Ignition講座03 SystemプラグインでModelを駆動する

# 環境

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

| 項目 |	値 |
|:--|:--|
| CPU | Core i5-3470  |
| Ubuntu | 22.04 |
| GPU| GTX 1050 Ti |
| GPU driver | 535.86.05  |
| ROS2 | Humble |
| Ignition Gazebo| Fortress |

# 概要

pluginをさせることでモデルをプログラムから操作することが出来ます。gazebo classicではmodel pluginという専用のplugin基本型がありましたが、ignition gazeboではsystem pluginに統合されています。

## C++コード

```c++:ignition_plugin_lecture/src/rotate_axis/rotate_axis.hpp
#include <ignition/gazebo/System.hh>
#include <ignition/gazebo/Model.hh>
#include <ignition/transport/Node.hh>

namespace iginition_plugin_lecture
{
  class RotateAxis:
    public ignition::gazebo::System,
    public ignition::gazebo::ISystemConfigure,
    public ignition::gazebo::ISystemPreUpdate,
    public ignition::gazebo::ISystemUpdate,
    public ignition::gazebo::ISystemPostUpdate
  {
    public: 
        RotateAxis();
        ~RotateAxis() override;
        void Configure(const ignition::gazebo::Entity &_entity,
            const std::shared_ptr<const sdf::Element> &_sdf,
            ignition::gazebo::EntityComponentManager &_ecm,
            ignition::gazebo::EventManager &_eventMgr) override;
        void PreUpdate(const ignition::gazebo::UpdateInfo &_info,
                ignition::gazebo::EntityComponentManager &_ecm) override;
        void Update(const ignition::gazebo::UpdateInfo &_info,
                ignition::gazebo::EntityComponentManager &_ecm) override;
        void PostUpdate(const ignition::gazebo::UpdateInfo &_info,
                const ignition::gazebo::EntityComponentManager &_ecm) override;
    private:
      void CreateIgnitionIf(void);
      void OnSpeedMessage(const ignition::msgs::Float & msg);

      ignition::gazebo::Model model_;
      ignition::transport::Node node_;
      std::string target_joint_name_{""};
      float target_speed_{0.0f};
  };
}
```

* `ignition::gazebo::System`を継承したpluginを作成します。
* system pluginでは4つのタイミングで処理を実行できます。必要な物だけ継承をします。
    * `ISystemConfigure`: シミュレーション起動時(`Configure()`)
    * `ISystemPreUpdate`: ワールド更新前(`PreUpdate()`)、軸の速度の設定などを行います。
    * `ISystemUpdate`: ワールド更新中(`Update()`)
    * `ISystemPostUpdate`: ワールド更新後(`PostUpdate()`)、テレメトリーの出力等を行います。

```c++:ignition_plugin_lecture/src/rotate_axis/rotate_axis.cpp
#include "rotate_axis.hpp"
#include <ignition/gazebo/components/JointVelocityCmd.hh>
#include <ignition/msgs/float.pb.h>

namespace iginition_plugin_lecture
{

RotateAxis::RotateAxis()
{
  CreateIgnitionIf();
}

RotateAxis::~RotateAxis()
{
}

void RotateAxis::Configure(const ignition::gazebo::Entity &_entity,
    const std::shared_ptr<const sdf::Element> &_sdf,
    ignition::gazebo::EntityComponentManager &_ecm,
    ignition::gazebo::EventManager &_eventMgr)
{
  (void)_ecm;
  (void)_eventMgr;
  model_ = ignition::gazebo::Model(_entity);

  auto ptr = const_cast<sdf::Element *>(_sdf.get());
  sdf::ElementPtr target_joint_elem = ptr->GetElement("target_joint");
  if (target_joint_elem) {
    target_joint_name_ = target_joint_elem->Get<std::string>();
  } else {
    ignerr << "sdf target_joint not found" << std::endl;
  }
}

void RotateAxis::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
    ignition::gazebo::EntityComponentManager &_ecm)
{
    (void)_info;
    (void)_ecm;
    ignition::gazebo::Entity joint = model_.JointByName(_ecm, target_joint_name_);
    if (joint == ignition::gazebo::kNullEntity){
      ignerr << target_joint_name_ <<" not found" << std::endl;
      return;
    }

    auto vel = _ecm.Component<ignition::gazebo::components::JointVelocityCmd>(joint);
    if (vel != nullptr)
    {
      *vel = ignition::gazebo::components::JointVelocityCmd({target_speed_});
    }
    else {
      _ecm.CreateComponent(joint, ignition::gazebo::components::JointVelocityCmd({target_speed_}));
    }
 }

void RotateAxis::Update(const ignition::gazebo::UpdateInfo &_info,
    ignition::gazebo::EntityComponentManager &_ecm)
{
  (void)_info;
  (void)_ecm;
}

void RotateAxis::PostUpdate(const ignition::gazebo::UpdateInfo &_info,
    const ignition::gazebo::EntityComponentManager &_ecm)
{
  (void)_info;
  (void)_ecm;
}

void RotateAxis::CreateIgnitionIf(void){
  this->node_.Subscribe("target_speed", &RotateAxis::OnSpeedMessage, this);
}

void RotateAxis::OnSpeedMessage(const ignition::msgs::Float & msg)
{
  target_speed_ = msg.data();
}

}

#include <ignition/plugin/Register.hh>
IGNITION_ADD_PLUGIN(
    iginition_plugin_lecture::RotateAxis,
    ignition::gazebo::System,
    iginition_plugin_lecture::RotateAxis::ISystemConfigure,
    iginition_plugin_lecture::RotateAxis::ISystemPreUpdate,
    iginition_plugin_lecture::RotateAxis::ISystemUpdate,
    iginition_plugin_lecture::RotateAxis::ISystemPostUpdate)
```

* `Configure()`ではsdfで記述した子要素の値を受け取ります。
* 受信コールバックに登録した`OnSpeedMessage()`で角速度の指令値を受け取ります。
* `model_.JointByName(_ecm, "joint_name");`で、過去に設定したジョイントの設定が取れます。
* 空でないときは`JointVelocityCmd()`で上書きをします。そうでないときは`CreateComponent()`でリストに追加します。

## 設定ファイル

```xml:ignition_lecture/worlds/model_example1.sdf
<?xml version="1.0" ?>

<sdf version="1.6">
  <world name="visualize_lidar_world">
    
    <gui fullscreen="0">
      <plugin filename="GzScene3D" name="3D View">
        <camera_pose>10 0 6 0 0.5 3.14</camera_pose>
        <ignition-gui>
          <title>3D View</title>
          <property type="bool" key="showTitleBar">false</property>
          <property type="string" key="state">docked</property>
        </ignition-gui>
      </plugin>

      <plugin filename="SwitchPanel" name="iginition_plugin_lecture::SwitchPanel">
        <forward_speed>0.3</forward_speed>
        <reverse_speed>-0.1</reverse_speed>
        <ignition-gui>
          <title>SwitchPanel</title>
          <property type="bool" key="showTitleBar">true</property>
          <property type="string" key="state">docked</property>
          <property key="resizable" type="bool">true</property>
        </ignition-gui>
      </plugin>
    </gui>

    <physics name="1ms" type="ignored">
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1.0</real_time_factor>
    </physics>
    <plugin
      filename="libignition-gazebo-physics-system.so"
      name="ignition::gazebo::systems::Physics">
    </plugin>
    <plugin
      filename="libignition-gazebo-sensors-system.so"
      name="ignition::gazebo::systems::Sensors">
      <render_engine>ogre2</render_engine>
    </plugin>
    <plugin
      filename="libignition-gazebo-scene-broadcaster-system.so"
      name="ignition::gazebo::systems::SceneBroadcaster">
    </plugin>

    <light name="sun" type="directional">
      <cast_shadows>true</cast_shadows>
      <pose>0 0 10 0 0 0</pose>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.2 0.2 0.2 1</specular>
      <attenuation>
        <range>1000</range>
        <constant>0.9</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <direction>-0.5 0.1 -0.9</direction>
    </light>

    <model name="ground_plane">
      <static>true</static>
      <pose>0 0 0 0 0 0</pose>
      <link name="link">
        <collision name="collision">
          <geometry>
            <box>
              <size>20 20 0.1</size>
            </box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>20 20 0.1</size>
            </box>
          </geometry>
          <material>
            <ambient>0.8 0.8 0.8 1</ambient>
            <diffuse>0.8 0.8 0.8 1</diffuse>
            <specular>0.8 0.8 0.8 1</specular>
          </material>
        </visual>
      </link>
    </model>

    <model name="box">
      <plugin filename="libRotateAxis.so" name="iginition_plugin_lecture::RotateAxis">
        <target_joint>arm_joint</target_joint>
      </plugin>

      <pose>0 -1 0.5 0 0 0</pose>
      <link name="box_link">
        <inertial>
          <inertia>
            <ixx>1</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>1</iyy>
            <iyz>0</iyz>
            <izz>1</izz>
          </inertia>
          <mass>1.0</mass>
        </inertial>
        <collision name="box_collision">
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
        </collision>

        <visual name="box_visual">
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
          <material>
            <ambient>1 0 0 1</ambient>
            <diffuse>1 0 0 1</diffuse>
            <specular>1 0 0 1</specular>
          </material>
        </visual>
      </link>

      <joint name="arm_joint" type="revolute">
        <pose>0 0 -0.5 0 0 0</pose>
        <parent>box_link</parent>
        <child>arm_link</child>
        <axis>
          <xyz>1 0 0</xyz>
          <limit>
            <upper>1.57</upper>
            <lower>-1.57</lower>
            <effort>1.0</effort>
          </limit>
        </axis>
      </joint>

      <link name="arm_link">
        <pose>0 0 1.2 0 0 0</pose>
        <inertial>
          <inertia>
            <ixx>0.008</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>0.008</iyy>
            <iyz>0</iyz>
            <izz>0.00016</izz>
          </inertia>
          <mass>0.1</mass>
        </inertial>
        <collision name="arm_collision">
          <geometry>
            <box>
              <size>0.1 0.1 1</size>
            </box>
          </geometry>
        </collision>

        <visual name="arm_visual">
          <geometry>
            <box>
              <size>0.1 0.1 1</size>
            </box>
          </geometry>
          <material>
            <ambient>1 0 0 1</ambient>
            <diffuse>1 0 0 1</diffuse>
            <specular>1 0 0 1</specular>
          </material>
        </visual>
      </link>
    </model>

    <include>
      <pose>5 0 0 0 0 1.57</pose>
      <uri>https://fuel.gazebosim.org/1.0/nate/models/Valve</uri>
    </include>

    <include>
      <uri>package://ignition_lecture/models/my_box</uri>
      <pose>0 2 0 0 0 0</pose>
    </include>

  </world>
</sdf>
```

* `<model>`要素直下の`<plugin>`でプラグインの設定を書きます。`filename`でshared object名を、`name`ではclass名を指定します。

## CMakeList

```cmake:ignition_plugin_lecture/CMakeLists.txt
cmake_minimum_required(VERSION 3.8)
project(ignition_plugin_lecture)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

set(CMAKE_AUTOMOC ON)

find_package(ament_cmake REQUIRED)
find_package(ignition-plugin1 REQUIRED COMPONENTS register)
find_package(ignition-gazebo6 REQUIRED)

include_directories(SYSTEM
  ${IGNITION-COMMON_INCLUDE_DIRS}
  ${IGNITION-GUI_INCLUDE_DIRS}
)

# gui plugin
qt5_add_resources(resources_rcc src/switch_panel/SwitchPanel.qrc)
add_library(SwitchPanel SHARED
  src/switch_panel/SwitchPanel.cpp
  ${resources_rcc}
)
target_link_libraries(SwitchPanel
  ${IGNITION-COMMON_LIBRARY_DIRS}
  ${IGNITION-GUI_LIBRARIES}
)

# system plugin
add_library(RotateAxis SHARED
  src/rotate_axis/rotate_axis.cpp
)
target_link_libraries(RotateAxis
  ${IGNITION-PLUGIN_LIBRARIES}
  ${IGNITION-GAZEBO_LIBRARIES}
)

install(
  TARGETS SwitchPanel RotateAxis
  LIBRARY DESTINATION lib
)

ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in")

ament_package()
```

## ビルド&実行

```shell:ビルド
source /opt/ros/humble/setup.bash
cd ros2_ws
colcon build
```

```shell:実行
source ~/ros2_ws/install/setup.bash
ros2 launch ignition_lecture gazebo.launch.py world:=model_example1.sdf
```

![model_plugin.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/bbe38baf-3ab5-31ab-a876-f4f79a337b17.gif)

前に作成したGUIプラグインで指定した角速度で軸が動きます。

# 参考
[system pluginパス](https://gazebosim.org/api/gazebo/6.0/createsystemplugins.html)

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