🐢

ROS 2(iron)でMoveIt2のdemo.launch.pyを動かすまで

2024/06/16に公開1件

意外と落とし穴があったのでメモ書き。

前提

項目	内容
PC	Thinkpad X260
メモリ	8GB
OS	Ubuntu 22.04LTS

ROS 2(iron)のインストール

変わったことはしません。以下のドキュメントの通りセットアップするだけ。

上記ドキュメントの以下の節を実施しました。

System setup
Install ROS 2
Setup environment

MoveIt2のインストール

こちらもROS 2と同様、ドキュメントどおりにセットアップ

Build your Colcon Workspace節でPCが固まりました。
具体的には

colcon build --mixin release

のところ。PCの搭載メモリ量8GBはやはり少ないそうです。最小でも16GB、推奨32GBの搭載メモリが必要らしいです。

多分デフォルトでは使えるCPUすべてを使って並行にコンパイルをしているからでしょう。ドキュメントのWarningの記載内容に従い並行コンパイル数を減らします(2並行）。

colcon build --mixin release --parallel-workers 2

これをすることでcolcon buildは無事完走。その後のsetup.bash記載も指示通りにやってインストールは完了しました。

MoveIt Quickstart in RViz

デモプログラムを起動するだけでしょ?と思いましたが甘かった。

起動1回目

ros2 launch moveit2_tutorials demo.launch.py

RViz2は起動するけど、ターミナルには怪しいメッセージがある。

[move_group-4] [INFO] [1718499210.472236424] [move_group.moveit.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry
[move_group-4] [WARN] [1718499210.475600913] [move_group.moveit.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[move_group-4] [ERROR] [1718499210.475654121] [move_group.moveit.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates
[rviz2-1] Warning: class_loader.impl: SEVERE WARNING!!! A namespace collision has occurred with plugin factory for class rviz_default_plugins::displays::InteractiveMarkerDisplay. New factory will OVERWRITE existing one. This situation occurs when libraries containing plugins are directly linked against an executable (the one running right now generating this message). Please separate plugins out into their own library or just don't link against the library and use either class_loader::ClassLoader/MultiLibraryClassLoader to open.
[rviz2-1]          at line 321 in /opt/ros/iron/include/class_loader/class_loader/class_loader_core.hpp
[spawner-8] [INFO] [1718499210.533247763] [spawner_robotiq_gripper_controller]: Loaded robotiq_gripper_controller
[ros2_control_node-5] [INFO] [1718499210.536809941] [controller_manager]: Configuring controller 'robotiq_gripper_controller'
[ros2_control_node-5] [INFO] [1718499210.536965053] [robotiq_gripper_controller]: Action status changes will be monitored at 20Hz.
[spawner-8] [INFO] [1718499210.583726276] [spawner_robotiq_gripper_controller]: Configured and activated robotiq_gripper_controller
[INFO] [spawner-8]: process has finished cleanly [pid 23147]
[move_group-4] terminate called after throwing an instance of 'rclcpp::ParameterTypeException'
[move_group-4]   what():  expected [string_array] got [string]
[move_group-4] Stack trace (most recent call last):
[move_group-4] #17   Object "", at 0xffffffffffffffff, in 
[move_group-4] #16   Object "/home/itohemon/ws_moveit/install/moveit_ros_move_group/lib/moveit_ros_move_group/move_group", at 0x5c06a0e51204, in _start
[move_group-4] #15   Source "../csu/libc-start.c", line 392, in __libc_start_main_impl [0x73aed5429e3f]
[move_group-4] #14   Source "../sysdeps/nptl/libc_start_call_main.h", line 58, in __libc_start_call_main [0x73aed5429d8f]
[move_group-4] #13   Object "/home/itohemon/ws_moveit/install/moveit_ros_move_group/lib/moveit_ros_move_group/move_group", at 0x5c06a0e501c3, in main
[move_group-4] #12   Object "/home/itohemon/ws_moveit/install/moveit_ros_planning/lib/libmoveit_cpp.so.2.10.0", at 0x73aed63bc6a7, in moveit_cpp::MoveItCpp::MoveItCpp(std::shared_ptr<rclcpp::Node> const&, moveit_cpp::MoveItCpp::Options const&)
[move_group-4] #11   Object "/home/itohemon/ws_moveit/install/moveit_ros_planning/lib/libmoveit_cpp.so.2.10.0", at 0x73aed63bb5cb, in moveit_cpp::MoveItCpp::loadPlanningPipelines(moveit_cpp::MoveItCpp::PlanningPipelineOptions const&)
[move_group-4] #10   Object "/home/itohemon/ws_moveit/install/moveit_ros_planning/lib/libmoveit_planning_pipeline_interfaces.so.2.10.0", at 0x73aed5e7efab, in moveit::planning_pipeline_interfaces::createPlanningPipelineMap(std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&, std::shared_ptr<moveit::core::RobotModel const> const&, std::shared_ptr<rclcpp::Node> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)
[ros2_control_node-5] [INFO] [1718499210.913198461] [controller_manager]: Loading controller 'joint_trajectory_controller'
[move_group-4] #9    Object "/home/itohemon/ws_moveit/install/moveit_ros_planning/lib/libmoveit_planning_pipeline.so.2.10.0", at 0x73aed5ab8fc5, in planning_pipeline::PlanningPipeline::PlanningPipeline(std::shared_ptr<moveit::core::RobotModel const> const&, std::shared_ptr<rclcpp::Node> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)
[move_group-4] #8    Object "/home/itohemon/ws_moveit/install/moveit_ros_planning/lib/libmoveit_planning_pipeline.so.2.10.0", at 0x73aed5afa63d, in planning_pipeline_parameters::ParamListener::declare_params()
[move_group-4] #7    Object "/opt/ros/iron/lib/librclcpp.so", at 0x73aed5cd9869, in 
[move_group-4] #6    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x73aed58ae4d7, in __cxa_throw
[move_group-4] #5    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x73aed58ae276, in std::terminate()
[move_group-4] #4    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x73aed58ae20b, in 
[move_group-4] #3    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x73aed58a2b9d, in 
[move_group-4] #2    Source "./stdlib/abort.c", line 79, in abort [0x73aed54287f2]
[move_group-4] #1    Source "../sysdeps/posix/raise.c", line 26, in raise [0x73aed5442475]
[move_group-4] #0  | Source "./nptl/pthread_kill.c", line 89, in __pthread_kill_internal
[move_group-4]     | Source "./nptl/pthread_kill.c", line 78, in __pthread_kill_implementation
[move_group-4]       Source "./nptl/pthread_kill.c", line 44, in __pthread_kill [0x73aed54969fc]
[move_group-4] Aborted (Signal sent by tkill() 23137 1000)
[ros2_control_node-5] [WARN] [1718499210.968131536] [joint_trajectory_controller]: "allow_nonzero_velocity_at_trajectory_end" is set to false. (The default behavior changed to false, and trajectories might get discarded.)
[spawner-7] [INFO] [1718499210.973469510] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller
[ros2_control_node-5] [INFO] [1718499210.977080614] [controller_manager]: Configuring controller 'joint_trajectory_controller'
[ros2_control_node-5] [INFO] [1718499210.977394356] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[ros2_control_node-5] [INFO] [1718499210.977447266] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity].
[ros2_control_node-5] [INFO] [1718499210.977476848] [joint_trajectory_controller]: Using 'splines' interpolation method.
[ros2_control_node-5] [INFO] [1718499210.982319799] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz.
[spawner-7] [INFO] [1718499211.011974483] [spawner_joint_trajectory_controller]: Configured and activated joint_trajectory_controller
[ERROR] [move_group-4]: process has died [pid 23137, exit code -6, cmd '/home/itohemon/ws_moveit/install/moveit_ros_move_group/lib/moveit_ros_move_group/move_group --ros-args --params-file /tmp/launch_params_dhre4v21'].
[INFO] [spawner-7]: process has finished cleanly [pid 23145]
[spawner-6] [INFO] [1718499211.988505010] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' node to exist

怪しいのはこのあたり。「パラメータタイプ例外」って何よ?

[move_group-4] terminate called after throwing an instance of 'rclcpp::ParameterTypeException'
[move_group-4]   what():  expected [string_array] got [string]
[move_group-4] Stack trace (most recent call last):
[move_group-4] #17   Object "", at 0xffffffffffffffff, in 
****

起動しているのがmoveit2_tutorialsパッケージなのでGitHubにIssueがあるか確認するとありました。Issue

ompl_planning.yamlを読んでrequest_adaptersを変えてね、ってある。

抽出したエラーメッセージをよく見ると、

[move_group-4]   what():  expected [string_array] got [string]

string配列型を予想したけどstring型を得た、ってあるのでYAMLの書き方ミスっぽい。ompl_planning.yamlのコメントの通りに修正を加える。修正した結果のgit diff。

diff --git a/kortex_moveit_config/kinova_gen3_7dof_robotiq_2f_85_moveit_config/config/ompl_planning.yaml b/kortex_moveit_config/kinova_gen3_7dof_robotiq_2f_85_moveit_config/config/ompl_planning.yaml
index 44372b0..bdbb2b3 100644
--- a/kortex_moveit_config/kinova_gen3_7dof_robotiq_2f_85_moveit_config/config/ompl_planning.yaml
+++ b/kortex_moveit_config/kinova_gen3_7dof_robotiq_2f_85_moveit_config/config/ompl_planning.yaml
@@ -21,13 +21,13 @@
 ###################################### NOTE #############################################
 
 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
+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
 planner_configs:
   SBLkConfigDefault:

再びビルド。

起動2回目

ros2 launch moveit2_tutorials demo.launch.py

やっぱりターミナルには怪しいメッセージがある。ただ、1回目の修正は効いてるのかも。メッセージが変わった。

[move_group-4] terminate called after throwing an instance of 'std::runtime_error'
[move_group-4]   what():  Planning plugin name is empty or not defined in namespace 'ompl'. Please choose one of the available plugins: chomp_interface/CHOMPPlanner, ompl_interface/OMPLPlanner, pilz_industrial_motion_planner/CommandPlanner, planning_pipeline_test/DummyPlannerManager, stomp_moveit/StompPlanner
[move_group-4] Stack trace (most recent call last):
[move_group-4] #16   Object "", at 0xffffffffffffffff, in 
[move_group-4] #15   Object "/home/itohemon/ws_moveit/install/moveit_ros_move_group/lib/moveit_ros_move_group/move_group", at 0x56a24223a204, in _start

1回目で修正したompl_planner.yamlの修正箇所の上の行に

planning_plugin: ompl_interface/OMPLPlanner

って書いてあるにも関わらずエラーメッセージが出てるように見える。
ぐぐってみたら、パラメータ名がplanning_pluginからplanning_pluginsに変わり値の型を文字列ベクタにする必要があるらしい。
参照

ということで変更。

diff --git a/kortex_moveit_config/kinova_gen3_7dof_robotiq_2f_85_moveit_config/config/ompl_planning.yaml b/kortex_moveit_config/kinova_gen3_7dof_robotiq_2f_85_moveit_config/config/ompl_planning.yaml
index 44372b0..ba3f49c 100644
--- a/kortex_moveit_config/kinova_gen3_7dof_robotiq_2f_85_moveit_config/config/ompl_planning.yaml
+++ b/kortex_moveit_config/kinova_gen3_7dof_robotiq_2f_85_moveit_config/config/ompl_planning.yaml
@@ -20,14 +20,15 @@
 #
 ###################################### NOTE #############################################
 
-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
+planning_plugins:
+ - 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
 planner_configs:
   SBLkConfigDefault:

再々ビルド。

起動3回目

ros2 launch moveit2_tutorials demo.launch.py

まだまだターミナルには怪しいメッセージがある。ただ、2回目の修正も効いてるっぽい。メッセージが変わった。

[move_group-4] [INFO] [1718501128.566339920] [move_group]: Try loading adapter 'default_planner_request_adapters/AddTimeOptimalParameterization'
[move_group-4] [FATAL] [1718501128.566541868] [move_group]: Exception while loading planning adapter plugin 'default_planner_request_adapters/AddTimeOptimalParameterization': According to the loaded plugin descriptions the class default_planner_request_adapters/AddTimeOptimalParameterization with base class type planning_interface::PlanningRequestAdapter does not exist. Declared types are  default_planning_request_adapters/CheckForStackedConstraints default_planning_request_adapters/CheckStartStateBounds default_planning_request_adapters/CheckStartStateCollision default_planning_request_adapters/ResolveConstraintFrames default_planning_request_adapters/ValidateWorkspaceBounds planning_pipeline_test/AlwaysSuccessRequestAdapter
[move_group-4] terminate called after throwing an instance of 'pluginlib::LibraryLoadException'
[move_group-4]   what():  According to the loaded plugin descriptions the class default_planner_request_adapters/AddTimeOptimalParameterization with base class type planning_interface::PlanningRequestAdapter does not exist. Declared types are  default_planning_request_adapters/CheckForStackedConstraints default_planning_request_adapters/CheckStartStateBounds default_planning_request_adapters/CheckStartStateCollision default_planning_request_adapters/ResolveConstraintFrames default_planning_request_adapters/ValidateWorkspaceBounds planning_pipeline_test/AlwaysSuccessRequestAdapter
[move_group-4] Stack trace (most recent call last):
[move_group-4] #17   Object "", at 0xffffffffffffffff, in 
[move_group-4] #16   Object "/home/itohemon/ws_moveit/install/moveit_ros_move_group/lib/moveit_ros_move_group/move_group", at 0x570644613204, in _start

FATALエラーのところが怪しい。

[move_group-4] [FATAL] [1718501128.566541868] [move_group]: Exception while loading planning adapter plugin 'default_planner_request_adapters/AddTimeOptimalParameterization': According to the loaded plugin descriptions the class default_planner_request_adapters/AddTimeOptimalParameterization with base class type planning_interface::PlanningRequestAdapter does not exist. Declared types are  default_planning_request_adapters/CheckForStackedConstraints default_planning_request_adapters/CheckStartStateBounds default_planning_request_adapters/CheckStartStateCollision default_planning_request_adapters/ResolveConstraintFrames default_planning_request_adapters/ValidateWorkspaceBounds planning_pipeline_test/AlwaysSuccessRequestAdapter

その周辺を見ると「宣言されてるタイプは…」と書かれてるので、それをそのまま書けば良いんじゃね？ということで修正。

diff --git a/kortex_moveit_config/kinova_gen3_7dof_robotiq_2f_85_moveit_config/config/ompl_planning.yaml b/kortex_moveit_config/kinova_gen3_7dof_robotiq_2f_85_moveit_config/config/ompl_planning.yaml
index 44372b0..072793b 100644
--- a/kortex_moveit_config/kinova_gen3_7dof_robotiq_2f_85_moveit_config/config/ompl_planning.yaml
+++ b/kortex_moveit_config/kinova_gen3_7dof_robotiq_2f_85_moveit_config/config/ompl_planning.yaml
@@ -20,14 +20,15 @@
 #
 ###################################### NOTE #############################################
 
-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
+planning_plugins:
+ - ompl_interface/OMPLPlanner
+request_adapters:
+ - default_planning_request_adapters/CheckForStackedConstraints
+ - default_planning_request_adapters/CheckStartStateBounds
+ - default_planning_request_adapters/CheckStartStateCollision
+ - default_planning_request_adapters/ResolveConstraintFrames
+ - default_planning_request_adapters/ValidateWorkspaceBounds
+ - planning_pipeline_test/AlwaysSuccessRequestAdapter
 start_state_max_bounds_error: 0.1
 planner_configs:
   SBLkConfigDefault:

再々々ビルド。

起動4回目

ros2 launch moveit2_tutorials demo.launch.py

ついにエラー無く起動した模様。You can start planning now!出たし。

[move_group-4] [INFO] [1718502069.449737819] [move_group.moveit.moveit.ros.move_group.executable]: 
[move_group-4] 
[move_group-4] ********************************************************
[move_group-4] * MoveGroup using: 
[move_group-4] *     - apply_planning_scene_service
[move_group-4] *     - clear_octomap_service
[move_group-4] *     - get_group_urdf
[move_group-4] *     - CartesianPathService
[move_group-4] *     - execute_trajectory_action
[move_group-4] *     - get_planning_scene_service
[move_group-4] *     - kinematics_service
[move_group-4] *     - move_action
[move_group-4] *     - motion_plan_service
[move_group-4] *     - query_planners_service
[move_group-4] *     - state_validation_service
[move_group-4] *     - SequenceAction
[move_group-4] *     - SequenceService
[move_group-4] ********************************************************
[move_group-4] 
[move_group-4] [INFO] [1718502069.449888560] [move_group.moveit.moveit.ros.move_group.context]: MoveGroup context using pipeline ompl
[move_group-4] [INFO] [1718502069.449928054] [move_group.moveit.moveit.ros.move_group.context]: MoveGroup context initialization complete
[move_group-4] Loading 'move_group/ApplyPlanningSceneService'...
[move_group-4] Loading 'move_group/ClearOctomapService'...
[move_group-4] Loading 'move_group/GetUrdfService'...
[move_group-4] Loading 'move_group/MoveGroupCartesianPathService'...
[move_group-4] Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
[move_group-4] Loading 'move_group/MoveGroupGetPlanningSceneService'...
[move_group-4] Loading 'move_group/MoveGroupKinematicsService'...
[move_group-4] Loading 'move_group/MoveGroupMoveAction'...
[move_group-4] Loading 'move_group/MoveGroupPlanService'...
[move_group-4] Loading 'move_group/MoveGroupQueryPlannersService'...
[move_group-4] Loading 'move_group/MoveGroupStateValidationService'...
[move_group-4] Loading 'pilz_industrial_motion_planner/MoveGroupSequenceAction'...
[move_group-4] Loading 'pilz_industrial_motion_planner/MoveGroupSequenceService'...
[move_group-4] 
[move_group-4] You can start planning now!
[move_group-4]

起動画面。

Motion planningでアームを動かす

MotionPlanningのPlanningタブにあるQuery>Planning Groupをmanipulatorに切り替える。

グリッパーのところにボールが出て、アーム全体がオレンジ色になる。

グリッパーのところのボールをドラッグして適当にアームの姿勢を変える。

MotionPlanningのPlanningタブにあるCommands>Planボタンを押す。

MotionPlanningのPlanningタブにあるCommands>Executeボタンを押す。
あれ？Failedになった。アーム動かない。

MotionPlanningのContextタブにあるPlanning Libraryを変えてみる。
変更前

変更後

MotionPlanningのPlanningタブに戻ってCommands>Planボタンを押す。
今度は動いた。

MotionPlanningのPlanningタブにあるCommands>Executeボタンを押す。

やっと動かせた！

結論

configファイルをいくつか修正してとりあえず動かすことはできました。
チュートリアルのもうちょっと先まで行ってみたいと思います。

お疲れ様でした！
（Planning Libraryって何にするのが正解なんでしょう？）

Discussion

royhax

Thank you - ありがとう！！

OPML planner fix - ompl_planning.yaml:

planning_plugins:
  - ompl_interface/OMPLPlanner
request_adapters:
  - default_planning_request_adapters/CheckForStackedConstraints
  - default_planning_request_adapters/CheckStartStateBounds
  - default_planning_request_adapters/CheckStartStateCollision
  - default_planning_request_adapters/ResolveConstraintFrames
  - default_planning_request_adapters/ValidateWorkspaceBounds
  - planning_pipeline_test/AlwaysSuccessRequestAdapter
response_adapters:
 - default_planning_response_adapters/AddTimeOptimalParameterization
 - default_planning_response_adapters/ValidateSolution
 - default_planning_response_adapters/DisplayMotionPath

See: