ROSアプリの検証によく使われる3DシミュレーターGazeboであるが、Gazebo自体はROSとは独立しているため単体でも動作は可能である。

手っ取り早くはGazeboモデルがsubscribeする*/gazebo/default/<モデル名>/joint_cmd*に対してコマンドを投げるgz jointコマンドが使用可能であるが、この記事ではいろいろ細かい制御が可能となるプラグインを作成して動作させてみることにする。

環境はGazebo9を想定。

リポジトリはこちら
https://github.com/maueki/example_gazebo_standalone_plugin

流れ

作るものはメッセージ、モデル、プラグインの３つ。
このプラグインが

Gazeboメッセージ(≠ROSメッセージ)を受信し
シミュレーターのアップデートごとにコールバックを仕掛けて
コールバック内で受信したモーター値をモデルに設定

することで動作をおこなう。

準備

$ mkdir standalone_plugin

以下このディレクトリで作業をおこなう

プラグイン作成

Gazeboプラグインを作ったこと無いという人は以下を読んでおくと良い。
http://gazebosim.org/tutorials/?tut=plugins_hello_world

まずはシミュレーター更新毎にコールバックを呼び出すだけのプラグインを作ってみる。

standalone_plugin/plugin.hh

# pragma once

# include <gazebo/common/Plugin.hh>
# include <gazebo/common/UpdateInfo.hh>

namespace gazebo
{

class GAZEBO_VISIBLE StandalonePlugin : public ModelPlugin
{
    event::ConnectionPtr update_conn_;
public:
    StandalonePlugin();
    ~StandalonePlugin();

    void Load(physics::ModelPtr model, sdf::ElementPtr _sdf) override;
    void Reset() override;

    void OnUpdate();
};

}

standalone_plugin/plugin.cc

# include "plugin.hh"
# include <gazebo/common/Events.hh>

namespace gazebo {

GZ_REGISTER_MODEL_PLUGIN(StandalonePlugin)

StandalonePlugin::StandalonePlugin(){}
StandalonePlugin::~StandalonePlugin(){}

void StandalonePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
    gzmsg << "Load StandalonePlugin\n";

    update_conn_ = event::Events::ConnectWorldUpdateBegin(std::bind(&StandalonePlugin::OnUpdate, this));
}

void StandalonePlugin::OnUpdate() {
    gzmsg << "call OnUpdate\n";
}

void StandalonePlugin::Reset() {
}

}

プラグインのロード時に呼ばれるLoad()でコールバックの設定をおこなっている。
残念ながらevent::Events::ConnectWorldUpdateBegin()にラムダ式を渡すことはできないようだ。

standalone_plugin/CMakeLists.txt

# 2.8.8 required to use PROTOBUF_IMPORT_DIRS
cmake_minimum_required(VERSION 2.8.8 FATAL_ERROR)
set (CMAKE_CXX_FLAGS "-g -Wall -std=gnu++17")

include (FindPkgConfig)
pkg_check_modules(GAZEBO gazebo)
pkg_check_modules(SDF sdformat)

include_directories(
  ${GAZEBO_INCLUDE_DIRS}
  )

add_library(StandalonePlugin SHARED plugin.cc )

ここではC++17を指定しているがC++11でも十分動くはずだ。

試しにビルドしてみる

$ mkdir build
$ cd build
$ cmake ..
$ make

問題なければlibStandalonePlugin.soができているはずだ。

モデル作成

作業ディレクトリ直下で以下のコマンドでGazeboを起動する。

GAZEBO_PLUGIN_PATH=build gazebo --verbose

プラグインが見えるようにGAZEBO_PLUGIN_PATHを指定。
--verboseはgzmsgの出力を確認したいため。

モデルはこんな感じ。

四輪
今回は実装しないが操舵も見据えてジョイント種はUniversal
Axis1: Z, Axis2: -Y
ジョイント名はそれぞれ、front_right, front_left, rear_right, rear_left

そして「Addボタン」から先程作成したプラグインを指定する。

モデルを保存してGazeboを再起動し、作成したモデルを置いてやると以下のように出力されプラグインが動作していることがわかる。

[Msg] Load StandalonePlugin
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
...

　メッセージ

メッセージについてのチュートリアルは以下、一読しておくと良い
http://gazebosim.org/tutorials?tut=custom_messages

今回はとりあえず速度([rad/s])をint32で送ることにする

standalone_plugin/msgs/velocity.proto

syntax = "proto2";
package standalone_plugin.msgs;

message Velocity
{
  required int32 vel = 1;
}

GazeboのメッセージはROSのメッセージとは異なりProtobufフォーマットで定義するようだ。

CMakeLists.txtは以下の通り

standalone_plugin/msgs/CMakeLists.txt

cmake_minimum_required(VERSION 3.0)
find_package(Protobuf REQUIRED)

include (FindPkgConfig)
pkg_check_modules(GAZEBO gazebo)

set (msgs
  velocity.proto
)

PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS ${msgs})

add_library(msgs SHARED ${PROTO_SRCS})
target_link_libraries(msgs ${PROTOBUF_LIBRARY})

作業ディレクトリ直下のCMakeLists.txtにadd_subdirectory(msgs)を追加して

standalone_plugin/CMakeLists.txt

@@ -10,4 +10,6 @@ include_directories(
   ${GAZEBO_INCLUDE_DIRS}
   )
 
+add_subdirectory(msgs)
+
 add_library(StandalonePlugin SHARED plugin.cc )

ビルドできることを確認する。ビルドが成功すればbuild/msgs/libmsgs.soができているはず。

Gazeboメッセージ受信と速度指定

コードは以下
https://github.com/maueki/example_gazebo_standalone_plugin/blob/master/plugin.hh
https://github.com/maueki/example_gazebo_standalone_plugin/blob/master/plugin.cc

`StandalonePlugin::Load()`

定義したメッセージを受け取るため、subscribeをおこなう。
ここでは*/velocity*というパスでメッセージが送られてくることを仮定。

standalone_plugin/plugin.cc

    node_ = gazebo::transport::NodePtr(new gazebo::transport::Node());
    node_->Init();

    sub_ = node_->Subscribe("/velocity", &StandalonePlugin::CbMsgVelocity, this);

`StandalonePlugin::CbMsgVelocity()`

Gazeboメッセージで送られてくる速度を受信しメンバに格納する。

standalone_plugin/plugin.hh

using MsgVelocityPtr = const boost::shared_ptr<const standalone_plugin::msgs::Velocity>;

standalone_plugin/plugin.cc

void StandalonePlugin::CbMsgVelocity(MsgVelocityPtr& recv_vel) {
    vel_ = recv_vel->vel();
}

StandalonePlugin::OnUpdate()と同一スレッドで動作するか不明だったためvel_はstd::atomic<int32_t>型にしてある。

`StandalonePlugin::OnUpdate()`

速度値をジョイントに設定する。

standalone_plugin/plugin.cc

void StandalonePlugin::OnUpdate() {
    model_->GetJoint("front_right")->SetVelocity(0, vel_);
    model_->GetJoint("front_left")->SetVelocity(0, vel_);
    model_->GetJoint("rear_right")->SetVelocity(0, vel_);
    model_->GetJoint("rear_left")->SetVelocity(0, vel_);
}

SetVelocity()の第一引数は軸のインデックス。

CMakeLists.txt

特筆すべきものはないが、target_link_librariesは忘れないように。

standalone_plugin/CMakeLists.txt

# 2.8.8 required to use PROTOBUF_IMPORT_DIRS
cmake_minimum_required(VERSION 2.8.8 FATAL_ERROR)
set (CMAKE_CXX_FLAGS "-g -Wall -std=gnu++17")

include (FindPkgConfig)
pkg_check_modules(GAZEBO gazebo)
pkg_check_modules(SDF sdformat)

include_directories(
  ${GAZEBO_INCLUDE_DIRS}
  ${CMAKE_CURRENT_BINARY_DIR}/msgs
  )
link_directories(${GAZEBO_LIBRARY_DIRS} ${CMAKE_CURRENT_BINARY_DIR}/msgs)

add_subdirectory(msgs)

add_library(StandalonePlugin SHARED plugin.cc )
target_link_libraries(StandalonePlugin ${GAZEBO_LIBRARIES} msgs)

実行

さて準備ができたので動作させたいわけだが、ROSと異なりGazeboメッセージはターミナルからpublishする手段がどうやら無いらしい。

仕方ないので、publishするコードを書いてみた。

standalone_plugin/publish_vel.cc

# include <gazebo/transport/transport.hh>
# include <gazebo/msgs/msgs.hh>
# include <gazebo/gazebo_client.hh>

# include "velocity.pb.h"

int main(int _argc, char **_argv)
{
  // Load gazebo
  gazebo::client::setup(_argc, _argv);

  // Create our node for communication
  gazebo::transport::NodePtr node(new gazebo::transport::Node());
  node->Init();

  auto adv = node->Advertise<standalone_plugin::msgs::Velocity>("/velocity");

  gazebo::common::Time::MSleep(1000);
  standalone_plugin::msgs::Velocity vel;
  vel.set_vel(1);
  adv->Publish(vel);

  // Make sure to shut everything down.
  gazebo::client::shutdown();
}

CMakeLists.txtに追加してビルド

standalone_plugin/CMakeLists.txt

@@ -17,3 +17,5 @@ add_subdirectory(msgs)
 add_library(StandalonePlugin SHARED plugin.cc )
 target_link_libraries(StandalonePlugin ${GAZEBO_LIBRARIES} msgs)
 
+add_executable(publish_vel publish_vel.cc msgs)
+target_link_libraries(publish_vel ${GAZEBO_LIBRARIES} msgs)

gazebo起動してもでるを配置した後実行する。

$ ./publish_vel

動いた。