import os from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess ) from ament_index_python.packages import get_package_share_directory from launch.conditions import IfCondition, 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 from ur_moveit_config.launch_common import load_yaml def generate_launch_description(): declared_arguments = [] # UR specific arguments declared_arguments.append( DeclareLaunchArgument( "rbs_robot_type", description="Type of robot by name", choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"], default_value="ur5e", ) ) declared_arguments.append( DeclareLaunchArgument( "with_gripper", default_value="true", description="With gripper or not?", ) ) declared_arguments.append( DeclareLaunchArgument( "safety_limits", default_value="true", description="Enables the safety limits controller if true.", ) ) declared_arguments.append( DeclareLaunchArgument( "safety_pos_margin", default_value="0.15", description="The margin to lower and upper limits in the safety controller.", ) ) declared_arguments.append( DeclareLaunchArgument( "safety_k_position", default_value="20", description="k-position factor in the safety controller.", ) ) # General arguments declared_arguments.append( DeclareLaunchArgument( "runtime_config_package", default_value="ur_moveit_config", description='Package with the controller\'s configuration in "config" folder. \ Usually the argument is not set, it enables use of a custom setup.', ) ) declared_arguments.append( DeclareLaunchArgument( "controllers_file", default_value="ur_controllers.yaml", description="YAML file with the controllers configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "controllers_with_gripper_file", default_value="ur_plus_gripper_controllers.yaml", description="YAML file with the UR + gripper_controller configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "description_package", default_value="ur_description", description="Description package with robot URDF/XACRO files. Usually the argument \ is not set, it enables use of a custom description.", ) ) declared_arguments.append( DeclareLaunchArgument( "description_file", default_value="ur.urdf.xacro", description="URDF/XACRO description file with the robot.", ) ) declared_arguments.append( DeclareLaunchArgument( "prefix", default_value='""', description="Prefix of the joint names, useful for \ multi-robot setup. If changed than also joint names in the controllers' configuration \ have to be updated.", ) ) declared_arguments.append( DeclareLaunchArgument( "start_joint_controller", default_value="true", description="Enable headless mode for robot control", ) ) declared_arguments.append( DeclareLaunchArgument( "initial_joint_controller", default_value="joint_trajectory_controller", description="Robot controller to start.", ) ) declared_arguments.append( DeclareLaunchArgument( "initial_gripper_controller", default_value="gripper_controller", description="Robot controller to start.", ) ) declared_arguments.append( DeclareLaunchArgument( "moveit_config_package", default_value="ur_moveit_config", description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \ is not set, it enables use of a custom moveit config.", ) ) declared_arguments.append( DeclareLaunchArgument( "moveit_config_file", default_value="ur.srdf.xacro", description="MoveIt SRDF/XACRO description file with the robot.", ) ) declared_arguments.append( DeclareLaunchArgument( "use_sim_time", default_value="true", description="Make MoveIt to use simulation time. This is needed for the trajectory planing in simulation.", ) ) declared_arguments.append( DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?") ) # Initialize Arguments rbs_robot_type = LaunchConfiguration("rbs_robot_type") safety_limits = LaunchConfiguration("safety_limits") safety_pos_margin = LaunchConfiguration("safety_pos_margin") safety_k_position = LaunchConfiguration("safety_k_position") # General arguments runtime_config_package = LaunchConfiguration("runtime_config_package") with_gripper_condition = LaunchConfiguration("with_gripper") controllers_file = LaunchConfiguration("controllers_with_gripper_file") description_package = LaunchConfiguration("description_package") description_file = LaunchConfiguration("description_file") prefix = LaunchConfiguration("prefix") start_joint_controller = LaunchConfiguration("start_joint_controller") initial_joint_controller = LaunchConfiguration("initial_joint_controller") initial_gripper_controller = LaunchConfiguration("initial_gripper_controller") launch_rviz = LaunchConfiguration("launch_rviz") moveit_config_package = LaunchConfiguration("moveit_config_package") moveit_config_file = LaunchConfiguration("moveit_config_file") use_sim_time = LaunchConfiguration("use_sim_time") initial_joint_controllers_file_path = PathJoinSubstitution( [FindPackageShare(runtime_config_package), "config", controllers_file] ) rviz_config_file = PathJoinSubstitution( [FindPackageShare(moveit_config_package), "rviz", "view_robot.rviz"] ) world_config_file = PathJoinSubstitution( [FindPackageShare("rbs_simulation"), "worlds", "mir.sdf"] ) robot_description_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), " ", PathJoinSubstitution( [FindPackageShare(description_package), "urdf", description_file] ), " ", "safety_limits:=", safety_limits, " ", "safety_pos_margin:=", safety_pos_margin, " ", "safety_k_position:=", safety_k_position, " ", "name:=", "ur", " ", "ur_type:=", rbs_robot_type, " ", "prefix:=", prefix, " ", "sim_ignition:=true", " ", "simulation_controllers:=", initial_joint_controllers_file_path, " ", "with_gripper:=", with_gripper_condition ] ) robot_description = {"robot_description": robot_description_content} robot_state_publisher_node = Node( package="robot_state_publisher", executable="robot_state_publisher", output="both", parameters=[{"use_sim_time": True}, robot_description], ) joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], ) # There may be other controllers of the joints, but this is the initially-started one initial_joint_controller_spawner_started = Node( package="controller_manager", executable="spawner", arguments=[initial_joint_controller, "--controller-manager", "/controller_manager"], condition=IfCondition(start_joint_controller), ) initial_joint_controller_spawner_stopped = Node( package="controller_manager", executable="spawner", arguments=[initial_joint_controller, "--controller-manager", "/controller_manager", "--stopped"], condition=UnlessCondition(start_joint_controller), ) gripper_controller = ExecuteProcess( cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', "gripper_controller"], output='screen', condition=IfCondition(with_gripper_condition) ) # Gazebo nodes gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource( [os.path.join(get_package_share_directory('ros_ign_gazebo'), 'launch', 'ign_gazebo.launch.py')]), launch_arguments=[('ign_args', [' -r ',world_config_file, " --physics-engine ignition-physics-dartsim-plugin --render-engine ogre2"])]) # Spawn robot gazebo_spawn_robot = Node(package='ros_ign_gazebo', executable='create', arguments=[ '-name', rbs_robot_type, '-x', '0.0', '-z', '0.0', '-y', '0.0', '-topic', '/robot_description'], output='screen') # MoveIt Configuration robot_description_semantic_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), " ", PathJoinSubstitution( [FindPackageShare(moveit_config_package), "srdf", moveit_config_file] ), " ", "name:=", "ur", " ", "prefix:=", prefix, " ", "with_gripper:=", with_gripper_condition ] ) robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content} robot_description_kinematics = PathJoinSubstitution( [FindPackageShare(moveit_config_package), "config", "kinematics.yaml"] ) # Planning Configuration 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("ur_moveit_config", "config/ompl_planning.yaml") ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) controllers_yaml = load_yaml("ur_moveit_config", "config/controllers.yaml") moveit_controllers = { "moveit_simple_controller_manager": controllers_yaml, "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", } trajectory_execution = { "moveit_manage_controllers": True, "trajectory_execution.allowed_execution_duration_scaling": 100.0, "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, } move_group_node = Node( package="moveit_ros_move_group", executable="move_group", output="screen", parameters=[ robot_description, robot_description_semantic, robot_description_kinematics, ompl_planning_pipeline_config, trajectory_execution, moveit_controllers, planning_scene_monitor_parameters, {"use_sim_time": use_sim_time}, ], ) rviz_node = Node( package="rviz2", executable="rviz2", name="rviz2", output="log", arguments=["-d", rviz_config_file], parameters=[ robot_description, robot_description_semantic, robot_description_kinematics, ], condition=IfCondition(launch_rviz), ) # TODO: Launch skill servers in other launch file move_topose_action_server = Node( package="rbs_skill_servers", executable="move_topose_action_server", parameters=[ robot_description, robot_description_semantic, robot_description_kinematics, {"use_sim_time": use_sim_time}, ] ) gripper_control_node = Node( package="rbs_skill_servers", executable="gripper_control_action_server", parameters= [ robot_description, robot_description_semantic, robot_description_kinematics, {"use_sim_time": use_sim_time}, ] ) move_topose_action_server = Node( package="rbs_skill_servers", executable="move_topose_action_server", parameters=[ robot_description, robot_description_semantic, robot_description_kinematics, {"use_sim_time": use_sim_time}, ] ) move_cartesian_path_action_server = Node( package="rbs_skill_servers", executable="move_cartesian_path_action_server", parameters=[ robot_description, robot_description_semantic, robot_description_kinematics, {"use_sim_time": use_sim_time}, ] ) move_joint_state_action_server = Node( package="rbs_skill_servers", executable="move_to_joint_states_action_server", parameters=[ robot_description, robot_description_semantic, robot_description_kinematics, {"use_sim_time": use_sim_time}, ] ) points_params = load_yaml("rbs_bt_executor", "config/gripperPositions.yaml") grasp_pose_loader = Node( package="rbs_skill_servers", executable="pick_place_pose_loader_service_server", output="screen", emulate_tty=True, parameters=[ points_params ] ) # add_planning_scene_object = Node( # package="rbs_skill_servers", # executable="add_planning_scene_object_service", # output="screen", # parameters=[ # robot_description, # robot_description_semantic, # robot_description_kinematics, # {"use_sim_time": use_sim_time}, # ] # ) nodes_to_start = [ robot_state_publisher_node, joint_state_broadcaster_spawner, rviz_node, initial_joint_controller_spawner_stopped, initial_joint_controller_spawner_started, gazebo, gazebo_spawn_robot, move_group_node, gripper_controller, gripper_control_node, move_topose_action_server, move_cartesian_path_action_server, move_joint_state_action_server, grasp_pose_loader, #add_planning_scene_object ] return LaunchDescription(declared_arguments + nodes_to_start)