Add subframe tutorial

CMakeLists.txt

@@ -65,7 +65,7 @@ add_subdirectory(doc/planning_scene_ros_api)
 add_subdirectory(doc/quickstart_in_rviz)
 # add_subdirectory(doc/robot_model_and_robot_state)
 # add_subdirectory(doc/state_display)
-# add_subdirectory(doc/subframes)
+add_subdirectory(doc/subframes)
 # add_subdirectory(doc/tests)
 # add_subdirectory(doc/trajopt_planner)
 # add_subdirectory(doc/creating_moveit_plugins/lerp_motion_planner)

doc/move_group_interface/launch/move_group.launch.py

@@ -57,7 +57,7 @@ def generate_launch_description():
     ompl_planning_pipeline_config = {
         "move_group": {
             "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""",
+            "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames 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,
         }
     }

doc/moveit_cpp/launch/moveit_cpp_tutorial.launch.py

@@ -68,7 +68,7 @@ def generate_launch_description():
     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""",
+            "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames 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,
         }
     }

doc/quickstart_in_rviz/launch/demo.launch.py

@@ -65,7 +65,7 @@ def generate_launch_description():
     ompl_planning_pipeline_config = {
         "move_group": {
             "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""",
+            "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames 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,
         }
     }

doc/subframes/CMakeLists.txt

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

doc/subframes/launch/subframes_tutorial.launch.py

@@ -0,0 +1,63 @@
+import os
+import xacro
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+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():
+    # planning_context
+    robot_description_config = xacro.process_file(
+        os.path.join(
+            get_package_share_directory("moveit_resources_panda_moveit_config"),
+            "config",
+            "panda.urdf.xacro",
+        )
+    )
+    robot_description = {"robot_description": robot_description_config.toxml()}
+
+    robot_description_semantic_config = load_file(
+        "moveit_resources_panda_moveit_config", "config/panda.srdf"
+    )
+    robot_description_semantic = {
+        "robot_description_semantic": robot_description_semantic_config
+    }
+
+    kinematics_yaml = load_yaml(
+        "moveit_resources_panda_moveit_config", "config/kinematics.yaml"
+    )
+
+    # MoveGroupInterface demo executable
+    subframes = Node(
+        name="subframes_tutorial",
+        package="moveit2_tutorials",
+        executable="subframes_tutorial",
+        prefix="xterm -e",
+        output="screen",
+        parameters=[robot_description, robot_description_semantic, kinematics_yaml],
+    )
+
+    return LaunchDescription([subframes])