環境

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

項目	値
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++コード

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())、テレメトリーの出力等を行います。

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()でリストに追加します。

設定ファイル

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

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()

ビルド&実行

ビルド

source /opt/ros/humble/setup.bash
cd ros2_ws
colcon build

実行

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

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

参考

system pluginパス

目次ページへのリンク

ROS2講座の目次へのリンク