Backport of Pilz examples and update to ROS2 (#548) to humble (#797)

* Pilz examples and update to ROS2 (#548)

* Add bare-bones Pilz code examples

* Fill in Pilz MTC example

* Clean up MTC example

* Fill in MoveGroup Interface example + more cleanup

* Update docs

* clang-format

* Fix HTML targets

* Update code links to use codedir

* PR comments

* Fix URL

(cherry picked from commit 7498cc2)

# Conflicts:
#	CMakeLists.txt

* Fix conflicts from mergify bot

---------

Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
Co-authored-by: Sebastian Castro <sebas.a.castro@gmail.com>

CMakeLists.txt

@@ -67,6 +67,7 @@ add_subdirectory(doc/examples/move_group_interface)
 # add_subdirectory(doc/examples/move_group_python_interface)
 # add_subdirectory(doc/examples/perception_pipeline)
 # add_subdirectory(doc/examples/pick_place)
+add_subdirectory(doc/examples/pilz_industrial_motion_planner)
 # add_subdirectory(doc/examples/planning)
 add_subdirectory(doc/examples/planning_scene)
 add_subdirectory(doc/examples/planning_scene_ros_api)

doc/examples/pilz_industrial_motion_planner/CMakeLists.txt

@@ -0,0 +1,19 @@
+add_executable(pilz_move_group src/pilz_move_group.cpp)
+target_include_directories(pilz_move_group PUBLIC include)
+ament_target_dependencies(pilz_move_group ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+add_executable(pilz_mtc src/pilz_mtc.cpp)
+target_include_directories(pilz_mtc PUBLIC include)
+ament_target_dependencies(pilz_mtc ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+install(
+  TARGETS
+    pilz_move_group
+    pilz_mtc
+  DESTINATION
+    lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)

doc/examples/pilz_industrial_motion_planner/launch/pilz_mtc.launch.py

@@ -0,0 +1,19 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from moveit_configs_utils import MoveItConfigsBuilder
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder("moveit_resources_panda").to_dict()
+
+    # Pilz + MTC Demo node
+    pilz_mtc_demo = Node(
+        package="moveit2_tutorials",
+        executable="pilz_mtc",
+        output="screen",
+        parameters=[
+            moveit_config,
+        ],
+    )
+
+    return LaunchDescription([pilz_mtc_demo])

doc/examples/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst

@@ -1,56 +1,48 @@
-:moveit1:
-
-..
-   Once updated for MoveIt 2, remove all lines above title (including this comment and :moveit1: tag)
-
 Pilz Industrial Motion Planner
 ==============================
 
 ``pilz_industrial_motion_planner`` provides a trajectory generator to plan standard robot
-motions like PTP, LIN, CIRC with the interface of a MoveIt PlannerManager
-plugin.
+motions like point-to-point, linear, and circular with MoveIt.
 
-User Interface MoveGroup
-------------------------
-
-This package implements the ``planning_interface::PlannerManager``
-interface of MoveIt. By loading the corresponding planning pipeline
-(``pilz_industrial_motion_planner_planning_pipeline.launch.xml`` in your
+By loading the corresponding planning pipeline
+(``pilz_industrial_motion_planner_planning_planner.yaml`` in your
 ``*_moveit_config`` package), the trajectory generation
-functionalities can be accessed through the user interface (c++, python
-or rviz) provided by the ``move_group`` node, e.g.
-``/plan_kinematics_path`` service and ``/move_group`` action. For
-detailed usage tutorials please refer to :doc:`/doc/examples/move_group_interface/move_group_interface_tutorial`.
+functionalities can be accessed through the user interface (C++, Python
+or RViz) provided by the ``move_group`` node, e.g.
+``/plan_kinematic_path`` service and ``/move_action`` action.
+For detailed usage tutorials, please refer to
+:doc:`/doc/examples/moveit_cpp/moveitcpp_tutorial` and
+:doc:`/doc/examples/move_group_interface/move_group_interface_tutorial`.
 
 Joint Limits
 ------------
 
-For all commands the planner uses maximum velocities and accelerations from
-the parameter server. Using the MoveIt setup assistant the file ``joint_limits.yaml``
+The planner uses maximum velocities and accelerations from the
+parameters of the ROS node that is operating the Pilz planning pipeline.
+Using the MoveIt Setup Assistant the file ``joint_limits.yaml``
 is auto-generated with proper defaults and loaded during startup.
 
-The limits on the parameter server override the limits from the URDF robot description.
+The specified limits override the limits from the URDF robot description.
 Note that while setting position limits and velocity limits is possible
-in both the URDF and the parameter server setting acceleration limits is
-only possible via the parameter server. In extension to the common
-``has_acceleration`` and ``max_acceleration`` parameter we added the
-ability to also set ``has_deceleration`` and
-``max_deceleration``\ (<0.0).
+in both the URDF and a parameter file, setting acceleration limits is
+only possible using a parameter file. In addition to the common
+``has_acceleration`` and ``max_acceleration`` parameters, we added the
+ability to also set ``has_deceleration`` and ``max_deceleration``\ (<0.0).
 
 The limits are merged under the premise that the limits from the
-parameter server must be stricter or at least equal to the parameters
+node parameters must be stricter or at least equal to the parameters
 set in the URDF.
 
-Currently the calculated trajectory will respect the limits by using the
+Currently, the calculated trajectory will respect the limits by using the
 strictest combination of all limits as a common limit for all joints.
 
 Cartesian Limits
 ----------------
 
-For cartesian trajectory generation (LIN/CIRC) the planner needs an
-information about the maximum speed in 3D cartesian space. Namely
+For Cartesian trajectory generation (LIN/CIRC), the planner needs
+information about the maximum speed in 3D Cartesian space. Namely,
 translational/rotational velocity/acceleration/deceleration need to be
-set on the parameter server like this:
+set in the node parameters like this:
 
 .. code:: yaml
 
@@ -60,22 +52,25 @@ set on the parameter server like this:
       max_trans_dec: -5
       max_rot_vel: 1.57
 
+You can specify Cartesian velocity and acceleration limits in a file named
+``pilz_cartesian_limits.yaml`` in your ``*_moveit_config`` package.
+
 The planners assume the same acceleration ratio for translational and
-rotational trapezoidal shapes. So the rotational acceleration is
-calculated as ``max\_trans\_acc / max\_trans\_vel \* max\_rot\_vel`` (and
-for deceleration accordingly).
+rotational trapezoidal shapes. The rotational acceleration is
+calculated as ``max_trans_acc / max_trans_vel * max_rot_vel``
+(and for deceleration accordingly).
 
 Planning Interface
 ------------------
 
-This package uses ``moveit_msgs::MotionPlanRequest`` and ``moveit_msgs::MotionPlanResponse``
+This package uses ``moveit_msgs::msgs::MotionPlanRequest`` and ``moveit_msgs::msg::MotionPlanResponse``
 as input and output for motion planning. The parameters needed for each planning algorithm
 are explained below.
 
-For a general introduction on how to fill a ``MotionPlanRequest`` see
+For a general introduction on how to fill a ``MotionPlanRequest``, see
 :ref:`move_group_interface-planning-to-pose-goal`.
 
-You can specify "PTP", "LIN" or "CIRC" as the ``planner\_id``of the ``MotionPlanRequest``.
+You can specify ``"PTP"``, ``"LIN"`` or ``"CIRC"`` as the ``planner_id`` of the ``MotionPlanRequest``.
 
 The PTP motion command
 ----------------------
@@ -95,12 +90,12 @@ phases as the lead axis.
 PTP Input Parameters in ``moveit_msgs::MotionPlanRequest``
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
-- ``planner_id``: PTP
-- ``group_name``: name of the planning group
+- ``planner_id``: ``"PTP"``
+- ``group_name``: the name of the planning group
 - ``max_velocity_scaling_factor``: scaling factor of maximal joint velocity
 - ``max_acceleration_scaling_factor``: scaling factor of maximal joint acceleration/deceleration
-- ``start_state/joint_state/(name, position and velocity``: joint name/position/velocity(optional) of the start state.
-- ``goal_constraints`` (goal can be given in joint space or Cartesian space)
+- ``start_state/joint_state/(name, position and velocity)``: joint name/position/velocity (optional) of the start state.
+- ``goal_constraints``: (goal can be given in joint space or Cartesian space)
 - for a goal in joint space
     - ``goal_constraints/joint_constraints/joint_name``: goal joint name
     - ``goal_constraints/joint_constraints/position``: goal joint position
@@ -123,13 +118,13 @@ PTP Planning Result in ``moveit_msg::MotionPlanResponse``
    positions/velocities/accelerations of all joints (same order as the
    joint names) and time from start. The last point will have zero
    velocity and acceleration.
--  ``group_name``: name of the planning group
+-  ``group_name``: the name of the planning group
 -  ``error_code/val``: error code of the motion planning
 
 The LIN motion command
 ----------------------
 
-This planner generates linear Cartesian trajectory between goal and
+This planner generates a linear Cartesian trajectory between goal and
 start poses. The planner uses the Cartesian limits to generate a
 trapezoidal velocity profile in Cartesian space. The translational
 motion is a linear interpolation between start and goal position vector.
@@ -143,8 +138,8 @@ violation of joint space limits.
 LIN Input Parameters in ``moveit_msgs::MotionPlanRequest``
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
--  ``planner_id``: LIN
--  ``group_name``: name of the planning group
+-  ``planner_id``: ``"LIN"``
+-  ``group_name``: the name of the planning group
 -  ``max_velocity_scaling_factor``: scaling factor of maximal Cartesian
    translational/rotational velocity
 -  ``max_acceleration_scaling_factor``: scaling factor of maximal
@@ -184,7 +179,7 @@ LIN Planning Result in ``moveit_msg::MotionPlanResponse``
    positions/velocities/accelerations of all joints (same order as the
    joint names) and time from start. The last point will have zero
    velocity and acceleration.
--  ``group_name``: name of the planning group
+-  ``group_name``: the name of the planning group
 -  ``error_code/val``: error code of the motion planning
 
 The CIRC motion command
@@ -206,16 +201,16 @@ velocity/acceleration/deceleration need to be set and the planner uses
 these limits to generate a trapezoidal velocity profile in Cartesian
 space. The rotational motion is quaternion slerp between start and goal
 orientation. The translational and rotational motion is synchronized in
-time. This planner only accepts start state with zero velocity. Planning
+time. This planner only accepts start state with zero velocity. The planning
 result is a joint trajectory. The user needs to adapt the Cartesian
 velocity/acceleration scaling factor if motion plan fails due to
 violation of joint limits.
 
 CIRC Input Parameters in ``moveit_msgs::MotionPlanRequest``
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
--  ``planner_id``: CIRC
--  ``group_name``: name of the planning group
+-  ``planner_id``: ``"CIRC"``
+-  ``group_name``: the name of the planning group
 -  ``max_velocity_scaling_factor``: scaling factor of maximal Cartesian
    translational/rotational velocity
 -  ``max_acceleration_scaling_factor``: scaling factor of maximal
@@ -262,42 +257,87 @@ CIRC Planning Result in ``moveit_msg::MotionPlanResponse``
    positions/velocities/accelerations of all joints (same order as the
    joint names) and time from start. The last point will have zero
    velocity and acceleration.
--  ``group_name``: name of the planning group
+-  ``group_name``: the name of the planning group
 -  ``error_code/val``: error code of the motion planning
 
-Example
--------
+Examples
+--------
 
 By running
 
 ::
 
-    roslaunch prbt_moveit_config demo.launch
+    ros2 launch moveit2_tutorials demo.launch.py rviz_config:=panda_hello_moveit.rviz
 
-the user can interact with the planner through rviz.
+you can interact with the planner through the RViz MotionPlanning panel.
 
 .. figure:: rviz_planner.png
    :alt: rviz figure
 
+To use the planner through the MoveGroup Interface, refer to
+:codedir:`the MoveGroup Interface C++ example <examples/pilz_industrial_motion_planner/src/pilz_move_group.cpp>`.
+To run this, execute the following commands in separate Terminals:
+
+::
+
+    ros2 launch moveit2_tutorials demo.launch.py rviz_config:=panda_hello_moveit.rviz
+    ros2 run moveit2_tutorials pilz_move_group
+
+
+To use the planner using MoveIt Task Constructor, refer to
+:codedir:`the MoveIt Task Constructor C++ example <examples/pilz_industrial_motion_planner/src/pilz_mtc.cpp>`.
+To run this, execute the following commands in separate Terminals:
+
+::
+
+    ros2 launch moveit2_tutorials mtc_demo.launch.py
+    ros2 launch moveit2_tutorials pilz_mtc.launch.py
+
 Using the planner
 -----------------
 
 The *pilz_industrial_motion_planner::CommandPlanner* is provided as a MoveIt Motion Planning
 Pipeline and, therefore, can be used with all other manipulators using
 MoveIt. Loading the plugin requires the param
-``/move_group/planning_plugin`` to be set to ``pilz_industrial_motion_planner::CommandPlanner``
+``/move_group/<pipeline_name>/planning_plugin`` to be set to ``pilz_industrial_motion_planner/CommandPlanner``
 before the ``move_group`` node is started.
+For example, the `panda_moveit_config package
+<https://github.com/ros-planning/moveit_resources/blob/ros2/panda_moveit_config>`_
+has a ``pilz_industrial_motion_planner`` pipeline set up as follows:
+
+
+::
+
+    ros2 param get /move_group pilz_industrial_motion_planner.planning_plugin
+
+    String value is: pilz_industrial_motion_planner/CommandPlanner
+
 
-To use the command planner cartesian limits have to be defined. The
+To use the command planner, Cartesian limits have to be defined. The
 limits are expected to be under the namespace
-``<robot_description>_planning``. Where ``<robot_description>`` refers
-to the parameter under which the URDF is loaded. E.g. if the URDF was
-loaded into ``/robot_description`` the cartesian limits have to be
-defined at ``/robot_description_planning``.
+``<robot_description>_planning``, where ``<robot_description>`` refers
+to the parameter name under which the URDF is loaded.
+For example, if the URDF was loaded into ``/robot_description`` the
+Cartesian limits have to be defined at ``/robot_description_planning``.
+
+You can set these using a ``pilz_cartesian_limits.yaml`` file in your
+``*_moveit_config`` package.
+An example showing this file can be found in `panda_moveit_config
+<https://github.com/ros-planning/moveit_resources/blob/ros2/panda_moveit_config/config/pilz_cartesian_limits.yaml>`_.
+
+To verify the limits were set correctly, you can check the parameters for your
+``move_group`` node. For example,
+
+::
+
+    ros2 param list /move_group --filter .*cartesian_limits
+
+    /move_group:
+        robot_description_planning.cartesian_limits.max_rot_vel
+        robot_description_planning.cartesian_limits.max_trans_acc
+        robot_description_planning.cartesian_limits.max_trans_dec
+        robot_description_planning.cartesian_limits.max_trans_vel
 
-An example showing the cartesian limits which have to be defined can be
-found in `prbt_moveit_config
-<https://github.com/ros-planning/moveit_resources/blob/master/prbt_moveit_config/config/cartesian_limits.yaml>`_.
 
 Sequence of multiple segments
 =============================
@@ -316,9 +356,10 @@ multiple groups (e.g. "Manipulator", "Gripper")
 User interface sequence capability
 ----------------------------------
 
-A specialized MoveIt capability takes a
-``moveit_msgs::MotionSequenceRequest`` as input. The request contains a
-list of subsequent goals as described above and an additional
+A specialized MoveIt functionality known as the
+:moveit_codedir:`command list manager<moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.h>`
+takes a ``moveit_msgs::msg::MotionSequenceRequest`` as input.
+The request contains a list of subsequent goals as described above and an additional
 ``blend_radius`` parameter. If the given ``blend_radius`` in meter is
 greater than zero, the corresponding trajectory is merged together with
 the following goal such that the robot does not stop at the current
@@ -330,8 +371,8 @@ the trajectory it would have taken without blending.
 .. figure:: blend_radius.png
    :alt: blend figure
 
-Implementation details are available `as pdf
-<https://github.com/ros-planning/moveit/blob/master/moveit_planners/pilz_industrial_motion_planner/doc/MotionBlendAlgorithmDescription.pdf>`_.
+Implementation details are available
+:moveit_codedir:`as PDF<moveit_planners/pilz_industrial_motion_planner/doc/MotionBlendAlgorithmDescription.pdf>`.
 
 Restrictions for ``MotionSequenceRequest``
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
@@ -345,24 +386,24 @@ Restrictions for ``MotionSequenceRequest``
 Action interface
 ~~~~~~~~~~~~~~~~
 
-In analogy to the ``MoveGroup`` action interface the user can plan and
+In analogy to the ``MoveGroup`` action interface, the user can plan and
 execute a ``moveit_msgs::MotionSequenceRequest`` through the action server
 at ``/sequence_move_group``.
 
 In one point the ``MoveGroupSequenceAction`` differs from the standard
 MoveGroup capability: If the robot is already at the goal position, the
 path is still executed. The underlying PlannerManager can check, if the
-constraints of an individual ``moveit_msgs::MotionPlanRequest`` are
+constraints of an individual ``moveit_msgs::msg::MotionPlanRequest`` are
 already satisfied but the ``MoveGroupSequenceAction`` capability doesn't
 implement such a check to allow moving on a circular or comparable path.
 
-See the ``pilz_robot_programming`` package for an `example python script
+See the ``pilz_robot_programming`` package for an `ROS1 python script
 <https://github.com/PilzDE/pilz_industrial_motion/blob/melodic-devel/pilz_robot_programming/examples/demo_program.py>`_
 that shows how to use the capability.
 
 Service interface
 ~~~~~~~~~~~~~~~~~
 
 The service ``plan_sequence_path`` allows the user to generate a joint
-trajectory for a ``moveit_msgs::MotionSequenceRequest``. The trajectory is
-returned and not executed.
+trajectory for a ``moveit_msgs::msg::MotionSequenceRequest``.
+The trajectory is returned and not executed.