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]>
(cherry picked from commit 41d7393)

# Conflicts:
#	controller_manager/controller_manager/controller_manager_services.py
#	controller_manager/doc/userdoc.rst
#	doc/release_notes.rst

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,11 @@ def set_hardware_component_state(
         f"{controller_manager_name}/set_hardware_component_state",
         SetHardwareComponentState,
         request,
+<<<<<<< HEAD
+=======
+        service_timeout,
+        call_timeout,
+>>>>>>> 41d7393 (Add service call timeout argument in spawner (#1808))
     )
 
 
@@ -217,6 +244,7 @@ def switch_controllers(
     strict,
     activate_asap,
     timeout,
+    call_timeout=10.0,
 ):
     request = SwitchController.Request()
     request.activate_controllers = activate_controllers
@@ -228,11 +256,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 +275,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

@@ -90,8 +90,13 @@ There are two scripts to interact with controller manager from launch files:
 .. code-block:: console
 
     $ ros2 run controller_manager spawner -h
+<<<<<<< HEAD
     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]
+=======
+    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] [--service-call-timeout SERVICE_CALL_TIMEOUT]
+>>>>>>> 41d7393 (Add service call timeout argument in spawner (#1808))
                   controller_names [controller_names ...]
 
     positional arguments:
@@ -112,7 +117,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

@@ -21,4 +21,76 @@ controller_manager
 * Added support for the wildcard entries for the controller configuration files (`#1724 <https://github.com/ros-controls/ros2_control/pull/1724>`_).
 * 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>`_).
+<<<<<<< HEAD
 * ``--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>`_).
+
+hardware_interface
+******************
+* A portable version for string-to-double conversion was added: ``hardware_interface::stod`` (`#1257 <https://github.com/ros-controls/ros2_control/pull/1257>`_)
+* ``test_components`` was moved to its own package (`#1325 <https://github.com/ros-controls/ros2_control/pull/1325>`_)
+* The ``ros2_control`` tag now supports parsing of the limits from the URDF into the ``HardwareInfo`` structure. More conservative limits can be defined using the ``min`` and ``max`` attributes per interface (`#1472 <https://github.com/ros-controls/ros2_control/pull/1472>`_)
+
+  .. code:: xml
+
+    <ros2_control name="RRBotSystemMutipleGPIOs" type="system">
+      <hardware>
+        <plugin>ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware</plugin>
+        <param name="example_param_hw_start_duration_sec">2.0</param>
+        <param name="example_param_hw_stop_duration_sec">3.0</param>
+        <param name="example_param_hw_slowdown">2.0</param>
+      </hardware>
+      <joint name="joint1">
+        <command_interface name="position">
+          <param name="min">-1</param>
+          <param name="max">1</param>
+        </command_interface>
+        <command_interface name="velocity">
+          <limits enable="false"/>
+        </command_interface>
+        <state_interface name="position"/>
+      </joint>
+    </ros2_control>
+
+* Soft limits are also parsed from the URDF into the ``HardwareInfo`` structure for the defined joints (`#1488 <https://github.com/ros-controls/ros2_control/pull/1488>`_)
+* Access to logger and clock through ``get_logger`` and ``get_clock`` methods in ResourceManager and HardwareComponents ``Actuator``, ``Sensor`` and ``System`` (`#1585 <https://github.com/ros-controls/ros2_control/pull/1585>`_)
+* Added ``get_hardware_info`` method to the hardware components interface to access the ``HardwareInfo`` instead of accessing the variable ``info_`` directly (`#1643 <https://github.com/ros-controls/ros2_control/pull/1643>`_)
+* With (`#1683 <https://github.com/ros-controls/ros2_control/pull/1683>`_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 <https://github.com/ros-controls/ros2_control/pull/1240>`_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles.
+* With (`#1421 <https://github.com/ros-controls/ros2_control/pull/1421>`_) a key-value storage is added to InterfaceInfo. This allows to define extra params with per Command-/StateInterface in the ``.ros2_control.xacro`` file.
+* With (`#1763 <https://github.com/ros-controls/ros2_control/pull/1763>`_) parsing for SDF published to ``robot_description`` topic is now also supported.
+
+joint_limits
+************
+* Add header to import limits from standard URDF definition (`#1298 <https://github.com/ros-controls/ros2_control/pull/1298>`_)
+
+Adaption of Command-/StateInterfaces
+***************************************
+Changes from `(PR #1688) <https://github.com/ros-controls/ros2_control/pull/1688>`_ for an overview of related changes and discussion refer to `(PR #1240) <https://github.com/ros-controls/ros2_control/pull/1240>`_.
+
+* ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` methods based on the interfaces defined in the ``ros2_control`` XML-tag, which gets parsed and the ``InterfaceDescription`` is created accordingly (check the `hardware_info.hpp <https://github.com/ros-controls/ros2_control/tree/{REPOS_FILE_BRANCH}/hardware_interface/include/hardware_interface/hardware_info.hpp>`__).
+* The memory for storing the value of a ``Command-/StateInterfaces`` is no longer allocated in the hardware but instead in the ``Command-/StateInterfaces`` itself.
+* To access the automatically created ``Command-/StateInterfaces`` we provide the ``std::unordered_map<std::string, InterfaceDescription>``, where the string is the fully qualified name of the interface and the ``InterfaceDescription`` is the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the  ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the ``ros2_control`` XML-tag but were created by overriding the ``export_unlisted_command_interfaces()`` or ``export_unlisted_state_interfaces()`` function by creating some custom ``Command-/StateInterfaces``.
+
+
+ros2controlcli
+**************
+* Spawner colours were added to ``list_controllers`` depending upon active or inactive (`#1409 <https://github.com/ros-controls/ros2_control/pull/1409>`_)
+* The ``set_hardware_component_state`` verb was added (`#1248 <https://github.com/ros-controls/ros2_control/pull/1248>`_). Use the following command to set the state of a hardware component
+
+  .. code-block:: bash
+
+    ros2 control set_hardware_component_state <hardware_component_name> <state>
+
+* The ``load_controller`` now supports parsing of the params file (`#1703 <https://github.com/ros-controls/ros2_control/pull/1703>`_).
+
+  .. code-block:: bash
+
+    ros2 control load_controller <controller_name> <realtive_or_absolute_file_path>
+
+* All the ros2controlcli verbs now support the namespacing through the ROS 2 standard way (`#1703 <https://github.com/ros-controls/ros2_control/pull/1703>`_).
+
+  .. code-block:: bash
+
+    ros2 control <verb> <arguments> --ros-args -r __ns:=<namespace>
+>>>>>>> 41d7393 (Add service call timeout argument in spawner (#1808))