From 0372e6d76467d2632b2e5a13981b3ca7f5c1f993 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 25 Nov 2024 21:17:09 +0100 Subject: [PATCH] Add service call timeout argument in spawner (#1808) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --------- Signed-off-by: Angsa Deployment Team Signed-off-by: Tony Najjar Co-authored-by: Angsa Deployment Team Co-authored-by: Christoph Fröhlich Co-authored-by: Sai Kishor Kothakota (cherry picked from commit 41d73939597a9332779ebdab82a72f4d13125328) # Conflicts: # controller_manager/controller_manager/controller_manager_services.py # controller_manager/doc/userdoc.rst # doc/release_notes.rst --- .../controller_manager_services.py | 55 +++++++++++--- .../controller_manager/spawner.py | 35 +++++++-- controller_manager/doc/userdoc.rst | 9 ++- doc/release_notes.rst | 72 +++++++++++++++++++ 4 files changed, 155 insertions(+), 16 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index cc0b6f4645..17c4a68aa4 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/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,10 +126,11 @@ 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, @@ -135,10 +138,11 @@ def list_controllers(node, controller_manager_name, service_timeout=0.0): 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, @@ -146,10 +150,13 @@ def list_controller_types(node, controller_manager_name, service_timeout=0.0): 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, @@ -157,10 +164,13 @@ def list_hardware_components(node, controller_manager_name, service_timeout=0.0) 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, @@ -168,10 +178,13 @@ def list_hardware_interfaces(node, controller_manager_name, service_timeout=0.0) 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, ) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 83b1e2f89d..47911296fa 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/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( diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 8f1c57161c..f83553a0e2 100644 --- a/controller_manager/doc/userdoc.rst +++ b/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 diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 400e234d23..d70e84110b 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -21,4 +21,76 @@ controller_manager * Added support for the wildcard entries for the controller configuration files (`#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 `_). * 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 `_). +<<<<<<< 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 `_). +======= +* 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 `_). + +hardware_interface +****************** +* A portable version for string-to-double conversion was added: ``hardware_interface::stod`` (`#1257 `_) +* ``test_components`` was moved to its own package (`#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 `_) + + .. code:: xml + + + + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + 2.0 + 3.0 + 2.0 + + + + -1 + 1 + + + + + + + + +* Soft limits are also parsed from the URDF into the ``HardwareInfo`` structure for the defined joints (`#1488 `_) +* Access to logger and clock through ``get_logger`` and ``get_clock`` methods in ResourceManager and HardwareComponents ``Actuator``, ``Sensor`` and ``System`` (`#1585 `_) +* Added ``get_hardware_info`` method to the hardware components interface to access the ``HardwareInfo`` instead of accessing the variable ``info_`` directly (`#1643 `_) +* With (`#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 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. +* With (`#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 `_) parsing for SDF published to ``robot_description`` topic is now also supported. + +joint_limits +************ +* Add header to import limits from standard URDF definition (`#1298 `_) + +Adaption of Command-/StateInterfaces +*************************************** +Changes from `(PR #1688) `_ for an overview of related changes and discussion refer to `(PR #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 `__). +* 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``, 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 `_) +* The ``set_hardware_component_state`` verb was added (`#1248 `_). Use the following command to set the state of a hardware component + + .. code-block:: bash + + ros2 control set_hardware_component_state + +* The ``load_controller`` now supports parsing of the params file (`#1703 `_). + + .. code-block:: bash + + ros2 control load_controller + +* All the ros2controlcli verbs now support the namespacing through the ROS 2 standard way (`#1703 `_). + + .. code-block:: bash + + ros2 control --ros-args -r __ns:= +>>>>>>> 41d7393 (Add service call timeout argument in spawner (#1808))