New Launch

stretch_demo.repos

@@ -5,8 +5,8 @@ repositories:
     version: diff_drive_plus_fake
   srdfdom:
     type: git
-    url: https://github.com/DLu/srdfdom
-    version: diff_drive_joint2
+    url: https://github.com/ros-planning/srdfdom
+    version: ros2
   stretch_kinematics_plugin:
     type: git
     url: https://github.com/PickNikRobotics/stretch_kinematics_plugin
@@ -17,6 +17,14 @@ repositories:
     version: ros2_diffdrive
 
   # Unrelated to this demo, but needed for building
+  ros2_control:
+    type: git
+    url: https://github.com/ros-controls/ros2_control
+    version: master
+  ros2_controllers:
+    type: git
+    url: https://github.com/ros-controls/ros2_controllers
+    version: master
   warehouse_ros_mongo:
     type: git
     url: https://github.com/ros-planning/warehouse_ros_mongo

stretch_moveit_config/launch/demo.launch.py

@@ -109,20 +109,6 @@ def generate_launch_description():
         parameters=[robot_description,  os.path.join(get_package_share_directory("stretch_moveit_config"), "config", "ros_controllers.yaml")],
     )
 
-    client_node = Node(
-        package='stretch_plan_client',
-        executable='stretch_plan_client',
-        parameters=[robot_description,
-                    robot_description_semantic,
-                    ompl_planning_pipeline_config,
-                    kinematics_yaml],
-        output={
-            'stdout': 'screen',
-            'stderr': 'screen',
-        },
-    )
-
-
     # load joint_state_controller
     load_joint_state_controller = ExecuteProcess(cmd=['ros2 control load_start_controller joint_state_controller'], shell=True, output='screen')
 
@@ -130,4 +116,4 @@ def generate_launch_description():
     for controller in ['stretch_arm_controller', 'gripper_controller']:
         load_controllers += [ExecuteProcess(cmd=['ros2 control load_configure_controller', controller], shell=True, output='screen', on_exit=[ExecuteProcess(cmd=['ros2 control switch_controllers --start-controllers', controller], shell=True, output='screen')])]
 
-    return LaunchDescription([client_node, rviz_node, static_tf, robot_state_publisher, run_move_group_node, fake_joint_driver_node ] + load_controllers)
+    return LaunchDescription([rviz_node, static_tf, robot_state_publisher, run_move_group_node, fake_joint_driver_node ] + load_controllers)

stretch_plan_client/CMakeLists.txt

@@ -13,3 +13,5 @@ ament_package()
 install(TARGETS stretch_plan_client
         RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}
+  PATTERN "setup_assistant.launch" EXCLUDE)

stretch_plan_client/launch/demo.launch.py

@@ -0,0 +1,65 @@
+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():
+
+    # planning_context
+    robot_description_config = xacro.process_file(os.path.join(get_package_share_directory('stretch_moveit_config'),
+                                                               'config',
+                                                               'stretch.xacro'))
+    robot_description = {'robot_description' : robot_description_config.toxml()}
+
+    robot_description_semantic_config = load_file('stretch_moveit_config', 'config/stretch_description.srdf')
+    robot_description_semantic = {'robot_description_semantic' : robot_description_semantic_config}
+
+    kinematics_yaml = load_yaml('stretch_moveit_config', 'config/kinematics.yaml')
+
+    # Planning Functionality
+    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""" ,
+        'start_state_max_bounds_error' : 0.1 } }
+    ompl_planning_yaml = load_yaml('stretch_moveit_config', 'config/ompl_planning.yaml')
+    ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
+
+
+    client_node = Node(
+        package='stretch_plan_client',
+        executable='stretch_plan_client',
+        parameters=[robot_description,
+                    robot_description_semantic,
+                    ompl_planning_pipeline_config,
+                    kinematics_yaml],
+        output={
+            'stdout': 'screen',
+            'stderr': 'screen',
+        },
+    )
+
+    return LaunchDescription([client_node])