import os from launch import LaunchDescription, LaunchContext from launch.actions import ( DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess, OpaqueFunction ) 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 import xacro 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="false", 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_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"] ) mujoco_model = PathJoinSubstitution( [FindPackageShare("rbs_simulation"), "mujoco_model", "current_mj.xml"] ) 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_mujoco:=true", " ", "simulation_controllers:=", initial_joint_controllers_file_path, " ", "with_gripper:=", with_gripper_condition, " ", "mujoco_model:=", mujoco_model, ] ) robot_description = {"robot_description": robot_description_content} control_node = Node( package="controller_manager", executable="ros2_control_node", parameters=[robot_description, initial_joint_controllers_file_path], #prefix=['xterm -e gdb -ex run --args'], output="both", remappings=[ ('motion_control_handle/target_frame', 'target_frame'), ('cartesian_motion_controller/target_frame', 'target_frame'), # ('cartesian_compliance_controller/target_frame', 'target_frame'), # ('cartesian_force_controller/target_wrench', 'target_wrench'), # ('cartesian_compliance_controller/target_wrench', 'target_wrench'), # ('cartesian_force_controller/ft_sensor_wrench', 'ft_sensor_wrench'), # ('cartesian_compliance_controller/ft_sensor_wrench', 'ft_sensor_wrench'), ] ) 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", "-c", "/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, "-c", "/controller_manager"], condition=IfCondition(start_joint_controller), ) initial_joint_controller_spawner_stopped = Node( package="controller_manager", executable="spawner", arguments=[initial_joint_controller, "-c", "/controller_manager", "--incactive"], 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) ) cartesian_motion_controller_spawner = Node( package="controller_manager", executable="spawner", arguments=["cartesian_motion_controller", "--inactive", "-c", "/controller_manager"], ) motion_control_handle_spawner = Node( package="controller_manager", executable="spawner", arguments=["motion_control_handle", "--inactive", "-c", "/controller_manager"], ) # 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 ] ) assemble_state = Node( package="rbs_skill_servers", executable="assemble_state_service_server", output="screen", parameters=[ {'assemble_prefix': 'ASSEMBLE_'}, {'assemble_dir': '/home/splinter1984/tmp_ws/src/robossembler-ros2/rbs_task_planner/example/sdf_models/'} ] ) moveit_planning_scene_init = Node( package="rbs_skill_servers", executable="moveit_update_planning_scene_service_server", output="screen", parameters=[ {'init_scene': world_config_file}, {'models_paths': os.environ['IGN_GAZEBO_RESOURCE_PATH']} ] ) # 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}, # ] # ) # remappings = [('/camera', '/camera/image'), # ('/camera_info', '/camera/camera_info')] # # Bridge # bridge = Node( # package='ros_gz_bridge', # executable='parameter_bridge', # arguments=[ # '/camera@sensor_msgs/msg/Image@gz.msgs.Image', # '/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo', # '/rgbd_camera/image@sensor_msgs/msg/Image@gz.msgs.Image', # '/rgbd_camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo', # '/rgbd_camera/depth_image@sensor_msgs/msg/Image@gz.msgs.Image', # '/rgbd_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked' # ], # output='screen', # remappings=remappings, # ) # pc_filter = Node( # package="rbs_perception", # executable="pc_filter", # output="screen", # #prefix=['xterm -e gdb -ex run --args'], # ) grasp_marker = Node( package="rbs_perception", executable="grasp_marker_publish.py", name="hyak", ) nodes_to_start = [ control_node, 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, assemble_state, moveit_planning_scene_init, #bridge, cartesian_motion_controller_spawner, motion_control_handle_spawner, #pc_filter #add_planning_scene_object ] return LaunchDescription(declared_arguments + nodes_to_start)