Pilz examples and update to ROS2

CMakeLists.txt

@@ -58,6 +58,7 @@ add_subdirectory(doc/examples/motion_planning_pipeline)
 add_subdirectory(doc/examples/move_group_interface)
 add_subdirectory(doc/examples/moveit_cpp)
 add_subdirectory(doc/examples/persistent_scenes_and_states)
+add_subdirectory(doc/examples/pilz_industrial_motion_planner)
 add_subdirectory(doc/examples/planning_scene_ros_api)
 add_subdirectory(doc/examples/planning_scene)
 add_subdirectory(doc/examples/realtime_servo)

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,32 +1,27 @@
-: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
+motions like point-to-point, linear, and circular with the interface of a MoveIt PlannerManager
 plugin.
 
 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
+(``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.
+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`.
 
 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 parameter server. 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.
@@ -47,8 +42,8 @@ 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:
 
@@ -60,22 +55,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
+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
 :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,7 +93,7 @@ phases as the lead axis.
 Input parameters in ``moveit_msgs::MotionPlanRequest``
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
-- ``planner_id``: PTP
+- ``planner_id``: ``"PTP"``
 - ``group_name``: 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
@@ -129,7 +127,7 @@ Planning results in ``moveit_msg::MotionPlanResponse``
 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,7 +141,7 @@ violation of joint space limits.
 Input parameters in ``moveit_msgs::MotionPlanRequest``
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
--  ``planner_id``: LIN
+-  ``planner_id``: ``"LIN"``
 -  ``group_name``: name of the planning group
 -  ``max_velocity_scaling_factor``: scaling factor of maximal Cartesian
    translational/rotational velocity
@@ -214,7 +212,7 @@ violation of joint limits.
 Input parameters in ``moveit_msgs::MotionPlanRequest``
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
--  ``planner_id``: CIRC
+-  ``planner_id``: ``"CIRC"``
 -  ``group_name``: name of the planning group
 -  ``max_velocity_scaling_factor``: scaling factor of maximal Cartesian
    translational/rotational velocity
@@ -265,39 +263,84 @@ Planning results in ``moveit_msg::MotionPlanResponse``
 -  ``group_name``: 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:
 
-To use the command planner cartesian limits have to be defined. The
+
+::
+
+    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
 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 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 these 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
 =============================
@@ -317,7 +360,7 @@ User interface sequence capability
 ----------------------------------
 
 A specialized MoveIt capability takes a
-``moveit_msgs::MotionSequenceRequest`` as input. The request contains 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
@@ -330,7 +373,7 @@ the trajectory it would have taken without blending.
 .. figure:: blend_radius.png
    :alt: blend figure
 
-Implementation details are available `as pdf
+Implementation details are available `as PDF
 <https://github.com/ros-planning/moveit/blob/master/moveit_planners/pilz_industrial_motion_planner/doc/MotionBlendAlgorithmDescription.pdf>`_.
 
 Restrictions for ``MotionSequenceRequest``
@@ -352,17 +395,17 @@ 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.