[WIP]: motion_planning_pipeline tutorial ported to ROS2

CMakeLists.txt

@@ -16,7 +16,7 @@ find_package(moveit_ros_perception REQUIRED)
 find_package(moveit_servo REQUIRED)
 find_package(interactive_markers REQUIRED)
 find_package(rviz_visual_tools REQUIRED)
-#find_package(moveit_visual_tools REQUIRED)
+find_package(moveit_visual_tools REQUIRED)
 find_package(geometric_shapes REQUIRED)
 #find_package(pcl_ros REQUIRED)
 #find_package(pcl_conversions REQUIRED)
@@ -40,7 +40,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   tf2_ros
   moveit_core
   rviz_visual_tools
-  # moveit_visual_tools
+  moveit_visual_tools
   moveit_ros_planning_interface
   interactive_markers
   tf2_geometry_msgs
@@ -57,8 +57,8 @@ include_directories(${THIS_PACKAGE_INCLUDE_DIRS})
 # add_subdirectory(doc/controller_configuration)
 # add_subdirectory(doc/interactivity)
 # add_subdirectory(doc/kinematics)
-# add_subdirectory(doc/motion_planning_api)
-# add_subdirectory(doc/motion_planning_pipeline)
+add_subdirectory(doc/motion_planning_api)
+add_subdirectory(doc/motion_planning_pipeline)
 add_subdirectory(doc/move_group_interface)
 # add_subdirectory(doc/move_group_python_interface)
 # add_subdirectory(doc/perception_pipeline)

doc/motion_planning_api/CMakeLists.txt

@@ -1,8 +1,13 @@
 add_executable(motion_planning_api_tutorial
     src/motion_planning_api_tutorial.cpp)
-target_link_libraries(motion_planning_api_tutorial
-    ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-install(TARGETS motion_planning_api_tutorial DESTINATION
-    ${CATKIN_PACKAGE_BIN_DESTINATION})
+target_include_directories(motion_planning_api_tutorial
+    PUBLIC include)
+ament_target_dependencies(motion_planning_api_tutorial
+    ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
 
-install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(TARGETS motion_planning_api_tutorial
+  DESTINATION lib/${PROJECT_NAME}
+)
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)

doc/motion_planning_api/launch/motion_planning_api_tutorial.launch.py

@@ -0,0 +1,73 @@
+import os
+import yaml
+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
+import xacro
+
+
+def load_file(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, "r") as file:
+            return file.read()
+    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+def load_yaml(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, "r") as file:
+            return yaml.safe_load(file)
+    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+def generate_launch_description():
+
+    robot_description_config = load_file(
+        "moveit_resources_panda_description", "urdf/panda.urdf"
+    )
+
+    robot_description_semantic_config = load_file(
+        "moveit_resources_panda_moveit_config", "config/panda.srdf"
+    )
+
+    kinematics_yaml = load_yaml(
+        "moveit_resources_panda_moveit_config", "config/kinematics.yaml"
+    )
+
+    ompl_planning_pipeline_config = {
+        "ompl": {
+            "planning_plugin": "ompl_interface/OMPLPlanner",
+            "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization 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,
+        }
+    }
+    ompl_planning_yaml = load_yaml(
+        "moveit_resources_panda_moveit_config", "config/ompl_planning.yaml"
+    )
+    ompl_planning_pipeline_config["ompl"].update(ompl_planning_yaml)
+
+    # Start the actual move_group node
+    motion_planning_node = Node(
+        package="moveit2_tutorials",
+        executable="motion_planning_api_tutorial",
+        prefix="xterm -e",
+        output="screen",
+        parameters=[
+            {"robot_description": robot_description_config},
+            {"robot_description_semantic": robot_description_semantic_config},
+            {"planning_plugin": "ompl_interface/OMPLPlanner"},
+            ompl_planning_pipeline_config,
+            kinematics_yaml,
+        ],
+    )
+
+    return LaunchDescription([motion_planning_node])

doc/motion_planning_api/motion_planning_api_tutorial.rst

@@ -1,8 +1,3 @@
-:moveit1:
-
-..
-   Once updated for MoveIt 2, remove all lines above title (including this comment and :moveit1: tag)
-
 Motion Planning API
 ==================================
 .. image:: motion_planning_api_tutorial_robot_move_arm_1st.png
@@ -16,20 +11,20 @@ Getting Started
 ---------------
 If you haven't already done so, make sure you've completed the steps in `Getting Started <../getting_started/getting_started.html>`_.
 
+This tutoral has made use of **xterm** and a simple prompter to help the user progress through each demo step.
+To install xterm please run the following command: ::
+
+   sudo apt-get install -y xterm
+
 Running the Demo
 ----------------
 Open two shells. In the first shell start RViz and wait for everything to finish loading: ::
 
-  roslaunch panda_moveit_config demo.launch
+  ros2 launch moveit_resources_panda_moveit_config demo.launch.py
 
 In the second shell, run the launch file: ::
 
-  roslaunch moveit_tutorials motion_planning_api_tutorial.launch
-
-**Note:** This tutorial uses the **RvizVisualToolsGui** panel to step through the demo. To add this panel to RViz, follow the instructions in the `Visualization Tutorial <../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#rviz-visual-tools>`_.
-
-After a short moment, the RViz window should appear and look similar to the one at the top of this page. To progress through each demo step either press the **Next** button in the **RvizVisualToolsGui** panel at the bottom of the screen or select **Key Tool** in the **Tools** panel at the top of the screen and then press **N** on your keyboard while RViz is focused.
-
+  ros2 launch moveit2_tutorials motion_planning_api_tutorial.launch.py
 
 Expected Output
 ---------------