Add service call timeout argument in spawner (#1808)

---------

Signed-off-by: Angsa Deployment Team <[email protected]>
Signed-off-by: Tony Najjar <[email protected]>
Co-authored-by: Angsa Deployment Team <[email protected]>
Co-authored-by: Christoph Fröhlich <[email protected]>
Co-authored-by: Sai Kishor Kothakota <[email protected]>

controller_manager/controller_manager/controller_manager_services.py

@@ -115,7 +115,9 @@ def service_caller(
     )
 
 
-def configure_controller(node, controller_manager_name, controller_name, service_timeout=0.0):
+def configure_controller(
+    node, controller_manager_name, controller_name, service_timeout=0.0, call_timeout=10.0
+):
     request = ConfigureController.Request()
     request.name = controller_name
     return service_caller(
@@ -124,54 +126,65 @@ def configure_controller(node, controller_manager_name, controller_name, service
         ConfigureController,
         request,
         service_timeout,
+        call_timeout,
     )
 
 
-def list_controllers(node, controller_manager_name, service_timeout=0.0):
+def list_controllers(node, controller_manager_name, service_timeout=0.0, call_timeout=10.0):
     request = ListControllers.Request()
     return service_caller(
         node,
         f"{controller_manager_name}/list_controllers",
         ListControllers,
         request,
         service_timeout,
+        call_timeout,
     )
 
 
-def list_controller_types(node, controller_manager_name, service_timeout=0.0):
+def list_controller_types(node, controller_manager_name, service_timeout=0.0, call_timeout=10.0):
     request = ListControllerTypes.Request()
     return service_caller(
         node,
         f"{controller_manager_name}/list_controller_types",
         ListControllerTypes,
         request,
         service_timeout,
+        call_timeout,
     )
 
 
-def list_hardware_components(node, controller_manager_name, service_timeout=0.0):
+def list_hardware_components(
+    node, controller_manager_name, service_timeout=0.0, call_timeout=10.0
+):
     request = ListHardwareComponents.Request()
     return service_caller(
         node,
         f"{controller_manager_name}/list_hardware_components",
         ListHardwareComponents,
         request,
         service_timeout,
+        call_timeout,
     )
 
 
-def list_hardware_interfaces(node, controller_manager_name, service_timeout=0.0):
+def list_hardware_interfaces(
+    node, controller_manager_name, service_timeout=0.0, call_timeout=10.0
+):
     request = ListHardwareInterfaces.Request()
     return service_caller(
         node,
         f"{controller_manager_name}/list_hardware_interfaces",
         ListHardwareInterfaces,
         request,
         service_timeout,
+        call_timeout,
     )
 
 
-def load_controller(node, controller_manager_name, controller_name, service_timeout=0.0):
+def load_controller(
+    node, controller_manager_name, controller_name, service_timeout=0.0, call_timeout=10.0
+):
     request = LoadController.Request()
     request.name = controller_name
     return service_caller(
@@ -180,10 +193,13 @@ def load_controller(node, controller_manager_name, controller_name, service_time
         LoadController,
         request,
         service_timeout,
+        call_timeout,
     )
 
 
-def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=0.0):
+def reload_controller_libraries(
+    node, controller_manager_name, force_kill, service_timeout=0.0, call_timeout=10.0
+):
     request = ReloadControllerLibraries.Request()
     request.force_kill = force_kill
     return service_caller(
@@ -192,11 +208,17 @@ def reload_controller_libraries(node, controller_manager_name, force_kill, servi
         ReloadControllerLibraries,
         request,
         service_timeout,
+        call_timeout,
     )
 
 
 def set_hardware_component_state(
-    node, controller_manager_name, component_name, lifecyle_state, service_timeout=0.0
+    node,
+    controller_manager_name,
+    component_name,
+    lifecyle_state,
+    service_timeout=0.0,
+    call_timeout=10.0,
 ):
     request = SetHardwareComponentState.Request()
     request.name = component_name
@@ -206,6 +228,8 @@ def set_hardware_component_state(
         f"{controller_manager_name}/set_hardware_component_state",
         SetHardwareComponentState,
         request,
+        service_timeout,
+        call_timeout,
     )
 
 
@@ -217,6 +241,7 @@ def switch_controllers(
     strict,
     activate_asap,
     timeout,
+    call_timeout=10.0,
 ):
     request = SwitchController.Request()
     request.activate_controllers = activate_controllers
@@ -228,11 +253,17 @@ def switch_controllers(
     request.activate_asap = activate_asap
     request.timeout = rclpy.duration.Duration(seconds=timeout).to_msg()
     return service_caller(
-        node, f"{controller_manager_name}/switch_controller", SwitchController, request
+        node,
+        f"{controller_manager_name}/switch_controller",
+        SwitchController,
+        request,
+        call_timeout=call_timeout,
     )
 
 
-def unload_controller(node, controller_manager_name, controller_name, service_timeout=0.0):
+def unload_controller(
+    node, controller_manager_name, controller_name, service_timeout=0.0, call_timeout=10.0
+):
     request = UnloadController.Request()
     request.name = controller_name
     return service_caller(
@@ -241,6 +272,7 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti
         UnloadController,
         request,
         service_timeout,
+        call_timeout,
     )
 
 

controller_manager/controller_manager/spawner.py

@@ -61,8 +61,12 @@ def has_service_names(node, node_name, node_namespace, service_names):
     return all(service in client_names for service in service_names)
 
 
-def is_controller_loaded(node, controller_manager, controller_name, service_timeout=0.0):
-    controllers = list_controllers(node, controller_manager, service_timeout).controller
+def is_controller_loaded(
+    node, controller_manager, controller_name, service_timeout=0.0, call_timeout=10.0
+):
+    controllers = list_controllers(
+        node, controller_manager, service_timeout, call_timeout
+    ).controller
     return any(c.name == controller_name for c in controllers)
 
 
@@ -120,7 +124,7 @@ def main(args=None):
     )
     parser.add_argument(
         "--controller-manager-timeout",
-        help="Time to wait for the controller manager",
+        help="Time to wait for the controller manager service to be available",
         required=False,
         default=0.0,
         type=float,
@@ -134,6 +138,13 @@ def main(args=None):
         default=5.0,
         type=float,
     )
+    parser.add_argument(
+        "--service-call-timeout",
+        help="Time to wait for the service response from the controller manager",
+        required=False,
+        default=10.0,
+        type=float,
+    )
     parser.add_argument(
         "--activate-as-group",
         help="Activates all the parsed controllers list together instead of one by one."
@@ -148,6 +159,7 @@ def main(args=None):
     controller_manager_name = args.controller_manager
     param_file = args.param_file
     controller_manager_timeout = args.controller_manager_timeout
+    service_call_timeout = args.service_call_timeout
     switch_timeout = args.switch_timeout
 
     if param_file and not os.path.isfile(param_file):
@@ -175,7 +187,11 @@ def main(args=None):
     try:
         for controller_name in controller_names:
             if is_controller_loaded(
-                node, controller_manager_name, controller_name, controller_manager_timeout
+                node,
+                controller_manager_name,
+                controller_name,
+                controller_manager_timeout,
+                service_call_timeout,
             ):
                 node.get_logger().warn(
                     bcolors.WARNING
@@ -217,7 +233,13 @@ def main(args=None):
                 )
 
             if not args.load_only:
-                ret = configure_controller(node, controller_manager_name, controller_name)
+                ret = configure_controller(
+                    node,
+                    controller_manager_name,
+                    controller_name,
+                    controller_manager_timeout,
+                    service_call_timeout,
+                )
                 if not ret.ok:
                     node.get_logger().error(
                         bcolors.FAIL + "Failed to configure controller" + bcolors.ENDC
@@ -233,6 +255,7 @@ def main(args=None):
                         True,
                         True,
                         switch_timeout,
+                        service_call_timeout,
                     )
                     if not ret.ok:
                         node.get_logger().error(
@@ -257,6 +280,7 @@ def main(args=None):
                 True,
                 True,
                 switch_timeout,
+                service_call_timeout,
             )
             if not ret.ok:
                 node.get_logger().error(
@@ -291,6 +315,7 @@ def main(args=None):
                     True,
                     True,
                     switch_timeout,
+                    service_call_timeout,
                 )
                 if not ret.ok:
                     node.get_logger().error(

controller_manager/doc/userdoc.rst

@@ -91,7 +91,8 @@ There are two scripts to interact with controller manager from launch files:
 
     $ ros2 run controller_manager spawner -h
     usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--stopped] [--inactive] [-t CONTROLLER_TYPE] [-u]
-                  [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group]
+                  [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] [--switch-timeout SWITCH_TIMEOUT]
+                  [--service-call-timeout SERVICE_CALL_TIMEOUT] [--activate-as-group]
                   controller_names [controller_names ...]
 
     positional arguments:
@@ -112,7 +113,9 @@ There are two scripts to interact with controller manager from launch files:
                             If not provided it should exist in the controller manager namespace
       -u, --unload-on-kill  Wait until this application is interrupted and unload controller
       --controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT
-                            Time to wait for the controller manager
+                            Time to wait for the controller manager service to be available
+      --service-call-timeout SERVICE_CALL_TIMEOUT
+                            Time to wait for the service response from the controller manager
       --switch-timeout SWITCH_TIMEOUT
                             Time to wait for a successful state switch of controllers. Useful when switching cannot be performed immediately, e.g.,
                             paused simulations at startup

doc/release_notes.rst

@@ -22,3 +22,4 @@ controller_manager
 * The ``ros2_control_node`` node has a new ``lock_memory`` parameter to lock memory at startup to physical RAM in order to avoid page faults (`#1822 <https://github.com/ros-controls/ros2_control/pull/1822>`_).
 * The ``ros2_control_node`` node has a new ``cpu_affinity`` parameter to bind the process to a specific CPU core. By default, this is not enabled. (`#1852 <https://github.com/ros-controls/ros2_control/pull/1852>`_).
 * ``--switch-timeout`` was added as parameter to the helper scripts ``spawner.py`` and ``unspawner.py``. Useful if controllers cannot be switched immediately, e.g., paused simulations at startup (`#1790 <https://github.com/ros-controls/ros2_control/pull/1790>`_).
+* The ``--service-call-timeout`` was added as parameter to the helper scripts ``spawner.py``. Useful when the CPU load is high at startup and the service call does not return immediately (`#1808 <https://github.com/ros-controls/ros2_control/pull/1808>`_).