Handle on waiting

controller_manager/controller_manager/controller_manager_services.py

@@ -28,15 +28,17 @@
 import rclpy
 
 
-def service_caller(node, service_name, service_type, request, service_timeout=10.0):
+def service_caller(node, service_name, service_type, request, service_timeout=0.0):
     cli = node.create_client(service_type, service_name)
 
-    if not cli.service_is_ready():
-        node.get_logger().debug(
-            f"waiting {service_timeout} seconds for service {service_name} to become available..."
-        )
-        if not cli.wait_for_service(service_timeout):
-            raise RuntimeError(f"Could not contact service {service_name}")
+    while not cli.service_is_ready():
+        node.get_logger().debug(f"waiting for service {service_name} to become available...")
+        if service_timeout:
+            if not cli.wait_for_service(service_timeout):
+                node.get_logger().fatal(f"Could not contact service {service_name}")
+                raise RuntimeError(f"Could not contact service {service_name}")
+        elif not cli.wait_for_service(10.0):
+            node.get_logger().warn(f"Could not contact service {service_name}")
 
     node.get_logger().debug(f"requester: making request: {request}\n")
     future = cli.call_async(request)
@@ -47,7 +49,7 @@ def service_caller(node, service_name, service_type, request, service_timeout=10
         raise RuntimeError(f"Exception while calling service: {future.exception()}")
 
 
-def configure_controller(node, controller_manager_name, controller_name, service_timeout=10.0):
+def configure_controller(node, controller_manager_name, controller_name, service_timeout=0.0):
     request = ConfigureController.Request()
     request.name = controller_name
     return service_caller(
@@ -59,7 +61,7 @@ def configure_controller(node, controller_manager_name, controller_name, service
     )
 
 
-def list_controllers(node, controller_manager_name, service_timeout=10.0):
+def list_controllers(node, controller_manager_name, service_timeout=0.0):
     request = ListControllers.Request()
     return service_caller(
         node,
@@ -70,7 +72,7 @@ def list_controllers(node, controller_manager_name, service_timeout=10.0):
     )
 
 
-def list_controller_types(node, controller_manager_name, service_timeout=10.0):
+def list_controller_types(node, controller_manager_name, service_timeout=0.0):
     request = ListControllerTypes.Request()
     return service_caller(
         node,
@@ -81,7 +83,7 @@ def list_controller_types(node, controller_manager_name, service_timeout=10.0):
     )
 
 
-def list_hardware_components(node, controller_manager_name, service_timeout=10.0):
+def list_hardware_components(node, controller_manager_name, service_timeout=0.0):
     request = ListHardwareComponents.Request()
     return service_caller(
         node,
@@ -92,7 +94,7 @@ def list_hardware_components(node, controller_manager_name, service_timeout=10.0
     )
 
 
-def list_hardware_interfaces(node, controller_manager_name, service_timeout=10.0):
+def list_hardware_interfaces(node, controller_manager_name, service_timeout=0.0):
     request = ListHardwareInterfaces.Request()
     return service_caller(
         node,
@@ -103,7 +105,7 @@ def list_hardware_interfaces(node, controller_manager_name, service_timeout=10.0
     )
 
 
-def load_controller(node, controller_manager_name, controller_name, service_timeout=10.0):
+def load_controller(node, controller_manager_name, controller_name, service_timeout=0.0):
     request = LoadController.Request()
     request.name = controller_name
     return service_caller(
@@ -115,7 +117,7 @@ def load_controller(node, controller_manager_name, controller_name, service_time
     )
 
 
-def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=10.0):
+def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=0.0):
     request = ReloadControllerLibraries.Request()
     request.force_kill = force_kill
     return service_caller(
@@ -128,7 +130,7 @@ def reload_controller_libraries(node, controller_manager_name, force_kill, servi
 
 
 def set_hardware_component_state(
-    node, controller_manager_name, component_name, lifecyle_state, service_timeout=10.0
+    node, controller_manager_name, component_name, lifecyle_state, service_timeout=0.0
 ):
     request = SetHardwareComponentState.Request()
     request.name = component_name
@@ -165,7 +167,7 @@ def switch_controllers(
     )
 
 
-def unload_controller(node, controller_manager_name, controller_name, service_timeout=10.0):
+def unload_controller(node, controller_manager_name, controller_name, service_timeout=0.0):
     request = UnloadController.Request()
     request.name = controller_name
     return service_caller(

controller_manager/controller_manager/hardware_spawner.py

@@ -15,13 +15,11 @@
 
 import argparse
 import sys
-import time
 
 from controller_manager import set_hardware_component_state
 
 from lifecycle_msgs.msg import State
 import rclpy
-from rclpy.duration import Duration
 from rclpy.node import Node
 from rclpy.signals import SignalHandlerOptions
 
@@ -43,17 +41,6 @@ def first_match(iterable, predicate):
     return next((n for n in iterable if predicate(n)), None)
 
 
-def wait_for_value_or(function, node, timeout, default, description):
-    while node.get_clock().now() < timeout:
-        if result := function():
-            return result
-        node.get_logger().info(
-            f"Waiting for {description}", throttle_duration_sec=2, skip_first=True
-        )
-        time.sleep(0.2)
-    return default
-
-
 def combine_name_and_namespace(name_and_namespace):
     node_name, namespace = name_and_namespace
     return namespace + ("" if namespace.endswith("/") else "/") + node_name
@@ -75,37 +62,6 @@ def has_service_names(node, node_name, node_namespace, service_names):
     return all(service in client_names for service in service_names)
 
 
-def wait_for_controller_manager(node, controller_manager, timeout_duration):
-    # List of service names from controller_manager we wait for
-    service_names = (
-        f"{controller_manager}/list_hardware_components",
-        f"{controller_manager}/set_hardware_component_state",
-    )
-
-    # Wait for controller_manager
-    timeout = node.get_clock().now() + Duration(seconds=timeout_duration)
-    node_and_namespace = wait_for_value_or(
-        lambda: find_node_and_namespace(node, controller_manager),
-        node,
-        timeout,
-        None,
-        f"'{controller_manager}' node to exist",
-    )
-
-    # Wait for the services if the node was found
-    if node_and_namespace:
-        node_name, namespace = node_and_namespace
-        return wait_for_value_or(
-            lambda: has_service_names(node, node_name, namespace, service_names),
-            node,
-            timeout,
-            False,
-            f"'{controller_manager}' services to be available",
-        )
-
-    return False
-
-
 def handle_set_component_state_service_call(
     node, controller_manager_name, component, target_state, action
 ):
@@ -163,14 +119,6 @@ def main(args=None):
         default="controller_manager",
         required=False,
     )
-    parser.add_argument(
-        "--controller-manager-timeout",
-        help="Time to wait for the controller manager",
-        required=False,
-        default=10,
-        type=int,
-    )
-
     # add arguments which are mutually exclusive
     activate_or_confiigure_grp.add_argument(
         "--activate",
@@ -188,7 +136,6 @@ def main(args=None):
     command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
     args = parser.parse_args(command_line_args)
     controller_manager_name = args.controller_manager
-    controller_manager_timeout = args.controller_manager_timeout
     hardware_component = [args.hardware_component_name]
     activate = args.activate
     configure = args.configure
@@ -202,12 +149,6 @@ def main(args=None):
             controller_manager_name = f"/{controller_manager_name}"
 
     try:
-        if not wait_for_controller_manager(
-            node, controller_manager_name, controller_manager_timeout
-        ):
-            node.get_logger().error("Controller manager not available")
-            return 1
-
         if activate:
             activate_components(node, controller_manager_name, hardware_component)
         elif configure:
@@ -218,6 +159,8 @@ def main(args=None):
             )
             parser.print_help()
             return 0
+    except KeyboardInterrupt:
+        pass
     finally:
         rclpy.shutdown()
 

controller_manager/controller_manager/spawner.py

@@ -30,7 +30,6 @@
 
 import rclpy
 from rcl_interfaces.msg import Parameter
-from rclpy.duration import Duration
 from rclpy.node import Node
 
 # @note: The versions conditioning is added here to support the source-compatibility with Humble
@@ -61,17 +60,6 @@ def first_match(iterable, predicate):
     return next((n for n in iterable if predicate(n)), None)
 
 
-def wait_for_value_or(function, node, timeout, default, description):
-    while node.get_clock().now() < timeout:
-        if result := function():
-            return result
-        node.get_logger().info(
-            f"Waiting for {description}", throttle_duration_sec=2, skip_first=True
-        )
-        time.sleep(0.2)
-    return default
-
-
 def combine_name_and_namespace(name_and_namespace):
     node_name, namespace = name_and_namespace
     return namespace + ("" if namespace.endswith("/") else "/") + node_name
@@ -92,46 +80,8 @@ def has_service_names(node, node_name, node_namespace, service_names):
     return all(service in client_names for service in service_names)
 
 
-def wait_for_controller_manager(node, controller_manager, timeout_duration):
-    # List of service names from controller_manager we wait for
-    service_names = (
-        f"{controller_manager}/configure_controller",
-        f"{controller_manager}/list_controllers",
-        f"{controller_manager}/list_controller_types",
-        f"{controller_manager}/list_hardware_components",
-        f"{controller_manager}/list_hardware_interfaces",
-        f"{controller_manager}/load_controller",
-        f"{controller_manager}/reload_controller_libraries",
-        f"{controller_manager}/switch_controller",
-        f"{controller_manager}/unload_controller",
-    )
-
-    # Wait for controller_manager
-    timeout = node.get_clock().now() + Duration(seconds=timeout_duration)
-    node_and_namespace = wait_for_value_or(
-        lambda: find_node_and_namespace(node, controller_manager),
-        node,
-        timeout,
-        None,
-        f"'{controller_manager}' node to exist",
-    )
-
-    # Wait for the services if the node was found
-    if node_and_namespace:
-        node_name, namespace = node_and_namespace
-        return wait_for_value_or(
-            lambda: has_service_names(node, node_name, namespace, service_names),
-            node,
-            timeout,
-            False,
-            f"'{controller_manager}' services to be available",
-        )
-
-    return False
-
-
-def is_controller_loaded(node, controller_manager, controller_name):
-    controllers = list_controllers(node, controller_manager).controller
+def is_controller_loaded(node, controller_manager, controller_name, service_timeout=0.0):
+    controllers = list_controllers(node, controller_manager, service_timeout).controller
     return any(c.name == controller_name for c in controllers)
 
 
@@ -184,8 +134,8 @@ def main(args=None):
         "--controller-manager-timeout",
         help="Time to wait for the controller manager",
         required=False,
-        default=10,
-        type=int,
+        default=0,
+        type=float,
     )
     parser.add_argument(
         "--activate-as-group",
@@ -202,7 +152,7 @@ def main(args=None):
     controller_namespace = args.namespace
     param_file = args.param_file
     controller_type = args.controller_type
-    controller_manager_timeout = args.controller_manager_timeout
+    timeout = args.controller_manager_timeout
 
     if param_file and not os.path.isfile(param_file):
         raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file)
@@ -217,20 +167,14 @@ def main(args=None):
             controller_manager_name = f"/{controller_manager_name}"
 
     try:
-        if not wait_for_controller_manager(
-            node, controller_manager_name, controller_manager_timeout
-        ):
-            node.get_logger().error(
-                bcolors.FAIL + "Controller manager not available" + bcolors.ENDC
-            )
-            return 1
-
         for controller_name in controller_names:
             prefixed_controller_name = controller_name
             if controller_namespace:
                 prefixed_controller_name = controller_namespace + "/" + controller_name
 
-            if is_controller_loaded(node, controller_manager_name, prefixed_controller_name):
+            if is_controller_loaded(
+                node, controller_manager_name, prefixed_controller_name, timeout
+            ):
                 node.get_logger().warn(
                     bcolors.WARNING
                     + "Controller already loaded, skipping load_controller"
@@ -392,6 +336,8 @@ def main(args=None):
 
             node.get_logger().info("Unloaded controller")
         return 0
+    except KeyboardInterrupt:
+        pass
     finally:
         rclpy.shutdown()
 

controller_manager/controller_manager/unspawner.py

@@ -57,6 +57,8 @@ def main(args=None):
             node.get_logger().info("Unloaded controller")
 
         return 0
+    except KeyboardInterrupt:
+        pass
     finally:
         rclpy.shutdown()
 

controller_manager/doc/userdoc.rst

@@ -136,6 +136,7 @@ There are two scripts to interact with controller manager from launch files:
 
   1. ``spawner`` - loads, configures and start a controller on startup.
   2. ``unspawner`` - stops and unloads a controller.
+  3. ``hardware_spawner`` - activates and configures a hardware component.
 
 
 ``spawner``
@@ -184,6 +185,26 @@ There are two scripts to interact with controller manager from launch files:
       -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER
                             Name of the controller manager ROS node
 
+
+``hardware_spawner``
+^^^^^^^^^^^^^^^^^^^^^^
+
+.. code-block:: console
+
+    $ ros2 run controller_manager hardware_spawner -h
+    usage: hardware_spawner [-h] [-c CONTROLLER_MANAGER] (--activate | --configure) hardware_component_name
+
+    positional arguments:
+      hardware_component_name
+                            The name of the hardware component which should be activated.
+
+    options:
+      -h, --help            show this help message and exit
+      -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER
+                            Name of the controller manager ROS node
+      --activate            Activates the given components. Note: Components are by default configured before activated.
+      --configure           Configures the given components.
+
 Using the Controller Manager in a Process
 -----------------------------------------