refactor: (env_manager, rbs_runtime) and add YAML config loading

- Updated `camera.py` to use consistent float literals for default spawn positions.
- Removed unused import statements and organized imports in `robot.py` and `scene.py`.
- Removed debugging import from `scene.py` to clean up codebase.
- Added a new YAML configuration file (`default-scene-config.yaml`) for scene settings.
- Implemented a scene configuration loader function in `rbs_runtime/__init__.py` to read and parse YAML config files.
- Modified `runtime.py` to utilize the newly added scene config loader and removed in-code default scene setup.
- Updated `setup.py` to include `config` directory in package data.
- Changed `rbs_robot.launch.py` to use position control strategy by default.
- Refactored `skills.launch.py` with cleaner parameter definitions and reactivated commented-out nodes.

env_manager/env_manager/env_manager/models/configs/camera.py

@@ -1,4 +1,5 @@
 from dataclasses import dataclass, field
+
 import numpy as np
 
 
@@ -93,9 +94,9 @@ class CameraData:
     publish_depth: bool = field(default=False)
     publish_points: bool = field(default=False)
 
-    spawn_position: tuple[float, float, float] = field(default=(0, 0, 1))
+    spawn_position: tuple[float, float, float] = field(default=(0.0, 0.0, 1.0))
     spawn_quat_xyzw: tuple[float, float, float, float] = field(
-        default=(0, 0.70710678118, 0, 0.70710678118)
+        default=(0.0, 0.70710678118, 0.0, 0.70710678118)
     )
 
     random_pose_rollouts_num: int = field(default=1)

env_manager/env_manager/env_manager/models/configs/robot.py

@@ -34,7 +34,6 @@ class GripperEnum(Enum):
     vacuum = VacuumGripperData
 
 
-
 @dataclass
 class RobotRandomizerData:
     """

env_manager/env_manager/env_manager/models/configs/scene.py

@@ -1,9 +1,10 @@
 from dataclasses import dataclass, field
+
+from .camera import CameraData
+from .light import LightData
 from .objects import ObjectData
 from .robot import RobotData
 from .terrain import TerrainData
-from .light import LightData
-from .camera import CameraData
 
 
 @dataclass

env_manager/env_manager/env_manager/scene/scene.py

@@ -1,6 +1,5 @@
 import asyncio
 from dataclasses import asdict
-# import debugpy
 
 import numpy as np
 from rcl_interfaces.srv import GetParameters

env_manager/rbs_runtime/config/default-scene-config.yaml

@@ -0,0 +1,152 @@
+camera:
+- clip_color: !tuple
+  - 0.01
+  - 1000.0
+  clip_depth: !tuple
+  - 0.05
+  - 10.0
+  enable: true
+  height: 128
+  horizontal_fov: 1.0471975511965976
+  image_format: R8G8B8
+  name: robot_camera
+  noise_mean: null
+  noise_stddev: null
+  publish_color: true
+  publish_depth: true
+  publish_points: true
+  random_pose_focal_point_z_offset: 0.0
+  random_pose_mode: orbit
+  random_pose_orbit_distance: 1.0
+  random_pose_orbit_height_range: !tuple
+  - 0.1
+  - 0.7
+  random_pose_orbit_ignore_arc_behind_robot: 0.39269908169872414
+  random_pose_rollout_counter: 0.0
+  random_pose_rollouts_num: 1
+  random_pose_select_position_options: []
+  relative_to: base_link
+  spawn_position: !tuple
+  - 0.0
+  - 0.0
+  - 1.0
+  spawn_quat_xyzw: !tuple
+  - 0.0
+  - 0.70710678118
+  - 0.0
+  - 0.70710678118
+  type: rgbd_camera
+  update_rate: 10
+  vertical_fov: 1.0471975511965976
+  width: 128
+gravity: !tuple
+- 0.0
+- 0.0
+- -9.80665
+gravity_std: !tuple
+- 0.0
+- 0.0
+- 0.0232
+light:
+  color: !tuple
+  - 1.0
+  - 1.0
+  - 1.0
+  - 1.0
+  direction: !tuple
+  - 0.5
+  - -0.25
+  - -0.75
+  distance: 1000.0
+  model_rollouts_num: 1
+  radius: 25.0
+  random_minmax_elevation: !tuple
+  - -0.15
+  - -0.65
+  type: sun
+  visual: true
+objects:
+- color: null
+  name: bishop
+  orientation: !tuple
+  - 1.0
+  - 0.0
+  - 0.0
+  - 0.0
+  position: !tuple
+  - 0.0
+  - 1.0
+  - 0.3
+  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'
+physics_rollouts_num: 0
+plugins:
+  fts_broadcaster: false
+  scene_broadcaster: false
+  sensors_render_engine: ogre2
+  user_commands: false
+robot:
+  gripper_joint_positions: 0.0
+  joint_positions:
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  name: rbs_arm
+  randomizer:
+    joint_positions: false
+    joint_positions_above_object_spawn: false
+    joint_positions_above_object_spawn_elevation: 0.2
+    joint_positions_above_object_spawn_xy_randomness: 0.2
+    joint_positions_std: 0.1
+    pose: false
+    spawn_volume: !tuple
+    - 1.0
+    - 1.0
+    - 0.0
+  spawn_position: !tuple
+  - 0.0
+  - 0.0
+  - 0.0
+  spawn_quat_xyzw: !tuple
+  - 0.0
+  - 0.0
+  - 0.0
+  - 1.0
+  urdf_string: ''
+  with_gripper: true
+terrain:
+  model_rollouts_num: 1
+  name: ground
+  size: !tuple
+  - 1.5
+  - 1.5
+  spawn_position: !tuple
+  - 0
+  - 0
+  - 0
+  spawn_quat_xyzw: !tuple
+  - 0
+  - 0
+  - 0
+  - 1
+  type: flat

env_manager/rbs_runtime/rbs_runtime/__init__.py

@@ -0,0 +1,18 @@
+from pathlib import Path
+
+import yaml
+from dacite import from_dict
+from env_manager.models.configs import SceneData
+
+
+def scene_config_loader(file: str | Path) -> SceneData:
+    def tuple_constructor(loader, node):
+        return tuple(loader.construct_sequence(node))
+
+    with open(file, "r") as yaml_file:
+        yaml.SafeLoader.add_constructor("!tuple", tuple_constructor)
+        scene_cfg = yaml.load(yaml_file, Loader=yaml.SafeLoader)
+
+    scene_data = from_dict(data_class=SceneData, data=scene_cfg)
+
+    return scene_data

env_manager/rbs_runtime/rbs_runtime/scripts/runtime.py

@@ -1,50 +1,42 @@
 #!/usr/bin/python3
 
-from env_manager.models.configs.objects import ObjectRandomizerData
-from rclpy.node import Node
-from env_manager.scene import Scene
-from env_manager.models.configs import (
-    CameraData,
-    SceneData,
-    RobotData,
-    MeshData,
-    ModelData,
-)
 import rclpy
-from scenario.bindings.gazebo import GazeboSimulator
+from ament_index_python.packages import get_package_share_directory
+from env_manager.scene import Scene
 from env_manager_interfaces.srv import ResetEnv
 from rbs_assets_library import get_world_file
+from rclpy.node import Node
+from scenario.bindings.gazebo import GazeboSimulator
 
-
-DEFAULT_SCENE: SceneData = SceneData(
-    robot=RobotData(
-        name="rbs_arm",
-        spawn_position = (0.0, 0.0, 0.0),
-        with_gripper=True,
-        joint_positions=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-        gripper_joint_positions=0.0,
-    ),
-    objects=[
-        ModelData("bishop", position=(0.0, 1.0, 0.3)),
-    ],
-    camera=[CameraData("robot_camera", publish_color=True, publish_depth=True, publish_points=True)],
-)
+from .. import scene_config_loader
 
 
 class GazeboRuntime(Node):
     def __init__(self) -> None:
         super().__init__(node_name="rbs_gz_runtime")
         self.declare_parameter("robot_description", "")
+        self.declare_parameter(
+            "config_file",
+            get_package_share_directory("rbs_runtime")
+            + "/config/default-scene-config.yaml",
+        )
+
         self.gazebo = GazeboSimulator(step_size=0.001, rtf=1.0, steps_per_run=1)
 
         self.gazebo.insert_world_from_sdf(get_world_file("default"))
         if not self.gazebo.initialize():
             raise RuntimeError("Gazebo cannot be initialized")
 
+        config_file = (
+            self.get_parameter("config_file").get_parameter_value().string_value
+        )
+
+        scene_data = scene_config_loader(config_file)
+
         self.scene = Scene(
             node=self,
             gazebo=self.gazebo,
-            scene=DEFAULT_SCENE,
+            scene=scene_data,
             robot_urdf_string=self.get_parameter("robot_description")
             .get_parameter_value()
             .string_value,

env_manager/rbs_runtime/setup.py

@@ -16,6 +16,10 @@ setup(
             os.path.join("share", package_name, "launch"),
             glob(os.path.join("launch", "*launch.[pxy][yma]*")),
         ),
+        (
+            os.path.join("share", package_name, "config"),
+            glob(os.path.join("config", "*config.[yma]*")),
+        ),
     ],
     install_requires=["setuptools"],
     zip_safe=True,

rbs_bringup/launch/rbs_robot.launch.py

@@ -175,7 +175,7 @@ def launch_setup(context, *args, **kwargs):
         ),
         launch_arguments={
             "control_space": "task",
-            "control_strategy": "effort",
+            "control_strategy": "position",
             "has_gripper": "true",
             "namespace": namespace,
         }.items(),

rbs_skill_servers/launch/skills.launch.py

@@ -1,16 +1,11 @@
-from collections import namedtuple
-from os import name
 from launch import LaunchDescription
-from launch.actions import (
-    DeclareLaunchArgument,
-    OpaqueFunction
-)
-from ament_index_python.packages import get_package_share_directory
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 from rbs_launch_utils.launch_common import load_yaml
 
+
 def launch_setup(context, *args, **kwargs):
     robot_description_decl = LaunchConfiguration("robot_description")
     robot_description_semantic_decl = LaunchConfiguration("robot_description_semantic")
@@ -19,12 +14,12 @@ def launch_setup(context, *args, **kwargs):
     with_gripper_condition = LaunchConfiguration("with_gripper_condition")
     points_params_filepath_decl = LaunchConfiguration("points_params_filepath")
     robot_description = {"robot_description": robot_description_decl}
-    robot_description_semantic = {"robot_description_semantic": robot_description_semantic_decl}
+    robot_description_semantic = {
+        "robot_description_semantic": robot_description_semantic_decl
+    }
     namespace = LaunchConfiguration("namespace")
 
-    points_params = load_yaml(
-        "rbs_skill_servers", "config/gripperPositions.yaml"
-    )
+    points_params = load_yaml("rbs_skill_servers", "config/gripperPositions.yaml")
 
     kinematics_yaml = load_yaml("rbs_arm", "config/kinematics.yaml")
 
@@ -39,20 +34,20 @@ def launch_setup(context, *args, **kwargs):
             robot_description_semantic,
             robot_description_kinematics,
             {"use_sim_time": use_sim_time},
-        ]
+        ],
     )
 
     gripper_control_node = Node(
         package="rbs_skill_servers",
         executable="gripper_control_action_server",
         namespace=namespace,
-        parameters= [
+        parameters=[
             robot_description,
             robot_description_semantic,
             robot_description_kinematics,
             {"use_sim_time": use_sim_time},
         ],
-        condition=IfCondition(with_gripper_condition)
+        condition=IfCondition(with_gripper_condition),
     )
 
     move_cartesian_path_action_server = Node(
@@ -64,18 +59,15 @@ def launch_setup(context, *args, **kwargs):
             robot_description_semantic,
             robot_description_kinematics,
             {"use_sim_time": use_sim_time},
-        ]
+        ],
     )
 
-    # cartesian_move_to_pose_action_server = Node(
-    #     package="rbs_skill_servers",
-    #     executable="move_to_pose.py",
-    #     namespace=namespace,
-    #     parameters=[
-    #         {"use_sim_time": use_sim_time},
-    #         {"robot_name": namespace}
-    #         ]
-    # )
+    cartesian_move_to_pose_action_server = Node(
+        package="rbs_skill_servers",
+        executable="move_to_pose.py",
+        namespace=namespace,
+        parameters=[{"use_sim_time": use_sim_time}, {"robot_name": namespace}],
+    )
 
     move_joint_state_action_server = Node(
         package="rbs_skill_servers",
@@ -86,30 +78,29 @@ def launch_setup(context, *args, **kwargs):
             robot_description_semantic,
             robot_description_kinematics,
             {"use_sim_time": use_sim_time},
-        ]
+        ],
     )
 
     assembly_config = Node(
         package="rbs_utils",
         executable="assembly_config_service.py",
         namespace=namespace,
-        output="screen"
+        output="screen",
     )
 
-
-    nodes_to_start =[
+    nodes_to_start = [
         assembly_config,
         move_topose_action_server,
         gripper_control_node,
         move_cartesian_path_action_server,
         move_joint_state_action_server,
-        # cartesian_move_to_pose_action_server,
+        cartesian_move_to_pose_action_server,
         # grasp_pose_loader
     ]
     return nodes_to_start
 
-def generate_launch_description():
 
+def generate_launch_description():
     declared_arguments = []
 
     declared_arguments.append(
@@ -125,18 +116,15 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument("robot_description_kinematics", default_value="")
     )
-    declared_arguments.append(
-        DeclareLaunchArgument("use_sim_time", default_value="")
-    )
+    declared_arguments.append(DeclareLaunchArgument("use_sim_time", default_value=""))
     declared_arguments.append(
         DeclareLaunchArgument("with_gripper_condition", default_value="")
     )
     declared_arguments.append(
         DeclareLaunchArgument("points_params_filepath", default_value="")
     )
-    declared_arguments.append(
-        DeclareLaunchArgument("namespace", default_value="")
+    declared_arguments.append(DeclareLaunchArgument("namespace", default_value=""))
+
+    return LaunchDescription(
+        declared_arguments + [OpaqueFunction(function=launch_setup)]
     )
-
-
-    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])