From de0d5d44400ca54608821c9dfa24e40953038028 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Sat, 30 Nov 2024 19:30:13 +0300 Subject: [PATCH] refactor launch configuration for robot and environment setup - Simplified and consolidated launch arguments by removing unused configurations. - Enhanced `test_env.launch.py` to support dynamic loading of xacro and SRDF arguments via YAML files. - Added new launch parameters for control space, strategy, and interactive mode to improve flexibility. - Updated robot description and MoveIt integration to use dynamic configurations. - Improved clarity and modularity in scene and robot setup processes. --- env_manager/rbs_gym/launch/test_env.launch.py | 254 +++++++++-------- env_manager/rbs_gym/launch/train.launch.py | 268 ++++++++++-------- env_manager/rbs_gym/rbs_gym/envs/__init__.py | 4 +- .../rbs_runtime/rbs_runtime/__init__.py | 17 +- rbs_bringup/launch/rbs_bringup.launch.py | 2 +- rbs_bringup/launch/rbs_robot.launch.py | 12 +- 6 files changed, 313 insertions(+), 244 deletions(-) diff --git a/env_manager/rbs_gym/launch/test_env.launch.py b/env_manager/rbs_gym/launch/test_env.launch.py index c2667d6..98a39b9 100644 --- a/env_manager/rbs_gym/launch/test_env.launch.py +++ b/env_manager/rbs_gym/launch/test_env.launch.py @@ -15,6 +15,7 @@ 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 +import yaml def launch_setup(context, *args, **kwargs): @@ -22,25 +23,21 @@ def launch_setup(context, *args, **kwargs): 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") + use_moveit = LaunchConfiguration("use_moveit") 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") - gripper_name = LaunchConfiguration("gripper_name") + scene_config_file = LaunchConfiguration("scene_config_file").perform(context) + + ee_link_name = LaunchConfiguration("ee_link_name").perform(context) + base_link_name = LaunchConfiguration("base_link_name").perform(context) + + control_space = LaunchConfiguration("control_space").perform(context) + control_strategy = LaunchConfiguration("control_strategy").perform(context) + + interactive = LaunchConfiguration("interactive").perform(context) # training arguments env = LaunchConfiguration("env") @@ -48,28 +45,72 @@ def launch_setup(context, *args, **kwargs): log_level = LaunchConfiguration("log_level") env_kwargs = LaunchConfiguration("env_kwargs") - sim_gazebo = LaunchConfiguration("sim_gazebo") - launch_simulation = LaunchConfiguration("launch_sim") - - initial_joint_controllers_file_path = os.path.join( - get_package_share_directory("rbs_arm"), "config", "controllers.yaml" + 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_semantic_content = "" + + if use_moveit.perform(context) == "true": + srdf_config_file = f"{description_package_abs_path}/config/srdf_xacro_args.yaml" + srdf_file = os.path.join( + get_package_share_directory(moveit_config_package.perform(context)), + "srdf", + moveit_config_file.perform(context), + ) + srdf_mappings = load_xacro_args(srdf_config_file, locals()) + robot_description_semantic_content = xacro.process_file(srdf_file, mappings=srdf_mappings) + robot_description_semantic_content = robot_description_semantic_content.toprettyxml(indent=" ") + control_space = "joint" + control_strategy = "position" + interactive = "false" + robot_description_content = robot_description_doc.toprettyxml(indent=" ") robot_description = {"robot_description": robot_description_content} @@ -83,28 +124,25 @@ def launch_setup(context, *args, **kwargs): ] ), launch_arguments={ - "env_manager": env_manager, "with_gripper": with_gripper_condition, - "gripper_name": gripper_name, - "controllers_file": initial_joint_controllers_file_path, + "controllers_file": simulation_controllers, "robot_type": robot_type, - "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, + "use_moveit": "false", "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", + "use_sim_time": "true", + "use_skills": "false", + "use_controllers": "true", "robot_description": robot_description_content, + "robot_description_semantic": robot_description_semantic_content, + "base_link_name": base_link_name, + "ee_link_name": ee_link_name, + "control_space": control_space, + "control_strategy": "effort", + "interactive_control": "false", }.items(), ) @@ -137,7 +175,7 @@ def launch_setup(context, *args, **kwargs): nodes_to_start = [ # env, - rl_task, + # rl_task, clock_bridge, delay_robot_control_stack, ] @@ -150,18 +188,20 @@ def generate_launch_description(): DeclareLaunchArgument( "robot_type", description="Type of robot 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", ) ) - # General arguments - declared_arguments.append( - DeclareLaunchArgument( - "controllers_file", - default_value="controllers_gazebosim.yaml", - description="YAML file with the controllers configuration.", - ) - ) declared_arguments.append( DeclareLaunchArgument( "description_package", @@ -184,20 +224,6 @@ def generate_launch_description(): 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", @@ -221,14 +247,6 @@ def generate_launch_description(): 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?" @@ -236,25 +254,7 @@ def generate_launch_description(): ) 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?" + "use_moveit", default_value="false", description="Launch moveit?" ) ) declared_arguments.append( @@ -264,39 +264,55 @@ def generate_launch_description(): ) 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", + "use_controllers", default_value="true", description="Launch controllers?", ) ) declared_arguments.append( DeclareLaunchArgument( - "gazebo_gui", default_value="true", description="Launch gazebo with gui?" + "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", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "control_space", + default_value="task", + choices=["task", "joint"], + description="Specify the control space for the robot (e.g., task space).", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "control_strategy", + default_value="position", + choices=["position", "velocity", "effort"], + description="Specify the control strategy (e.g., position control).", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "interactive", + default_value="true", + description="Wheter to run the motion_control_handle controller", + ), + ) # training arguments declared_arguments.append( DeclareLaunchArgument( diff --git a/env_manager/rbs_gym/launch/train.launch.py b/env_manager/rbs_gym/launch/train.launch.py index cda85d6..5721f74 100644 --- a/env_manager/rbs_gym/launch/train.launch.py +++ b/env_manager/rbs_gym/launch/train.launch.py @@ -13,31 +13,29 @@ from launch_ros.actions import Node import os from os import cpu_count from ament_index_python.packages import get_package_share_directory +import yaml +import xacro 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") + use_moveit = LaunchConfiguration("use_moveit") 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") - gripper_name = LaunchConfiguration("gripper_name") + scene_config_file = LaunchConfiguration("scene_config_file").perform(context) + + ee_link_name = LaunchConfiguration("ee_link_name").perform(context) + base_link_name = LaunchConfiguration("base_link_name").perform(context) + + control_space = LaunchConfiguration("control_space").perform(context) + control_strategy = LaunchConfiguration("control_strategy").perform(context) + + interactive = LaunchConfiguration("interactive").perform(context) # training arguments env = LaunchConfiguration("env") @@ -64,47 +62,104 @@ def launch_setup(context, *args, **kwargs): track = LaunchConfiguration("track") - - sim_gazebo = LaunchConfiguration("sim_gazebo") - launch_simulation = LaunchConfiguration("launch_sim") - - initial_joint_controllers_file_path = os.path.join( - get_package_share_directory('rbs_arm'), 'config', 'controllers.yaml' + 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( + description_package_abs_path, + "urdf", + description_file.perform(context), + ) + + 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_semantic_content = "" + + if use_moveit.perform(context) == "true": + srdf_config_file = f"{description_package_abs_path}/config/srdf_xacro_args.yaml" + srdf_file = os.path.join( + get_package_share_directory(moveit_config_package.perform(context)), + "srdf", + moveit_config_file.perform(context), + ) + srdf_mappings = load_xacro_args(srdf_config_file, locals()) + robot_description_semantic_content = xacro.process_file(srdf_file, mappings=srdf_mappings) + robot_description_semantic_content = robot_description_semantic_content.toprettyxml(indent=" ") + control_space = "joint" + control_strategy = "position" + interactive = "false" + + + robot_description_content = robot_description_doc.toprettyxml(indent=" ") + robot_description = {"robot_description": robot_description_content} + single_robot_setup = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - PathJoinSubstitution([ - FindPackageShare('rbs_bringup'), - "launch", - "rbs_robot.launch.py" - ]) - ]), + 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, + "controllers_file": simulation_controllers, "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, + "robot_name": robot_type, + "use_moveit": use_moveit, "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 - }.items() + "use_controllers": "true", + "robot_description": robot_description_content, + "robot_description_semantic": robot_description_semantic_content, + "base_link_name": base_link_name, + "ee_link_name": ee_link_name, + "control_space": control_space, + "control_strategy": control_strategy, + "interactive_control": interactive, + }.items(), ) args = [ @@ -188,18 +243,20 @@ def generate_launch_description(): DeclareLaunchArgument( "robot_type", description="Type of robot 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", ) ) - # General arguments - declared_arguments.append( - DeclareLaunchArgument( - "controllers_file", - default_value="controllers_gazebosim.yaml", - description="YAML file with the controllers configuration.", - ) - ) declared_arguments.append( DeclareLaunchArgument( "description_package", @@ -222,20 +279,6 @@ def generate_launch_description(): 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", @@ -261,69 +304,69 @@ def generate_launch_description(): ) declared_arguments.append( DeclareLaunchArgument( - "gripper_name", - default_value="rbs_gripper", - choices=["rbs_gripper", ""], - description="choose gripper by name (leave empty if hasn't)", + "with_gripper", default_value="true", description="With gripper or not?" ) ) declared_arguments.append( - DeclareLaunchArgument("with_gripper", - default_value="true", - description="With gripper or not?") + DeclareLaunchArgument( + "use_moveit", default_value="false", description="Launch moveit?" + ) ) declared_arguments.append( - DeclareLaunchArgument("sim_gazebo", - default_value="true", - description="Gazebo Simulation") + DeclareLaunchArgument( + "launch_perception", default_value="false", description="Launch perception?" + ) ) declared_arguments.append( - DeclareLaunchArgument("env_manager", - default_value="false", - description="Launch env_manager?") + DeclareLaunchArgument( + "use_controllers", + default_value="true", + description="Launch controllers?", + ) ) declared_arguments.append( - DeclareLaunchArgument("launch_sim", - default_value="true", - description="Launch simulator (Gazebo)?\ - Most general arg") + DeclareLaunchArgument( + "scene_config_file", + default_value="", + description="Path to a scene configuration file", + ) ) declared_arguments.append( - DeclareLaunchArgument("launch_moveit", - default_value="false", - description="Launch moveit?") + DeclareLaunchArgument( + "ee_link_name", + default_value="", + description="End effector name of robot arm", + ) ) declared_arguments.append( - DeclareLaunchArgument("launch_perception", - default_value="false", - description="Launch perception?") + DeclareLaunchArgument( + "base_link_name", + default_value="", + description="Base link name if robot arm", + ) ) declared_arguments.append( - DeclareLaunchArgument("launch_task_planner", - default_value="false", - description="Launch task_planner?") + DeclareLaunchArgument( + "control_space", + default_value="task", + choices=["task", "joint"], + description="Specify the control space for the robot (e.g., task space).", + ) ) declared_arguments.append( - DeclareLaunchArgument("cartesian_controllers", - default_value="true", - description="Load cartesian\ - controllers?") + DeclareLaunchArgument( + "control_strategy", + default_value="position", + choices=["position", "velocity", "effort"], + description="Specify the control strategy (e.g., position control).", + ) ) 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?") + DeclareLaunchArgument( + "interactive", + default_value="true", + description="Wheter to run the motion_control_handle controller", + ), ) # training arguments declared_arguments.append( @@ -448,13 +491,12 @@ def generate_launch_description(): default_value="1", description="Verbose mode (0: no output, 1: INFO).", )) - # HER specifics declared_arguments.append( DeclareLaunchArgument( "truncate_last_trajectory", default_value="True", description="When using HER with online sampling the last trajectory in the replay buffer will be truncated after) reloading the replay buffer." - )), + )) declared_arguments.append( DeclareLaunchArgument( "log_level", diff --git a/env_manager/rbs_gym/rbs_gym/envs/__init__.py b/env_manager/rbs_gym/rbs_gym/envs/__init__.py index ea59065..9061b87 100644 --- a/env_manager/rbs_gym/rbs_gym/envs/__init__.py +++ b/env_manager/rbs_gym/rbs_gym/envs/__init__.py @@ -105,14 +105,14 @@ SCENE_CONFIGURATION: SceneData = SceneData( name="rbs_arm", joint_positions=[0.0, 0.5, 3.14159, 1.5, 0.0, 1.4, 0.0], with_gripper=True, - gripper_joint_positions=0.064, + gripper_joint_positions=0.00, randomizer=RobotRandomizerData(joint_positions=True), ), objects=[ SphereObjectData( name="sphere", relative_to="base_link", - position=(0.0, 0.4, 1.0), + position=(0.0, 0.3, 0.5), static=True, collision=False, color=(0.0, 1.0, 0.0, 0.8), diff --git a/env_manager/rbs_runtime/rbs_runtime/__init__.py b/env_manager/rbs_runtime/rbs_runtime/__init__.py index 3dec5f1..029c56e 100644 --- a/env_manager/rbs_runtime/rbs_runtime/__init__.py +++ b/env_manager/rbs_runtime/rbs_runtime/__init__.py @@ -6,22 +6,25 @@ from env_manager.models.configs import SceneData from typing import Dict, Any from env_manager.models.configs import ( - BoxObjectData, CylinderObjectData, MeshData, ModelData, ObjectData + BoxObjectData, CylinderObjectData, MeshData, ModelData, ObjectData, ObjectRandomizerData ) def object_factory(obj_data: Dict[str, Any]) -> ObjectData: obj_type = obj_data.get('type', '') + if "randomize" in obj_data and isinstance(obj_data["randomize"], dict): + obj_data["randomize"] = from_dict(data_class=ObjectRandomizerData, data=obj_data["randomize"]) + if obj_type == 'box': - return BoxObjectData(**obj_data) + return from_dict(data_class=BoxObjectData, data=obj_data) elif obj_type == 'cylinder': - return CylinderObjectData(**obj_data) + return from_dict(data_class=CylinderObjectData, data=obj_data) elif obj_type == 'mesh': - return MeshData(**obj_data) + return from_dict(data_class=MeshData, data=obj_data) elif obj_type == 'model': - return ModelData(**obj_data) + return from_dict(data_class=ModelData, data=obj_data) else: - return ObjectData(**obj_data) + return from_dict(data_class=ObjectData, data=obj_data) def scene_config_loader(file: str | Path) -> SceneData: def tuple_constructor(loader, node): @@ -35,6 +38,4 @@ def scene_config_loader(file: str | Path) -> SceneData: scene_data.objects = [object_factory(obj) for obj in scene_cfg.get('objects', [])] - print(scene_data) - return scene_data diff --git a/rbs_bringup/launch/rbs_bringup.launch.py b/rbs_bringup/launch/rbs_bringup.launch.py index 844a50c..b23d71c 100644 --- a/rbs_bringup/launch/rbs_bringup.launch.py +++ b/rbs_bringup/launch/rbs_bringup.launch.py @@ -25,7 +25,7 @@ def launch_setup(context, *args, **kwargs): "description_package": "rbs_arm", "description_file": "rbs_arm_modular.xacro", "robot_name": "rbs_arm", - "use_moveit": "true", + "use_moveit": "false", "moveit_config_package": "rbs_arm", "moveit_config_file": "rbs_arm.srdf.xacro", "use_sim_time": "true", diff --git a/rbs_bringup/launch/rbs_robot.launch.py b/rbs_bringup/launch/rbs_robot.launch.py index 5195ebd..2c2ff21 100644 --- a/rbs_bringup/launch/rbs_robot.launch.py +++ b/rbs_bringup/launch/rbs_robot.launch.py @@ -2,7 +2,7 @@ import os import xacro from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription +from launch import LaunchDescription, condition from launch.actions import ( DeclareLaunchArgument, IncludeLaunchDescription, @@ -30,6 +30,7 @@ def launch_setup(context, *args, **kwargs): moveit_config_package = LaunchConfiguration("moveit_config_package") use_sim_time = LaunchConfiguration("use_sim_time") use_controllers = LaunchConfiguration("use_controllers") + use_skills = LaunchConfiguration("use_skills") namespace = LaunchConfiguration("namespace") multi_robot = LaunchConfiguration("multi_robot") robot_name = robot_name.perform(context) @@ -144,6 +145,7 @@ def launch_setup(context, *args, **kwargs): "ee_link_name": ee_link_name, "base_link_name": base_link_name, }.items(), + condition=IfCondition(use_skills), ) nodes_to_start = [ @@ -247,6 +249,14 @@ def generate_launch_description(): description="Specify if MoveIt should be launched.", ) ) + + declared_arguments.append( + DeclareLaunchArgument( + "use_skills", + default_value="true", + description="Specify if skills should be launched.", + ) + ) declared_arguments.append( DeclareLaunchArgument( "use_controllers",