From cfc94bc0dcf880296c37920cf527b881efc7cb4a Mon Sep 17 00:00:00 2001
From: "Addisu Z. Taddese" <addisu@openrobotics.org>
Date: Thu, 30 May 2024 11:45:57 -0500
Subject: [PATCH] Ensure the same container is used for the bridge and
 gz_server

This also adds a required `name` parameter for the bridge so that
multiple different bridges can be created without name collision

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>
---
 ros_gz_bridge/launch/ros_gz_bridge.launch.py  | 21 ++++++++++++-------
 .../ros_gz_bridge/actions/ros_gz_bridge.py    | 14 ++++++++++++-
 2 files changed, 26 insertions(+), 9 deletions(-)

diff --git a/ros_gz_bridge/launch/ros_gz_bridge.launch.py b/ros_gz_bridge/launch/ros_gz_bridge.launch.py
index 7809299c..5ad46b9d 100644
--- a/ros_gz_bridge/launch/ros_gz_bridge.launch.py
+++ b/ros_gz_bridge/launch/ros_gz_bridge.launch.py
@@ -18,12 +18,13 @@
 from launch.actions import DeclareLaunchArgument, GroupAction
 from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration, PythonExpression
-from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.actions import LoadComposableNodes, Node
 from launch_ros.descriptions import ComposableNode
 
 
 def generate_launch_description():
 
+    name = LaunchConfiguration('name')
     config_file = LaunchConfiguration('config_file')
     container_name = LaunchConfiguration('container_name')
     namespace = LaunchConfiguration('namespace')
@@ -31,6 +32,10 @@ def generate_launch_description():
     use_respawn = LaunchConfiguration('use_respawn')
     log_level = LaunchConfiguration('log_level')
 
+    declare_name_cmd = DeclareLaunchArgument(
+        'name', description='Name of ros_gz_bridge node'
+    )
+
     declare_config_file_cmd = DeclareLaunchArgument(
         'config_file', default_value='', description='YAML config file'
     )
@@ -65,6 +70,8 @@ def generate_launch_description():
             Node(
                 package='ros_gz_bridge',
                 executable='bridge_node',
+                name=name,
+                namespace=namespace,
                 output='screen',
                 respawn=use_respawn,
                 respawn_delay=2.0,
@@ -74,28 +81,26 @@ def generate_launch_description():
         ],
     )
 
-    load_composable_nodes = ComposableNodeContainer(
+    load_composable_nodes = LoadComposableNodes(
         condition=IfCondition(use_composition),
-        name=container_name,
-        namespace=namespace,
-        package='rclcpp_components',
-        executable='component_container',
+        target_container=container_name,
         composable_node_descriptions=[
             ComposableNode(
                 package='ros_gz_bridge',
                 plugin='ros_gz_bridge::RosGzBridge',
-                name='ros_gz_bridge',
+                name=name,
+                namespace=namespace,
                 parameters=[{'config_file': config_file}],
                 extra_arguments=[{'use_intra_process_comms': True}],
             ),
         ],
-        output='screen',
     )
 
     # Create the launch description and populate
     ld = LaunchDescription()
 
     # Declare the launch options
+    ld.add_action(declare_name_cmd)
     ld.add_action(declare_config_file_cmd)
     ld.add_action(declare_container_name_cmd)
     ld.add_action(declare_namespace_cmd)
diff --git a/ros_gz_bridge/ros_gz_bridge/actions/ros_gz_bridge.py b/ros_gz_bridge/ros_gz_bridge/actions/ros_gz_bridge.py
index e9cf15ee..9704f44e 100644
--- a/ros_gz_bridge/ros_gz_bridge/actions/ros_gz_bridge.py
+++ b/ros_gz_bridge/ros_gz_bridge/actions/ros_gz_bridge.py
@@ -34,6 +34,7 @@ class RosGzBridge(Action):
     def __init__(
         self,
         *,
+        name: SomeSubstitutionsType,
         config_file: Optional[SomeSubstitutionsType] = None,
         container_name: Optional[SomeSubstitutionsType] = None,
         namespace: Optional[SomeSubstitutionsType] = None,
@@ -48,6 +49,7 @@ def __init__(
         All arguments are forwarded to `ros_gz_bridge.launch.ros_gz_bridge.launch.py`,
         so see the documentation of that class for further details.
 
+        :param: name Name of ros_gz_bridge  node
         :param: config_file YAML config file.
         :param: container_name Name of container that nodes will load in if use composition.
         :param: namespace Top-level namespace.
@@ -56,6 +58,7 @@ def __init__(
         :param: log_level Log level.
         """
         super().__init__(**kwargs)
+        self.__name = name
         self.__config_file = config_file
         self.__container_name = container_name
         self.__namespace = namespace
@@ -68,6 +71,10 @@ def parse(cls, entity: Entity, parser: Parser):
         """Parse ros_gz_bridge."""
         _, kwargs = super().parse(entity, parser)
 
+        name = entity.get_attr(
+            'name', data_type=str,
+            optional=False)
+
         config_file = entity.get_attr(
             'config_file', data_type=str,
             optional=False)
@@ -92,6 +99,10 @@ def parse(cls, entity: Entity, parser: Parser):
             'log_level', data_type=str,
             optional=True)
 
+        if isinstance(name, str):
+            name = parser.parse_substitution(name)
+            kwargs['name'] = name
+
         if isinstance(config_file, str):
             config_file = parser.parse_substitution(config_file)
             kwargs['config_file'] = config_file
@@ -125,7 +136,8 @@ def execute(self, context: LaunchContext) -> Optional[List[Action]]:
                 [PathJoinSubstitution([FindPackageShare('ros_gz_bridge'),
                                        'launch',
                                        'ros_gz_bridge.launch.py'])]),
-            launch_arguments=[('config_file', self.__config_file),
+            launch_arguments=[('name', self.__name),
+                              ('config_file', self.__config_file),
                               ('container_name', self.__container_name),
                               ('namespace',   self.__namespace),
                               ('use_composition',  self.__use_composition),