From cb91599f8f66aaf39b7485a2f7e131157f633474 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Fri, 25 Oct 2024 15:18:23 +0200 Subject: [PATCH 01/12] Fix timeout value in std output (#1807) We were using the wrong timeout in the terminal output there. --- .../controller_manager/controller_manager_services.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 102531f8e8..909e681ce6 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -114,7 +114,7 @@ def service_caller( if future.result() is None: node.get_logger().warning( f"Failed getting a result from calling {fully_qualified_service_name} in " - f"{service_timeout}. (Attempt {attempt+1} of {max_attempts}.)" + f"{call_timeout}. (Attempt {attempt+1} of {max_attempts}.)" ) else: return future.result() From b071c1e903ce7e1d6088246912e76cb2d51a649b Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 26 Oct 2024 21:30:20 +0100 Subject: [PATCH 02/12] Update changelogs --- controller_interface/CHANGELOG.rst | 9 +++++++++ controller_manager/CHANGELOG.rst | 14 ++++++++++++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 5 +++++ hardware_interface_testing/CHANGELOG.rst | 3 +++ joint_limits/CHANGELOG.rst | 3 +++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 3 +++ ros2controlcli/CHANGELOG.rst | 5 +++++ rqt_controller_manager/CHANGELOG.rst | 5 +++++ transmission_interface/CHANGELOG.rst | 3 +++ 11 files changed, 56 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 173f6f1bcf..a6173c246e 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [CM] Async Function Handler for Controllers (`#1489 `_) +* Check the update_rate set to the controllers to be a valid one (`#1788 `_) +* [PR-1689] Follow-up PR of the controller interface variants integration (`#1779 `_) +* Add `PoseSensor` semantic component (`#1775 `_) +* [RM/HW] Constify the exported state interfaces using ConstSharedPtr (`#1767 `_) +* Contributors: RobertWilbrandt, Sai Kishor Kothakota + 4.18.0 (2024-10-07) ------------------- * Adapt controller Reference/StateInterfaces to New Way of Exporting (variant support) (`#1689 `_) diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index f714f056d9..ec8091feba 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix timeout value in std output (`#1807 `_) +* [CM] Async Function Handler for Controllers (`#1489 `_) +* [Spawner] Add support for wildcard entries in the controller param files (`#1724 `_) +* [Feature] Fallback controllers (`#1789 `_) +* Check the update_rate set to the controllers to be a valid one (`#1788 `_) +* [PR-1689] Follow-up PR of the controller interface variants integration (`#1779 `_) +* Improve diagnostics of Controllers, Hardware Components and Controller Manager (`#1764 `_) +* Improve launch utils to support the multiple controller names (`#1782 `_) +* [RM/HW] Constify the exported state interfaces using ConstSharedPtr (`#1767 `_) +* [CM] Throw an exception when the components initially fail to be in the required state (`#1729 `_) +* Contributors: Felix Exner (fexner), Sai Kishor Kothakota + 4.18.0 (2024-10-07) ------------------- * Adapt controller Reference/StateInterfaces to New Way of Exporting (variant support) (`#1689 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 6b5007c6a9..d27896c61a 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.18.0 (2024-10-07) ------------------- diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index b21be43f60..a3b9e951b9 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [RM/HW] Constify the exported state interfaces using ConstSharedPtr (`#1767 `_) +* Contributors: Sai Kishor Kothakota + 4.18.0 (2024-10-07) ------------------- * Adapt controller Reference/StateInterfaces to New Way of Exporting (variant support) (`#1689 `_) diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 8a9835038c..796dfa3387 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.18.0 (2024-10-07) ------------------- * Adapt controller Reference/StateInterfaces to New Way of Exporting (variant support) (`#1689 `_) diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index cc31c4505f..6e34f1b758 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.18.0 (2024-10-07) ------------------- diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 8d78dbdd63..7d2919d647 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.18.0 (2024-10-07) ------------------- diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index a4750ed2e4..793aa0a0cb 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.18.0 (2024-10-07) ------------------- * Automatic Creation of Handles in HW, Adding Getters/Setters (variant support) (`#1688 `_) diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index f9c320c74f..51cdafad82 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [ros2controlcli] Fix the missing exported state interface printing (`#1800 `_) +* Contributors: Sai Kishor Kothakota + 4.18.0 (2024-10-07) ------------------- * [ros2controlcli] add params file parsing to load_controller verb and add namespacing support (`#1703 `_) diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 386b3b7f78..31b725a4bc 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix: call configure_controller on 'unconfigured' state instead load_controller (`#1794 `_) +* Contributors: Santosh Govindaraj + 4.18.0 (2024-10-07) ------------------- diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 1875547497..e5e551ac12 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.18.0 (2024-10-07) ------------------- From b0da4a16201b7429444a094f236449b1469c229e Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 26 Oct 2024 21:30:48 +0100 Subject: [PATCH 03/12] 4.19.0 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- hardware_interface_testing/CHANGELOG.rst | 4 ++-- hardware_interface_testing/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- rqt_controller_manager/CHANGELOG.rst | 4 ++-- rqt_controller_manager/package.xml | 2 +- rqt_controller_manager/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 24 files changed, 35 insertions(+), 35 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index a6173c246e..be2de417d6 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2024-10-26) +------------------- * [CM] Async Function Handler for Controllers (`#1489 `_) * Check the update_rate set to the controllers to be a valid one (`#1788 `_) * [PR-1689] Follow-up PR of the controller interface variants integration (`#1779 `_) diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 09955c46c9..55c47473f0 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.18.0 + 4.19.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index ec8091feba..7d9fb12a0d 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2024-10-26) +------------------- * Fix timeout value in std output (`#1807 `_) * [CM] Async Function Handler for Controllers (`#1489 `_) * [Spawner] Add support for wildcard entries in the controller param files (`#1724 `_) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 030fcd3f7a..69f5f6e4ce 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.18.0 + 4.19.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index d27896c61a..48802d740a 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2024-10-26) +------------------- 4.18.0 (2024-10-07) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index e733b3921b..fbd4859323 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.18.0 + 4.19.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index a3b9e951b9..913d4cc36b 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2024-10-26) +------------------- * [RM/HW] Constify the exported state interfaces using ConstSharedPtr (`#1767 `_) * Contributors: Sai Kishor Kothakota diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 6af9bd002f..96d593e8f6 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.18.0 + 4.19.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 796dfa3387..8cab703114 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2024-10-26) +------------------- 4.18.0 (2024-10-07) ------------------- diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index 7d8ad0a7cd..1c4f2f1b73 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,7 @@ hardware_interface_testing - 4.18.0 + 4.19.0 ros2_control hardware interface testing Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 6e34f1b758..7c1e88b2f9 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2024-10-26) +------------------- 4.18.0 (2024-10-07) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 5f929868e0..a945aa4710 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.18.0 + 4.19.0 Package with interfaces for handling of joint limits in controllers or in hardware. The package also implements Saturation Joint Limiter for position-velocity-acceleration set and other individual interfaces. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 7d2919d647..771506022e 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2024-10-26) +------------------- 4.18.0 (2024-10-07) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 08f08112df..71ec807544 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.18.0 + 4.19.0 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 793aa0a0cb..8a2406ca4e 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2024-10-26) +------------------- 4.18.0 (2024-10-07) ------------------- diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index c3f22e7917..9752b87fca 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 4.18.0 + 4.19.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 51cdafad82..546356417a 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2024-10-26) +------------------- * [ros2controlcli] Fix the missing exported state interface printing (`#1800 `_) * Contributors: Sai Kishor Kothakota diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index c2e39ca926..67b651e3ee 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.18.0 + 4.19.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 2634d726ef..244359601e 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.18.0", + version="4.19.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 31b725a4bc..da13697e81 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2024-10-26) +------------------- * fix: call configure_controller on 'unconfigured' state instead load_controller (`#1794 `_) * Contributors: Santosh Govindaraj diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index ad25593e34..ca6e74373c 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.18.0 + 4.19.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index 4e1bb84627..70cf0f8984 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.18.0", + version="4.19.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index e5e551ac12..68b6ec5054 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2024-10-26) +------------------- 4.18.0 (2024-10-07) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 1531e1e173..22d0c90ba1 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.18.0 + 4.19.0 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl From 832602dc327386a989ef447906a75295eba31210 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 30 Oct 2024 21:23:54 +0100 Subject: [PATCH 04/12] add thread_priority option to the ros2_control_node (#1820) --- controller_manager/src/ros2_control_node.cpp | 8 ++++++-- doc/release_notes.rst | 1 + 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index e0094b7e01..13af864c2f 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -58,11 +58,15 @@ int main(int argc, char ** argv) executor, manager_node_name, "", cm_node_options); RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate()); + const int thread_priority = cm->get_parameter_or("thread_priority", kSchedPriority); + RCLCPP_INFO( + cm->get_logger(), "Spawning %s RT thread with scheduler priority: %d", cm->get_name(), + thread_priority); std::thread cm_thread( - [cm]() + [cm, thread_priority]() { - if (!realtime_tools::configure_sched_fifo(kSchedPriority)) + if (!realtime_tools::configure_sched_fifo(thread_priority)) { RCLCPP_WARN( cm->get_logger(), diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 611e8d8176..a7c1242ffc 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -76,6 +76,7 @@ controller_manager * The ``--controller-type`` or ``-t`` spawner arg is removed. Now the controller type is defined in the controller configuration file with ``type`` field (`#1639 `_). * The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 `_). * Added support for the wildcard entries for the controller configuration files (`#1724 `_). +* The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 `_). hardware_interface ****************** From 5dbd6d55c60dd6ad8d0244dca900e9a52f5b3c25 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Thu, 31 Oct 2024 13:05:27 +0100 Subject: [PATCH 05/12] Fix Hardware spawner and add tests for it (#1759) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Tests for hardware spawner. * Fully support spawning a list of hardware components * Use python3 coverage instead of ros2 run in tests * Actually check component's state after changing it * Update hardware_spawner's user documentation --------- Co-authored-by: Dr. Denis Štogl Co-authored-by: Bence Magyar Co-authored-by: Christoph Fröhlich --- controller_manager/CMakeLists.txt | 9 + .../controller_manager/hardware_spawner.py | 70 ++--- controller_manager/doc/userdoc.rst | 10 +- .../test/test_hardware_spawner.cpp | 282 ++++++++++++++++++ 4 files changed, 333 insertions(+), 38 deletions(-) create mode 100644 controller_manager/test/test_hardware_spawner.cpp diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index e44fb5fb9b..a1a98bc59a 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -194,6 +194,15 @@ if(BUILD_TESTING) ros2_control_test_assets::ros2_control_test_assets ) + ament_add_gmock(test_hardware_spawner + test/test_hardware_spawner + TIMEOUT 120 + ) + target_link_libraries(test_hardware_spawner + controller_manager + ros2_control_test_assets::ros2_control_test_assets + ) + install(FILES test/test_controller_spawner_with_fallback_controllers.yaml DESTINATION test) diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index 29c0b5e97c..323e02584a 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -57,7 +57,7 @@ def has_service_names(node, node_name, node_namespace, service_names): def is_hardware_component_loaded( node, controller_manager, hardware_component, service_timeout=0.0 ): - components = list_hardware_components(node, hardware_component, service_timeout).component + components = list_hardware_components(node, controller_manager, service_timeout).component return any(c.name == hardware_component for c in components) @@ -82,34 +82,33 @@ def handle_set_component_state_service_call( ) -def activate_components(node, controller_manager_name, components_to_activate): +def activate_component(node, controller_manager_name, component_to_activate): active_state = State() active_state.id = State.PRIMARY_STATE_ACTIVE active_state.label = "active" - for component in components_to_activate: - handle_set_component_state_service_call( - node, controller_manager_name, component, active_state, "activated" - ) + handle_set_component_state_service_call( + node, controller_manager_name, component_to_activate, active_state, "activated" + ) -def configure_components(node, controller_manager_name, components_to_configure): +def configure_component(node, controller_manager_name, component_to_configure): inactive_state = State() inactive_state.id = State.PRIMARY_STATE_INACTIVE inactive_state.label = "inactive" - for component in components_to_configure: - handle_set_component_state_service_call( - node, controller_manager_name, component, inactive_state, "configured" - ) + handle_set_component_state_service_call( + node, controller_manager_name, component_to_configure, inactive_state, "configured" + ) def main(args=None): rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO) parser = argparse.ArgumentParser() - activate_or_confiigure_grp = parser.add_mutually_exclusive_group(required=True) + activate_or_configure_grp = parser.add_mutually_exclusive_group(required=True) parser.add_argument( - "hardware_component_name", - help="The name of the hardware component which should be activated.", + "hardware_component_names", + help="The name of the hardware components which should be activated.", + nargs="+", ) parser.add_argument( "-c", @@ -126,13 +125,13 @@ def main(args=None): type=float, ) # add arguments which are mutually exclusive - activate_or_confiigure_grp.add_argument( + activate_or_configure_grp.add_argument( "--activate", help="Activates the given components. Note: Components are by default configured before activated. ", action="store_true", required=False, ) - activate_or_confiigure_grp.add_argument( + activate_or_configure_grp.add_argument( "--configure", help="Configures the given components.", action="store_true", @@ -141,9 +140,9 @@ def main(args=None): command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] args = parser.parse_args(command_line_args) + hardware_components = args.hardware_component_names controller_manager_name = args.controller_manager controller_manager_timeout = args.controller_manager_timeout - hardware_component = [args.hardware_component_name] activate = args.activate configure = args.configure @@ -156,24 +155,25 @@ def main(args=None): controller_manager_name = f"/{controller_manager_name}" try: - if not is_hardware_component_loaded( - node, controller_manager_name, hardware_component, controller_manager_timeout - ): - node.get_logger().warn( - bcolors.WARNING - + "Hardware Component is not loaded - state can not be changed." - + bcolors.ENDC - ) - elif activate: - activate_components(node, controller_manager_name, hardware_component) - elif configure: - configure_components(node, controller_manager_name, hardware_component) - else: - node.get_logger().error( - 'You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag' - ) - parser.print_help() - return 0 + for hardware_component in hardware_components: + if not is_hardware_component_loaded( + node, controller_manager_name, hardware_component, controller_manager_timeout + ): + node.get_logger().warn( + bcolors.WARNING + + "Hardware Component is not loaded - state can not be changed." + + bcolors.ENDC + ) + elif activate: + activate_component(node, controller_manager_name, hardware_component) + elif configure: + configure_component(node, controller_manager_name, hardware_component) + else: + node.get_logger().error( + 'You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag' + ) + parser.print_help() + return 0 except KeyboardInterrupt: pass except ServiceNotFoundError as err: diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 312e7c68ef..fa673cff0a 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -259,16 +259,20 @@ The parsed controller config file can follow the same conventions as the typical .. code-block:: console $ ros2 run controller_manager hardware_spawner -h - usage: hardware_spawner [-h] [-c CONTROLLER_MANAGER] (--activate | --configure) hardware_component_name + usage: hardware_spawner [-h] [-c CONTROLLER_MANAGER] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] + (--activate | --configure) + hardware_component_names [hardware_component_names ...] positional arguments: - hardware_component_name - The name of the hardware component which should be activated. + hardware_component_names + The name of the hardware components which should be activated. options: -h, --help show this help message and exit -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node + --controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT + Time to wait for the controller manager --activate Activates the given components. Note: Components are by default configured before activated. --configure Configures the given components. diff --git a/controller_manager/test/test_hardware_spawner.cpp b/controller_manager/test/test_hardware_spawner.cpp new file mode 100644 index 0000000000..6fd1269fa9 --- /dev/null +++ b/controller_manager/test/test_hardware_spawner.cpp @@ -0,0 +1,282 @@ +// Copyright 2021 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "controller_manager_test_common.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "test_chainable_controller/test_chainable_controller.hpp" +#include "test_controller/test_controller.hpp" + +using ::testing::_; +using ::testing::Return; + +class RMServiceCaller +{ +public: + explicit RMServiceCaller(const std::string & cm_name) + { + list_srv_node_ = std::make_shared("list_srv_client"); + srv_executor_.add_node(list_srv_node_); + list_hw_components_client_ = + list_srv_node_->create_client( + cm_name + "/list_hardware_components"); + } + + lifecycle_msgs::msg::State get_component_state(const std::string & component_name) + { + auto request = + std::make_shared(); + EXPECT_TRUE(list_hw_components_client_->wait_for_service(std::chrono::milliseconds(500))); + auto future = list_hw_components_client_->async_send_request(request); + EXPECT_EQ(srv_executor_.spin_until_future_complete(future), rclcpp::FutureReturnCode::SUCCESS); + auto result = future.get(); + + auto it = std::find_if( + std::begin(result->component), std::end(result->component), + [&component_name](const auto & cmp) { return cmp.name == component_name; }); + + EXPECT_NE(it, std::end(result->component)); + + return it->state; + }; + +protected: + rclcpp::executors::SingleThreadedExecutor srv_executor_; + rclcpp::Node::SharedPtr list_srv_node_; + rclcpp::Client::SharedPtr + list_hw_components_client_; +}; + +using namespace std::chrono_literals; +class TestHardwareSpawner : public ControllerManagerFixture, + public RMServiceCaller +{ +public: + TestHardwareSpawner() + : ControllerManagerFixture(), RMServiceCaller(TEST_CM_NAME) + { + cm_->set_parameter( + rclcpp::Parameter("hardware_components_initial_state.unconfigured", "TestSystemHardware")); + } + + void SetUp() override + { + ControllerManagerFixture::SetUp(); + + update_executor_ = + std::make_shared(rclcpp::ExecutorOptions(), 2); + + update_executor_->add_node(cm_); + update_executor_spin_future_ = + std::async(std::launch::async, [this]() -> void { update_executor_->spin(); }); + // This sleep is needed to prevent a too fast test from ending before the + // executor has began to spin, which causes it to hang + std::this_thread::sleep_for(50ms); + } + + void TearDown() override { update_executor_->cancel(); } + +protected: + // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks + std::shared_ptr update_executor_; + std::future update_executor_spin_future_; +}; + +int call_spawner(const std::string extra_args) +{ + std::string spawner_script = + "python3 -m coverage run --append --branch $(ros2 pkg prefix " + "controller_manager)/lib/controller_manager/hardware_spawner "; + return std::system((spawner_script + extra_args).c_str()); +} + +TEST_F(TestHardwareSpawner, spawner_with_no_arguments_errors) +{ + EXPECT_NE(call_spawner(""), 0) << "Missing mandatory arguments"; +} + +TEST_F(TestHardwareSpawner, spawner_without_manager_errors_with_given_timeout) +{ + EXPECT_NE(call_spawner("TestSystemHardware --controller-manager-timeout 1.0"), 0) + << "Wrong controller manager name"; +} + +TEST_F(TestHardwareSpawner, spawner_without_component_name_argument) +{ + EXPECT_NE(call_spawner("-c test_controller_manager"), 0) + << "Missing component name argument parameter"; +} + +TEST_F(TestHardwareSpawner, spawner_non_exising_hardware_component) +{ + EXPECT_NE(call_spawner("TestSystemHardware1 -c test_controller_manager"), 0) + << "Missing component name argument parameter"; +} + +TEST_F(TestHardwareSpawner, set_component_to_configured_state_and_back_to_activated) +{ + EXPECT_EQ(call_spawner("TestSystemHardware --configure -c test_controller_manager"), 0); + EXPECT_EQ( + get_component_state("TestSystemHardware").id, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + EXPECT_EQ(call_spawner("TestSystemHardware --activate -c test_controller_manager"), 0); + EXPECT_EQ( + get_component_state("TestSystemHardware").id, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); +} + +class TestHardwareSpawnerWithoutRobotDescription +: public ControllerManagerFixture, + public RMServiceCaller +{ +public: + TestHardwareSpawnerWithoutRobotDescription() + : ControllerManagerFixture(""), + RMServiceCaller(TEST_CM_NAME) + { + cm_->set_parameter(rclcpp::Parameter( + "hardware_components_initial_state.unconfigured", + std::vector{"TestSystemHardware"})); + } + +public: + void SetUp() override + { + ControllerManagerFixture::SetUp(); + + update_timer_ = cm_->create_wall_timer( + std::chrono::milliseconds(10), + [&]() + { + cm_->read(time_, PERIOD); + cm_->update(time_, PERIOD); + cm_->write(time_, PERIOD); + }); + + update_executor_ = + std::make_shared(rclcpp::ExecutorOptions(), 2); + + update_executor_->add_node(cm_); + update_executor_spin_future_ = + std::async(std::launch::async, [this]() -> void { update_executor_->spin(); }); + // This sleep is needed to prevent a too fast test from ending before the + // executor has began to spin, which causes it to hang + std::this_thread::sleep_for(50ms); + } + + void TearDown() override { update_executor_->cancel(); } + + rclcpp::TimerBase::SharedPtr robot_description_sending_timer_; + +protected: + rclcpp::TimerBase::SharedPtr update_timer_; + + // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks + std::shared_ptr update_executor_; + std::future update_executor_spin_future_; +}; + +TEST_F(TestHardwareSpawnerWithoutRobotDescription, when_no_robot_description_spawner_times_out) +{ + EXPECT_EQ( + call_spawner( + "TestSystemHardware --configure -c test_controller_manager --controller-manager-timeout 1.0"), + 256) + << "could not change hardware state because not robot description and not services for " + "controller " + "manager are active"; +} + +TEST_F(TestHardwareSpawnerWithoutRobotDescription, spawner_with_later_load_of_robot_description) +{ + // Delay sending robot description + robot_description_sending_timer_ = cm_->create_wall_timer( + std::chrono::milliseconds(2500), [&]() { pass_robot_description_to_cm_and_rm(); }); + + EXPECT_EQ( + call_spawner( + "TestSystemHardware --configure -c test_controller_manager --controller-manager-timeout 1.0"), + 256) + << "could not activate control because not robot description"; + EXPECT_EQ( + call_spawner( + "TestSystemHardware --configure -c test_controller_manager --controller-manager-timeout 2.5"), + 0); + EXPECT_EQ( + get_component_state("TestSystemHardware").id, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); +} + +class TestHardwareSpawnerWithNamespacedCM +: public ControllerManagerFixture, + public RMServiceCaller +{ +public: + TestHardwareSpawnerWithNamespacedCM() + : ControllerManagerFixture( + ros2_control_test_assets::minimal_robot_urdf, "foo_namespace"), + RMServiceCaller("foo_namespace/" + std::string(TEST_CM_NAME)) + { + cm_->set_parameter( + rclcpp::Parameter("hardware_components_initial_state.unconfigured", "TestSystemHardware")); + } + +public: + void SetUp() override + { + ControllerManagerFixture::SetUp(); + + update_executor_ = + std::make_shared(rclcpp::ExecutorOptions(), 2); + + update_executor_->add_node(cm_); + update_executor_spin_future_ = + std::async(std::launch::async, [this]() -> void { update_executor_->spin(); }); + // This sleep is needed to prevent a too fast test from ending before the + // executor has began to spin, which causes it to hang + std::this_thread::sleep_for(50ms); + } + + void TearDown() override { update_executor_->cancel(); } + +protected: + // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks + std::shared_ptr update_executor_; + std::future update_executor_spin_future_; +}; + +TEST_F(TestHardwareSpawnerWithNamespacedCM, set_component_to_configured_state_cm_namespace) +{ + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + call_spawner( + "TestSystemHardware --configure -c test_controller_manager --controller-manager-timeout 1.0"), + 256) + << "Should fail without defining the namespace"; + EXPECT_EQ( + call_spawner("TestSystemHardware --configure -c test_controller_manager --ros-args -r " + "__ns:=/foo_namespace"), + 0); + + EXPECT_EQ( + get_component_state("TestSystemHardware").id, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); +} From 0be43748de967d1d410385411baf45107145d24d Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Fri, 1 Nov 2024 09:50:10 +0100 Subject: [PATCH 06/12] Bump version of pre-commit hooks (#1831) Co-authored-by: christophfroehlich <3367244+christophfroehlich@users.noreply.github.com> --- .pre-commit-config.yaml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 63e7f08682..205e0f63ab 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -16,7 +16,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.6.0 + rev: v5.0.0 hooks: - id: check-added-large-files - id: check-ast @@ -37,7 +37,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.17.0 + rev: v3.19.0 hooks: - id: pyupgrade args: [--py36-plus] @@ -50,7 +50,7 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/psf/black - rev: 24.8.0 + rev: 24.10.0 hooks: - id: black args: ["--line-length=99"] @@ -63,7 +63,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v19.1.0 + rev: v19.1.3 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] @@ -133,7 +133,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.29.3 + rev: 0.29.4 hooks: - id: check-github-workflows args: ["--verbose"] From 58805d35b75a19ff289c2dfd1582c8a353368980 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 3 Nov 2024 18:05:36 +0100 Subject: [PATCH 07/12] Update mergify.yml author entries (#1841) --- .github/mergify.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/mergify.yml b/.github/mergify.yml index 39ee6b6bc0..fd185e02d0 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -32,7 +32,7 @@ pull_request_rules: - author=mergify[bot] actions: comment: - message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich? + message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich @saikishor? - name: development targets master branch conditions: @@ -40,6 +40,7 @@ pull_request_rules: - author!=bmagyar - author!=destogl - author!=christophfroehlich + - author!=saikishor - author!=mergify[bot] - author!=dependabot[bot] actions: From be0a339ac447cf259b560f13f634dd0ced6028ff Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Mon, 4 Nov 2024 17:18:19 +0100 Subject: [PATCH 08/12] fix: typo use thread_priority (#1844) --- controller_manager/src/ros2_control_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 13af864c2f..c0a61d6cda 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -79,7 +79,7 @@ int main(int argc, char ** argv) { RCLCPP_INFO( cm->get_logger(), "Successful set up FIFO RT scheduling policy with priority %i.", - kSchedPriority); + thread_priority); } // for calculating sleep time From 434f7f54c91114028b8f73c927ecee41588f2bce Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 4 Nov 2024 19:44:34 +0100 Subject: [PATCH 09/12] Fix CMP0115 (#1830) --- controller_manager/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index a1a98bc59a..6e004ffc4a 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -195,7 +195,7 @@ if(BUILD_TESTING) ) ament_add_gmock(test_hardware_spawner - test/test_hardware_spawner + test/test_hardware_spawner.cpp TIMEOUT 120 ) target_link_libraries(test_hardware_spawner From e6e69c1052b91d52de792f49d89c6c54bf4ec18a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 4 Nov 2024 20:53:55 +0100 Subject: [PATCH 10/12] [ros2_control_node] Add the realtime_tools lock_memory method to prevent page faults (#1822) --- controller_manager/src/ros2_control_node.cpp | 7 +++++++ doc/release_notes.rst | 1 + 2 files changed, 8 insertions(+) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index c0a61d6cda..6de49166a9 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -57,6 +57,13 @@ int main(int argc, char ** argv) auto cm = std::make_shared( executor, manager_node_name, "", cm_node_options); + const bool lock_memory = cm->get_parameter_or("lock_memory", true); + std::string message; + if (lock_memory && !realtime_tools::lock_memory(message)) + { + RCLCPP_WARN(cm->get_logger(), "Unable to lock the memory : '%s'", message.c_str()); + } + RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate()); const int thread_priority = cm->get_parameter_or("thread_priority", kSchedPriority); RCLCPP_INFO( diff --git a/doc/release_notes.rst b/doc/release_notes.rst index a7c1242ffc..9b70016184 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -77,6 +77,7 @@ controller_manager * The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 `_). * Added support for the wildcard entries for the controller configuration files (`#1724 `_). * The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#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 `_). hardware_interface ****************** From 98f079598a6444d7043236c2e9fec04d1f7dae02 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 5 Nov 2024 11:08:17 +0100 Subject: [PATCH 11/12] [ros2_control_node] Add option to set the CPU affinity (#1852) --- controller_manager/src/ros2_control_node.cpp | 11 +++++++++++ doc/release_notes.rst | 1 + 2 files changed, 12 insertions(+) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 6de49166a9..51e7e83d23 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -64,6 +64,17 @@ int main(int argc, char ** argv) RCLCPP_WARN(cm->get_logger(), "Unable to lock the memory : '%s'", message.c_str()); } + const int cpu_affinity = cm->get_parameter_or("cpu_affinity", -1); + if (cpu_affinity >= 0) + { + const auto affinity_result = realtime_tools::set_current_thread_affinity(cpu_affinity); + if (!affinity_result.first) + { + RCLCPP_WARN( + cm->get_logger(), "Unable to set the CPU affinity : '%s'", affinity_result.second.c_str()); + } + } + RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate()); const int thread_priority = cm->get_parameter_or("thread_priority", kSchedPriority); RCLCPP_INFO( diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 9b70016184..12ded009dc 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -78,6 +78,7 @@ controller_manager * Added support for the wildcard entries for the controller configuration files (`#1724 `_). * The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#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 `_). +* 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 `_). hardware_interface ****************** From 814137dd3a87ac21c70c84420c985e436e1f5f7c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 5 Nov 2024 13:40:17 +0100 Subject: [PATCH 12/12] [CM] Fix controller missing update cycles in a real setup (#1774) --- .../controller_interface_base.hpp | 2 +- controller_manager/CMakeLists.txt | 1 + .../controller_manager/controller_manager.hpp | 2 +- .../controller_manager/controller_spec.hpp | 2 +- controller_manager/src/controller_manager.cpp | 34 ++++---- .../test/test_controller_manager.cpp | 77 ++++++++++--------- 6 files changed, 66 insertions(+), 52 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 211716f301..ae3804919c 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -160,7 +160,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * **The method called in the (real-time) control loop.** * * \param[in] time The time at the start of this control loop iteration - * \param[in] period The measured time taken by the last control loop iteration + * \param[in] period The measured time since the last control loop iteration * \returns return_type::OK if update is successfully, otherwise return_type::ERROR. */ CONTROLLER_INTERFACE_PUBLIC diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 6e004ffc4a..1bb84eb32c 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -87,6 +87,7 @@ if(BUILD_TESTING) ament_add_gmock(test_controller_manager test/test_controller_manager.cpp + TIMEOUT 180 ) target_link_libraries(test_controller_manager controller_manager diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 6c0b4fde9b..068eefc1f9 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -123,7 +123,7 @@ class ControllerManager : public rclcpp::Node controller_spec.c = controller; controller_spec.info.name = controller_name; controller_spec.info.type = controller_type; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); return add_controller_impl(controller_spec); } diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 9ce1aab8b6..0f44867814 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -37,7 +37,7 @@ struct ControllerSpec { hardware_interface::ControllerInfo info; controller_interface::ControllerInterfaceBaseSharedPtr c; - std::shared_ptr next_update_cycle_time; + std::shared_ptr last_update_cycle_time; }; struct ControllerChainSpec diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a8668f7f1b..d7e924a863 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -543,7 +543,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.c = controller; controller_spec.info.name = controller_name; controller_spec.info.type = controller_type; - controller_spec.next_update_cycle_time = std::make_shared( + controller_spec.last_update_cycle_time = std::make_shared( 0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); // We have to fetch the parameters_file at the time of loading the controller, because this way we @@ -1668,8 +1668,8 @@ void ControllerManager::activate_controllers( continue; } auto controller = found_it->c; - // reset the next update cycle time for newly activated controllers - *found_it->next_update_cycle_time = + // reset the last update cycle time for newly activated controllers + *found_it->last_update_cycle_time = rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); bool assignment_successful = true; @@ -2354,21 +2354,31 @@ controller_interface::return_type ControllerManager::update( run_controller_at_cm_rate ? period : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); + const rclcpp::Time current_time = get_clock()->now(); if ( - *loaded_controller.next_update_cycle_time == + *loaded_controller.last_update_cycle_time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) { // it is zero after activation + *loaded_controller.last_update_cycle_time = current_time - controller_period; RCLCPP_DEBUG( - get_logger(), "Setting next_update_cycle_time to %fs for the controller : %s", - time.seconds(), loaded_controller.info.name.c_str()); - *loaded_controller.next_update_cycle_time = time; + get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s", + loaded_controller.last_update_cycle_time->seconds(), loaded_controller.info.name.c_str()); } - - bool controller_go = + const auto controller_actual_period = + (current_time - *loaded_controller.last_update_cycle_time); + + /// @note The factor 0.99 is used to avoid the controllers skipping update cycles due to the + /// jitter in the system sleep cycles. + // For instance, A controller running at 50 Hz and the CM running at 100Hz, then when we have + // an update cycle at 0.019s (ideally, the controller should only trigger >= 0.02s), if we + // wait for next cycle, then trigger will happen at ~0.029 sec and this is creating an issue + // to keep up with the controller update rate (see issue #1769). + const bool controller_go = + run_controller_at_cm_rate || (time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) || - (time.seconds() >= loaded_controller.next_update_cycle_time->seconds()); + (controller_actual_period.seconds() * controller_update_rate >= 0.99); RCLCPP_DEBUG( get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '", @@ -2377,8 +2387,6 @@ controller_interface::return_type ControllerManager::update( if (controller_go) { - const auto controller_actual_period = - (time - *loaded_controller.next_update_cycle_time) + controller_period; auto controller_ret = controller_interface::return_type::OK; bool trigger_status = true; // Catch exceptions thrown by the controller update function @@ -2402,7 +2410,7 @@ controller_interface::return_type ControllerManager::update( controller_ret = controller_interface::return_type::ERROR; } - *loaded_controller.next_update_cycle_time += controller_period; + *loaded_controller.last_update_cycle_time = current_time; if (controller_ret != controller_interface::return_type::OK) { diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 8ff844adc2..0c4e51985f 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -575,10 +575,8 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd // In case of a non perfect divisor, the update period should respect the rule // [cm_update_rate, 2*cm_update_rate) EXPECT_THAT( - test_controller->update_period_, - testing::AllOf( - testing::Ge(rclcpp::Duration::from_seconds(1.0 / cm_update_rate)), - testing::Lt(rclcpp::Duration::from_seconds(2.0 / cm_update_rate)))); + test_controller->update_period_.seconds(), + testing::AllOf(testing::Ge(0.9 / cm_update_rate), testing::Lt((1.1 / cm_update_rate)))); loop_rate.sleep(); } // if we do 2 times of the controller_manager update rate, the internal counter should be @@ -640,6 +638,9 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) ControllerManagerRunner cm_runner(this); cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); } + time_ = test_controller->get_node()->now(); // set to something nonzero + cm_->get_clock()->sleep_until(time_ + PERIOD); + time_ = cm_->get_clock()->now(); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -650,7 +651,6 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) test_controller->get_lifecycle_state().id()); // Start controller, will take effect at the end of the update function - time_ = test_controller->get_node()->now(); // set to something nonzero const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; std::vector stop_controllers = {}; @@ -661,7 +661,8 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) << "switch_controller should be blocking until next update cycle"; - time_ += rclcpp::Duration::from_seconds(0.01); + cm_->get_clock()->sleep_until(time_ + PERIOD); + time_ = cm_->get_clock()->now(); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -677,25 +678,29 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate); const auto cm_update_rate = cm_->get_update_rate(); const auto controller_update_rate = test_controller->get_update_rate(); + const double controller_period = 1.0 / controller_update_rate; const auto initial_counter = test_controller->internal_counter; // don't start with zero to check if the period is correct if controller is activated anytime rclcpp::Time time = time_; for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter) { + rclcpp::Time old_time = time; + cm_->get_clock()->sleep_until(old_time + PERIOD); + time = cm_->get_clock()->now(); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time, rclcpp::Duration::from_seconds(0.01))); // In case of a non perfect divisor, the update period should respect the rule // [controller_update_rate, 2*controller_update_rate) EXPECT_THAT( - test_controller->update_period_, + test_controller->update_period_.seconds(), testing::AllOf( - testing::Ge(rclcpp::Duration::from_seconds(1.0 / controller_update_rate)), - testing::Lt(rclcpp::Duration::from_seconds(2.0 / controller_update_rate)))) - << "update_counter: " << update_counter; + testing::Gt(0.99 * controller_period), + testing::Lt((1.05 * controller_period) + PERIOD.seconds()))) + << "update_counter: " << update_counter << " desired controller period: " << controller_period + << " actual controller period: " << test_controller->update_period_.seconds(); - time += rclcpp::Duration::from_seconds(0.01); if (update_counter % cm_update_rate == 0) { const double no_of_secs_passed = static_cast(update_counter) / cm_update_rate; @@ -708,15 +713,15 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_THAT( test_controller->internal_counter - initial_counter, testing::AnyOf( - testing::Eq(controller_update_rate * no_of_secs_passed), - testing::Eq((controller_update_rate * no_of_secs_passed) - 1))); + testing::Ge((controller_update_rate - 1) * no_of_secs_passed), + testing::Lt((controller_update_rate * no_of_secs_passed)))); } } } INSTANTIATE_TEST_SUITE_P( per_controller_update_rate_check, TestControllerUpdateRates, - testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 98)); + testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 90)); class TestControllerManagerFallbackControllers : public ControllerManagerFixture, @@ -764,7 +769,7 @@ TEST_F(TestControllerManagerFallbackControllers, test_failure_on_fallback_contro controller_spec.info.name = test_controller_1_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {test_controller_2_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); ControllerManagerRunner cm_runner(this); cm_->add_controller(controller_spec); // add controller_1 @@ -772,7 +777,7 @@ TEST_F(TestControllerManagerFallbackControllers, test_failure_on_fallback_contro controller_spec.info.name = test_controller_2_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_2 } EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); @@ -846,7 +851,7 @@ TEST_F(TestControllerManagerFallbackControllers, test_fallback_controllers_activ controller_spec.info.name = test_controller_1_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {test_controller_2_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); ControllerManagerRunner cm_runner(this); cm_->add_controller(controller_spec); // add controller_1 @@ -854,7 +859,7 @@ TEST_F(TestControllerManagerFallbackControllers, test_fallback_controllers_activ controller_spec.info.name = test_controller_2_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_2 } EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); @@ -944,7 +949,7 @@ TEST_F( controller_spec.info.name = test_controller_1_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {test_controller_2_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); ControllerManagerRunner cm_runner(this); cm_->add_controller(controller_spec); // add controller_1 @@ -952,7 +957,7 @@ TEST_F( controller_spec.info.name = test_controller_2_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_2 } EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); @@ -1033,7 +1038,7 @@ TEST_F( controller_spec.info.name = test_controller_1_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {test_controller_2_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); ControllerManagerRunner cm_runner(this); cm_->add_controller(controller_spec); // add controller_1 @@ -1041,7 +1046,7 @@ TEST_F( controller_spec.info.name = test_controller_2_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_2 } EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); @@ -1129,7 +1134,7 @@ TEST_F( controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = { test_controller_2_name, test_controller_3_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); ControllerManagerRunner cm_runner(this); cm_->add_controller(controller_spec); // add controller_1 @@ -1137,14 +1142,14 @@ TEST_F( controller_spec.info.name = test_controller_2_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_2 controller_spec.c = test_controller_3; controller_spec.info.name = test_controller_3_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_3 } @@ -1279,7 +1284,7 @@ TEST_F( controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = { test_controller_2_name, test_controller_3_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); ControllerManagerRunner cm_runner(this); cm_->add_controller(controller_spec); // add controller_1 @@ -1287,21 +1292,21 @@ TEST_F( controller_spec.info.name = test_controller_2_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_2 controller_spec.c = test_controller_3; controller_spec.info.name = test_controller_3_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_3 controller_spec.c = test_controller_4; controller_spec.info.name = test_controller_4_name; controller_spec.info.type = "test_chainable_controller::TestChainableController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_4 } @@ -1381,7 +1386,7 @@ TEST_F( controller_spec.info.name = test_controller_1_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {test_controller_3_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_1 EXPECT_EQ( @@ -1413,7 +1418,7 @@ TEST_F( // available controller_spec.info.fallback_controllers_names = { test_controller_4_name, test_controller_3_name, test_controller_2_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_1 EXPECT_EQ( @@ -1509,7 +1514,7 @@ TEST_F( controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = { test_controller_2_name, test_controller_4_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); ControllerManagerRunner cm_runner(this); cm_->add_controller(controller_spec); // add controller_1 @@ -1517,21 +1522,21 @@ TEST_F( controller_spec.info.name = test_controller_2_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_2 controller_spec.c = test_controller_3; controller_spec.info.name = test_controller_3_name; controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_3 controller_spec.c = test_controller_4; controller_spec.info.name = test_controller_4_name; controller_spec.info.type = "test_chainable_controller::TestChainableController"; controller_spec.info.fallback_controllers_names = {}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_4 } @@ -1617,7 +1622,7 @@ TEST_F( controller_spec.info.type = "test_controller::TestController"; controller_spec.info.fallback_controllers_names = { test_controller_3_name, test_controller_4_name}; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.last_update_cycle_time = std::make_shared(0); cm_->add_controller(controller_spec); // add controller_1 EXPECT_EQ(