🐋

ROS2: gazebo_ros2_controlでGazeboとMoveitを連携する

2022/10/06に公開

海洋ロボコンをやってた人です。
今回は、URDFで記述したオリジナルロボットをGazeboシミュレーターと連携して動かせるようにする方法を紹介します。

Gazeboはじめましての方のお力添えになれば幸いです。


※2023/02/26追記: 本記事の「ROS2」表記、正しくは「ROS 2」です。

ROS2 gazebo_ros2_controlでシミュレーションする

今回は、前回・前々回で紹介したURDFおよび、MoveitパッケージをGazeboと連携してシミュレーターで動かす方法を紹介します。

Moveitパッケージの作成方法は下記よりご覧ください。
https://zenn.dev/tasada038/articles/57e446285e60de

この記事を読むことで、下図のように、自身のオリジナルロボットをMoveitを用いてGazebo上で動かせるようになります。

また使用するモデルは、アイコンでも表示されているマンタ型ロボットで紹介していきます。
なお、ロボットのハード設計からソフト設計まで全て私が担当しており、所属研究室の教授からも承諾を得ています。

本記事のプログラム

下記のGithubにて公開しているので、こちらを参考にしてください。
スター頂けると、大変嬉しいです・ω・

https://github.com/tasada038/manta_v2

Installation & Environment

はじめにROS経由でGazeboを使用するために、以下のパッケージをインストールします。

sudo apt install ros-$ROS_DISTRO-gazebo-ros2-control

sudo apt install ros-$ROS_DISTRO-gazebo-ros

sudo apt install ros-$ROS_DISTRO-xacro

また、実行テスト環境は

  • Ubuntu 20.04 Foxy
  • Gazebo 11
    です。

gazebo_ros2_controlパッケージを作成する

次にgazebo_ros2_control用のパッケージを作成します。
パッケージ名は任意だと思いますが、今回はxxxxx_gazebo_ros2_controlと作成します。

cd ~/xxxxx_ws
. install/setup.bash
ros2 pkg create --build-type ament_cmake xxxxx_gazebo_ros2_control

続いて、作成したパッケージ内に以下のフォルダを準備しましょう。

  • config
  • launch
  • urdf
  • worlds


まずはGazeboでロボットモデルを表示するためにURDFファイルを作成していきます。

Gazeboでは、xxxxx.xacroファイルを読み込むのが苦手なので、xxxxx.urdfを新たに作成します。

手順としては、前回作成したxxxxx_description/urdf下の

  • xxxxx_description.xacro
  • xxxxx.trans
  • xxxxx.gazebo
  • materials.xacro
    をxxxxx.urdf内に全てまとめれば大丈夫です。

また、GazeboとROS2を連携させるために、gazebo_ros2_controlプラグインとlibgazebo_ros2_control.soを追記します。

manta_v2_moveit.urdf
<?xml version="1.0" ?>

<robot name="manta_v2">
  <material name="silver">
    <color rgba="0.700 0.700 0.700 1.000"/>
  </material>
  <material name="white">
    <color rgba="1.0 1.0 1.0 1.0"/>
  </material>
  <material name="grey">
    <color rgba="0.2 0.2 0.2 1.0"/>
  </material>

  <transmission name="rev_link_head_tran">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="rev_link_head">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="rev_link_head_actr">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>  

  <!-- 略 -->
  <joint name="rev_link_Rbtm_roll1" type="revolute">
    <origin xyz="-0.056269 -0.104821 0.050256" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="link_Rbtm_roll1_1"/>
    <axis xyz="0.983165 -0.182721 -0.0"/>
    <limit upper="0.523599" lower="-0.523599" effort="100" velocity="100"/>
    <dynamics damping="0.1" friction="0.0"/>
  </joint>
	
  <!-- ros2_control -->
  <ros2_control name="GazeboSystem" type="system">
    <hardware>
      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
    </hardware>
  </ros2_control>
  <gazebo>
    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
      <robot_param>robot_description</robot_param>
      <robot_param_node>robot_state_publisher</robot_param_node>
      <parameters>$(find manta_v2_gazebo_ros2_control)/config/manta_v2_move_group_controller.yaml</parameters>
      <!-- <parameters>$(find manta_v2_gazebo_ros2_control)/config/manta_v2_trajectory_controller.yaml</parameters> -->
    </plugin>
  </gazebo>
</robot>


gazeboタグで記述するparametersは制御するcontrollerに応じて変えてもらえれば問題ありません。
ここでは、moveitのコントローラーを利用するので、moveit用のyamlファイルを宣言しています。

※2022/12/30追記

上記ではtransmissonタグでは、ユーザーが実装するHardware Components(Sensor, Actuator, System)の3種類の内から、Actuatorというコンポーネントを実装しており、Actuatorと指定することで、Command Interfaceをエクスポートします。

Actuatorの指定により、Command Interfaceをエクスポートできるようになったため、ros2_controlのControllerがハードウェア動作のアクセス権を得ることができ、command_interfaceにて各Controllerを動かすことができるようになります。

xacro import for Gazebo

また、The Constructらのグループで、xacro をGazeboでインポートする方法がいくつか見られたので、こちらも記載しておきます。

こちらの例では、以下を実行するとxacroをGazeboで表示できるようです。

ros2 launch box_car_gazebo box_bot_launch.py

プログラムとして、xacroを展開・列挙したものをurdfとして認識させている感じだと思います。
自分の環境ではxacroモデルをGazeboで表示することができなかったので、こちらは残念ながら断念しました。

gazeboパラメータについて

URDF内のgazeboタグには、linkやjointを正しくシミュレーションするためのパラメータを設定できます。

<gazebo reference="link_Rtop_roll1_1">
    <material>Gazebo/Grey</material>
    <mu1>500</mu1>
    <mu2>500</mu2>
    <kp>5000</kp>
    <kd>10</kd>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.001</minDepth>
    <selfCollide>true</selfCollide>
  </gazebo>


各パラメータの詳細は以下にまとまっているので、こちらも記載。

URDFでロボットモデルの作成

4脚ロボットでgazeboシミュレーション。歩行モーションを入れてみた。


  • configファイルの作成

続いて、先程urdfで定義したgazebo_ros2_controlの設定ファイルを作成していきます。

manta_v2_move_group_controller.yaml
controller_manager:
  ros__parameters:
    update_rate: 100  # Hz

    rtop_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    rmid_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    rbtm_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    joint_state_controller:
      type: joint_state_controller/JointStateController

rtop_controller:
  ros__parameters:
    joints:
      - rev_link_Rtop_roll1
      - rev_link_Rtop_pitch
      - rev_link_Rtop_roll2
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
rmid_controller:
  ros__parameters:
    joints:
      - rev_link_Rmid_roll1
      - rev_link_Rmid_pitch
      - rev_link_Rmid_roll2
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
rbtm_controller:
  ros__parameters:
    joints:
      - rev_link_Rbtm_roll1
      - rev_link_Rbtm_pitch
      - rev_link_Rbtm_roll2
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity

この設定ファイルがコントローラーの詳細になっています。


  • worldsファイルを作成する

Gazeboのworldsファイルもデフォルトの状態でいいので作成しておきます。
このworldsファイルのインクルード内容を変更してやれば、さまざまなモデルをGazebo内で表示できることになります。

basic.world
<?xml version="1.0" ?>
<sdf version="1.5">
  <world name="default">
    <include>
      <uri>model://ground_plane</uri>
    </include>
    <include>
      <uri>model://sun</uri>
    </include>
  </world>
</sdf>

Gazeboにオリジナルstlモデルを追加する場合は以下参照
https://zenn.dev/tasada038/articles/54f6111fb6e907


  • launchファイルをつくる
    ここまでできたら、gazeboとros2 controllerをローンチするプログラムを記述するだけです。
move_group_controller.launch.py
from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
import xacro
import os

def generate_launch_description():
    # Constants for paths to different files and folders
    gazebo_pkg_name = 'manta_v2_gazebo_ros2_control'
    robot_name_in_model = 'manta_v2'
    urdf_file_path = 'urdf/manta_v2_moveit.urdf'
    world_file_path = 'worlds/basic.world'
    # Pose where we want to spawn the robot
    spawn_x_val = '0.0'
    spawn_y_val = '0.0'
    spawn_z_val = '0.0'
    spawn_yaw_val = '0.0'

    gazebo_pkg_share = FindPackageShare(package=gazebo_pkg_name).find(gazebo_pkg_name)
    world_path = os.path.join(gazebo_pkg_share, world_file_path)

    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
            launch_arguments={'world': world_path}.items(),
    )

    urdf_model_path = os.path.join(gazebo_pkg_share, urdf_file_path)

    doc = xacro.parse(open(urdf_model_path))
    xacro.process_doc(doc)
    robot_description = {'robot_description': doc.toxml()}

    gazebo_robot_state_pub_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        output='screen',
        parameters=[robot_description]
    ) 

    spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
                        arguments=['-topic', 'robot_description',
                                   '-entity', robot_name_in_model,
                                   '-x', spawn_x_val,
                                   '-y', spawn_y_val,
                                   '-z', spawn_z_val,
                                   '-Y', spawn_yaw_val,
                                   ],
                        output='screen')

    load_joint_state_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', 'joint_state_broadcaster'],
        output='screen'
    )

    # load_joint_trajectory_controller = ExecuteProcess(
    #     cmd=['ros2', 'control', 'load_start_controller', 'joint_trajectory_controller'],
    #     output='screen'
    # )

    load_rtop_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_start_controller', 'rtop_controller'],
        output='screen'
    )
    load_rmid_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_start_controller', 'rmid_controller'],
        output='screen'
    )
    load_rbtm_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_start_controller', 'rbtm_controller'],
        output='screen'
    )
    load_ltop_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_start_controller', 'ltop_controller'],
        output='screen'
    )
    load_lmid_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_start_controller', 'lmid_controller'],
        output='screen'
    )
    load_lbtm_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_start_controller', 'lbtm_controller'],
        output='screen'
    )

    nodes = [
        # Gazebo
        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=spawn_entity,
                on_exit=[load_joint_state_controller],
            )
        ),
        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=load_joint_state_controller,
                on_exit=[
                    load_rtop_controller,
                    load_rmid_controller,
                    load_rbtm_controller,
                    load_ltop_controller,
                    load_lmid_controller,
                    load_lbtm_controller,
                    # load_joint_trajectory_controller,
                ],
            )
        ),
        gazebo,
        gazebo_robot_state_pub_node,
        spawn_entity,
    ]

    return LaunchDescription(nodes)


  • CMakeLists.txtの記述
    各フォルダを追加しているので、CMakeListsに依存関係も追記しておきます。
CMakeLists.txt
+install(DIRECTORY
+  urdf
+  launch
+  worlds
+  config
+  DESTINATION share/${PROJECT_NAME}/
+)

GazeboとMoveitの連携を確認する

buildを通したら、gazebo_ros2_controlのlaunchとMoveitのlaunchを実行してみましょう。

First Shell

ros2 launch manta_v2_gazebo_ros2_control move_group_controller.launch.py

Second Shell

ros2 launch manta_v2_moveit_config moveit_demo.launch.py


GazeboとRviz2が立ち上がったら、Plan & Executeで実際に動作確認して動けばOKです。

Gazebo関係のパス一覧メモ

  • Default Gazebo Plugins
    /usr/lib/x86_64-linux-gnu/gazebo11/Plugins

  • locate libgazebo_ros2_control.so
    /opt/ros/foxy/lib/libgazebo_ros2_control.so

  • Model
    Home/.gazebo/models  (Show Hidden Folder)

moveitのROS2パッケージを作成していれば、簡単にGazeboとつなげることができます。

ぜひ試してみてください。
以上。

Discussion