モチベーション
ros2_controlの仕様変更によってロボットシミュレータの修正が必要になったので、いっそのことGazebo ClassicからIgnition Gazeboに移行しようかと思い立って作業してみると、結構大変だったので共有兼備忘録として投稿します。
この記事でできること
- Gazebo Classicで使っていた２輪型移動ロボットのURDFをIgnition Gazeboで使えるようになる
- LiDAR、カメラ、デプスカメラといったセンサもIgnition Gazebo上でシミュレーションできるようになる
検証環境
- ソースコード：https://github.com/hijimasa/how-to-make-package-for-ignition-gazebo
- ROS2 バージョン：humble- Ignition Gazebo バージョン：fortress
- 特記事項：gz_ros2_controlはaptでは入らなかったので、githubからサブモジュールとして導入しています。
ign_ros2_controlがapt側に存在していることから、ign->gzへのリファクタリングの最中のようです（2023/4/9時点）。
ソースコードの構成
ソースコードは主に以下の4つのパッケージで構成されています。
- classic_diffbot_description
Gazebo Classicで動作する２輪型移動ロボットのURDFなどが格納されています。 - classic_diffbot_sim
Gazebo Classicでシミュレーションする際のlaunchファイルなどが格納されています。 - ignition_diffbot_description
Ignition Gazeboで動作する２輪型移動ロボットのURDFなどが格納されています。 - ignition_diffbot_sim
Ignition Gazeboでシミュレーションする際のlaunchファイルなどが格納されています。
また、以下のパッケージは実行時に補助する役割で含まれています。
- velocity_pub
teleop_twist_keyboard実行時に生成されるTwistメッセージをTwistStampedに変換します。
本題
Ignition Gazeboに移行するための作業は以下の3つです。
本稿では、classic_diffbotパッケージとignition_diffbotパッケージを比較して、変更内容を明らかにします。
(1) ros2_control設定の変更
ros2_controlに関する設定は、各descriptionパッケージのros2_controlディレクトリにxacroとして記述しています。
変更するのは、hardwareタグ内のプラグインです。
以下に２つのパッケージのhardwareタグの中身を示します。
classic_diffbot_description/ros2_control/difbot.ros2_control.xacro
<hardware> <xacro:if value="${use_fake_hardware}">
<plugin>fake_components/GenericSystem</plugin>
<param name="fake_sensor_commands">${fake_sensor_commands}</param>
<param name="state_following_offset">0.0</param>
</xacro:if>
<xacro:if value="${use_sim}">
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</xacro:if>
<xacro:unless value="${use_fake_hardware or use_sim}">
<!-- You can add your hardware to use real robot.
<plugin>your_hardware/YourHardware</plugin>
-->
</xacro:unless>
</hardware>
ignition_diffbot_description/ros2_control/difbot.ros2_control.xacro
<hardware>
<xacro:if value="${use_fake_hardware}">
<plugin>fake_components/GenericSystem</plugin>
<param name="fake_sensor_commands">${fake_sensor_commands}</param>
<param name="state_following_offset">0.0</param>
</xacro:if>
<xacro:if value="${use_sim}">
<plugin>gz_ros2_control/GazeboSimSystem</plugin>
</xacro:if>
<xacro:unless value="${use_fake_hardware or use_sim}">
<!--
<plugin>your_hardware/YourHardware</plugin>
-->
</xacro:unless>
</hardware>
少しわかりにくいですが、gazeboがgzに、GazeboSystemがGazeboSimSystemに変更になっています。
また、gazeboディレクトリ内のdiffbot.gazebo.xacroでも使用するプラグインに変更があります。
classic_diffbot_description/gazebo/diffbot.gazebo.xacro
<gazebo>
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
<parameters>$(find classic_diffbot_description)/config/diffbot_sim.yaml</parameters>
</plugin>
</gazebo>
ignition_diffbot_description/gazebo/diffbot.gazebo.xacro
<gazebo>
<plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
<parameters>$(find ignition_diffbot_description)/config/diffbot_sim.yaml</parameters>
</plugin>
</gazebo>
センサ設定の変更
ros2_controlでは使用するプラグインの名前を変更するだけでしたが、センサの場合はより多くの変更点があります。
大きな変更点としては、Gazebo Classicでは各センサごとにプラグインを記述する必要がありましたが、Ignition Gazeboでは1つのプラグインを最初に記述するだけになりました。具体的な例として、LiDAR、カメラ、デプスカメラの例を以下に示します。
-
LiDAR
classic_diffbot_description/gazebo/diffbot.gazebo.xacro<gazebo reference="lidar_link"> <sensor name="lidar" type="ray"> <always_on>true</always_on> <visualize>false</visualize> <pose>0.0 0 0.0 0 0 0</pose> <update_rate>30</update_rate> <ray> <scan> <horizontal> <samples>2800</samples> <resolution>1</resolution> <min_angle>-3.14</min_angle> <max_angle>3.14</max_angle> </horizontal> </scan> <range> <min>0.2</min> <max>30.0</max> <resolution>0.01</resolution> </range> <noise> <type>gaussian</type> <mean>0.0</mean> <stddev>0.01</stddev> </noise> </ray> <plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so"> <ros> <namespace>lidar</namespace> <remapping>~/out:=scan</remapping> </ros> <output_type>sensor_msgs/LaserScan</output_type> <frame_name>lidar_link</frame_name> </plugin> </sensor> <material>Gazebo/White</material> </gazebo>ignition_diffbot_description/gazebo/diffbot.gazebo.xacro<gazebo reference="lidar_link" > <sensor name="lidar_link" type="gpu_lidar"> <ignition_frame_id>lidar_link</ignition_frame_id> <topic>scan</topic> <update_rate>30</update_rate> <lidar> <scan> <horizontal> <samples>2800</samples> <resolution>1</resolution> <min_angle>-3.14</min_angle> <max_angle>3.14</max_angle> </horizontal> <vertical> <samples>1</samples> <resolution>0.1</resolution> <min_angle>0.0</min_angle> <max_angle>0.0</max_angle> </vertical> </scan> <range> <min>0.2</min> <max>30.0</max> <resolution>0.01</resolution> </range> <frame_id>/lidar_link</frame_id> </lidar> <alwaysOn>1</alwaysOn> <visualize>true</visualize> </sensor> </gazebo> -
カメラ
classic_diffbot_description/gazebo/diffbot.gazebo.xacro<gazebo reference="camera_link"> <sensor type="camera" name="camera"> <alwaysOn>true</alwaysOn> <visualize>true</visualize> <update_rate>10.0</update_rate> <camera name="diff_bot_camera"> <horizontal_fov>1.3962634</horizontal_fov> <image> <width>600</width> <height>600</height> <format>R8G8B8</format> </image> <clip> <near>0.02</near> <far>300</far> </clip> <noise> <type>gaussian</type> <mean>0.0</mean> <stddev>0.007</stddev> </noise> </camera> <plugin name="camera_controller" filename="libgazebo_ros_camera.so"> <cameraName>/camera</cameraName> <imageTopicName>image_raw</imageTopicName> <cameraInfoTopicName>camera_info</cameraInfoTopicName> <frameName>camera_link</frameName> <hackBaseline>0.07</hackBaseline> </plugin> </sensor> <material>Gazebo/White</material> </gazebo>ignition_diffbot_description/gazebo/diffbot.gazebo.xacro<gazebo reference="camera_link" > <sensor name="camera_link" type="camera"> <update_rate>10.0</update_rate> <always_on>true</always_on> <ignition_frame_id>camera_link</ignition_frame_id> <pose>0 0 0 0 0 0</pose> <topic>/image_raw</topic> <camera name="diff_bot_camera"> <horizontal_fov>1.3962634</horizontal_fov> <image> <width>600</width> <height>600</height> <format>R8G8B8</format> </image> <clip> <near>0.02</near> <far>300</far> </clip> </camera> </sensor> </gazebo> -
デプスカメラ
classic_diffbot_description/gazebo/diffbot.gazebo.xacro<gazebo reference="depth_camera_link"> <sensor name="depth_camera" type="depth"> <always_on>true</always_on> <update_rate>15</update_rate> <pose>0 0 0 0 0 0</pose> <camera name="realsense_depth_camera"> <horizontal_fov>1.46608</horizontal_fov> <image> <width>424</width> <height>240</height> <format>B8G8R8</format> </image> <clip> <near>0.05</near> <far>8</far> </clip> </camera> <plugin name="intel_realsense_d430_depth_driver" filename="libgazebo_ros_camera.so"> <ros> <namespace>d430</namespace> <remapping>depth_camera/image_raw:=color/image_raw</remapping> <remapping>depth_camera/depth/image_raw:=depth/image_rect_raw</remapping> <remapping>depth_camera/camera_info:=camera_info</remapping> <remapping>depth_camera/depth/camera_info:=depth/camera_info</remapping> <remapping>depth_camera/points:=depth/points</remapping> </ros> <camera_name>depth_camera</camera_name> <frame_name>depth_camera_link</frame_name> <hack_baseline>0.07</hack_baseline> <min_depth>0.05</min_depth> <max_depth>8.0</max_depth> </plugin> </sensor> <material>Gazebo/White</material> </gazebo>ignition_diffbot_description/gazebo/diffbot.gazebo.xacro<gazebo reference="depth_camera_link" > <sensor name="depth_camera_link" type="depth_camera"> <ignition_frame_id>depth_camera_link</ignition_frame_id> <pose>0 0 0 0 0 0</pose> <camera> <horizontal_fov>1.047</horizontal_fov> <image> <width>320</width> <height>240</height> <format>R_FLOAT32</format> </image> <clip> <near>0.1</near> <far>100</far> </clip> </camera> <always_on>true</always_on> <update_rate>10</update_rate> <visualize>true</visualize> <topic>depth_camera/image_raw</topic> </sensor> </gazebo>
最後に、Ignition Gazeboでのセンサ用のプラグインの記述を示す。
ignition_diffbot_description/gazebo/diffbot.gazebo.xacro
<gazebo>
<plugin filename="ignition-gazebo-sensors-system" name="ignition::gazebo::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>
</gazebo>
launchファイルの書き換え
launchファイルは導入するパッケージの違いなどがあるので、全文を以下に示します。
classic_diffbot_sim/launch/diffbot_spawn.launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
import xacro
def generate_launch_description():
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
)
classic_diffbot_description_path = os.path.join(
get_package_share_directory('classic_diffbot_description'))
xacro_file = os.path.join(classic_diffbot_description_path,
'robots',
'diffbot.urdf.xacro')
# xacroをロード
doc = xacro.process_file(xacro_file, mappings={'use_sim' : 'true'})
# xacroを展開してURDFを生成
robot_desc = doc.toprettyxml(indent=' ')
params = {'robot_description': robot_desc}
node_robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[params]
)
spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
arguments=['-topic', 'robot_description',
'-entity', 'diffbot'],
output='screen')
load_joint_state_controller = ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
'joint_state_broadcaster'],
output='screen'
)
load_diff_drive_controller = ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
'diff_drive_controller'],
output='screen'
)
velocity_converter = Node(
package='velocity_pub',
name='velocity_pub',
executable='velocity_pub',
remappings=[
('/cmd_vel_stamped', '/diff_drive_controller/cmd_vel'),
],
)
return LaunchDescription([
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_diff_drive_controller],
)
),
gazebo,
node_robot_state_publisher,
spawn_entity,
velocity_converter,
])
ignition_diffbot_sim/launch/diffbot_spawn.lanch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
from launch.actions import RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
import xacro
def generate_launch_description():
# Launch Arguments
use_sim_time = LaunchConfiguration('use_sim_time', default=True)
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('ros_gz_sim'), 'launch'), '/gz_sim.launch.py']),
launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]
)
ignition_diffbot_description_path = os.path.join(
get_package_share_directory('ignition_diffbot_description'))
xacro_file = os.path.join(ignition_diffbot_description_path,
'robots',
'diffbot.urdf.xacro')
# xacroをロード
doc = xacro.process_file(xacro_file, mappings={'use_sim' : 'true'})
# xacroを展開してURDFを生成
robot_desc = doc.toprettyxml(indent=' ')
params = {'robot_description': robot_desc}
rviz_config_file = os.path.join(ignition_diffbot_description_path, 'config', 'diffbot_config.rviz')
node_robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[params]
)
gz_spawn_entity = Node(
package='ros_gz_sim',
executable='create',
output='screen',
arguments=['-string', robot_desc,
'-name', 'diff_bot',
'-allow_renaming', 'false'],
)
load_joint_state_controller = ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
'joint_state_broadcaster'],
output='screen'
)
load_diff_drive_controller = ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
'diff_drive_controller'],
output='screen'
)
# Bridge
bridge = Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=['/scan@sensor_msgs/msg/LaserScan@ignition.msgs.LaserScan',
'/camera_info@sensor_msgs/msg/CameraInfo@ignition.msgs.CameraInfo',
'/image_raw@sensor_msgs/msg/Image@ignition.msgs.Image',
'/depth_camera/camera_info@sensor_msgs/msg/CameraInfo@ignition.msgs.CameraInfo',
'/depth_camera/image_raw@sensor_msgs/msg/Image@ignition.msgs.Image',
'/depth_camera/image_raw/points@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked'],
output='screen'
)
velocity_converter = Node(
package='velocity_pub',
name='velocity_pub',
executable='velocity_pub',
remappings=[
('/cmd_vel_stamped', '/diff_drive_controller/cmd_vel'),
],
)
rviz = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
)
return LaunchDescription([
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=gz_spawn_entity,
on_exit=[load_joint_state_controller],
)
),
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=load_joint_state_controller,
on_exit=[load_diff_drive_controller],
)
),
gazebo,
node_robot_state_publisher,
gz_spawn_entity,
bridge,
velocity_converter,
rviz,
])
基本的に使用するパッケージが異なるだけで、ClassicでもIgnitionでも大まかな流れは違いません。ただ、Ignition GazeboではセンサデータをIgnition専用のメッセージで出力してしまうため、bridgeによって変換する必要があります。各センサデータの変換に関する記述はlanuchファイルを参照してください。
注：最新のros_gzパッケージ（ https://github.com/gazebosim/ros_gz ）では、Ignition用メッセージをgz.msgs.xxxと記述していました。2023/4/9時点では、ignition.msgs.xxxの記述で問題なさそうですが、いずれgz.msgs.xxxに変更となる可能性があります。
今回の記事の執筆の要因となったのは、load_controllerのオプションがstartからactiveに変更になったことでした。
まとめ
本稿では、Gazebo Classicで使用していたパッケージをIgnition Gazebo用に変更するための修正点についてまとめました。classic_diffbot_description/urdfとignition_diffbot_description/urdfの中身を見てもらうと分かるように、メインとなるurdf.xacroファイルの中身はほぼ同じです。変更箇所も書き方が分かってしまえば何ということはありません。ぜひともあなたのプロジェクトもIgnition Gazeboへの移行にチャレンジしてみてください。Ignition Gazeboのユーザが増えて開発がますます発展することを願って結びとさせていただきます。