ROS2: gazebo_ros2_controlでGazeboとMoveitを連携する
海洋ロボコンをやってた人です。
今回は、URDFで記述したオリジナルロボットをGazeboシミュレーターと連携して動かせるようにする方法を紹介します。
Gazeboはじめましての方のお力添えになれば幸いです。
※2023/02/26追記: 本記事の「ROS2」表記、正しくは「ROS 2」です。
ROS2 gazebo_ros2_controlでシミュレーションする
今回は、前回・前々回で紹介したURDFおよび、MoveitパッケージをGazeboと連携してシミュレーターで動かす方法を紹介します。
Moveitパッケージの作成方法は下記よりご覧ください。
この記事を読むことで、下図のように、自身のオリジナルロボットをMoveitを用いてGazebo上で動かせるようになります。
また使用するモデルは、アイコンでも表示されているマンタ型ロボットで紹介していきます。
なお、ロボットのハード設計からソフト設計まで全て私が担当しており、所属研究室の教授からも承諾を得ています。
本記事のプログラム
下記のGithubにて公開しているので、こちらを参考にしてください。
スター頂けると、大変嬉しいです・ω・
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を追記します。
<?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でインポートする方法がいくつか見られたので、こちらも記載しておきます。
-
The Construct Youtube
https://www.youtube.com/watch?v=SgYiiraDL7A -
ros answers
https://answers.ros.org/question/361166/how-to-use-xacro-for-ros2/ -
Example
https://bitbucket.org/theconstructcore/box_car/src/foxy/
こちらの例では、以下を実行すると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>
各パラメータの詳細は以下にまとまっているので、こちらも記載。
4脚ロボットでgazeboシミュレーション。歩行モーションを入れてみた。
- configファイルの作成
続いて、先程urdfで定義したgazebo_ros2_controlの設定ファイルを作成していきます。
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内で表示できることになります。
<?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モデルを追加する場合は以下参照
- launchファイルをつくる
ここまでできたら、gazeboとros2 controllerをローンチするプログラムを記述するだけです。
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に依存関係も追記しておきます。
+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