ROS抜きでGazeboのモデルを動かす


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

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

環境はGazebo9を想定。

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

# 流れ

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

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

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

# 準備

```bash
$ mkdir standalone_plugin
```

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

# プラグイン作成

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

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

```cpp: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();
};

}
```

```cpp: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()`にラムダ式を渡すことはできないようだ。

```cmake: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でも十分動くはずだ。

試しにビルドしてみる

```bash
$ mkdir build
$ cd build
$ cmake ..
$ make
```

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

# モデル作成

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

```bash
GAZEBO_PLUGIN_PATH=build gazebo --verbose
```

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

モデルはこんな感じ。

![Screenshot from 2019-03-11 14-40-44.png](https://qiita-image-store.s3.amazonaws.com/0/39647/a9fcec67-73a4-ec3c-6bd4-f7f08f8fd7ed.png)

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

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


![select_plugin.png](https://qiita-image-store.s3.amazonaws.com/0/39647/501ae560-0aa1-ce5a-a188-4673be0bc842.png)


モデルを保存して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で送ることにする

```proto:standalone_plugin/msgs/velocity.proto
syntax = "proto2";
package standalone_plugin.msgs;

message Velocity
{
  required int32 vel = 1;
}
```

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

CMakeLists.txtは以下の通り

```cmake: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)`を追加して

```diff: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*というパスでメッセージが送られてくることを仮定。

```cpp:standalone_plugin/plugin.cc
    node_ = gazebo::transport::NodePtr(new gazebo::transport::Node());
    node_->Init();

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

## `StandalonePlugin::CbMsgVelocity()`

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

```cpp:standalone_plugin/plugin.hh
using MsgVelocityPtr = const boost::shared_ptr<const standalone_plugin::msgs::Velocity>;
```

```cpp:standalone_plugin/plugin.cc
void StandalonePlugin::CbMsgVelocity(MsgVelocityPtr& recv_vel) {
    vel_ = recv_vel->vel();
}
```

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


## `StandalonePlugin::OnUpdate()`

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

```cpp: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`は忘れないように。

```cmake: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するコードを書いてみた。

```cpp: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に追加してビルド

```diff: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起動してもでるを配置した後実行する。

```bash
$ ./publish_vel
```

動いた。

![standalone.gif](https://qiita-image-store.s3.amazonaws.com/0/39647/d0edf5ea-4371-3602-1afc-0b64850fd4f3.gif)