Add assembly configuration integration and refactor launches

- **Added support for assembly configuration in `rbs_bringup` and `rbs_robot` launches**:
  - Introduced new launch arguments: `use_rbs_utils` and `assembly_config_name`.
  - Integrated `rbs_utils`'s `utils.launch.py` for handling assembly configurations.
- **Simplified `skills.launch.py`**:
  - Removed redundant `assembly_config` node setup.
- **Enhanced `rbs_utils`**:
  - Added installation of `launch` files in `CMakeLists.txt`.
  - Created a new `utils.launch.py` for dynamically loading assembly configurations.
  - Refactored `assembly_config_service.py` to utilize `get_asm_config` for streamlined configuration file resolution.
- Improved `rbs_bringup` setup to include additional parameters and nodes for assembly configuration.

These changes centralize assembly configuration handling and enhance modularity across launch setups.

rbs_utils/rbs_utils/CMakeLists.txt

@@ -49,6 +49,11 @@ install(
   DESTINATION include
 )
 
+install(
+  DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
 ament_target_dependencies(${PROJECT_NAME} PUBLIC ${deps})
 
 # target_include_directories(asm_folder_process

rbs_utils/rbs_utils/launch/utils.launch.py

@@ -0,0 +1,40 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def launch_setup(context, *args, **kwargs):
+    asm_config_name = LaunchConfiguration("assembly_config_name")
+    namespace = LaunchConfiguration("namespace")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+
+    assembly_config = Node(
+        package="rbs_utils",
+        executable="assembly_config_service.py",
+        namespace=namespace,
+        parameters=[{"use_sim_time": use_sim_time}, {"assembly_config_name": asm_config_name.perform(context)}],
+        output="screen",
+    )
+
+    nodes_to_start = [
+        assembly_config,
+    ]
+    return nodes_to_start
+
+
+def generate_launch_description():
+    declared_arguments = []
+
+    declared_arguments.append(
+        DeclareLaunchArgument("assembly_config_name", default_value="")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("use_sim_time", default_value="false")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("namespace", default_value="")
+    )
+    return LaunchDescription(
+        declared_arguments + [OpaqueFunction(function=launch_setup)]
+    )

rbs_utils/rbs_utils/scripts/assembly_config_service.py

@@ -5,11 +5,9 @@ import rclpy
 from rclpy.node import Node
 import yaml
 from geometry_msgs.msg import Point, Pose, Quaternion
-from visualization_msgs.msg import Marker, MarkerArray
 from rbs_utils_interfaces.msg import NamedPose, RelativeNamedPose, AssemblyConfig
 from rbs_utils_interfaces.srv import GetGraspPose, GetWorkspace
-from tf2_ros import TransformListener, Buffer, TransformException
-from rbs_assets_library import get_model_meshes_info
+from rbs_assets_library import get_asm_config
 from env_manager.utils import Tf2Broadcaster
 
 
@@ -18,12 +16,12 @@ class AssemblyConfigService(Node):
         super().__init__(node_name)
 
         # Initialize parameters
-        self.declare_parameter("db_folder", "asp-example")
-        db_folder = self.get_parameter("db_folder").get_parameter_value().string_value
-
-        # Parse the YAML file
+        self.declare_parameter("assembly_config_name", "board_pick_and_place")
+        config_name = self.get_parameter("assembly_config_name").get_parameter_value().string_value
+        self.get_logger().info(f"Loading assembly config with name: {config_name}")
+        asm_config_filepath = get_asm_config(config_name)
         yaml_file = os.path.join(
-            os.getenv("RBS_ASSEMBLY_DIR", ""), db_folder, "rbs_db.yaml"
+            asm_config_filepath
         )
         self.assembly_config = parse_yaml(yaml_file)