diff --git a/env_manager/rbs_gym/launch/evaluate.launch.py b/env_manager/rbs_gym/launch/evaluate.launch.py index df4c4a5..4e69f23 100644 --- a/env_manager/rbs_gym/launch/evaluate.launch.py +++ b/env_manager/rbs_gym/launch/evaluate.launch.py @@ -65,7 +65,7 @@ def launch_setup(context, *args, **kwargs): launch_simulation = LaunchConfiguration("launch_sim") initial_joint_controllers_file_path = os.path.join( - get_package_share_directory('rbs_arm'), 'config', 'rbs_arm0_controllers.yaml' + get_package_share_directory('rbs_arm'), 'config', 'controllers.yaml' ) single_robot_setup = IncludeLaunchDescription( @@ -176,7 +176,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "controllers_file", - default_value="rbs_arm_controllers_gazebosim.yaml", + default_value="controllers_gazebosim.yaml", description="YAML file with the controllers configuration.", ) ) diff --git a/env_manager/rbs_gym/launch/optimize.launch.py b/env_manager/rbs_gym/launch/optimize.launch.py index 40008d2..7e06a9c 100644 --- a/env_manager/rbs_gym/launch/optimize.launch.py +++ b/env_manager/rbs_gym/launch/optimize.launch.py @@ -70,7 +70,7 @@ def launch_setup(context, *args, **kwargs): launch_simulation = LaunchConfiguration("launch_sim") initial_joint_controllers_file_path = os.path.join( - get_package_share_directory('rbs_arm'), 'config', 'rbs_arm0_controllers.yaml' + get_package_share_directory('rbs_arm'), 'config', 'controllers.yaml' ) single_robot_setup = IncludeLaunchDescription( @@ -195,7 +195,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "controllers_file", - default_value="rbs_arm_controllers_gazebosim.yaml", + default_value="controllers_gazebosim.yaml", description="YAML file with the controllers configuration.", ) ) diff --git a/env_manager/rbs_gym/launch/test_env.launch.py b/env_manager/rbs_gym/launch/test_env.launch.py index b9ac3a7..c2667d6 100644 --- a/env_manager/rbs_gym/launch/test_env.launch.py +++ b/env_manager/rbs_gym/launch/test_env.launch.py @@ -52,7 +52,7 @@ def launch_setup(context, *args, **kwargs): launch_simulation = LaunchConfiguration("launch_sim") initial_joint_controllers_file_path = os.path.join( - get_package_share_directory("rbs_arm"), "config", "rbs_arm0_controllers.yaml" + get_package_share_directory("rbs_arm"), "config", "controllers.yaml" ) xacro_file = os.path.join( @@ -158,7 +158,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "controllers_file", - default_value="rbs_arm_controllers_gazebosim.yaml", + default_value="controllers_gazebosim.yaml", description="YAML file with the controllers configuration.", ) ) diff --git a/env_manager/rbs_gym/launch/train.launch.py b/env_manager/rbs_gym/launch/train.launch.py index 60db4db..cda85d6 100644 --- a/env_manager/rbs_gym/launch/train.launch.py +++ b/env_manager/rbs_gym/launch/train.launch.py @@ -69,7 +69,7 @@ def launch_setup(context, *args, **kwargs): launch_simulation = LaunchConfiguration("launch_sim") initial_joint_controllers_file_path = os.path.join( - get_package_share_directory('rbs_arm'), 'config', 'rbs_arm0_controllers.yaml' + get_package_share_directory('rbs_arm'), 'config', 'controllers.yaml' ) single_robot_setup = IncludeLaunchDescription( @@ -196,7 +196,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "controllers_file", - default_value="rbs_arm_controllers_gazebosim.yaml", + default_value="controllers_gazebosim.yaml", description="YAML file with the controllers configuration.", ) ) diff --git a/env_manager/rbs_runtime/launch/runtime.launch.py b/env_manager/rbs_runtime/launch/runtime.launch.py index f479d6c..f142865 100644 --- a/env_manager/rbs_runtime/launch/runtime.launch.py +++ b/env_manager/rbs_runtime/launch/runtime.launch.py @@ -14,6 +14,10 @@ from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare +from robot_builder.parser.urdf import URDF_parser +from robot_builder.external.ros2_control import ControllerManager +import yaml + def launch_setup(context, *args, **kwargs): # Initialize Arguments @@ -29,29 +33,70 @@ def launch_setup(context, *args, **kwargs): use_sim_time = LaunchConfiguration("use_sim_time") hardware = LaunchConfiguration("hardware") gripper_name = LaunchConfiguration("gripper_name") + scene_config_file = LaunchConfiguration("scene_config_file").perform(context) - initial_joint_controllers_file_path = os.path.join( - get_package_share_directory("rbs_arm"), "config", "rbs_arm0_controllers.yaml" + ee_link_name = LaunchConfiguration("ee_link_name").perform(context) + base_link_name = LaunchConfiguration("base_link_name").perform(context) + + if not scene_config_file == "": + config_file = {"config_file": scene_config_file} + else: + config_file = {} + + description_package_abs_path = get_package_share_directory(description_package.perform(context)) + + simulation_controllers = os.path.join( + description_package_abs_path, "config", "controllers.yaml" ) xacro_file = os.path.join( - get_package_share_directory(description_package.perform(context)), + description_package_abs_path, "urdf", description_file.perform(context), ) - robot_description_doc = xacro.process_file( - xacro_file, - mappings={ - "gripper_name": gripper_name.perform(context), - "hardware": hardware.perform(context), - "simulation_controllers": initial_joint_controllers_file_path, - "namespace": "", - }, - ) + xacro_config_file = f"{description_package_abs_path}/config/xacro_args.yaml" + + # TODO: hide this to another place + # Load xacro_args + def param_constructor(loader, node, local_vars): + value = loader.construct_scalar(node) + return LaunchConfiguration(value).perform( + local_vars.get("context", "Launch context if not defined") + ) + + def variable_constructor(loader, node, local_vars): + value = loader.construct_scalar(node) + return local_vars.get(value, f"Variable '{value}' not found") + + def load_xacro_args(yaml_file, local_vars): + # Get valut from ros2 argument + yaml.add_constructor( + "!param", lambda loader, node: param_constructor(loader, node, local_vars) + ) + + # Get value from local variable in this code + # The local variable should be initialized before the loader was called + yaml.add_constructor( + "!variable", + lambda loader, node: variable_constructor(loader, node, local_vars), + ) + + with open(yaml_file, "r") as file: + return yaml.load(file, Loader=yaml.FullLoader) + + mappings_data = load_xacro_args(xacro_config_file, locals()) + + robot_description_doc = xacro.process_file(xacro_file, mappings=mappings_data) robot_description_content = robot_description_doc.toprettyxml(indent=" ") robot_description = {"robot_description": robot_description_content} + + # Parse robot and configure controller's file for ControllerManager + robot = URDF_parser.load_string(robot_description_content, ee_link_name="gripper_grasp_point") + ControllerManager.save_to_yaml( + robot, description_package_abs_path, "controllers.yaml" + ) rbs_robot_setup = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -64,7 +109,7 @@ def launch_setup(context, *args, **kwargs): launch_arguments={ "with_gripper": with_gripper_condition, "gripper_name": gripper_name, - "controllers_file": initial_joint_controllers_file_path, + "controllers_file": simulation_controllers, "robot_type": robot_type, "description_package": description_package, "description_file": description_file, @@ -76,13 +121,15 @@ def launch_setup(context, *args, **kwargs): "hardware": hardware, "use_controllers": "true", "robot_description": robot_description_content, + "base_link_name": base_link_name, + "ee_link_name": ee_link_name }.items(), ) rbs_runtime = Node( package="rbs_runtime", executable="runtime", - parameters=[robot_description, {"use_sim_time": True}], + parameters=[robot_description, config_file, {"use_sim_time": True}], ) clock_bridge = Node( @@ -109,13 +156,13 @@ def generate_launch_description(): ) ) # General arguments - declared_arguments.append( - DeclareLaunchArgument( - "controllers_file", - default_value="rbs_arm0_controllers.yaml", - description="YAML file with the controllers configuration.", - ) - ) + # declared_arguments.append( + # DeclareLaunchArgument( + # "controllers_file", + # default_value="controllers.yaml", + # description="YAML file with the controllers configuration.", + # ) + # ) declared_arguments.append( DeclareLaunchArgument( "description_package", @@ -199,6 +246,27 @@ def generate_launch_description(): description="Launch controllers?", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "scene_config_file", + default_value="", + description="Path to a scene configuration file", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "ee_link_name", + default_value="", + description="End effector name of robot arm", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "base_link_name", + default_value="", + description="Base link name if robot arm", + ) + ) return LaunchDescription( declared_arguments + [OpaqueFunction(function=launch_setup)] diff --git a/rbs_bringup/launch/control.launch.py b/rbs_bringup/launch/control.launch.py new file mode 100644 index 0000000..91ab0b9 --- /dev/null +++ b/rbs_bringup/launch/control.launch.py @@ -0,0 +1,87 @@ +from launch_ros.actions import Node +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction + +# from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration + + +def create_spawner(controller_name, namespace, condition=None, inactive=False): + args = [controller_name, "-c", f"{namespace}/controller_manager"] + if inactive: + args.append("--inactive") + return Node( + package="controller_manager", + executable="spawner", + namespace=namespace, + arguments=args, + condition=condition, + ) + + +def launch_setup(context, *args, **kwargs): + namespace = LaunchConfiguration("namespace").perform(context) + control_strategy = LaunchConfiguration("control_strategy").perform(context) + control_space = LaunchConfiguration("control_space").perform(context) + has_gripper = LaunchConfiguration("has_gripper").perform(context) + interactive_control = LaunchConfiguration("interactive_control").perform(context) + + controllers_config = { + "joint_state_broadcaster": True, + "gripper_controller": has_gripper == "true", + "joint_trajectory_controller": control_strategy == "position" + and control_space == "joint", + "cartesian_motion_controller": control_strategy == "position" + and control_space == "task", + "cartesian_force_controller": control_strategy == "effort" + and control_space == "task", + "joint_effort_controller": control_strategy == "effort" + and control_space == "joint", + "force_torque_sensor_broadcaster": control_strategy == "effort" + and control_space == "task", + "motion_control_handle": interactive_control == "true" + and control_space == "task" + and control_strategy == "position", + } + + nodes_to_start = [] + for controller_name, should_start in controllers_config.items(): + if should_start: + nodes_to_start.append(create_spawner(controller_name, namespace)) + + return nodes_to_start + + +def generate_launch_description(): + declared_arguments = [ + DeclareLaunchArgument( + "namespace", default_value="", description="A robot's namespace" + ), + DeclareLaunchArgument( + "control_space", + default_value="task", + choices=["task", "joint"], + description="Control type: 'task' for Cartesian, 'joint' for joint space control.", + ), + DeclareLaunchArgument( + "control_strategy", + default_value="position", + choices=["position", "velocity", "effort"], + description="Control strategy: 'position', 'velocity', or 'effort'.", + ), + DeclareLaunchArgument( + "has_gripper", + default_value="true", + description="Whether to activate the gripper_controller.", + ), + + DeclareLaunchArgument( + "interactive_control", + default_value="true", + description="Whether to activate the gripper_controller.", + ), + ] + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) diff --git a/rbs_bringup/launch/launch_env.launch.py b/rbs_bringup/launch/launch_env.launch.py deleted file mode 100644 index 20bcf42..0000000 --- a/rbs_bringup/launch/launch_env.launch.py +++ /dev/null @@ -1,254 +0,0 @@ -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - IncludeLaunchDescription, - OpaqueFunction -) -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare -from launch_ros.actions import Node -import os -from ament_index_python.packages import get_package_share_directory -from nav2_common.launch import RewrittenYaml - -def launch_setup(context, *args, **kwargs): - # Initialize Arguments - robot_type = LaunchConfiguration("robot_type") - # General arguments - with_gripper_condition = LaunchConfiguration("with_gripper") - controllers_file = LaunchConfiguration("controllers_file") - cartesian_controllers = LaunchConfiguration("cartesian_controllers") - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - robot_name = LaunchConfiguration("robot_name") - start_joint_controller = LaunchConfiguration("start_joint_controller") - initial_joint_controller = LaunchConfiguration("initial_joint_controller") - launch_simulation = LaunchConfiguration("launch_sim") - launch_moveit = LaunchConfiguration("launch_moveit") - launch_task_planner = LaunchConfiguration("launch_task_planner") - launch_perception = LaunchConfiguration("launch_perception") - 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") - hardware = LaunchConfiguration("hardware") - env_manager = LaunchConfiguration("env_manager") - launch_controllers = LaunchConfiguration("launch_controllers") - gazebo_gui = LaunchConfiguration("gazebo_gui") - gripper_name = LaunchConfiguration("gripper_name") - - - sim_gazebo = LaunchConfiguration("sim_gazebo") - launch_simulation = LaunchConfiguration("launch_sim") - - configured_params = RewrittenYaml( - source_file=os.path.join( - get_package_share_directory( - description_package.perform(context)), - "config", - controllers_file.perform(context)), - root_key=robot_name.perform(context), - param_rewrites={}, - convert_types=True, - ) - initial_joint_controllers_file_path = os.path.join( - get_package_share_directory('rbs_arm'), 'config', 'rbs_arm0_controllers.yaml' - ) - controller_paramfile = configured_params.perform(context) - namespace = "/" + robot_name.perform(context) - - # spawner = Node( - # package="gz_enviroment_python", - # executable="spawner.py", - # namespace=namespace - # ) - - single_robot_setup = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - PathJoinSubstitution([ - FindPackageShare('rbs_bringup'), - "launch", - "rbs_robot.launch.py" - ]) - ]), - launch_arguments={ - "env_manager": env_manager, - "with_gripper": with_gripper_condition, - "gripper_name": gripper_name, - "controllers_file": controllers_file, - "robot_type": robot_type, - "controllers_file": initial_joint_controllers_file_path, - "cartesian_controllers": cartesian_controllers, - "description_package": description_package, - "description_file": description_file, - "robot_name": robot_name, - "start_joint_controller": start_joint_controller, - "initial_joint_controller": initial_joint_controller, - "launch_simulation": launch_simulation, - "launch_moveit": launch_moveit, - "launch_task_planner": launch_task_planner, - "launch_perception": launch_perception, - "moveit_config_package": moveit_config_package, - "moveit_config_file": moveit_config_file, - "use_sim_time": use_sim_time, - "sim_gazebo": sim_gazebo, - "hardware": hardware, - "launch_controllers": "true", - "gazebo_gui": gazebo_gui, - }.items() - ) - - nodes_to_start = [ - # spawner, - single_robot_setup - ] - return nodes_to_start - - -def generate_launch_description(): - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "robot_type", - description="Type of robot by name", - choices=["rbs_arm","ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"], - default_value="rbs_arm", - ) - ) - # General arguments - declared_arguments.append( - DeclareLaunchArgument( - "controllers_file", - default_value="rbs_arm_controllers_gazebosim.yaml", - description="YAML file with the controllers configuration.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="rbs_arm", - 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="rbs_arm_modular.xacro", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "robot_name", - default_value="arm0", - description="Name for robot, used to apply namespace for specific robot in multirobot setup", - ) - ) - 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( - "moveit_config_package", - default_value="rbs_arm", - 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="rbs_arm.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( - "gripper_name", - default_value="rbs_gripper", - choices=["rbs_gripper", ""], - description="choose gripper by name (leave empty if hasn't)", - ) - ) - declared_arguments.append( - DeclareLaunchArgument("with_gripper", - default_value="true", - description="With gripper or not?") - ) - declared_arguments.append( - DeclareLaunchArgument("sim_gazebo", - default_value="true", - description="Gazebo Simulation") - ) - declared_arguments.append( - DeclareLaunchArgument("env_manager", - default_value="false", - description="Launch env_manager?") - ) - declared_arguments.append( - DeclareLaunchArgument("launch_sim", - default_value="true", - description="Launch simulator (Gazebo)?\ - Most general arg") - ) - declared_arguments.append( - DeclareLaunchArgument("launch_moveit", - default_value="false", - description="Launch moveit?") - ) - declared_arguments.append( - DeclareLaunchArgument("launch_perception", - default_value="false", - description="Launch perception?") - ) - declared_arguments.append( - DeclareLaunchArgument("launch_task_planner", - default_value="false", - description="Launch task_planner?") - ) - declared_arguments.append( - DeclareLaunchArgument("cartesian_controllers", - default_value="true", - description="Load cartesian\ - controllers?") - ) - declared_arguments.append( - DeclareLaunchArgument("hardware", - choices=["gazebo", "mock"], - default_value="gazebo", - description="Choose your harware_interface") - ) - declared_arguments.append( - DeclareLaunchArgument("launch_controllers", - default_value="true", - description="Launch controllers?") - ) - declared_arguments.append( - DeclareLaunchArgument("gazebo_gui", - default_value="true", - description="Launch gazebo with gui?") - ) - - return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/rbs_bringup/launch/multi_robot.launch.py b/rbs_bringup/launch/multi_robot.launch.py deleted file mode 100644 index e3b19e4..0000000 --- a/rbs_bringup/launch/multi_robot.launch.py +++ /dev/null @@ -1,292 +0,0 @@ -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - IncludeLaunchDescription, - OpaqueFunction, -) -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare -from launch_ros.actions import Node -from nav2_common.launch import RewrittenYaml -import os -from ament_index_python import get_package_share_directory -from rbs_launch_utils.merged_yaml import MergedYaml -from rbs_launch_utils.launch_common import load_yaml -import yaml - -def launch_setup(context, *args, **kwargs): - # Initialize Arguments - robot_type = LaunchConfiguration("robot_type") - # General arguments - with_gripper_condition = LaunchConfiguration("with_gripper") - controllers_file = LaunchConfiguration("controllers_file") - cartesian_controllers = LaunchConfiguration("cartesian_controllers") - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - robot_name = LaunchConfiguration("robot_name") - start_joint_controller = LaunchConfiguration("start_joint_controller") - initial_joint_controller = LaunchConfiguration("initial_joint_controller") - launch_simulation = LaunchConfiguration("launch_sim") - launch_moveit = LaunchConfiguration("launch_moveit") - launch_task_planner = LaunchConfiguration("launch_task_planner") - launch_perception = LaunchConfiguration("launch_perception") - 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") - hardware = LaunchConfiguration("hardware") - env_manager = LaunchConfiguration("env_manager") - launch_controllers = LaunchConfiguration("launch_controllers") - gazebo_gui = LaunchConfiguration("gazebo_gui") - gripper_name = LaunchConfiguration("gripper_name") - robots_config_file = LaunchConfiguration("robots_config_file") - gazebo_world_filename = LaunchConfiguration("gazebo_world_filename") - - ld = [] - ld.append(IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - PathJoinSubstitution([ - FindPackageShare('rbs_simulation'), - 'launch', - 'simulation_gazebo.launch.py' - ]) - ]), - launch_arguments={ - 'sim_gazebo': sim_gazebo, - 'debugger': "false", - 'launch_env_manager': "false", - "gazebo_world_filename": gazebo_world_filename - }.items(), - condition=IfCondition(launch_simulation) - )) - scene_file = robots_config_file.perform(context) - robots = load_yaml("rbs_bringup", "config/" + scene_file) - - description_package = description_package.perform(context) - controllers_file = controllers_file.perform(context) - config = MergedYaml(context, - os.path.join(get_package_share_directory(description_package), "config", controllers_file), - root_keys=[i["name"] for i in robots["scene_config"]], - param_rewrites={}, - convert_types=False, - ).merge_yamls() - for robot in robots["scene_config"]: - namespace: str = "/" + robot["name"] - ld.append(IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - PathJoinSubstitution([ - FindPackageShare('rbs_bringup'), - "launch", - "rbs_robot.launch.py" - ]) - ]), - launch_arguments={ - "env_manager": env_manager, - "with_gripper": with_gripper_condition, - "gripper_name": gripper_name, - "controllers_file": controllers_file, - "robot_type": robot["type"], - "controllers_file": config, - "cartesian_controllers": cartesian_controllers, - "description_package": description_package, - "description_file": description_file, - "robot_name": robot["name"], - "start_joint_controller": start_joint_controller, - "initial_joint_controller": initial_joint_controller, - "launch_simulation": launch_simulation, - "launch_moveit": launch_moveit, - "launch_task_planner": launch_task_planner, - "launch_perception": launch_perception, - "moveit_config_package": moveit_config_package, - "moveit_config_file": moveit_config_file, - "use_sim_time": use_sim_time, - "sim_gazebo": sim_gazebo, - "hardware": hardware, - "launch_controllers": launch_controllers, - "gazebo_gui": gazebo_gui, - "namespace": namespace, - "x": str(robot["pose"]["position"]["x"]), - "y": str(robot["pose"]["position"]["y"]), - "z": str(robot["pose"]["position"]["z"]), - "roll": str(robot["pose"]["orientation"]["x"]), - "pitch": str(robot["pose"]["orientation"]["y"]), - "yaw": str(robot["pose"]["orientation"]["z"]), - }.items() - )) - - gz_spawner = Node( - package='ros_gz_sim', - executable='create', - arguments=[ - '-name', robot_name, - # '-x', str(robot["pose"]["position"]["x"]), - # '-y', str(robot["pose"]["position"]["y"]), - # '-z', str(robot["pose"]["position"]["z"]), - # '-R', str(robot["pose"]["orientation"]["x"]), - # '-P', str(robot["pose"]["orientation"]["y"]), - # '-Y', str(robot["pose"]["orientation"]["z"]), - '-topic', namespace + '/robot_description'], - output='screen', - condition=IfCondition(sim_gazebo)) - ld.append(gz_spawner) - - return ld - - -def generate_launch_description(): - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "robot_type", - description="Type of robot by name", - choices=["rbs_arm","ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"], - default_value="rbs_arm", - ) - ) - # General arguments - declared_arguments.append( - DeclareLaunchArgument( - "controllers_file", - default_value="rbs_arm_controllers_gazebosim.yaml", - description="YAML file with the controllers configuration.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="rbs_arm", - 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="rbs_arm_modular.xacro", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "robot_name", - default_value="arm0", - description="Name for robot, used to apply namespace for specific robot in multirobot setup", - ) - ) - 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( - "moveit_config_package", - default_value="rbs_arm", - 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="rbs_arm.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( - "gripper_name", - default_value="rbs_gripper", - choices=["rbs_gripper", ""], - description="choose gripper by name (leave empty if hasn't)", - ) - ) - declared_arguments.append( - DeclareLaunchArgument("with_gripper", - default_value="true", - description="With gripper or not?") - ) - declared_arguments.append( - DeclareLaunchArgument("sim_gazebo", - default_value="true", - description="Gazebo Simulation") - ) - declared_arguments.append( - DeclareLaunchArgument("env_manager", - default_value="false", - description="Launch env_manager?") - ) - declared_arguments.append( - DeclareLaunchArgument("launch_sim", - default_value="true", - description="Launch simulator (Gazebo)?\ - Most general arg") - ) - declared_arguments.append( - DeclareLaunchArgument("launch_moveit", - default_value="false", - description="Launch moveit?") - ) - declared_arguments.append( - DeclareLaunchArgument("launch_perception", - default_value="false", - description="Launch perception?") - ) - declared_arguments.append( - DeclareLaunchArgument("launch_task_planner", - default_value="false", - description="Launch task_planner?") - ) - declared_arguments.append( - DeclareLaunchArgument("cartesian_controllers", - default_value="true", - description="Load cartesian\ - controllers?") - ) - declared_arguments.append( - DeclareLaunchArgument("hardware", - choices=["gazebo", "mock"], - default_value="gazebo", - description="Choose your harware_interface") - ) - declared_arguments.append( - DeclareLaunchArgument("launch_controllers", - default_value="true", - description="Launch controllers?") - ) - declared_arguments.append( - DeclareLaunchArgument("gazebo_gui", - default_value="true", - description="Launch gazebo with gui?") - ) - declared_arguments.append( - DeclareLaunchArgument("gazebo_world_filename", - default_value="asm2.sdf", - description="Filename of Gazebo world file to launch") - ) - declared_arguments.append( - DeclareLaunchArgument("robots_config_file", - default_value="roboclone.yaml", - description="Filename for config file with robots in scene") - ) - - return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/rbs_bringup/launch/rbs_bringup.launch.py b/rbs_bringup/launch/rbs_bringup.launch.py new file mode 100644 index 0000000..63253b4 --- /dev/null +++ b/rbs_bringup/launch/rbs_bringup.launch.py @@ -0,0 +1,52 @@ +from launch import LaunchDescription +from launch.actions import ( + IncludeLaunchDescription, + OpaqueFunction, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ( + PathJoinSubstitution, +) +from launch_ros.substitutions import FindPackageShare + + +def launch_setup(context, *args, **kwargs): + main_script = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [FindPackageShare("rbs_runtime"), "launch", "runtime.launch.py"] + ) + ] + ), + launch_arguments={ + "with_gripper": "true", + "gripper_name": "rbs_gripper", + "robot_type": "rbs_arm", + "description_package": "rbs_arm", + "description_file": "rbs_arm_modular.xacro", + "robot_name": "rbs_arm", + "use_moveit": "false", + "moveit_config_package": "rbs_arm", + "moveit_config_file": "rbs_arm.srdf.xacro", + "use_sim_time": "true", + "hardware": "gazebo", + "use_controllers": "true", + "scene_config_file": "", + "base_link_name": "base_link", + "ee_link_name": "gripper_grasp_point", + }.items(), + ) + + nodes_to_start = [ + main_script, + ] + return nodes_to_start + + +def generate_launch_description(): + declared_arguments = [] + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) diff --git a/rbs_bringup/launch/rbs_robot.launch.py b/rbs_bringup/launch/rbs_robot.launch.py index 8735f46..2d42d35 100644 --- a/rbs_bringup/launch/rbs_robot.launch.py +++ b/rbs_bringup/launch/rbs_robot.launch.py @@ -53,6 +53,8 @@ def launch_setup(context, *args, **kwargs): robot_description_semantic = LaunchConfiguration("robot_description_semantic") control_space = LaunchConfiguration("control_space") control_strategy = LaunchConfiguration("control_strategy") + ee_link_name = LaunchConfiguration("ee_link_name").perform(context) + base_link_name = LaunchConfiguration("base_link_name").perform(context) remappings = [] if multi_robot == "true": @@ -141,7 +143,7 @@ def launch_setup(context, *args, **kwargs): [ PathJoinSubstitution( [ - FindPackageShare(description_package), + FindPackageShare("rbs_bringup"), "launch", "control.launch.py", ] @@ -202,6 +204,8 @@ def launch_setup(context, *args, **kwargs): "with_gripper_condition": with_gripper_condition, "namespace": namespace, "use_moveit": use_moveit, + "ee_link_name": ee_link_name, + "base_link_name": base_link_name, }.items(), ) @@ -221,14 +225,14 @@ def generate_launch_description(): DeclareLaunchArgument( "robot_type", description="Type of robot to launch, specified by name.", - choices=["rbs_arm", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"], + choices=["rbs_arm", "ar4", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"], default_value="rbs_arm", ) ) declared_arguments.append( DeclareLaunchArgument( "controllers_file", - default_value="rbs_arm0_controllers.yaml", + default_value="controllers.yaml", description="YAML file containing configuration settings for the controllers.", ) ) @@ -397,6 +401,21 @@ def generate_launch_description(): description="Custom semantic robot description (SRDF) to override the default.", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "ee_link_name", + default_value="", + description="End effector name of robot arm", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "base_link_name", + default_value="", + description="Base link name if robot arm", + ) + ) + return LaunchDescription( declared_arguments + [OpaqueFunction(function=launch_setup)] diff --git a/rbs_bringup/launch/single_robot.launch.py b/rbs_bringup/launch/single_robot.launch.py deleted file mode 100644 index 5397885..0000000 --- a/rbs_bringup/launch/single_robot.launch.py +++ /dev/null @@ -1,301 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - IncludeLaunchDescription, - OpaqueFunction, -) -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def launch_setup(context, *args, **kwargs): - # Initialize Arguments - robot_type = LaunchConfiguration("robot_type") - # General arguments - with_gripper_condition = LaunchConfiguration("with_gripper") - controllers_file = LaunchConfiguration("controllers_file") - cartesian_controllers = LaunchConfiguration("cartesian_controllers") - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - robot_name = LaunchConfiguration("robot_name") - start_joint_controller = LaunchConfiguration("start_joint_controller") - initial_joint_controller = LaunchConfiguration("initial_joint_controller") - launch_simulation = LaunchConfiguration("launch_sim") - launch_moveit = LaunchConfiguration("launch_moveit") - launch_task_planner = LaunchConfiguration("launch_task_planner") - launch_perception = LaunchConfiguration("launch_perception") - 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") - hardware = LaunchConfiguration("hardware") - env_manager = LaunchConfiguration("env_manager") - launch_controllers = LaunchConfiguration("launch_controllers") - gazebo_gui = LaunchConfiguration("gazebo_gui") - gripper_name = LaunchConfiguration("gripper_name") - - sim_gazebo = LaunchConfiguration("sim_gazebo") - launch_simulation = LaunchConfiguration("launch_sim") - simulation = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [ - PathJoinSubstitution( - [ - FindPackageShare("rbs_simulation"), - "launch", - "simulation_gazebo.launch.py", - ] - ) - ] - ), - launch_arguments={ - "sim_gazebo": sim_gazebo, - "debugger": "false", - "launch_env_manager": env_manager, - "gazebo_world_filename": "asm2.sdf", - }.items(), - condition=IfCondition(launch_simulation), - ) - - # FIXME: namespaces - # configured_params = RewrittenYaml( - # source_file=os.path.join( - # get_package_share_directory( - # description_package.perform(context)), - # "config", - # controllers_file.perform(context)), - # root_key=robot_name.perform(context), - # param_rewrites={}, - # convert_types=True, - # ) - - initial_joint_controllers_file_path = os.path.join( - get_package_share_directory("rbs_arm"), "config", "rbs_arm0_controllers.yaml" - ) - - # controller_paramfile = configured_params.perform(context) - # controller_paramfile = PathJoinSubstitution([ - # FindPackageShare(robot_type), "config", "rbs_arm0_controllers.yaml" - # ]) - # namespace = "/" + robot_name.perform(context) - namespace = "" - - gz_spawner = Node( - package="ros_gz_sim", - executable="create", - # prefix=['gdbserver localhost:1234'], - arguments=[ - "-name", - robot_name.perform(context), - "-topic", - namespace + "/robot_description", - ], - output="screen", - condition=IfCondition(sim_gazebo), - ) - - single_robot_setup = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [ - PathJoinSubstitution( - [FindPackageShare("rbs_bringup"), "launch", "rbs_robot.launch.py"] - ) - ] - ), - launch_arguments={ - "env_manager": env_manager, - "with_gripper": with_gripper_condition, - "gripper_name": gripper_name, - "controllers_file": initial_joint_controllers_file_path, - "robot_type": robot_type, - # "controllers_file": controller_paramfile, - "cartesian_controllers": cartesian_controllers, - "description_package": description_package, - "description_file": description_file, - "robot_name": robot_type, - "start_joint_controller": start_joint_controller, - "initial_joint_controller": initial_joint_controller, - "launch_simulation": launch_simulation, - "launch_moveit": launch_moveit, - "launch_task_planner": launch_task_planner, - "launch_perception": launch_perception, - "moveit_config_package": moveit_config_package, - "moveit_config_file": moveit_config_file, - "use_sim_time": use_sim_time, - "sim_gazebo": sim_gazebo, - "hardware": hardware, - "launch_controllers": "true", - "gazebo_gui": gazebo_gui, - # "x": "0.5", - # "y": "0.5", - # "z": "0.5" - }.items(), - ) - - nodes_to_start = [simulation, gz_spawner, single_robot_setup] - return nodes_to_start - - -def generate_launch_description(): - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "robot_type", - description="Type of robot by name", - choices=["rbs_arm", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"], - default_value="rbs_arm", - ) - ) - # General arguments - declared_arguments.append( - DeclareLaunchArgument( - "controllers_file", - default_value="rbs_arm0_controllers.yaml", - description="YAML file with the controllers configuration.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="rbs_arm", - 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="rbs_arm_modular.xacro", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "robot_name", - default_value="arm0", - description="Name for robot, used to apply namespace for specific robot in multirobot setup", - ) - ) - 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( - "moveit_config_package", - default_value="rbs_arm", - 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="rbs_arm.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( - "gripper_name", - default_value="rbs_gripper", - choices=["rbs_gripper", ""], - description="choose gripper by name (leave empty if hasn't)", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "with_gripper", default_value="true", description="With gripper or not?" - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "sim_gazebo", default_value="true", description="Gazebo Simulation" - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "env_manager", default_value="true", description="Launch env_manager?" - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "launch_sim", - default_value="true", - description="Launch simulator (Gazebo)?\ - Most general arg", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "launch_moveit", default_value="false", description="Launch moveit?" - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "launch_perception", default_value="false", description="Launch perception?" - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "launch_task_planner", - default_value="false", - description="Launch task_planner?", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "cartesian_controllers", - default_value="true", - description="Load cartesian\ - controllers?", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "hardware", - choices=["gazebo", "mock"], - default_value="gazebo", - description="Choose your harware_interface", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "launch_controllers", - default_value="true", - description="Launch controllers?", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "gazebo_gui", default_value="true", description="Launch gazebo with gui?" - ) - ) - - return LaunchDescription( - declared_arguments + [OpaqueFunction(function=launch_setup)] - ) diff --git a/rbs_skill_servers/launch/skills.launch.py b/rbs_skill_servers/launch/skills.launch.py index f9e5692..8e5a34c 100644 --- a/rbs_skill_servers/launch/skills.launch.py +++ b/rbs_skill_servers/launch/skills.launch.py @@ -13,6 +13,8 @@ def launch_setup(context, *args, **kwargs): robot_description_kinematics = LaunchConfiguration("robot_description_kinematics") use_sim_time = LaunchConfiguration("use_sim_time") use_moveit = LaunchConfiguration("use_moveit") + ee_link_name = LaunchConfiguration("ee_link_name").perform(context) + base_link_name = LaunchConfiguration("base_link_name").perform(context) # with_gripper_condition = LaunchConfiguration("with_gripper_condition") robot_description = {"robot_description": robot_description_decl} @@ -64,6 +66,8 @@ def launch_setup(context, *args, **kwargs): parameters=[ {"use_sim_time": use_sim_time}, {"robot_name": namespace}, + {"base_link": base_link_name}, + {"robot_ee_link": ee_link_name}, robot_description, ], ), @@ -136,6 +140,12 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument("namespace", default_value="") ) + declared_arguments.append( + DeclareLaunchArgument("ee_link_name", default_value="") + ) + declared_arguments.append( + DeclareLaunchArgument("base_link_name", default_value="") + ) return LaunchDescription( declared_arguments + [OpaqueFunction(function=launch_setup)] ) diff --git a/rbs_skill_servers/src/mtp_jtc_cart.cpp b/rbs_skill_servers/src/mtp_jtc_cart.cpp index 363180b..4fdcc98 100644 --- a/rbs_skill_servers/src/mtp_jtc_cart.cpp +++ b/rbs_skill_servers/src/mtp_jtc_cart.cpp @@ -59,6 +59,8 @@ public: std::placeholders::_1), s_options); this->declare_parameter("robot_description", ""); + this->declare_parameter("base_link", ""); + this->declare_parameter("robot_ee_link", ""); auto robot_description = this->get_parameter("robot_description").as_string(); @@ -70,7 +72,15 @@ public: throw std::runtime_error("KDL Tree initialization failed"); } - if (!kdl_tree.getChain("base_link", "gripper_grasp_point", m_kdl_chain)) { + auto base_link = this->get_parameter("base_link").as_string(); + auto robot_ee_link = this->get_parameter("robot_ee_link").as_string(); + if (base_link.empty() or robot_ee_link.empty()) { + RCLCPP_ERROR(this->get_logger(), + "Describe robot end-effector link and base link to continue"); + throw std::runtime_error("Describe robot end-effector link and base link to continue"); + } + + if (!kdl_tree.getChain(base_link, robot_ee_link, m_kdl_chain)) { RCLCPP_ERROR(this->get_logger(), "Failed to obtain KDL chain from base to end-effector."); throw std::runtime_error("KDL Chain initialization failed");