Add service call timeout argument in spawner

controller_manager/controller_manager/controller_manager_services.py

@@ -123,7 +123,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(
@@ -132,54 +134,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(
@@ -188,10 +201,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(
@@ -200,11 +216,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
@@ -215,6 +237,7 @@ def set_hardware_component_state(
         SetHardwareComponentState,
         request,
         service_timeout,
+        call_timeout,
     )
 
 
@@ -226,6 +249,7 @@ def switch_controllers(
     strict,
     activate_asap,
     timeout,
+    call_timeout=10.0,
 ):
     request = SwitchController.Request()
     request.activate_controllers = activate_controllers
@@ -237,11 +261,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(
@@ -250,6 +280,7 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti
         UnloadController,
         request,
         service_timeout,
+        call_timeout,
     )
 
 

controller_manager/controller_manager/spawner.py

@@ -60,8 +60,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)
 
 
@@ -110,11 +114,18 @@ 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,
         type=float,
     )
+    parser.add_argument(
+        "--controller-manager-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."
@@ -129,6 +140,7 @@ def main(args=None):
     controller_manager_name = args.controller_manager
     param_file = args.param_file
     controller_manager_timeout = args.controller_manager_timeout
+    controller_manager_call_timeout = args.controller_manager_call_timeout
 
     if param_file and not os.path.isfile(param_file):
         raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file)
@@ -164,7 +176,11 @@ def main(args=None):
         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,
+                controller_manager_call_timeout,
             ):
                 node.get_logger().warn(
                     bcolors.WARNING
@@ -197,7 +213,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,
+                    controller_manager_call_timeout,
+                )
                 if not ret.ok:
                     node.get_logger().error(
                         bcolors.FAIL + "Failed to configure controller" + bcolors.ENDC
@@ -206,7 +228,14 @@ def main(args=None):
 
                 if not args.inactive and not args.activate_as_group:
                     ret = switch_controllers(
-                        node, controller_manager_name, [], [controller_name], True, True, 5.0
+                        node,
+                        controller_manager_name,
+                        [],
+                        [controller_name],
+                        True,
+                        True,
+                        controller_manager_timeout,
+                        controller_manager_call_timeout,
                     )
                     if not ret.ok:
                         node.get_logger().error(
@@ -224,7 +253,14 @@ def main(args=None):
 
         if not args.inactive and args.activate_as_group:
             ret = switch_controllers(
-                node, controller_manager_name, [], controller_names, True, True, 5.0
+                node,
+                controller_manager_name,
+                [],
+                controller_names,
+                True,
+                True,
+                controller_manager_timeout,
+                controller_manager_call_timeout,
             )
             if not ret.ok:
                 node.get_logger().error(
@@ -250,7 +286,14 @@ def main(args=None):
                 node.get_logger().info("Interrupt captured, deactivating and unloading controller")
                 # TODO(saikishor) we might have an issue in future, if any of these controllers is in chained mode
                 ret = switch_controllers(
-                    node, controller_manager_name, controller_names, [], True, True, 5.0
+                    node,
+                    controller_manager_name,
+                    controller_names,
+                    [],
+                    True,
+                    True,
+                    controller_manager_timeout,
+                    controller_manager_call_timeout,
                 )
                 if not ret.ok:
                     node.get_logger().error(

controller_manager/doc/userdoc.rst

@@ -158,7 +158,7 @@ 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] [--inactive] [-t CONTROLLER_TYPE] [-u]
-                      [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT]
+                      [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] [--controller-manager-call-timeout CONTROLLER_MANAGER_CALL_TIMEOUT]
                       controller_name
 
     positional arguments:
@@ -176,7 +176,9 @@ There are two scripts to interact with controller manager from launch files:
       --inactive            Load and configure the controller, however do not activate them
       -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 
+      --controller-manager-call-timeout CONTROLLER_MANAGER_CALL_TIMEOUT
+                            Time to wait for the service response from the controller manager
       --activate-as-group   Activates all the parsed controllers list together instead of one by one. Useful for activating all chainable controllers altogether
       --fallback_controllers FALLBACK_CONTROLLERS [FALLBACK_CONTROLLERS ...]
                             Fallback controllers list are activated as a fallback strategy when the spawned controllers fail. When the argument is provided, it takes precedence over the fallback_controllers list in the

doc/release_notes.rst

@@ -79,6 +79,7 @@ controller_manager
 * The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 <https://github.com/ros-controls/ros2_control/pull/1820>`_).
 * 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>`_).
+* The ``--controller-manager-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>`_).
 
 hardware_interface
 ******************