Add load_node_timeout parameter to ComposableNodeContainer and LoadComposableNodes actions

Signed-off-by: Błażej Sowa <bsowa123@gmail.com>

Author: bjsowa

launch_ros/launch_ros/actions/composable_node_container.py

@@ -15,14 +15,15 @@
 """Module for the ComposableNodeContainer action."""
 
 from typing import List
-from typing import Optional
+from typing import Optional, Union
 
 from launch.action import Action
 from launch.frontend import Entity
 from launch.frontend import expose_action
 from launch.frontend import Parser
 from launch.launch_context import LaunchContext
 from launch.some_substitutions_type import SomeSubstitutionsType
+from launch.utilities import type_utils
 
 from .node import Node
 
@@ -40,6 +41,7 @@ def __init__(
         name: SomeSubstitutionsType,
         namespace: SomeSubstitutionsType,
         composable_node_descriptions: Optional[List[ComposableNode]] = None,
+        load_node_timeout: Optional[Union[float, SomeSubstitutionsType]] = None,
         **kwargs
     ) -> None:
         """
@@ -52,15 +54,21 @@ def __init__(
         :param: namespace the ROS namespace for this Node, mandatory for full container node
              name resolution
         :param composable_node_descriptions: optional descriptions of composable nodes to be loaded
+        :param load_node_timeout: optional timeout for loading each composable node, in seconds
         """
         super().__init__(name=name, namespace=namespace, **kwargs)
         self.__composable_node_descriptions = composable_node_descriptions
+        self.__load_node_timeout = type_utils.normalize_typed_substitution(
+            load_node_timeout, float) if load_node_timeout is not None else None
 
     @classmethod
     def parse(cls, entity: Entity, parser: Parser):
         """Parse node_container."""
         _, kwargs = super().parse(entity, parser)
 
+        kwargs['load_node_timeout'] = parser.parse_if_substitutions(
+            entity.get_attr('load_node_timeout', data_type=float, optional=True, can_be_str=True))
+
         composable_nodes = entity.get_attr(
             'composable_node', data_type=List[Entity], optional=True)
         composable_lifecycle_nodes = entity.get_attr(
@@ -106,7 +114,8 @@ def execute(self, context: LaunchContext) -> Optional[List[Action]]:
             load_actions = [
                 LoadComposableNodes(
                     composable_node_descriptions=valid_composable_nodes,
-                    target_container=self
+                    target_container=self,
+                    load_node_timeout=self.__load_node_timeout
                 )
             ]
         container_actions = super().execute(context)  # type: Optional[List[Action]]

launch_ros/launch_ros/actions/load_composable_nodes.py

@@ -65,6 +65,7 @@ def __init__(
         *,
         composable_node_descriptions: List[ComposableNode],
         target_container: Union[SomeSubstitutionsType, ComposableNodeContainer],
+        load_node_timeout: Optional[Union[float, SomeSubstitutionsType]] = None,
         **kwargs,
     ) -> None:
         """
@@ -79,6 +80,7 @@ def __init__(
 
         :param composable_node_descriptions: descriptions of composable nodes to be loaded
         :param target_container: the container to load the nodes into
+        :param load_node_timeout: optional timeout for loading each composable node, in seconds
         """
         ensure_argument_type(
             target_container,
@@ -90,6 +92,8 @@ def __init__(
         super().__init__(**kwargs)
         self.__composable_node_descriptions = composable_node_descriptions
         self.__target_container = target_container
+        self.__load_node_timeout = type_utils.normalize_typed_substitution(
+            load_node_timeout, float) if load_node_timeout is not None else None
         self.__final_target_container_name: Optional[Text] = None
         self.__logger = launch.logging.get_logger(__name__)
 
@@ -101,6 +105,9 @@ def parse(cls, entity: Entity, parser: Parser):
         kwargs['target_container'] = parser.parse_substitution(
             entity.get_attr('target', data_type=str))
 
+        kwargs['load_node_timeout'] = parser.parse_if_substitutions(
+            entity.get_attr('load_node_timeout', data_type=float, optional=True, can_be_str=True))
+
         kwargs['composable_node_descriptions'] = []
         composable_nodes = entity.get_attr(
             'composable_node', data_type=List[Entity], optional=True) or []
@@ -149,7 +156,7 @@ def _load_node(
         # Asynchronously wait on service call so that we can periodically check for shutdown
         event = threading.Event()
 
-        def unblock(future):
+        def unblock(future=None):
             nonlocal event
             event.set()
 
@@ -162,13 +169,36 @@ def unblock(future):
         response_future = self.__rclpy_load_node_client.call_async(request)
         response_future.add_done_callback(unblock)
 
+        timeout_value = None
+        if self.__load_node_timeout is not None:
+            value = type_utils.perform_typed_substitution(context, self.__load_node_timeout, float)
+            if value > 0.0:
+                timeout_value = value
+                timeout_timer = get_ros_node(context).create_timer(timeout_value, unblock)
+
         while not event.wait(1.0):
             if context.is_shutdown:
                 self.__logger.warning(
                     "Abandoning wait for the '{}' service response, due to shutdown.".format(
                         self.__rclpy_load_node_client.srv_name),
                 )
                 response_future.cancel()
+                if timeout_value is not None:
+                    timeout_timer.cancel()
+                    timeout_timer.destroy()
+                return
+
+        if timeout_value is not None:
+            timeout_timer.cancel()
+            timeout_timer.destroy()
+
+            if not response_future.done():
+                self.__logger.error(
+                    "Load node request for node '{}' timed out after {} seconds".format(
+                        request.node_name, timeout_value
+                    )
+                )
+                response_future.cancel()
                 return
 
         # Get response

test_launch_ros/test/test_launch_ros/actions/test_load_composable_nodes.py

@@ -16,6 +16,7 @@
 
 import pathlib
 import threading
+import time
 
 from composition_interfaces.srv import LoadNode
 
@@ -44,9 +45,10 @@
 
 class MockComponentContainer(rclpy.node.Node):
 
-    def __init__(self, context):
+    def __init__(self, context, delay=0.0):
         # List of LoadNode requests received
         self.requests = []
+        self.delay = delay
 
         super().__init__(TEST_CONTAINER_NAME, context=context)
 
@@ -57,6 +59,8 @@ def __init__(self, context):
         )
 
     def load_node_callback(self, request, response):
+        if self.delay > 0.0:
+            time.sleep(self.delay)
         self.requests.append(request)
         response.success = True
         if request.node_namespace == '/':
@@ -84,10 +88,12 @@ def _load_composable_node(
     condition=None,
     parameters=None,
     remappings=None,
-    target_container=f'/{TEST_CONTAINER_NAME}'
+    target_container=f'/{TEST_CONTAINER_NAME}',
+    load_node_timeout=None
 ):
     return LoadComposableNodes(
         target_container=target_container,
+        load_node_timeout=load_node_timeout,
         composable_node_descriptions=[
             ComposableNode(
                 condition=condition,
@@ -119,6 +125,25 @@ def mock_component_container():
         thread.join()
 
 
+@pytest.fixture
+def slow_mock_component_container():
+    """Mock container with 3-second delay for timeout testing."""
+    context = rclpy.context.Context()
+    with rclpy.init(context=context):
+        executor = rclpy.executors.SingleThreadedExecutor(context=context)
+
+        container = MockComponentContainer(context, delay=3.0)
+        executor.add_node(container)
+
+        # Start spinning in a thread
+        thread = threading.Thread(target=lambda executor: executor.spin(), args=(executor,))
+        thread.start()
+        yield container
+        executor.remove_node(container)
+        executor.shutdown()
+        thread.join()
+
+
 def test_load_node(mock_component_container):
     """Test loading a node."""
     context = _assert_launch_no_errors([
@@ -654,3 +679,45 @@ def test_load_node_with_condition_in_group(mock_component_container):
     assert len(request.remap_rules) == 0
     assert len(request.parameters) == 0
     assert len(request.extra_arguments) == 0
+
+
+def test_load_node_with_timeout(slow_mock_component_container):
+    """Test that load_node_timeout actually times out slow services."""
+    # Test 1: Timeout is shorter than service delay - should timeout and not load node
+    start_time = time.time()
+    launch_context = _assert_launch_no_errors([
+        _load_composable_node(
+            package='foo_package',
+            plugin='bar_plugin',
+            name='test_node_timeout',
+            namespace='test_namespace',
+            load_node_timeout=0.5  # Shorter than the 3 second delay
+        )
+    ])
+    elapsed_time = time.time() - start_time
+
+    # Should timeout before the 3 second service response
+    assert elapsed_time < 3.0, f'Expected quick timeout, but took {elapsed_time:.2f} seconds'
+    # Node should NOT be registered since we skipped waiting for service response
+    assert get_node_name_count(launch_context, '/test_namespace/test_node_timeout') == 0
+
+    # Test 2: Timeout is longer than service delay - should succeed
+    start_time = time.time()
+    launch_context = _assert_launch_no_errors([
+        _load_composable_node(
+            package='foo_package',
+            plugin='bar_plugin',
+            name='test_node_success',
+            namespace='test_namespace',
+            load_node_timeout=5.0  # Longer than the 3 second delay
+        )
+    ])
+    elapsed_time = time.time() - start_time
+
+    # Should wait for the full service response (around 3-4 seconds including overhead)
+    assert elapsed_time >= 2.5, f'Should have waited for service, took {elapsed_time:.2f} seconds'
+    assert elapsed_time < 7.0, f'Should not have timed out, took {elapsed_time:.2f} seconds'
+    # Node SHOULD be registered since it completed successfully
+    assert get_node_name_count(launch_context, '/test_namespace/test_node_success') == 1
+    # Verify the container received both requests
+    assert len(slow_mock_component_container.requests) == 2

test_launch_ros/test/test_launch_ros/frontend/test_component_container.py

@@ -34,6 +34,7 @@ def test_launch_component_container_yaml():
                 name: my_container
                 namespace: ''
                 args: 'test_args'
+                load_node_timeout: 5.0
                 composable_node:
                     -   pkg: composition
                         plugin: composition::Talker
@@ -51,6 +52,7 @@ def test_launch_component_container_yaml():
 
             - load_composable_node:
                 target: my_container
+                load_node_timeout: 5.0
                 composable_node:
                     -   pkg: composition
                         plugin: composition::Listener
@@ -75,15 +77,15 @@ def test_launch_component_container_xml():
     xml_file = textwrap.dedent(
         r"""
         <launch>
-            <node_container pkg="rclcpp_components" exec="component_container" name="my_container" namespace="" args="test_args">
+            <node_container pkg="rclcpp_components" exec="component_container" name="my_container" namespace="" args="test_args" load_node_timeout="5.0">
                 <composable_node pkg="composition" plugin="composition::Talker" name="talker" namespace="test_namespace">
                     <remap from="chatter" to="/remap/chatter" />
                     <param name="use_sim_time" value="true"/>
                     <extra_arg name="use_intra_process_comms" value="true"/>
                 </composable_node>
             </node_container>
 
-            <load_composable_node target="my_container">
+            <load_composable_node target="my_container" load_node_timeout="5.0">
                 <composable_node pkg="composition" plugin="composition::Listener" name="listener" namespace="test_namespace">
                     <remap from="chatter" to="/remap/chatter" />
                     <param name="use_sim_time" value="true"/>
@@ -119,8 +121,10 @@ def perform(substitution):
     assert perform(node_container._Node__node_name) == 'my_container'
     assert perform(node_container._Node__node_namespace) == ''
     assert perform(node_container._Node__arguments[0]) == 'test_args'
+    assert node_container._ComposableNodeContainer__load_node_timeout == 5.0
 
     assert perform(load_composable_node._LoadComposableNodes__target_container) == 'my_container'
+    assert load_composable_node._LoadComposableNodes__load_node_timeout == 5.0
 
     # Check node parameters
     talker_remappings = list(talker._ComposableNode__remappings)