ROS講座106 GazeboのModelPluginを作る

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 20.04 |
| ROS | Noetic |
| Gazebo | 11.9.0 |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要
今までロボットを動かすために`DiffDrivePlugin`等のすでに用意されていたプラグインを使っていましたが、このようなプラグインを自分で記述することが出来ます。
特に駆動側のプラグインは自作したくなることがあります。その方法を説明します。

## Gazeboプラグインとは

Gazebo世界に作用したり、情報を取得する追加プログラムのpluginとして作成できます。主にロボットのアクチュエーター・センサーのシミュレーション用に用いられます。SDFファイルのどのタグに挿入するかで次の6種類があります。

* Model: モデルの設定が出来ます。jointを駆動することもできます。
* Sensor: センサー（カメラ、lidar等）のシミュレーションをできます。
* Visual: モデルの表示の変更が出来ます。
* World: シミュレーションの物理演算の設定や、複数のモデルにまたがる設定ができます。
* System: gzclientの表示の設定を扱えます。
* GUI: gzclientにGUI（ボタン等）を追加することが出来ます。

# プラグインの記述 

## pluginファイル

```c++:gazebo2_lecture/src/turret_plugin.hh
#pragma once

#include <ros/ros.h>
#include <geometry_msgs/Point.h>
#include <gazebo/common/Plugin.hh>
#include <gazebo/common/UpdateInfo.hh>
#include <gazebo/common/Time.hh>
#include <gazebo/physics/physics.hh>

namespace gazebo
{
class GAZEBO_VISIBLE TurretPlugin : public ModelPlugin{
    event::ConnectionPtr update_conn_;
public:
    TurretPlugin();
    ~TurretPlugin();

    void Load(physics::ModelPtr model, sdf::ElementPtr _sdf) override;
    void Reset() override;
    void OnUpdate();
    bool GetParam(sdf::ElementPtr sdf);
    void PointCallback(const geometry_msgs::Point& point_msg);

private: 
    physics::ModelPtr model_;
    physics::WorldPtr world_;
    common::Time last_time_;
    physics::LinkPtr link_;

    ros::NodeHandle nh_;
    ros::Subscriber point_sub_;

    std::string topic_name_;
    std::string yaw_joint_name_;
    std::string pitch_joint_name_;
    float yaw_p_;
    float pitch_p_;
    float yaw_target_;
    float pitch_target_;
};
}
```

```c++:gazebo2_lecture/src/turret_plugin.cc
#include "turret_plugin.hh"
#include <gazebo/common/Events.hh>
#include <cmath>

namespace gazebo
{
GZ_REGISTER_MODEL_PLUGIN(TurretPlugin)

TurretPlugin::TurretPlugin() : nh_()
{
}
TurretPlugin::~TurretPlugin()
{
}

void TurretPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  gzmsg << "Load TurretPlugin\n";
  GetParam(_sdf);
  model_ = _model;
  world_ = _model->GetWorld();
  link_ = _model->GetLink("link");
  update_conn_ = event::Events::ConnectWorldUpdateBegin(std::bind(&TurretPlugin::OnUpdate, this));
  point_sub_ = nh_.subscribe(topic_name_, 10, &TurretPlugin::PointCallback, this);
}

void TurretPlugin::OnUpdate()
{
#if GAZEBO_MAJOR_VERSION >= 8
  common::Time current_time = world_->SimTime();
#else
  common::Time current_time = world_->GetSimTime();
#endif
  if ((current_time - last_time_).Double() > (1.0 / 2.0))
  {
    last_time_ = current_time;
#if GAZEBO_MAJOR_VERSION >= 8
    ignition::math::Pose3d pose = this->link_->WorldPose();
#else
    ignition::math::Pose3d pose = this->link_->GetWorldPose().Ign();
#endif
    printf("pos: %f %f %f\n", pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z());
    printf("rot: %f %f %f\n", pose.Rot().Roll(), pose.Rot().Pitch(), pose.Rot().Yaw());
  }

  auto yaw = model_->GetJoint(yaw_joint_name_);
  auto pitch = model_->GetJoint(pitch_joint_name_);
#if GAZEBO_MAJOR_VERSION >= 8
  yaw->SetVelocity(0, -yaw_p_ * (yaw->Position(0) - yaw_target_));
  pitch->SetVelocity(0, -pitch_p_ * (pitch->Position(0) - pitch_target_));
#else
  yaw->SetVelocity(0, -yaw_p_ * (yaw->GetAngle(0).Radian() - yaw_target_));
  pitch->SetVelocity(0, -pitch_p_ * (pitch->GetAngle(0).Radian() - pitch_target_));
#endif
}

void TurretPlugin::Reset()
{
}

void TurretPlugin::PointCallback(const geometry_msgs::Point& point_msg)
{
  float hlen = sqrt(point_msg.x * point_msg.x + point_msg.y * point_msg.y);
  if (0.1 <= hlen)
  {
    yaw_target_ = atan2(point_msg.y, point_msg.x);
    pitch_target_ = -atan2(point_msg.z, hlen);
  }
}

bool TurretPlugin::GetParam(sdf::ElementPtr sdf)
{
  if (!sdf->HasElement("topic_name"))
  {
    gzmsg << "topic_name not set\n";
    return false;
  }
  else
  {
    topic_name_ = sdf->GetElement("topic_name")->Get<std::string>();
  }

  if (!sdf->HasElement("yaw_joint_name"))
  {
    gzmsg << "yaw_joint_name not set\n";
    return false;
  }
  else
  {
    yaw_joint_name_ = sdf->GetElement("yaw_joint_name")->Get<std::string>();
  }
  if (!sdf->HasElement("yaw_p"))
  {
    gzmsg << "yaw_p not set\n";
    return false;
  }
  else
    yaw_p_ = sdf->GetElement("yaw_p")->Get<float>();

  if (!sdf->HasElement("pitch_joint_name"))
  {
    gzmsg << "pitch_joint_name not set\n";
    return false;
  }
  else
  {
    pitch_joint_name_ = sdf->GetElement("pitch_joint_name")->Get<std::string>();
  }
  if (!sdf->HasElement("pitch_p"))
  {
    gzmsg << "pitch_p not set\n";
    return false;
  }
  else
    pitch_p_ = sdf->GetElement("pitch_p")->Get<float>();

  return true;
}
}  // end namespace gazebo
```

* 必須なのが`Load()`関数です。pluginがロードされたときに呼ばれます。ここで初期化をします。
* Load()関数の中でコールバック関数(`OnUpdate()`)を登録します。コールバック関数はgazeboのシミュレーションの更新のたび(初期設定だと1000回/秒)に呼ばれます。
* printf系は1000回/秒も呼ばれては大変なので上記のプログラムのように間引きます。
* Load関数の引数でsdfのパラメーターへアクセスするポインタとgazeboモデルへのアクセスをするポインタを取得できます。
* sdfパラメーターの操作
  * sdfでのパラメーターが存在するかは`sdf->HasElement("pitch_joint_name")‘のようにしてboolで判定できます。
  * 実際の値は`sdf->GetElement("pitch_joint_name")->Get<std::string>()`のようにして値を取得できます。
* gazeboモデルへの操作
  * `model_->GetJoint(joint_name)`でモデル中のjointへのアクセスするポインタを取得できます。ジョイントの名前が間違っているとNullが返ってきてそれを使おうとするとexceptionが起きるので注意しましょう。
  * `joint_ptr->GetAngle(0).Radian()`でjointの角度を取得できます。`GetAngle(0)`の0はjointの中の軸のindexです。2軸以上に曲がるジョイント(universalとかballとか)はここのindexでどの方向化を指定します。
  * `joint_ptr->SetVelocity(value)`でjointの回転の速度を指定できます。`SetForce()`など力の次元でも指定できます。

### CmakeListsの例
gazeboプラグインをビルドには以下の要素が必須です。

```cmake:cmakeの一部
cmake_minimum_required(VERSION 2.8.3)
project(gazebo_plugin)

add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS
  gazebo_plugins
)
find_package(gazebo REQUIRED)

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
  ${catkin_INCLUDE_DIRS}
  ${GAZEBO_INCLUDE_DIRS}
)
add_library(TurretPlugin SHARED 
  src/turret_plugin.cc
)
target_link_libraries(MyPlugin 
  ${catkin_LIBRARIES}
  ${GAZEBO_LIBRARIES}
)
```

* `add_compile_options(-std=c++11)`が必須です。
* gazebo関連のリンクが必要です。
  * `find_package(gazebo REQUIRED)`でcmakeパッケージを読み込みます。
  * `include_directories(${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})`でincludeディレクトリを追加します
  * `add_library(TurretPlugin SHARED src/turret_plugin.cc)`でライブラリのビルドを追加します。pluginlibと違い1つのsoファイルには1つのpluginのみを入れます。
  * `target_link_libraries(MyPlugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})`でライブラリをリンクします。

## ビルド

```shell
cd ~/catkin_ws
catkin build
```

# シミュレーションの起動

## ソースコード

[model.sdf](gazebo2_lecture/models/turret_robot/model.sdf)

modelプラグインは以下のようにmodelタグのの中で設定します。filenameのところには`lib<ライブラリ名>.so`と入れます。ライブラリ名はCmakeListsの中で定義したものです。
`plugin`タグの中で記述した要素がpluginのにsdf構造体へのポインタとして渡されます。

```xml:gazebo2_lecture/models/robot2/model.sdfの一部
<?xml version='1.0'?>
<sdf version="1.4">
  <model name="robot2">

    <!-- 省略 -->

    <plugin name="turret_controller" filename="libTurretPlugin.so">
      <topic_name>cmd_pos</topic_name>
      <yaw_joint_name>yaw_joint</yaw_joint_name>
      <yaw_p>1.0</yaw_p>
      <pitch_joint_name>pitch_joint</pitch_joint_name>
      <pitch_p>1.0</pitch_p>
    </plugin>
  </model>
</sdf>
```

## 実行

```shell:実行
roslaunch gazebo_ros empty_world.launch world_name:=turret_robot.world.world verbose:=true
```

`/cmd_pos`にtopicを投げると動きます。

```shell:rostopicの送信
ostopic pub -1 /cmd_pos geometry_msgs/Point {2.0,1.0,1.0}
```

![sim3_robot2.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/24d85961-6d27-43d7-fb54-6905dae99db2.gif)


# 参考
[plugin tutorial](https://classic.gazebosim.org/tutorials?tut=plugins_hello_world&cat=write_plugin)
[SDF tutorial](http://gazebosim.org/tutorials/?tut=build_robot)
[gazebo plugin tutorial](http://gazebosim.org/tutorials/?tut=plugins_hello_world)
[model plugin tutorial](http://gazebosim.org/tutorials?tut=plugins_model&cat=write_plugin)
[joint class referrence](http://osrf-distributions.s3.amazonaws.com/gazebo/api/1.4.0/classgazebo_1_1physics_1_1Joint.html)

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)