From 7576e617dae581d05a2894a36b98b754d1992ae7 Mon Sep 17 00:00:00 2001
From: Michael Carroll <michael@openrobotics.org>
Date: Thu, 16 Feb 2023 15:21:36 -0600
Subject: [PATCH] Make composable node container aware of ros domain

Signed-off-by: Michael Carroll <michael@openrobotics.org>
---
 .../actions/load_composable_nodes.py          | 13 ++++++--
 launch_ros/launch_ros/ros_adapters.py         | 31 +++++++++++++------
 2 files changed, 33 insertions(+), 11 deletions(-)

diff --git a/launch_ros/launch_ros/actions/load_composable_nodes.py b/launch_ros/launch_ros/actions/load_composable_nodes.py
index fda356a2..15be10fd 100644
--- a/launch_ros/launch_ros/actions/load_composable_nodes.py
+++ b/launch_ros/launch_ros/actions/load_composable_nodes.py
@@ -218,10 +218,18 @@ def execute(
         else:
             self.__logger.error(
                 'target container is neither a ComposableNodeContainer nor a SubstitutionType')
-            return
+            return None
+
+        domain_id = None
+        if self.__target_container.additional_env is not None:
+            for item in self.__target_container.additional_env:
+                key = item[0][0].text
+                value = item[1][0].text
+                if key == 'ROS_DOMAIN_ID':
+                    domain_id = value
 
         # Create a client to load nodes in the target container.
-        self.__rclpy_load_node_client = get_ros_node(context).create_client(
+        self.__rclpy_load_node_client = get_ros_node(context, domain_id=domain_id).create_client(
             composition_interfaces.srv.LoadNode, '{}/_container/load_node'.format(
                 self.__final_target_container_name
             )
@@ -242,6 +250,7 @@ def execute(
                     None, self._load_in_sequence, load_node_requests, context
                 )
             )
+        return None
 
 
 def get_composable_node_load_request(
diff --git a/launch_ros/launch_ros/ros_adapters.py b/launch_ros/launch_ros/ros_adapters.py
index 926644be..26b66255 100644
--- a/launch_ros/launch_ros/ros_adapters.py
+++ b/launch_ros/launch_ros/ros_adapters.py
@@ -33,7 +33,8 @@ def __init__(
         self,
         *,
         argv: Optional[List[str]] = None,
-        autostart: bool = True
+        autostart: bool = True,
+        domain_id: Optional[int] = None,
     ):
         """
         Construct adapter.
@@ -47,6 +48,7 @@ def __init__(
         self.__ros_node = None
         self.__ros_executor = None
         self.__is_running = False
+        self.__domain_id = domain_id
 
         if autostart:
             self.start()
@@ -57,8 +59,14 @@ def start(self):
             raise RuntimeError('Cannot start a ROS adapter that is already running')
         self.__ros_context = rclpy.Context()
         rclpy.init(args=self.__argv, context=self.__ros_context)
+
+        if self.__domain_id:
+            node_name = 'launch_ros_{}_{}'.format(os.getpid(), self.__domain_id)
+        else:
+            node_name = 'launch_ros_{}'.format(os.getpid())
+
         self.__ros_node = rclpy.create_node(
-            'launch_ros_{}'.format(os.getpid()),
+            node_name,
             context=self.__ros_context
         )
         self.__ros_executor = SingleThreadedExecutor(context=self.__ros_context)
@@ -106,7 +114,7 @@ def ros_executor(self):
         return self.__ros_executor
 
 
-def get_ros_adapter(context: launch.LaunchContext):
+def get_ros_adapter(context: launch.LaunchContext, domain_id: Optional[int] = None):
     """
     Get the ROS adapter managed by the given launch context.
 
@@ -115,16 +123,21 @@ def get_ros_adapter(context: launch.LaunchContext):
     This function is reentrant but concurrent calls on the
     same `context` are not safe.
     """
-    if not hasattr(context.locals, 'ros_adapter'):
-        ros_adapter = ROSAdapter()
-        context.extend_globals({'ros_adapter': ros_adapter})
+
+    if (not hasattr(context.locals, 'ros_adapter') or
+            domain_id not in context.locals.ros_adapter):
+        print(f"Start node with domain: {domain_id}")
+        ros_adapter = ROSAdapter(domain_id=domain_id)
+        context.extend_globals({'ros_adapter': {}})
+        context.locals.ros_adapter[domain_id] = ros_adapter
         context.register_event_handler(launch.event_handlers.OnShutdown(
             on_shutdown=lambda *args, **kwargs: ros_adapter.shutdown()
         ))
-    return context.locals.ros_adapter
+
+    return context.locals.ros_adapter[domain_id]
 
 
-def get_ros_node(context: launch.LaunchContext):
+def get_ros_node(context: launch.LaunchContext, domain_id: Optional[int] = None):
     """
     Get the ROS node managed by the given launch context.
 
@@ -133,4 +146,4 @@ def get_ros_node(context: launch.LaunchContext):
     This function is reentrant but concurrent calls on the
     same `context` are not safe.
     """
-    return get_ros_adapter(context).ros_node
+    return get_ros_adapter(context, domain_id).ros_node