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.

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

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",

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