From e4550af663a3303ca0e3bc10c28358d0fd33902d Mon Sep 17 00:00:00 2001 From: "David V. Lu" Date: Fri, 19 Mar 2021 13:54:10 -0400 Subject: [PATCH] New Launch --- stretch_demo.repos | 12 +++- stretch_moveit_config/launch/demo.launch.py | 16 +---- stretch_plan_client/CMakeLists.txt | 2 + stretch_plan_client/launch/demo.launch.py | 65 +++++++++++++++++++++ 4 files changed, 78 insertions(+), 17 deletions(-) create mode 100644 stretch_plan_client/launch/demo.launch.py diff --git a/stretch_demo.repos b/stretch_demo.repos index d57700f..333636d 100644 --- a/stretch_demo.repos +++ b/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 diff --git a/stretch_moveit_config/launch/demo.launch.py b/stretch_moveit_config/launch/demo.launch.py index 3cb4d6d..6dd3d1a 100644 --- a/stretch_moveit_config/launch/demo.launch.py +++ b/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) diff --git a/stretch_plan_client/CMakeLists.txt b/stretch_plan_client/CMakeLists.txt index 618955a..801b9a1 100644 --- a/stretch_plan_client/CMakeLists.txt +++ b/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) diff --git a/stretch_plan_client/launch/demo.launch.py b/stretch_plan_client/launch/demo.launch.py new file mode 100644 index 0000000..3702139 --- /dev/null +++ b/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])