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_bringup/launch/rbs_bringup.launch.py

@@ -35,7 +35,9 @@ def launch_setup(context, *args, **kwargs):
             "ee_link_name": "gripper_grasp_point",
             "control_space": "task",
             "control_strategy": "position",
-            "interactive": "false"
+            "interactive": "false",
+            "use_rbs_utils": "true",
+            "assembly_config_name": "board_pick_and_place"
         }.items(),
     )
 

rbs_bringup/launch/rbs_robot.launch.py

@@ -48,6 +48,9 @@ def launch_setup(context, *args, **kwargs):
     ee_link_name = LaunchConfiguration("ee_link_name").perform(context)
     base_link_name = LaunchConfiguration("base_link_name").perform(context)
 
+    use_rbs_utils = LaunchConfiguration("use_sim_time")
+    assembly_config_name = LaunchConfiguration("assembly_config_name")
+
     remappings = []
     if multi_robot == "true":
         remappings.append([("/tf", "tf"), ("/tf_static", "tf_static")])
@@ -148,11 +151,33 @@ def launch_setup(context, *args, **kwargs):
         condition=IfCondition(use_skills),
     )
 
+
+    # assembly config loader
+    utils = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare("rbs_utils"),
+                        "launch",
+                        "utils.launch.py",
+                    ]
+                )
+            ]
+        ),
+        launch_arguments={
+            "use_sim_time": use_sim_time,
+            "assembly_config_name": assembly_config_name
+        }.items(),
+        condition=IfCondition(use_rbs_utils),
+    )
+
     nodes_to_start = [
         robot_state_publisher,
         control,
         moveit,
         skills,
+        utils,
     ]
     return nodes_to_start
 
@@ -364,6 +389,20 @@ def generate_launch_description():
             description="Base link name if robot arm",
         )
     )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "assembly_config_name",
+            default_value="",
+            description="Assembly config name",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_rbs_utils",
+            default_value="true",
+            description="Whwther use utils",
+        )
+    )
 
     return LaunchDescription(
         declared_arguments + [OpaqueFunction(function=launch_setup)]

rbs_skill_servers/launch/skills.launch.py

@@ -99,16 +99,7 @@ def launch_setup(context, *args, **kwargs):
         ],
     )
 
-    assembly_config = Node(
-        package="rbs_utils",
-        executable="assembly_config_service.py",
-        namespace=namespace,
-        parameters=[{"use_sim_time": use_sim_time}],
-        output="screen",
-    )
-
     nodes_to_start = [
-        assembly_config,
         skills_container,
     ]
     return nodes_to_start

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)