WIP: Add planner chain tutorial

[sjahr]

Depends on

• Update tutorials to pipeline refactoring changes #834

Description

• Adds a demo to show how chaining planners works.

Just run

ros2 launch moveit2_tutorials pipeline_testbench.launch.py

Checklist

• Required by CI: Code is auto formatted using clang-format
• While waiting for someone to review your request, please consider reviewing another open pull request to support the maintainers

[sea-bass]

I compiled all your branches and only tested the basic tutorial.

ros2 launch moveit2_tutorials demo.launch.py 

As with the previous PR, the Kinova configs will need updates. The ompl_planning.yaml needs to bump up to use the syntax

planning_plugins: 
  - ompl_interface/OMPLPlanner

Then, I got an RCL segfault that goes away by switching to Cyclone DDS. This one is documented in this issue: ros2/rcl#1118

Finally, planning was successful but execution failed:

[move_group-4] [ERROR] [1701270061.334355974] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Action client not connected to action server: joint_trajectory_controller/follow_joint_trajectory
[move_group-4] [ERROR] [1701270061.334372717] [move_group.trajectory_execution_manager]: Failed to send trajectory part 1 of 1 to controller joint_trajectory_controller
[move_group-4] [INFO] [1701270061.334382042] [move_group.trajectory_execution_manager]: Completed trajectory execution with status ABORTED ...

In both the Kinova example and the Panda config example below, it seems the controller manager spawners are failing:

ros2 launch moveit_resources_panda_moveit_config demo.launch.py

The error is as follows ...

[spawner-7] /opt/ros/rolling/lib/python3.10/site-packages/rclpy/node.py:187: RuntimeWarning: Failed to fini rosout publisher: rcl node implementation is invalid, at ./src/rcl/node.c:391
[spawner-7]   self.__node = _rclpy.Node(
[spawner-6] Traceback (most recent call last):
[spawner-6]   File "/opt/ros/rolling/lib/controller_manager/spawner", line 33, in <module>
[spawner-6]     sys.exit(load_entry_point('controller-manager==3.19.1', 'console_scripts', 'spawner')())
[spawner-6]   File "/opt/ros/rolling/lib/python3.10/site-packages/controller_manager/spawner.py", line 204, in main
[spawner-6]     node = Node("spawner_" + controller_name)
[spawner-6]   File "/opt/ros/rolling/lib/python3.10/site-packages/rclpy/node.py", line 187, in __init__
[spawner-6]     self.__node = _rclpy.Node(
[spawner-6] rclpy._rclpy_pybind11.RCLError: error creating node: error not set, at ./src/rcl/node.c:253
[ERROR] [spawner-8]: process has died [pid 309439, exit code 1, cmd '/opt/ros/rolling/lib/controller_manager/spawner panda_hand_controller -c /controller_manager --ros-args'].
[spawner-7] Traceback (most recent call last):
[spawner-7]   File "/opt/ros/rolling/lib/controller_manager/spawner", line 33, in <module>
[spawner-7]     sys.exit(load_entry_point('controller-manager==3.19.1', 'console_scripts', 'spawner')())
[spawner-7]   File "/opt/ros/rolling/lib/python3.10/site-packages/controller_manager/spawner.py", line 204, in main
[spawner-7]     node = Node("spawner_" + controller_name)
[spawner-7]   File "/opt/ros/rolling/lib/python3.10/site-packages/rclpy/node.py", line 187, in __init__
[spawner-7]     self.__node = _rclpy.Node(
[spawner-7] rclpy._rclpy_pybind11.RCLError: error creating node: error not set, at ./src/rcl/node.c:253
[ERROR] [spawner-6]: process has died [pid 309435, exit code 1, cmd '/opt/ros/rolling/lib/controller_manager/spawner joint_state_broadcaster --controller-manager /controller_manager --ros-args'].
[ERROR] [spawner-7]: process has died [pid 309437, exit code 1, cmd '/opt/ros/rolling/lib/controller_manager/spawner panda_arm_controller -c /controller_manager --ros-args'

NOTE:

I could get around this issue by doing what was suggested here: ros2/rmw_cyclonedds#458 (comment)

... how's Zenoh looking? lol

[sea-bass]

why did we nee to jack up the tolerances? 👀

EDIT: Is this because we switched the pipeline from OMPL to STOMP?

[sjahr]

Planning always failed for me, maybe because I accidentally pushed the OMPL -> STOMP change. I'll try again

[sea-bass]

BTW this link is dead and making htmlproofer fail.

Can you change it / remove it?

Looks to me like removing it, since that entire folder went away, presumably because it's replaced by actually chaining planners :)

[mergify]

This pull request is in conflict. Could you fix it @sjahr?