ROS2: MoveIt2パッケージの作成方法
海洋ロボコンをやってた人です。
今回は、ROS2用のMoveIt2パッケージの作成方法について私の知見をまとめました。
これからMoveIt2を自分の研究機体に実装したい方のお力添えになれば幸いです。
※2023/02/26追記: 本記事の「ROS2」表記、正しくは「ROS 2」です。
ROS2 Moveit2パッケージの作成方法
この記事を読むことで、自身のオリジナルロボットをMoveit2で動かせるようになります。
また使用するモデルは、アイコンでも表示されているマンタ型ロボットで紹介していきます。
なお、ロボットのハード設計からソフト設計まで全て私が担当しており、所属研究室の教授からも承諾を得ています。
本記事のプログラム
下記のGithubにて公開しているので、こちらを参考にしてください。
スター頂けると、大変嬉しいです・ω・
moveit setup assistantでパッケージを準備
まずは、moveit setup assistant でmoveitに必要なconfigフォルダ群を準備していきます。
2022/10月現在では、まだsetup assistantがMoveit2に対応していないみたいなので、ROS1のsetup assistantを使用して設定ファイル群を作成していきます。
ROS1はご使用のUbuntuバージョンに合わせてインストールしてください。
また、setup assistantを使う際に、ロボットのURDF/xacroモデルが必要になりますので、まだ作成していない方は下記よりパッケージを作成してください。
私はROS1 Noeticで動作させており
sudo apt install ros-$ROS_DISTRO-moveit
でmoveitパッケージをインストールしておいてください。
まずはROS1用のワークスペースにxxxxx_descriptionフォルダを入れて、buildしておきます。
その後、setup assistantをローンチしましょう。
cd ~/catkin_manta_ws
source devel/setup.bash
roslaunch moveit_setup_assistant setup_assistant.launch
基本的には、setup assistant の使い方を紹介されている記事通りに進めてもらえれば問題なく作成できます。
以下は私がとても勉強になった記事です。非常に勉強になりました。ありがとうございました。
Moveit! Setup Assistant を使って簡単に Moveit! する
ROS入門 (46) - mycobot風ロボットのURDFを作成して、MoveIt構成ファイルを作成して、MoveItで制御
こちらでも、備忘録として、私なりの手順をまとめておきます。
- Create New Moveit Configuration Packageボタンから
- Browse > packages_name_description/urdf/packages_name.xacro > Load Files
によりxxxxx_descriptionフォルダ内のmanta_v2.xacroを読み込む
-
Self-Collisions
Generate Collision Matrixより、Collisionの設定をしてください。 -
Planning Groups
Add GroupよりPlanning Groupを設定します。
産業用ロボット/人協働ロボットなどはアーム部とエンドエフェクタ部を
- arm_controller
- gripper_controller
と分けることがあります。
今回は、マンタ型ロボットの各翼に対してcontrollerを適用させたいので、6つのPlanning Groupを作成しました。
- Robot Poses > Add Pose
こちらはPlanning/Query/Start State:, Goal State:で予め定義できるロボット位置になります。
ロボット起動時の初期位置や、良く使う位置を定義しておくと良いと思います。
エンドエフェクタを搭載している場合は、ハンドのOpen/Closeを定義しておくと良いです。
こちらは、パッケージ作成後にxxx.srdfファイルより再編集できるので、適当でも問題ありません。
-
End Effectors
作成したグループをエンドエフェクタへ指定する設定です。
今回は使用しないため割愛させていただきます。 -
Controllers
これはros_controlを使って制御するための設定です。
ros_controlを用いてロボットを動かす場合に、ここで設定したControllerをController Managerよりスポーンする必要があります。
今回はROS2対応ということで、ros2_controlを使用するためのyamlファイルを新たに作るので、設定しなくてもよいですが、ROS1でros_controlも使うという方は以下のどちらかで設定をしてください。
-
Auto Add FollowJointsTrajectory Controllers For Each Planning Group
からPlanning Groupより自動でControllerを生成 -
Add Controller
より、手動でControllerを追加
またros_controlについて、とても勉強になった記事も記載させていただきます。
ros_controls/ros_controllers の制御の仕組み (position/effort/velocity_controllers の基礎)
-
Simulation > Generate URDF
これはGazeboシミュレーターで使用するためのURDFを生成する項目です。
ROS2では.xacroを呼び出すのは（実行例はありますが）かなり困難なので、URDFを生成しておくのが良いかと思います。 -
Author Information
著者、メアドを適宜入力してください。
メアドを入力しないと、次の項目には進めない点も注意が必要です。 -
Configuration Files > Generate Package
パッケージを生成できたら、ROS1で確認してみるのも良いかと思います。
moveit2 パッケージを作成する
上記で、moveitに必要なパッケージを作成できたら、moveit用のROS2パッケージを作成します。
moveitのパッケージ名は"xxxxx_moveit_config"とつけることが多いです。
cd ~/xxxxx_ws
. install/setup.bash
ros2 pkg create --build-type ament_cmake xxxxx_moveit_config
パッケージを作成したら
- SetupAssistantで作成したmoveit_config/configをROS2下へコピー
config下へ
2. config下へyamlを作成
- packages_name_controllers.yaml
- packages_name_ros_controllers.yaml
します。
ロボット名_controllers.yamlファイルは、moveitの動かすために必要なコントローラー情報になっています。
コントローラーの記述方法ですが、moveit setup assistantで生成される"simple_moveit_controllers.yaml"を参考に記述してもらうと良いと思います。
下記では、"simple_moveit_controllers.yaml"内の-nameをcontroller_namesとして先にまとめていますが、プログラムの記述内容は"simple_moveit_controllers.yaml"と同じになっています。
言い換えれば、別に新しく作る必要なないという事ですね。(-ω-)/
controller_names:
- rtop_controller
- rmid_controller
- rbtm_controller
rtop_controller:
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- rev_link_Rtop_roll1
- rev_link_Rtop_pitch
- rev_link_Rtop_roll2
rmid_controller:
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- rev_link_Rmid_roll1
- rev_link_Rmid_pitch
- rev_link_Rmid_roll2
rbtm_controller:
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- rev_link_Rbtm_roll1
- rev_link_Rbtm_pitch
- rev_link_Rbtm_roll2
続いて、ロボット名_ros_controllers.yamlはmoveitをros2_controlで動かすためのファイルです。
こちらは、moveit setup assistantで生成される"ros_controllers.yaml"のcontrollerタイプを一部変更して記述しています。
ros2_controllersの公式ドキュメントのAvailable Controllersでは6種類のcontrollerが説明されていますので、ご自身のロボットに応じてタイプを変更してください。
また、ros-controls-GitHubのページに飛ぶと、より多くのコントローラーがあることも確認できます。
私のロボットでは各関節が動けば良いと思ったので、joint_trajectory_controllerを使用します。
この辺は私自身勉強中ですので、訂正等あればご教授お願い致します。
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
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
コントローラーの作成ができたら、ros2_controllerでmoveitのコントローラーを動かすためにURDF/xacroのロボットモデルに紐づけしていき、手順としては以下になります。
- xxxxx_description/rviz/run_move_group.rvizを作成
- xxxxx_description/ros2_control/xxxxx_ros2_control.xacroを作成
- xxxxx.xacro にxxxxx_ros2_control.xacroをインクルードするよう記述
- xxxxx_description/CMakeLisit.txtにros2_controlのincludeを追加
rvizファイルの作成やros2_control.xacroのディレクトリは任意です。
私はros2_control_demoのディレクトリ構成に沿ってxxxxx_description下にros2_controlのファイルを置いています。
また、rvizファイルはmoveitのデモで使用される
- /opt/ros/foxy/share/moveit_resources_panda_moveit_config/launch/moveit.rviz
を参考にしています。
こちらはrvizのDisplayプラグインからmoveit_ros_visualizationを追加すれば良いので任意で良いでしょう。
ros2_controlの記述としては以下のようになります。
<?xml version="1.0" ?>
<robot name="manta_v2" xmlns:xacro="http://www.ros.org/wiki/xacro" >
<xacro:macro name="manta_v2_ros2_control" params="name">
<ros2_control name="${name}" type="system">
<hardware>
<plugin>fake_components/GenericSystem</plugin>
</hardware>
<joint name="con_link_ballast">
<command_interface name="position">
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<!-- 以下略 -->
</ros2_control>
</xacro:macro>
</robot>
xacroをros2_control下に配置した場合は、CMakeListsにインクルード関係を記述しビルドを通しておきましょう。
- moveit launchファイルの作成
ここまでできたら、xxxxx_moveit_config/launch下に"moveit_demo.launch.pyを作成します。
また、以下のフォルダ名,load_controllersなどは任意に書き換えてください。
description_folder = "manta_v2_description"
moveit_config_folder = "manta_v2_moveit_config"
xacro_file = "urdf/manta_v2.xacro"
srdf_file = "config/manta_v2.srdf"
rviz_file = "rviz/run_move_group.rviz"
# Load controllers
load_controllers = []
for controller in [
"rtop_controller",
"rmid_controller",
"rbtm_controller",
"joint_state_broadcaster",
]:
以下、launch全文
import os
import yaml
from launch import LaunchDescription
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch.actions import ExecuteProcess
from ament_index_python.packages import get_package_share_directory
import xacro
def load_file(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return file.read()
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
return None
def load_yaml(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return yaml.safe_load(file)
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
return None
def generate_launch_description():
description_folder = "manta_v2_description"
moveit_config_folder = "manta_v2_moveit_config"
xacro_file = "urdf/manta_v2.xacro"
srdf_file = "config/manta_v2.srdf"
rviz_file = "rviz/run_move_group.rviz"
# planning_context
robot_description_config = xacro.process_file(
os.path.join(
get_package_share_directory(description_folder),
xacro_file,
)
)
robot_description = {"robot_description": robot_description_config.toxml()}
robot_description_semantic_config = load_file(
moveit_config_folder, srdf_file
)
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_config
}
kinematics_yaml = load_yaml(
moveit_config_folder, "config/kinematics.yaml"
)
robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml}
# Planning Functionality
ompl_planning_pipeline_config = {
"move_group": {
"planning_plugin": "ompl_interface/OMPLPlanner",
"request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
"start_state_max_bounds_error": 0.1,
}
}
ompl_planning_yaml = load_yaml(
moveit_config_folder, "config/ompl_planning.yaml"
)
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
# Trajectory Execution Functionality
moveit_simple_controllers_yaml = load_yaml(
moveit_config_folder, "config/manta_v2_controllers.yaml"
)
moveit_controllers = {
"moveit_simple_controller_manager": moveit_simple_controllers_yaml,
"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
}
trajectory_execution = {
"moveit_manage_controllers": True,
"trajectory_execution.allowed_execution_duration_scaling": 1.2,
"trajectory_execution.allowed_goal_duration_margin": 0.5,
"trajectory_execution.allowed_start_tolerance": 0.01,
}
planning_scene_monitor_parameters = {
"publish_planning_scene": True,
"publish_geometry_updates": True,
"publish_state_updates": True,
"publish_transforms_updates": True,
}
# Start the actual move_group node/action server
run_move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[
robot_description,
robot_description_semantic,
kinematics_yaml,
ompl_planning_pipeline_config,
trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
],
)
# RViz
rviz_config_file = PathJoinSubstitution(
[FindPackageShare(description_folder), rviz_file]
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
parameters=[
robot_description,
robot_description_semantic,
ompl_planning_pipeline_config,
kinematics_yaml,
],
)
# Static TF
static_tf = Node(
package="tf2_ros",
executable="static_transform_publisher",
name="static_transform_publisher",
output="log",
arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base_link"],
)
# Publish TF
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
output="both",
parameters=[robot_description],
)
# ros2_control using FakeSystem as hardware
ros2_controllers_path = os.path.join(
get_package_share_directory(moveit_config_folder),
"config",
"manta_v2_ros_controllers.yaml",
)
ros2_control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, ros2_controllers_path],
output="screen",
)
# Load controllers
load_controllers = []
for controller in [
"rtop_controller",
"rmid_controller",
"rbtm_controller",
"ltop_controller",
"lmid_controller",
"lbtm_controller",
"joint_state_broadcaster",
]:
load_controllers += [
ExecuteProcess(
cmd=["ros2 run controller_manager spawner.py {}".format(controller)],
shell=True,
output="screen",
)
]
# Warehouse mongodb server
mongodb_server_node = Node(
package="warehouse_ros_mongo",
executable="mongo_wrapper_ros.py",
parameters=[
{"warehouse_port": 33829},
{"warehouse_host": "localhost"},
{"warehouse_plugin": "warehouse_ros_mongo::MongoDatabaseConnection"},
],
output="screen",
)
return LaunchDescription(
[
rviz_node,
static_tf,
robot_state_publisher,
run_move_group_node,
ros2_control_node,
# mongodb_server_node,
]
+ load_controllers
)
+ cmd=["ros2 run controller_manager spawner {}".format(controller)],
- cmd=["ros2 run controller_manager spawner.py {}".format(controller)],
foxyでcontroller_mangagerをspawn(スポーン)する場合はspawner.pyという記述で良いみたいですが、Humbleで使用する際はspawner(スポナー)という記述でないとErrorが生じることを確認しています。
そのため、バージョンによって記述を変更する必要がありそうです。
moveit2 パッケージを実行する
パッケージの作成およびビルドができたら
cd ~/xxxxx_ws
. install/setup.bash
ros2 launch manta_v2_moveit_config moveit_demo.launch.py
で実行してみましょう
マーカーが表示されるので、適当に動かして、Plan & Executeを押すと
動きましたね！
軌道の可視化には、jsk_visualizationをROS2対応してくださった、Kottakuさんのパッケージ群を使用しています。
私自身、待ち望んでおり、非常に使いやすいのでjskの皆様、Kottaku様には感謝しきれませんね。
オプション：moveit2 group_state
moveitには"Start State:", "Goal State:"という項目があり、srdfファイル内にgroup_stateタグを記述することで、デフォルトポーズを呼び出すことができます。
+ <group_state group="Ltop" name="Init">
+ <joint name="rev_link_Ltop_roll1" value="0"/>
+ <joint name="rev_link_Ltop_pitch" value="0"/>
+ <joint name="rev_link_Ltop_roll2" value="0"/>
+ </group_state>
よくあるエラー
Controller Manager よりros2のcontrollerを読み込めないと、エラーになり、ロボットモデルが正常に動作しません。
エラーが発生しているんだなと分かる点を、正常動作時と比較して記載しておきます。
1. You can start planning now!が表示されるタイミング
launchファイルが正常に起動すると、You can start planning now!の緑色の文字が表示されるタイミングとほぼ同時にRvizのスプラッシュ画面が表示されます。
Rvizのスプラッシュ画面が表示されているのに、青文字、緑文字の表記がでていなかったら、controllerが上手く読み込めていない可能性が高いです。
- controller managerのスポーン
ros2_controlを使用する場合、設定したcontrollerを正常にスポーンできると
Configured and started xxx_controller
と色付き文字で表示されます。
逆に、この表記が見られなかったら、呼び出しに失敗しているのでプログラムを見直す必要があります。
疑問点、改善点等あればお気軽にご連絡ください。
また、この記事が良かったと感じたら、"いいね"ボタンよろしくお願いします。
以上。
Discussion