# Copyright (c) 2021 Franka Emika GmbH # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # This file is an adapted version of # https://github.com/ros-planning/moveit_resources/blob/ca3f7930c630581b5504f3b22c40b4f82ee6369d/panda_moveit_config/launch/demo.launch.py import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import (DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, Shutdown) from launch.conditions import UnlessCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare import yaml 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_ip_parameter_name = 'robot_ip' use_fake_hardware_parameter_name = 'use_fake_hardware' fake_sensor_commands_parameter_name = 'fake_sensor_commands' robot_ip = LaunchConfiguration(robot_ip_parameter_name) use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name) fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name) # Command-line arguments db_arg = DeclareLaunchArgument( 'db', default_value='False', description='Database flag' ) # planning_context franka_xacro_file = os.path.join(get_package_share_directory('franka_description'), 'robots', 'panda_arm.urdf.xacro') robot_description_config = Command( [FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=false', ' robot_ip:=', robot_ip, ' use_fake_hardware:=', use_fake_hardware, ' fake_sensor_commands:=', fake_sensor_commands]) robot_description = {'robot_description': robot_description_config} franka_semantic_xacro_file = os.path.join(get_package_share_directory('franka_moveit_config'), 'srdf', 'panda_arm.srdf.xacro') robot_description_semantic_config = Command( [FindExecutable(name='xacro'), ' ', franka_semantic_xacro_file, ' hand:=false'] ) robot_description_semantic = { 'robot_description_semantic': robot_description_semantic_config } kinematics_yaml = load_yaml( 'franka_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/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, } } ompl_planning_yaml = load_yaml( 'franka_moveit_config', 'config/ompl_planning.yaml' ) ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml) # Trajectory Execution Functionality moveit_simple_controllers_yaml = load_yaml( 'franka_moveit_config', 'config/panda_controllers.yaml' ) moveit_controllers = { 'moveit_simple_controller_manager': moveit_simple_controllers_yaml, 'moveit_controller_manager': 'moveit_simple_controller_manager' '/MoveItSimpleControllerManager', } trajectory_execution = { 'moveit_manage_controllers': True, 'trajectory_execution.allowed_execution_duration_scaling': 1.2, 'trajectory_execution.allowed_goal_duration_margin': 0.5, 'trajectory_execution.allowed_start_tolerance': 0.01, } planning_scene_monitor_parameters = { 'publish_planning_scene': True, 'publish_geometry_updates': True, 'publish_state_updates': True, 'publish_transforms_updates': True, } # Start the actual move_group node/action server run_move_group_node = Node( package='moveit_ros_move_group', executable='move_group', output='screen', parameters=[ robot_description, robot_description_semantic, kinematics_yaml, ompl_planning_pipeline_config, trajectory_execution, moveit_controllers, planning_scene_monitor_parameters, ], ) # RViz rviz_base = os.path.join(get_package_share_directory('franka_moveit_config'), 'rviz') rviz_full_config = os.path.join(rviz_base, 'moveit.rviz') rviz_node = Node( package='rviz2', executable='rviz2', name='rviz2', output='log', arguments=['-d', rviz_full_config], parameters=[ robot_description, robot_description_semantic, ompl_planning_pipeline_config, kinematics_yaml, ], ) # Publish TF robot_state_publisher = Node( package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', output='both', parameters=[robot_description], ) ros2_controllers_path = os.path.join( get_package_share_directory('franka_moveit_config'), 'config', 'panda_ros_controllers.yaml', ) ros2_control_node = Node( package='controller_manager', executable='ros2_control_node', parameters=[robot_description, ros2_controllers_path], remappings=[('joint_states', 'franka/joint_states')], output={ 'stdout': 'screen', 'stderr': 'screen', }, on_exit=Shutdown(), ) # Load controllers load_controllers = [] for controller in ['panda_arm_controller', 'joint_state_broadcaster']: load_controllers += [ ExecuteProcess( cmd=['ros2 run controller_manager spawner {}'.format(controller)], shell=True, output='screen', ) ] joint_state_publisher = Node( package='joint_state_publisher', executable='joint_state_publisher', name='joint_state_publisher', parameters=[ {'source_list': ['franka/joint_states'], 'rate': 30}], ) franka_robot_state_broadcaster = Node( package='controller_manager', executable='spawner', arguments=['franka_robot_state_broadcaster'], output='screen', condition=UnlessCondition(use_fake_hardware), ) moveit_interface = Node( package='franka_moveit_interface', executable='franka_moveit_interface', name='franka_moveit_interface', output='screen', parameters=[ robot_description, robot_description_semantic, kinematics_yaml, ompl_planning_pipeline_config, trajectory_execution, moveit_controllers, planning_scene_monitor_parameters, ], ) robot_arg = DeclareLaunchArgument( robot_ip_parameter_name, description='Hostname or IP address of the robot.') use_fake_hardware_arg = DeclareLaunchArgument( use_fake_hardware_parameter_name, default_value='false', description='Use fake hardware') fake_sensor_commands_arg = DeclareLaunchArgument( fake_sensor_commands_parameter_name, default_value='false', description="Fake sensor commands. Only valid when '{}' is true".format( use_fake_hardware_parameter_name)) return LaunchDescription( [robot_arg, use_fake_hardware_arg, fake_sensor_commands_arg, db_arg, rviz_node, robot_state_publisher, run_move_group_node, ros2_control_node, joint_state_publisher, franka_robot_state_broadcaster, moveit_interface, ] + load_controllers )