From fbddca1b8b4645f81f6c6644f0a9b5b49faaec31 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Fri, 22 Nov 2024 02:15:09 +0300 Subject: [PATCH] refactor and extend runtime configuration handling - **Scene configuration:** - Replaced `bishop` object with `hole` and added a new `peg` object. - Enhanced object properties with new attributes like `randomize`, `relative_to`, `static`, and `texture`. - Updated `robot` joint positions in the default scene configuration. - **Launch file updates:** - Added handling for MoveIt SRDF file processing, including semantic descriptions. - Refactored unused parameters (`gripper_name`, `hardware`) and streamlined arguments. - Extended `robot_type` choices in the `generate_launch_description` method. - **Runtime initialization:** - Introduced `object_factory` to handle dynamic object instantiation based on type (`box`, `cylinder`, `mesh`, or `model`). - Enhanced `scene_config_loader` to process and instantiate objects using the factory function. --- .../config/default-scene-config.yaml | 35 ++++++++++- .../rbs_runtime/launch/runtime.launch.py | 61 +++++++++++-------- .../rbs_runtime/rbs_runtime/__init__.py | 22 +++++++ 3 files changed, 91 insertions(+), 27 deletions(-) diff --git a/env_manager/rbs_runtime/config/default-scene-config.yaml b/env_manager/rbs_runtime/config/default-scene-config.yaml index e0b7f57..474f3b1 100644 --- a/env_manager/rbs_runtime/config/default-scene-config.yaml +++ b/env_manager/rbs_runtime/config/default-scene-config.yaml @@ -67,7 +67,36 @@ light: visual: true objects: - color: null - name: bishop + name: hole + orientation: !tuple + - 1.0 + - 0.0 + - 0.0 + - 0.0 + position: !tuple + - 0.1 + - 0.3 + - 0.1 + randomize: + count: 0 + models_rollouts_num: 0 + random_color: false + random_model: false + random_orientation: false + random_pose: false + random_position: false + random_spawn_position_segments: [] + random_spawn_position_update_workspace_centre: false + random_spawn_volume: !tuple + - 0.5 + - 0.5 + - 0.5 + relative_to: world + static: false + texture: [] + type: "model" + - color: null + name: peg orientation: !tuple - 1.0 - 0.0 @@ -75,8 +104,8 @@ objects: - 0.0 position: !tuple - 0.0 - - 1.0 - 0.3 + - 0.1 randomize: count: 0 models_rollouts_num: 0 @@ -104,7 +133,7 @@ plugins: robot: gripper_joint_positions: 0.0 joint_positions: - - 1.57 + - -1.57 - 0.5 - 3.14159 - 1.5 diff --git a/env_manager/rbs_runtime/launch/runtime.launch.py b/env_manager/rbs_runtime/launch/runtime.launch.py index 45d6af7..9656efa 100644 --- a/env_manager/rbs_runtime/launch/runtime.launch.py +++ b/env_manager/rbs_runtime/launch/runtime.launch.py @@ -1,5 +1,6 @@ import os +from launch.launch_introspector import indent import xacro from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription @@ -26,13 +27,10 @@ def launch_setup(context, *args, **kwargs): with_gripper_condition = LaunchConfiguration("with_gripper") description_package = LaunchConfiguration("description_package") description_file = LaunchConfiguration("description_file") - robot_name = LaunchConfiguration("robot_name") 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") - hardware = LaunchConfiguration("hardware") - gripper_name = LaunchConfiguration("gripper_name") scene_config_file = LaunchConfiguration("scene_config_file").perform(context) ee_link_name = LaunchConfiguration("ee_link_name").perform(context) @@ -64,6 +62,7 @@ def launch_setup(context, *args, **kwargs): 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): @@ -96,12 +95,29 @@ def launch_setup(context, *args, **kwargs): 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} # Parse robot and configure controller's file for ControllerManager robot = URDF_parser.load_string( - robot_description_content, ee_link_name="gripper_grasp_point" + robot_description_content, ee_link_name=ee_link_name ) ControllerManager.save_to_yaml( robot, description_package_abs_path, "controllers.yaml" @@ -117,7 +133,6 @@ def launch_setup(context, *args, **kwargs): ), launch_arguments={ "with_gripper": with_gripper_condition, - "gripper_name": gripper_name, "controllers_file": simulation_controllers, "robot_type": robot_type, "description_package": description_package, @@ -127,9 +142,9 @@ def launch_setup(context, *args, **kwargs): "moveit_config_package": moveit_config_package, "moveit_config_file": moveit_config_file, "use_sim_time": use_sim_time, - "hardware": hardware, "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, @@ -153,7 +168,11 @@ def launch_setup(context, *args, **kwargs): delay_robot_control_stack = TimerAction(period=10.0, actions=[rbs_robot_setup]) - nodes_to_start = [rbs_runtime, clock_bridge, delay_robot_control_stack] + nodes_to_start = [ + rbs_runtime, + clock_bridge, + delay_robot_control_stack + ] return nodes_to_start @@ -163,7 +182,17 @@ 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", ) ) @@ -212,14 +241,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?" @@ -235,14 +256,6 @@ def generate_launch_description(): "launch_perception", default_value="false", description="Launch perception?" ) ) - declared_arguments.append( - DeclareLaunchArgument( - "hardware", - choices=["gazebo", "mock"], - default_value="gazebo", - description="Choose your harware_interface", - ) - ) declared_arguments.append( DeclareLaunchArgument( "use_controllers", diff --git a/env_manager/rbs_runtime/rbs_runtime/__init__.py b/env_manager/rbs_runtime/rbs_runtime/__init__.py index 2659497..3dec5f1 100644 --- a/env_manager/rbs_runtime/rbs_runtime/__init__.py +++ b/env_manager/rbs_runtime/rbs_runtime/__init__.py @@ -4,6 +4,24 @@ import yaml from dacite import from_dict from env_manager.models.configs import SceneData +from typing import Dict, Any +from env_manager.models.configs import ( + BoxObjectData, CylinderObjectData, MeshData, ModelData, ObjectData +) + +def object_factory(obj_data: Dict[str, Any]) -> ObjectData: + obj_type = obj_data.get('type', '') + + if obj_type == 'box': + return BoxObjectData(**obj_data) + elif obj_type == 'cylinder': + return CylinderObjectData(**obj_data) + elif obj_type == 'mesh': + return MeshData(**obj_data) + elif obj_type == 'model': + return ModelData(**obj_data) + else: + return ObjectData(**obj_data) def scene_config_loader(file: str | Path) -> SceneData: def tuple_constructor(loader, node): @@ -15,4 +33,8 @@ def scene_config_loader(file: str | Path) -> SceneData: scene_data = from_dict(data_class=SceneData, data=scene_cfg) + scene_data.objects = [object_factory(obj) for obj in scene_cfg.get('objects', [])] + + print(scene_data) + return scene_data