From ef563a5dc02762f7ab773a419fa8d5292db41c41 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Mon, 24 Jul 2023 14:57:23 +0300 Subject: [PATCH] split and sort launch file --- rbs_bringup/CMakeLists.txt | 31 ++ rbs_bringup/launch/bringup.launch.py | 342 ++++++++++++++++++ rbs_bringup/launch/control.launch.py | 124 +++++++ rbs_bringup/launch/moveit.launch.py | 201 ++++++++++ rbs_bringup/launch/perception.launch.py | 27 ++ rbs_bringup/launch/simulation.launch.py | 65 ++++ rbs_bringup/package.xml | 18 + .../launch/rbs_simulation.launch.py | 1 - .../launch/task_planner.launch.py | 22 +- 9 files changed, 826 insertions(+), 5 deletions(-) create mode 100644 rbs_bringup/CMakeLists.txt create mode 100644 rbs_bringup/launch/bringup.launch.py create mode 100644 rbs_bringup/launch/control.launch.py create mode 100644 rbs_bringup/launch/moveit.launch.py create mode 100644 rbs_bringup/launch/perception.launch.py create mode 100644 rbs_bringup/launch/simulation.launch.py create mode 100644 rbs_bringup/package.xml diff --git a/rbs_bringup/CMakeLists.txt b/rbs_bringup/CMakeLists.txt new file mode 100644 index 0000000..45cf5a8 --- /dev/null +++ b/rbs_bringup/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.8) +project(rbs_bringup) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/rbs_bringup/launch/bringup.launch.py b/rbs_bringup/launch/bringup.launch.py new file mode 100644 index 0000000..8ef2102 --- /dev/null +++ b/rbs_bringup/launch/bringup.launch.py @@ -0,0 +1,342 @@ +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="false", + 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?") + ) + + declared_arguments.append( + DeclareLaunchArgument("sim_gazebo", default_value="false", description="Gazebo Simulation") + ) + declared_arguments.append( + DeclareLaunchArgument("sim_mujoco", default_value="true", description="Mujoco Simulation") + ) + declared_arguments.append( + DeclareLaunchArgument("sim_fake", default_value="false", description="Mock contollers") + ) + declared_arguments.append( + DeclareLaunchArgument("env_manager", default_value="false", description="Launch env_manager?") + ) + + # 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") + 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") + sim_gazebo = LaunchConfiguration("sim_gazebo") + sim_mujoco = LaunchConfiguration("sim_mujoco") + sim_fake = LaunchConfiguration("sim_fake") + env_manager = LaunchConfiguration("env_manager") + + + 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"] + ) + + 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:=", sim_mujoco, " ", + "sim_gazebo:=", sim_gazebo, " ", + "sim_fake:=", sim_fake, " ", + "simulation_controllers:=", initial_joint_controllers_file_path, " ", + "with_gripper:=", with_gripper_condition, " ", + "mujoco_model:=", mujoco_model, + ] + ) + robot_description = {"robot_description": robot_description_content} + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[{"use_sim_time": True}, robot_description], + ) + + rviz = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + robot_description + ], + condition=IfCondition(launch_rviz),) + + control = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('rbs_bringup'), + 'launch', + 'control.launch.py' + ]) + ]), + launch_arguments={ + 'with_gripper': with_gripper_condition, + 'robot_description': robot_description, + 'start_joint_controller': start_joint_controller, + 'initial_joint_controller': initial_joint_controller, + 'controllers_file': controllers_file + }.items()) + + simulation = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('rbs_bringup'), + 'launch', + 'simulation.launch.py' + ]) + ]), + launch_arguments={ + 'sim_gazebo': sim_gazebo, + 'rbs_robot_type': rbs_robot_type, + 'env_manager': env_manager + }.items()) + + moveit = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('rbs_bringup'), + 'launch', + 'moveit.launch.py' + ]) + ]), + launch_arguments={ + 'robot_description': robot_description, + 'moveit_config_package': moveit_config_package, + 'moveit_config_file': moveit_config_file, + 'use_sim_time': use_sim_time, + 'prefix': prefix, + 'with_gripper': with_gripper_condition + }.items()) + + task_planner = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('rbs_task_planner'), + 'launch', + 'task_planner.launch.py' + ]) + ]), + launch_arguments={ + # TBD + }.items()) + + task_planner = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('rbs_task_planner'), + 'launch', + 'task_planner.launch.py' + ]) + ]), + launch_arguments={ + # TBD + }.items()) + + perception = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('rbs_bringup'), + 'launch', + 'perception.launch.py' + ]) + ]), + launch_arguments={ + # TBD + }.items()) + + nodes_to_start = [ + robot_state_publisher, + rviz, + control, + simulation, + moveit, + task_planner, + perception + ] + return LaunchDescription(declared_arguments + nodes_to_start) + + + + diff --git a/rbs_bringup/launch/control.launch.py b/rbs_bringup/launch/control.launch.py new file mode 100644 index 0000000..e386b6b --- /dev/null +++ b/rbs_bringup/launch/control.launch.py @@ -0,0 +1,124 @@ +from launch_ros.actions import Node +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition, UnlessCondition + + +def generate_launch_description(): + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "with_gripper", + default_value="false", + description="With gripper or not?", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "robot_description", + default_value="", + description="robot description param", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "start_joint_controller", + default_value="false", + 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( + "controllers_file", + default_value="ur_controllers.yaml", + description="YAML file with the controllers configuration.", + ) + ) + robot_description = LaunchConfiguration("robot_description") + runtime_config_package = LaunchConfiguration("runtime_config_package") + controllers_file = LaunchConfiguration("controllers_file") + start_joint_controller = LaunchConfiguration("start_joint_controller") + with_gripper_condition = LaunchConfiguration("with_gripper") + initial_joint_controller = LaunchConfiguration("initial_joint_controller") + + initial_joint_controllers_file_path = PathJoinSubstitution( + [FindPackageShare(runtime_config_package), "config", controllers_file] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, initial_joint_controllers_file_path], + output="both", + remappings=[ + ('motion_control_handle/target_frame', 'target_frame'), + ('cartesian_compliance_controller/target_frame', 'target_frame'), + ('cartesian_compliance_controller/target_wrench', 'target_wrench'), + ('cartesian_compliance_controller/ft_sensor_wrench', 'ft_sensor_wrench'), + ] + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "-c", "/controller_manager"], + ) + + # FIXME: Start controllers one controller by one launch or launch it all and switch by runtime? + 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", "--inactive"], + 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"], + ) + + cartesian_compliance_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["cartesian_compliance_controller", "--inactive", "-c", "/controller_manager"], + ) + + nodes_to_start = [ + control_node, + joint_state_broadcaster_spawner, + initial_joint_controller_spawner_started, + initial_joint_controller_spawner_stopped, + gripper_controller, + cartesian_motion_controller_spawner, + motion_control_handle_spawner, + cartesian_compliance_controller_spawner + ] + + return LaunchDescription(declared_arguments + nodes_to_start) \ No newline at end of file diff --git a/rbs_bringup/launch/moveit.launch.py b/rbs_bringup/launch/moveit.launch.py new file mode 100644 index 0000000..4d46b76 --- /dev/null +++ b/rbs_bringup/launch/moveit.launch.py @@ -0,0 +1,201 @@ +import os +from launch_ros.actions import Node +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition +from ur_moveit_config.launch_common import load_yaml + + + + +def generate_launch_description(): + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "robot_description", + default_value="", + description="robot description param", + ) + ) + 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( + "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( + "with_gripper", + default_value="false", + description="With gripper or not?", + ) + ) + + prefix = LaunchConfiguration("prefix") + moveit_config_package = LaunchConfiguration("moveit_config_package") + moveit_config_file = LaunchConfiguration("moveit_config_file") + use_sim_time = LaunchConfiguration("use_sim_time") + with_gripper_condition = LaunchConfiguration("with_gripper") + robot_description = LaunchConfiguration("robot_description") + + # 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} + use_sim_time = {"use_sim_time": use_sim_time} + + world_config_file = PathJoinSubstitution( + [FindPackageShare("rbs_simulation"), "worlds", "mir.sdf"] + ) + + 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, + ], + ) + + 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, + ] + ) + + 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, + ], + condition=IfCondition(with_gripper_condition) + ) + + 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, + ] + ) + + 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, + ] + ) + + 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']} + ] + ) + nodes_to_start = [ + move_group_node, + move_topose_action_server, + gripper_control_node, + move_cartesian_path_action_server, + move_joint_state_action_server + ] + return LaunchDescription(declared_arguments + nodes_to_start) diff --git a/rbs_bringup/launch/perception.launch.py b/rbs_bringup/launch/perception.launch.py new file mode 100644 index 0000000..8c43c3e --- /dev/null +++ b/rbs_bringup/launch/perception.launch.py @@ -0,0 +1,27 @@ +from launch_ros.actions import Node +from ur_moveit_config.launch_common import load_yaml +from launch import LaunchDescription + +def generate_launch_description(): + declared_arguments = [] + + 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 + ] + ) + grasp_marker = Node( + package="rbs_perception", + executable="grasp_marker_publish.py", + ) + nodes_to_start = [ + grasp_pose_loader, + grasp_marker + ] + return LaunchDescription(declared_arguments + nodes_to_start) diff --git a/rbs_bringup/launch/simulation.launch.py b/rbs_bringup/launch/simulation.launch.py new file mode 100644 index 0000000..6b0ee24 --- /dev/null +++ b/rbs_bringup/launch/simulation.launch.py @@ -0,0 +1,65 @@ +import os +from launch_ros.actions import Node +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument("sim_gazebo", default_value="false", description="Gazebo Simulation") + ) + 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("env_manager", default_value="false", description="Launch env_manager?") + ) + + sim_gazebo = LaunchConfiguration("sim_gazebo") + rbs_robot_type = LaunchConfiguration("rbs_robot_type") + env_manager_cond = LaunchConfiguration("env_manager") + + # FIXME: To args when we'll have different files + world_config_file = PathJoinSubstitution( + [FindPackageShare("rbs_simulation"), "worlds", "mir.sdf"] + ) + # 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"])], + condition=IfCondition(sim_gazebo)) + # 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', + condition=IfCondition(sim_gazebo)) + + env_manager = Node(package="env_manager", + executable="main", + condition=IfCondition(env_manager_cond) + ) + + nodes_to_start = [ + gazebo, + gazebo_spawn_robot, + env_manager + ] + return LaunchDescription(declared_arguments + nodes_to_start) diff --git a/rbs_bringup/package.xml b/rbs_bringup/package.xml new file mode 100644 index 0000000..e8449c1 --- /dev/null +++ b/rbs_bringup/package.xml @@ -0,0 +1,18 @@ + + + + rbs_bringup + 0.0.0 + TODO: Package description + bill-finger + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/rbs_simulation/launch/rbs_simulation.launch.py b/rbs_simulation/launch/rbs_simulation.launch.py index 8858d9f..cc9f23e 100644 --- a/rbs_simulation/launch/rbs_simulation.launch.py +++ b/rbs_simulation/launch/rbs_simulation.launch.py @@ -515,7 +515,6 @@ def generate_launch_description(): grasp_marker = Node( package="rbs_perception", executable="grasp_marker_publish.py", - name="hyak", ) nodes_to_start = [ diff --git a/rbs_task_planner/launch/task_planner.launch.py b/rbs_task_planner/launch/task_planner.launch.py index 074c3d8..ccb0788 100644 --- a/rbs_task_planner/launch/task_planner.launch.py +++ b/rbs_task_planner/launch/task_planner.launch.py @@ -38,7 +38,10 @@ def generate_launch_description(): pkg_dir = get_package_share_directory('rbs_task_planner') bt_exec_dir = get_package_share_directory('rbs_bt_executor') namespace = LaunchConfiguration('namespace') - + assemble_dir = os.path.join( + get_package_share_directory("rbs_task_planner"), "example", "sdf_models" + ) + declare_namespace_cmd = DeclareLaunchArgument( name="namespace", default_value='', @@ -66,11 +69,22 @@ def generate_launch_description(): 'bt_xml_file': bt_exec_dir + '/bt_trees/assemble.xml', } ]) - ld = LaunchDescription() - ld.add_action(declare_namespace_cmd) - # Declare the launch options + assemble_state = Node( + package="rbs_skill_servers", + executable="assemble_state_service_server", + output="screen", + parameters=[ + {'assemble_prefix': 'ASSEMBLE_'}, + {'assemble_dir': assemble_dir} + ] + ) + ld = LaunchDescription() + # Args + ld.add_action(declare_namespace_cmd) + # Nodes ld.add_action(plansys2_cmd) ld.add_action(assemble) + ld.add_action(assemble_state) return ld