Pilz Sequence Motion MoveGroupSequence example (#917)

doc/how_to_guides/pilz_industrial_motion_planner/CMakeLists.txt

@@ -6,10 +6,15 @@ add_executable(pilz_mtc src/pilz_mtc.cpp)
 target_include_directories(pilz_mtc PUBLIC include)
 ament_target_dependencies(pilz_mtc ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
+add_executable(pilz_sequence src/pilz_sequence.cpp)
+target_include_directories(pilz_sequence PUBLIC include)
+ament_target_dependencies(pilz_sequence ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
 install(
   TARGETS
     pilz_move_group
     pilz_mtc
+    pilz_sequence
   DESTINATION
     lib/${PROJECT_NAME}
 )

doc/how_to_guides/pilz_industrial_motion_planner/launch/pilz_moveit.launch.py

@@ -0,0 +1,111 @@
+import os
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import ExecuteProcess
+from ament_index_python.packages import get_package_share_directory
+from moveit_configs_utils import MoveItConfigsBuilder
+
+
+def generate_launch_description():
+    moveit_config = (
+        MoveItConfigsBuilder("moveit_resources_panda")
+        .robot_description(file_path="config/panda.urdf.xacro")
+        .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
+        .planning_scene_monitor(
+            publish_robot_description=True, publish_robot_description_semantic=True
+        )
+        .planning_pipelines(pipelines=["pilz_industrial_motion_planner"])
+        .to_moveit_configs()
+    )
+
+    # Starts Pilz Industrial Motion Planner MoveGroupSequenceAction and MoveGroupSequenceService servers
+    move_group_capabilities = {
+        "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService"
+    }
+
+    # Start the actual move_group node/action server
+    run_move_group_node = Node(
+        package="moveit_ros_move_group",
+        executable="move_group",
+        output="screen",
+        parameters=[
+            moveit_config.to_dict(),
+            move_group_capabilities,
+        ],
+    )
+
+    # RViz
+    rviz_config_file = (
+        get_package_share_directory("moveit2_tutorials") + "/launch/move_group.rviz"
+    )
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config_file],
+        parameters=[
+            moveit_config.robot_description,
+            moveit_config.robot_description_semantic,
+            moveit_config.robot_description_kinematics,
+            moveit_config.planning_pipelines,
+            moveit_config.joint_limits,
+        ],
+    )
+
+    # 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", "panda_link0"],
+    )
+
+    # Publish TF
+    robot_state_publisher = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="both",
+        parameters=[moveit_config.robot_description],
+    )
+
+    # ros2_control using FakeSystem as hardware
+    ros2_controllers_path = os.path.join(
+        get_package_share_directory("moveit_resources_panda_moveit_config"),
+        "config",
+        "ros2_controllers.yaml",
+    )
+    ros2_control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[moveit_config.robot_description, ros2_controllers_path],
+        output="both",
+    )
+
+    # Load controllers
+    load_controllers = []
+    for controller in [
+        "panda_arm_controller",
+        "panda_hand_controller",
+        "joint_state_broadcaster",
+    ]:
+        load_controllers += [
+            ExecuteProcess(
+                cmd=["ros2 run controller_manager spawner {}".format(controller)],
+                shell=True,
+                output="screen",
+            )
+        ]
+
+    return LaunchDescription(
+        [
+            rviz_node,
+            static_tf,
+            robot_state_publisher,
+            run_move_group_node,
+            ros2_control_node,
+        ]
+        + load_controllers
+    )