Merge branch 'master' into sensor-joints

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,7 +114,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,
@@ -124,6 +128,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."
@@ -138,6 +149,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):
@@ -174,7 +186,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,
+                service_call_timeout,
             ):
                 node.get_logger().warn(
                     bcolors.WARNING
@@ -207,7 +223,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
@@ -223,6 +245,7 @@ def main(args=None):
                         True,
                         True,
                         switch_timeout,
+                        service_call_timeout,
                     )
                     if not ret.ok:
                         node.get_logger().error(
@@ -247,6 +270,7 @@ def main(args=None):
                 True,
                 True,
                 switch_timeout,
+                service_call_timeout,
             )
             if not ret.ok:
                 node.get_logger().error(
@@ -279,6 +303,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

@@ -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] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT]
-                  [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group]
+                  [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] [--service-call-timeout SERVICE_CALL_TIMEOUT]
                   controller_names [controller_names ...]
 
     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 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 if controllers cannot be switched immediately, e.g., paused
                             simulations at startup

doc/release_notes.rst

@@ -81,6 +81,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 ``--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>`_).
 
 hardware_interface
 ******************