LoginSignup
4
1
@srs(project_srs)

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

Last updated at Posted at 2023-09-06

環境

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

項目 値
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

model_plugin.gif

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

参考

system pluginパス

目次ページへのリンク

ROS2講座の目次へのリンク

4
1
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
4
1