From 054a4edb822a7b54593f8b247306717151f4e134 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 5 Aug 2024 10:23:31 +0100 Subject: [PATCH 01/10] Update changelogs --- controller_interface/CHANGELOG.rst | 3 +++ controller_manager/CHANGELOG.rst | 10 ++++++++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 6 ++++++ hardware_interface_testing/CHANGELOG.rst | 5 +++++ joint_limits/CHANGELOG.rst | 3 +++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 3 +++ ros2controlcli/CHANGELOG.rst | 3 +++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 3 +++ 11 files changed, 45 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index c8a69a2d50..2ba08903ad 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-07-23) ------------------- * Unused header cleanup (`#1627 `_) diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 397ed8b5ca..2310e8dfc0 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing include for executors (`#1653 `_) +* Fix the namespaced controller_manager spawner + added tests (`#1640 `_) +* CM: Add missing includes (`#1641 `_) +* Fix rst markup (`#1642 `_) +* Add a pytest launch file to test ros2_control_node (`#1636 `_) +* [CM] Remove deprecated spawner args (`#1639 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.14.0 (2024-07-23) ------------------- * Unused header cleanup (`#1627 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 9e210bc4e7..2fbdedeeaa 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.14.0 (2024-07-23) ------------------- diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 37f8b10795..1d6c0f6b0f 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [RM] Add `get_hardware_info` method to the Hardware Components (`#1643 `_) +* add missing rclcpp logging include for Humble compatibility build (`#1635 `_) +* Contributors: Sai Kishor Kothakota + 4.14.0 (2024-07-23) ------------------- * Unused header cleanup (`#1627 `_) diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index b0a25e3835..dfb9e4e03f 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [RM] Add `get_hardware_info` method to the Hardware Components (`#1643 `_) +* Contributors: Sai Kishor Kothakota + 4.14.0 (2024-07-23) ------------------- * Unused header cleanup (`#1627 `_) diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index da9bd28fed..e217465332 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-07-23) ------------------- * Unused header cleanup (`#1627 `_) diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 3c38360f50..a2968d85b9 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-07-23) ------------------- diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index a61a3273f7..e7bcde8086 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.14.0 (2024-07-23) ------------------- * Unused header cleanup (`#1627 `_) diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index b1d9dcd002..d840a55823 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-07-23) ------------------- diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index b5f9960575..0c3a905b70 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-07-23) ------------------- diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 694f4c72cf..5e8d45c60d 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-07-23) ------------------- * Unused header cleanup (`#1627 `_) From 32e471a0f85dfbb120891171f8c35059f612f012 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 5 Aug 2024 10:23:57 +0100 Subject: [PATCH 02/10] 4.15.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 2ba08903ad..7b2169ed00 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-08-05) +------------------- 4.14.0 (2024-07-23) ------------------- diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 30137a23e4..a60e5e90b6 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.14.0 + 4.15.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 2310e8dfc0..46d3663362 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-08-05) +------------------- * Add missing include for executors (`#1653 `_) * Fix the namespaced controller_manager spawner + added tests (`#1640 `_) * CM: Add missing includes (`#1641 `_) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index f969b76834..47b640c892 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.14.0 + 4.15.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 2fbdedeeaa..c8c11b580d 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.15.0 (2024-08-05) +------------------- 4.14.0 (2024-07-23) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 143117f512..b6a57c2636 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.14.0 + 4.15.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 1d6c0f6b0f..bb0b478e4b 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-08-05) +------------------- * [RM] Add `get_hardware_info` method to the Hardware Components (`#1643 `_) * add missing rclcpp logging include for Humble compatibility build (`#1635 `_) * Contributors: Sai Kishor Kothakota diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index c5f09e74bd..77d084ae7a 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.14.0 + 4.15.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index dfb9e4e03f..3a60a2f534 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.15.0 (2024-08-05) +------------------- * [RM] Add `get_hardware_info` method to the Hardware Components (`#1643 `_) * Contributors: Sai Kishor Kothakota diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index 7594d8e32c..646923b4e6 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,7 @@ hardware_interface_testing - 4.14.0 + 4.15.0 ros2_control hardware interface testing Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index e217465332..8581d26f72 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-08-05) +------------------- 4.14.0 (2024-07-23) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 9609522838..1ff460cf14 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.14.0 + 4.15.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 a2968d85b9..5106f98c38 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-08-05) +------------------- 4.14.0 (2024-07-23) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 501c543ccb..d5ef3ba862 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.14.0 + 4.15.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 e7bcde8086..f7e7d771b2 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.15.0 (2024-08-05) +------------------- 4.14.0 (2024-07-23) ------------------- diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index b6192e8c45..7be61a2f91 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 4.14.0 + 4.15.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index d840a55823..265084e74e 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-08-05) +------------------- 4.14.0 (2024-07-23) ------------------- diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 9adae5d2b5..f7ed71dae9 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.14.0 + 4.15.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 0a19536fe5..d1c8e2d150 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.14.0", + version="4.15.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 0c3a905b70..30a674acc5 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.15.0 (2024-08-05) +------------------- 4.14.0 (2024-07-23) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index e678b1f37a..fc3de21897 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.14.0 + 4.15.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 bbd9e75dcc..fad594c205 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.14.0", + version="4.15.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 5e8d45c60d..157a8ac09a 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-08-05) +------------------- 4.14.0 (2024-07-23) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 5740c2c68f..9ee959c5bc 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.14.0 + 4.15.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 a1ad5237c1ee0df7e82e9339b8fb566f265c19be Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 9 Aug 2024 14:03:12 +0200 Subject: [PATCH 03/10] refactor SwitchParams to fix the incosistencies in the spawner tests (#1638) --- .../controller_manager/controller_manager.hpp | 23 +++++++++--- controller_manager/src/controller_manager.cpp | 37 ++++++++++++++----- .../test/test_controller_manager_srvs.cpp | 31 +++++++++++++++- 3 files changed, 74 insertions(+), 17 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 2f326ff302..810936554e 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -583,14 +583,25 @@ class ControllerManager : public rclcpp::Node struct SwitchParams { - bool do_switch = {false}; - bool started = {false}; - rclcpp::Time init_time = {rclcpp::Time::max()}; + void reset() + { + do_switch = false; + started = false; + strictness = 0; + activate_asap = false; + } + + bool do_switch; + bool started; // Switch options - int strictness = {0}; - bool activate_asap = {false}; - rclcpp::Duration timeout = rclcpp::Duration{0, 0}; + int strictness; + bool activate_asap; + std::chrono::nanoseconds timeout; + + // conditional variable and mutex to wait for the switch to complete + std::condition_variable cv; + std::mutex mutex; }; SwitchParams switch_params_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 87414ce4ec..25add2045e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -849,6 +849,7 @@ controller_interface::return_type ControllerManager::configure_controller( void ControllerManager::clear_requests() { + switch_params_.do_switch = false; deactivate_request_.clear(); activate_request_.clear(); // Set these interfaces as unavailable when clearing requests to avoid leaving them in available @@ -878,7 +879,8 @@ controller_interface::return_type ControllerManager::switch_controller( return controller_interface::return_type::ERROR; } - switch_params_ = SwitchParams(); + // reset the switch param internal variables + switch_params_.reset(); if (!deactivate_request_.empty() || !activate_request_.empty()) { @@ -1299,19 +1301,27 @@ controller_interface::return_type ControllerManager::switch_controller( // start the atomic controller switching switch_params_.strictness = strictness; switch_params_.activate_asap = activate_asap; - switch_params_.init_time = rclcpp::Clock().now(); - switch_params_.timeout = timeout; + if (timeout == rclcpp::Duration{0, 0}) + { + RCLCPP_INFO_ONCE(get_logger(), "Switch controller timeout is set to 0, using default 1s!"); + switch_params_.timeout = std::chrono::nanoseconds(1'000'000'000); + } + else + { + switch_params_.timeout = timeout.to_chrono(); + } switch_params_.do_switch = true; - // wait until switch is finished RCLCPP_DEBUG(get_logger(), "Requested atomic controller switch from realtime loop"); - while (rclcpp::ok() && switch_params_.do_switch) + std::unique_lock switch_params_guard(switch_params_.mutex, std::defer_lock); + if (!switch_params_.cv.wait_for( + switch_params_guard, switch_params_.timeout, [this] { return !switch_params_.do_switch; })) { - if (!rclcpp::ok()) - { - return controller_interface::return_type::ERROR; - } - std::this_thread::sleep_for(std::chrono::microseconds(100)); + RCLCPP_ERROR( + get_logger(), "Switch controller timed out after %f seconds!", + static_cast(switch_params_.timeout.count()) / 1e9); + clear_requests(); + return controller_interface::return_type::ERROR; } // copy the controllers spec from the used to the unused list @@ -2147,6 +2157,12 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & void ControllerManager::manage_switch() { + std::unique_lock guard(switch_params_.mutex, std::try_to_lock); + if (!guard.owns_lock()) + { + RCLCPP_DEBUG(get_logger(), "Unable to lock switch mutex. Retrying in next cycle."); + return; + } // Ask hardware interfaces to change mode if (!resource_manager_->perform_command_mode_switch( activate_command_interface_request_, deactivate_command_interface_request_)) @@ -2175,6 +2191,7 @@ void ControllerManager::manage_switch() // All controllers switched --> switching done switch_params_.do_switch = false; + switch_params_.cv.notify_all(); } controller_interface::return_type ControllerManager::update( diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index af04cc5406..e53ad10578 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -168,9 +168,38 @@ TEST_F(TestControllerManagerSrvs, list_controllers_srv) "joint1/velocity", "joint2/acceleration", "joint2/position", "joint2/velocity", "joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity")); + // Switch with a very low timeout 1 ns and it should fail as there is no enough time to switch + ASSERT_EQ( + controller_interface::return_type::ERROR, + cm_->switch_controller( + {}, {test_controller::TEST_CONTROLLER_NAME}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 1))); + + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(1u, result->controller.size()); + ASSERT_EQ("active", result->controller[0].state); + ASSERT_THAT( + result->controller[0].claimed_interfaces, + UnorderedElementsAre( + "joint2/velocity", "joint3/velocity", "joint2/max_acceleration", "configuration/max_tcp_jerk", + "joint1/position", "joint1/max_velocity")); + ASSERT_THAT( + result->controller[0].required_command_interfaces, + UnorderedElementsAre( + "configuration/max_tcp_jerk", "joint1/max_velocity", "joint1/position", + "joint2/max_acceleration", "joint2/velocity", "joint3/velocity")); + ASSERT_THAT( + result->controller[0].required_state_interfaces, + UnorderedElementsAre( + "configuration/max_tcp_jerk", "joint1/position", "joint1/some_unlisted_interface", + "joint1/velocity", "joint2/acceleration", "joint2/position", "joint2/velocity", + "joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity")); + + // Try again with higher timeout cm_->switch_controller( {}, {test_controller::TEST_CONTROLLER_NAME}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(3, 0)); result = call_service_and_wait(*client, request, srv_executor); ASSERT_EQ(1u, result->controller.size()); From ace84165f3eda161db963415c71aa6d9da06ee34 Mon Sep 17 00:00:00 2001 From: sgmurray Date: Tue, 13 Aug 2024 06:18:18 -0700 Subject: [PATCH 04/10] Fix flaky transmission_interface tests by making them deterministic. (#1665) --- .../four_bar_linkage_transmission.hpp | 2 +- transmission_interface/test/random_generator_utils.hpp | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp index a8757e9c02..107133128c 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp @@ -289,7 +289,7 @@ inline void FourBarLinkageTransmission::actuator_to_joint() joint_eff[0].set_value(jr[0] * act_eff[0].get_value() * ar[0]); joint_eff[1].set_value( - jr[1] * (act_eff[1].get_value() * ar[1] - act_eff[0].get_value() * ar[0] * jr[0])); + jr[1] * (act_eff[1].get_value() * ar[1] - jr[0] * act_eff[0].get_value() * ar[0])); } } diff --git a/transmission_interface/test/random_generator_utils.hpp b/transmission_interface/test/random_generator_utils.hpp index fbacd1ba3b..01e1245e48 100644 --- a/transmission_interface/test/random_generator_utils.hpp +++ b/transmission_interface/test/random_generator_utils.hpp @@ -26,12 +26,14 @@ using std::vector; /// \brief Generator of pseudo-random double in the range [min_val, max_val]. // NOTE: Based on example code available at: // http://stackoverflow.com/questions/2860673/initializing-a-c-vector-to-random-values-fast +// Use a user specified seed instead of system time for deterministic tests struct RandomDoubleGenerator { public: - RandomDoubleGenerator(double min_val, double max_val) : min_val_(min_val), max_val_(max_val) + RandomDoubleGenerator(double min_val, double max_val, unsigned int seed = 1234) + : min_val_(min_val), max_val_(max_val) { - srand(static_cast(time(nullptr))); + srand(seed); } double operator()() { From 4919aba11469037cff455ce89367b0a633e12356 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 14 Aug 2024 12:59:28 +0200 Subject: [PATCH 05/10] [CM] Add more logging for easier debugging (#1645) --- controller_manager/src/controller_manager.cpp | 65 +++++++++++++++++-- 1 file changed, 58 insertions(+), 7 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 25add2045e..541fc57d02 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -530,12 +530,16 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c get_logger(), "The 'type' param was not defined for '%s'.", controller_name.c_str()); return nullptr; } + RCLCPP_INFO( + get_logger(), "Loading controller : '%s' of type '%s'", controller_name.c_str(), + controller_type.c_str()); return load_controller(controller_name, controller_type); } controller_interface::return_type ControllerManager::unload_controller( const std::string & controller_name) { + RCLCPP_INFO(get_logger(), "Unloading controller: '%s'", controller_name.c_str()); std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); const std::vector & from = rt_controllers_wrapper_.get_updated_list(guard); @@ -633,7 +637,7 @@ std::vector ControllerManager::get_loaded_controllers() const controller_interface::return_type ControllerManager::configure_controller( const std::string & controller_name) { - RCLCPP_INFO(get_logger(), "Configuring controller '%s'", controller_name.c_str()); + RCLCPP_INFO(get_logger(), "Configuring controller: '%s'", controller_name.c_str()); const auto & controllers = get_loaded_controllers(); @@ -919,16 +923,24 @@ controller_interface::return_type ControllerManager::switch_controller( strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; } - RCLCPP_DEBUG(get_logger(), "Activating controllers:"); + std::string activate_list, deactivate_list; + activate_list.reserve(500); + deactivate_list.reserve(500); for (const auto & controller : activate_controllers) { - RCLCPP_DEBUG(get_logger(), " - %s", controller.c_str()); + activate_list.append(controller); + activate_list.append(" "); } - RCLCPP_DEBUG(get_logger(), "Deactivating controllers:"); for (const auto & controller : deactivate_controllers) { - RCLCPP_DEBUG(get_logger(), " - %s", controller.c_str()); + deactivate_list.append(controller); + deactivate_list.append(" "); } + RCLCPP_INFO_EXPRESSION( + get_logger(), !activate_list.empty(), "Activating controllers: [ %s]", activate_list.c_str()); + RCLCPP_INFO_EXPRESSION( + get_logger(), !deactivate_list.empty(), "Deactivating controllers: [ %s]", + deactivate_list.c_str()); const auto list_controllers = [this, strictness]( const std::vector & controller_list, @@ -2141,13 +2153,32 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & if (!ok) { std::vector stop_request = {}; + std::string failed_hardware_string; + failed_hardware_string.reserve(500); // Determine controllers to stop for (const auto & hardware_name : failed_hardware_names) { + failed_hardware_string.append(hardware_name); + failed_hardware_string.append(" "); auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name); stop_request.insert(stop_request.end(), controllers.begin(), controllers.end()); } - + std::string stop_request_string; + stop_request_string.reserve(500); + for (const auto & controller : stop_request) + { + stop_request_string.append(controller); + stop_request_string.append(" "); + } + RCLCPP_ERROR( + get_logger(), + "Deactivating following hardware components as their read cycle resulted in an error: [ %s]", + failed_hardware_string.c_str()); + RCLCPP_ERROR_EXPRESSION( + get_logger(), !stop_request_string.empty(), + "Deactivating following controllers as their hardware components read cycle resulted in an " + "error: [ %s]", + stop_request_string.c_str()); std::vector & rt_controller_list = rt_controllers_wrapper_.update_and_get_used_by_rt_list(); deactivate_controllers(rt_controller_list, stop_request); @@ -2303,13 +2334,33 @@ void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration if (!ok) { std::vector stop_request = {}; + std::string failed_hardware_string; + failed_hardware_string.reserve(500); // Determine controllers to stop for (const auto & hardware_name : failed_hardware_names) { + failed_hardware_string.append(hardware_name); + failed_hardware_string.append(" "); auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name); stop_request.insert(stop_request.end(), controllers.begin(), controllers.end()); } - + std::string stop_request_string; + stop_request_string.reserve(500); + for (const auto & controller : stop_request) + { + stop_request_string.append(controller); + stop_request_string.append(" "); + } + RCLCPP_ERROR( + get_logger(), + "Deactivating following hardware components as their write cycle resulted in an error: [ " + "%s]", + failed_hardware_string.c_str()); + RCLCPP_ERROR_EXPRESSION( + get_logger(), !stop_request_string.empty(), + "Deactivating following controllers as their hardware components write cycle resulted in an " + "error: [ %s]", + stop_request_string.c_str()); std::vector & rt_controller_list = rt_controllers_wrapper_.update_and_get_used_by_rt_list(); deactivate_controllers(rt_controller_list, stop_request); From 32155cc3f400107c3e84979c1ffeb963458c9c8c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 14 Aug 2024 13:09:41 +0200 Subject: [PATCH 06/10] [RM] Decouple read/write cycles of each component with mutex to not block other components (#1646) --- .../include/hardware_interface/actuator.hpp | 4 ++- .../hardware_interface/actuator_interface.hpp | 2 +- .../include/hardware_interface/sensor.hpp | 4 ++- .../hardware_interface/sensor_interface.hpp | 2 +- .../include/hardware_interface/system.hpp | 4 ++- .../hardware_interface/system_interface.hpp | 3 +- hardware_interface/src/actuator.cpp | 30 +++++++++++++++++++ hardware_interface/src/resource_manager.cpp | 2 -- hardware_interface/src/sensor.cpp | 22 ++++++++++++++ hardware_interface/src/system.cpp | 29 ++++++++++++++++++ 10 files changed, 94 insertions(+), 8 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 3534d9f587..bb91b62057 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -42,7 +42,8 @@ class Actuator final HARDWARE_INTERFACE_PUBLIC explicit Actuator(std::unique_ptr impl); - Actuator(Actuator && other) = default; + HARDWARE_INTERFACE_PUBLIC + explicit Actuator(Actuator && other) noexcept; ~Actuator() = default; @@ -102,6 +103,7 @@ class Actuator final private: std::unique_ptr impl_; + mutable std::recursive_mutex actuators_mutex_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 1228d88bcd..a74d2a4964 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -228,7 +228,6 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } -protected: /// Get the logger of the ActuatorInterface. /** * \return logger of the ActuatorInterface. @@ -247,6 +246,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ const HardwareInfo & get_hardware_info() const { return info_; } +protected: HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 4126910d44..98197bd0e9 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -42,7 +42,8 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC explicit Sensor(std::unique_ptr impl); - Sensor(Sensor && other) = default; + HARDWARE_INTERFACE_PUBLIC + explicit Sensor(Sensor && other) noexcept; ~Sensor() = default; @@ -89,6 +90,7 @@ class Sensor final private: std::unique_ptr impl_; + mutable std::recursive_mutex sensors_mutex_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 6fde3f8891..0c7f43e7fc 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -167,7 +167,6 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } -protected: /// Get the logger of the SensorInterface. /** * \return logger of the SensorInterface. @@ -186,6 +185,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ const HardwareInfo & get_hardware_info() const { return info_; } +protected: HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 9308872ef8..1cc29a085d 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -42,7 +42,8 @@ class System final HARDWARE_INTERFACE_PUBLIC explicit System(std::unique_ptr impl); - System(System && other) = default; + HARDWARE_INTERFACE_PUBLIC + explicit System(System && other) noexcept; ~System() = default; @@ -102,6 +103,7 @@ class System final private: std::unique_ptr impl_; + mutable std::recursive_mutex system_mutex_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 06358f205e..54f1af33a7 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -25,6 +25,7 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -229,7 +230,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } -protected: /// Get the logger of the SystemInterface. /** * \return logger of the SystemInterface. @@ -248,6 +248,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ const HardwareInfo & get_hardware_info() const { return info_; } +protected: HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 66419402f8..52cfc681a1 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -25,6 +25,7 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -35,10 +36,17 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface Actuator::Actuator(std::unique_ptr impl) : impl_(std::move(impl)) {} +Actuator::Actuator(Actuator && other) noexcept +{ + std::lock_guard lock(other.actuators_mutex_); + impl_ = std::move(other.impl_); +} + const rclcpp_lifecycle::State & Actuator::initialize( const HardwareInfo & actuator_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { + std::unique_lock lock(actuators_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->init(actuator_info, logger, clock_interface)) @@ -60,6 +68,7 @@ const rclcpp_lifecycle::State & Actuator::initialize( const rclcpp_lifecycle::State & Actuator::configure() { + std::unique_lock lock(actuators_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { switch (impl_->on_configure(impl_->get_state())) @@ -83,6 +92,7 @@ const rclcpp_lifecycle::State & Actuator::configure() const rclcpp_lifecycle::State & Actuator::cleanup() { + std::unique_lock lock(actuators_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_cleanup(impl_->get_state())) @@ -103,6 +113,7 @@ const rclcpp_lifecycle::State & Actuator::cleanup() const rclcpp_lifecycle::State & Actuator::shutdown() { + std::unique_lock lock(actuators_mutex_); if ( impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -124,6 +135,7 @@ const rclcpp_lifecycle::State & Actuator::shutdown() const rclcpp_lifecycle::State & Actuator::activate() { + std::unique_lock lock(actuators_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_activate(impl_->get_state())) @@ -146,6 +158,7 @@ const rclcpp_lifecycle::State & Actuator::activate() const rclcpp_lifecycle::State & Actuator::deactivate() { + std::unique_lock lock(actuators_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { switch (impl_->on_deactivate(impl_->get_state())) @@ -168,6 +181,7 @@ const rclcpp_lifecycle::State & Actuator::deactivate() const rclcpp_lifecycle::State & Actuator::error() { + std::unique_lock lock(actuators_mutex_); if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->on_error(impl_->get_state())) @@ -221,6 +235,14 @@ const rclcpp_lifecycle::State & Actuator::get_state() const { return impl_->get_ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) { + std::unique_lock lock(actuators_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + impl_->get_logger(), "Skipping read() call for actuator '%s' since it is locked", + impl_->get_name().c_str()); + return return_type::OK; + } if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -243,6 +265,14 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & period) { + std::unique_lock lock(actuators_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + impl_->get_logger(), "Skipping write() call for actuator '%s' since it is locked", + impl_->get_name().c_str()); + return return_type::OK; + } if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 5b13e5d331..53fcad94d2 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1770,7 +1770,6 @@ void ResourceManager::shutdown_async_components() HardwareReadWriteStatus ResourceManager::read( const rclcpp::Time & time, const rclcpp::Duration & period) { - std::lock_guard guard(resources_lock_); read_write_status.ok = true; read_write_status.failed_hardware_names.clear(); @@ -1831,7 +1830,6 @@ HardwareReadWriteStatus ResourceManager::read( HardwareReadWriteStatus ResourceManager::write( const rclcpp::Time & time, const rclcpp::Duration & period) { - std::lock_guard guard(resources_lock_); read_write_status.ok = true; read_write_status.failed_hardware_names.clear(); diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 9b7f1f69d6..9f1d4b73e1 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -25,6 +25,7 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -34,10 +35,17 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface Sensor::Sensor(std::unique_ptr impl) : impl_(std::move(impl)) {} +Sensor::Sensor(Sensor && other) noexcept +{ + std::lock_guard lock(other.sensors_mutex_); + impl_ = std::move(other.impl_); +} + const rclcpp_lifecycle::State & Sensor::initialize( const HardwareInfo & sensor_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { + std::unique_lock lock(sensors_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->init(sensor_info, logger, clock_interface)) @@ -59,6 +67,7 @@ const rclcpp_lifecycle::State & Sensor::initialize( const rclcpp_lifecycle::State & Sensor::configure() { + std::unique_lock lock(sensors_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { switch (impl_->on_configure(impl_->get_state())) @@ -82,6 +91,7 @@ const rclcpp_lifecycle::State & Sensor::configure() const rclcpp_lifecycle::State & Sensor::cleanup() { + std::unique_lock lock(sensors_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_cleanup(impl_->get_state())) @@ -102,6 +112,7 @@ const rclcpp_lifecycle::State & Sensor::cleanup() const rclcpp_lifecycle::State & Sensor::shutdown() { + std::unique_lock lock(sensors_mutex_); if ( impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -123,6 +134,7 @@ const rclcpp_lifecycle::State & Sensor::shutdown() const rclcpp_lifecycle::State & Sensor::activate() { + std::unique_lock lock(sensors_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_activate(impl_->get_state())) @@ -145,6 +157,7 @@ const rclcpp_lifecycle::State & Sensor::activate() const rclcpp_lifecycle::State & Sensor::deactivate() { + std::unique_lock lock(sensors_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { switch (impl_->on_deactivate(impl_->get_state())) @@ -167,6 +180,7 @@ const rclcpp_lifecycle::State & Sensor::deactivate() const rclcpp_lifecycle::State & Sensor::error() { + std::unique_lock lock(sensors_mutex_); if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->on_error(impl_->get_state())) @@ -199,6 +213,14 @@ const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_st return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period) { + std::unique_lock lock(sensors_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + impl_->get_logger(), "Skipping read() call for the sensor '%s' since it is locked", + impl_->get_name().c_str()); + return return_type::OK; + } if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index ba327d8ab2..82473563b6 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -34,10 +34,17 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface System::System(std::unique_ptr impl) : impl_(std::move(impl)) {} +System::System(System && other) noexcept +{ + std::lock_guard lock(other.system_mutex_); + impl_ = std::move(other.impl_); +} + const rclcpp_lifecycle::State & System::initialize( const HardwareInfo & system_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { + std::unique_lock lock(system_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->init(system_info, logger, clock_interface)) @@ -59,6 +66,7 @@ const rclcpp_lifecycle::State & System::initialize( const rclcpp_lifecycle::State & System::configure() { + std::unique_lock lock(system_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { switch (impl_->on_configure(impl_->get_state())) @@ -82,6 +90,7 @@ const rclcpp_lifecycle::State & System::configure() const rclcpp_lifecycle::State & System::cleanup() { + std::unique_lock lock(system_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_cleanup(impl_->get_state())) @@ -102,6 +111,7 @@ const rclcpp_lifecycle::State & System::cleanup() const rclcpp_lifecycle::State & System::shutdown() { + std::unique_lock lock(system_mutex_); if ( impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -123,6 +133,7 @@ const rclcpp_lifecycle::State & System::shutdown() const rclcpp_lifecycle::State & System::activate() { + std::unique_lock lock(system_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_activate(impl_->get_state())) @@ -145,6 +156,7 @@ const rclcpp_lifecycle::State & System::activate() const rclcpp_lifecycle::State & System::deactivate() { + std::unique_lock lock(system_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { switch (impl_->on_deactivate(impl_->get_state())) @@ -167,6 +179,7 @@ const rclcpp_lifecycle::State & System::deactivate() const rclcpp_lifecycle::State & System::error() { + std::unique_lock lock(system_mutex_); if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->on_error(impl_->get_state())) @@ -218,6 +231,14 @@ const rclcpp_lifecycle::State & System::get_state() const { return impl_->get_st return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) { + std::unique_lock lock(system_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + impl_->get_logger(), "Skipping read() call for system '%s' since it is locked", + impl_->get_name().c_str()); + return return_type::OK; + } if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -240,6 +261,14 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & period) { + std::unique_lock lock(system_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + impl_->get_logger(), "Skipping write() call for system '%s' since it is locked", + impl_->get_name().c_str()); + return return_type::OK; + } if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) From 07fb4f3554645d76ba3187f88f4bc7e3ee977ab8 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 14 Aug 2024 13:10:45 +0200 Subject: [PATCH 07/10] Fix spawner unload on kill test (#1675) --- controller_manager/test/test_spawner_unspawner.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 58321d173d..83080ee0fa 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -298,11 +298,11 @@ TEST_F(TestLoadController, unload_on_kill) { // Launch spawner with unload on kill // timeout command will kill it after the specified time with signal SIGINT + cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); std::stringstream ss; ss << "timeout --signal=INT 5 " - << "ros2 run controller_manager spawner " - << "ctrl_3 -c test_controller_manager -t " - << std::string(test_controller::TEST_CONTROLLER_CLASS_NAME) << " --unload-on-kill"; + << "ros2 run controller_manager spawner ctrl_3 -c test_controller_manager" + << " --unload-on-kill"; EXPECT_NE(std::system(ss.str().c_str()), 0) << "timeout should have killed spawner and returned non 0 code"; From 3cf167d06c91a4da606e75895a37a3883dca8440 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 14 Aug 2024 14:51:31 +0200 Subject: [PATCH 08/10] [CI] Add coveragepy and remove ignore: test (#1668) --- codecov.yml | 2 -- controller_manager/package.xml | 1 + .../test/test_spawner_unspawner.cpp | 16 +++++++++++----- 3 files changed, 12 insertions(+), 7 deletions(-) diff --git a/codecov.yml b/codecov.yml index 97106f32ff..764afc34e6 100644 --- a/codecov.yml +++ b/codecov.yml @@ -14,8 +14,6 @@ fixes: comment: layout: "diff, flags, files" behavior: default -ignore: - - "**/test" flags: unittests: paths: diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 47b640c892..9f9a9bf796 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -31,6 +31,7 @@ ament_cmake_gmock ament_cmake_pytest + python3-coverage hardware_interface_testing ros2_control_test_assets diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 83080ee0fa..fa27ee2c97 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -27,6 +27,7 @@ using ::testing::_; using ::testing::Return; +const char coveragepy_script[] = "python3 -m coverage run --append --branch"; using namespace std::chrono_literals; class TestLoadController : public ControllerManagerFixture @@ -68,14 +69,18 @@ class TestLoadController : public ControllerManagerFixtureset_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); std::stringstream ss; ss << "timeout --signal=INT 5 " - << "ros2 run controller_manager spawner ctrl_3 -c test_controller_manager" - << " --unload-on-kill"; + << std::string(coveragepy_script) + + " $(ros2 pkg prefix controller_manager)/lib/controller_manager/spawner " + << "ctrl_3 -c test_controller_manager --unload-on-kill"; EXPECT_NE(std::system(ss.str().c_str()), 0) << "timeout should have killed spawner and returned non 0 code"; From cce79ebb8c5b509b8bd97e1be26f0061a7227a70 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 14 Aug 2024 15:15:11 +0200 Subject: [PATCH 09/10] Make list controller and list hardware components immediately visualize the state. (#1606) --- .../controller_manager/spawner.py | 2 +- .../ros2controlcli/verb/list_controllers.py | 14 +++++++++++--- .../verb/list_hardware_components.py | 17 ++++++++++++++--- 3 files changed, 26 insertions(+), 7 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index bc8d218ea8..a8b5d25eff 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -47,7 +47,7 @@ class bcolors: - HEADER = "\033[95m" + MAGENTA = "\033[95m" OKBLUE = "\033[94m" OKCYAN = "\033[96m" OKGREEN = "\033[92m" diff --git a/ros2controlcli/ros2controlcli/verb/list_controllers.py b/ros2controlcli/ros2controlcli/verb/list_controllers.py index 31ce814865..a26a1168a1 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/list_controllers.py @@ -22,7 +22,7 @@ from ros2controlcli.api import add_controller_mgr_parsers -def print_controller_state(c, args): +def print_controller_state(c, args, col_width_name, col_width_state, col_width_type): state_color = "" if c.state == "active": state_color = bcolors.OKGREEN @@ -31,7 +31,9 @@ def print_controller_state(c, args): elif c.state == "unconfigured": state_color = bcolors.WARNING - print(f"{c.name:20s}[{c.type:20s}] {state_color}{c.state:10s}{bcolors.ENDC}") + print( + f"{state_color}{c.name:<{col_width_name}}{bcolors.ENDC} {c.type:<{col_width_type}} {state_color}{c.state:<{col_width_state}}{bcolors.ENDC}" + ) if args.claimed_interfaces or args.verbose: print("\tclaimed interfaces:") for claimed_interface in c.claimed_interfaces: @@ -96,7 +98,13 @@ def add_arguments(self, parser, cli_name): def main(self, *, args): with NodeStrategy(args) as node: response = list_controllers(node, args.controller_manager) + + # Structure data as table for nicer output + col_width_name = max(len(ctrl.name) for ctrl in response.controller) + col_width_type = max(len(ctrl.type) for ctrl in response.controller) + col_width_state = max(len(ctrl.state) for ctrl in response.controller) + for c in response.controller: - print_controller_state(c, args) + print_controller_state(c, args, col_width_name, col_width_state, col_width_type) return 0 diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py index 8a5884f2cb..e2b427b233 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py @@ -15,6 +15,8 @@ from controller_manager import list_hardware_components from controller_manager.spawner import bcolors +from lifecycle_msgs.msg import State + from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy from ros2cli.verb import VerbExtension @@ -39,17 +41,26 @@ def main(self, *, args): hardware_components = list_hardware_components(node, args.controller_manager) for idx, component in enumerate(hardware_components.component): + # Set activity color for nicer visualization + activity_color = bcolors.FAIL + if component.state.id == State.PRIMARY_STATE_UNCONFIGURED: + activity_color = bcolors.WARNING + if component.state.id == State.PRIMARY_STATE_INACTIVE: + activity_color = bcolors.MAGENTA + if component.state.id == State.PRIMARY_STATE_ACTIVE: + activity_color = bcolors.OKGREEN + print( - f"Hardware Component {idx+1}\n\tname: {component.name}\n\ttype: {component.type}" + f"Hardware Component {idx+1}\n\tname: {activity_color}{component.name}{bcolors.ENDC}\n\ttype: {component.type}" ) if hasattr(component, "plugin_name"): - plugin_name = component.plugin_name + plugin_name = f"{component.plugin_name}" else: plugin_name = f"{bcolors.WARNING}plugin name missing!{bcolors.ENDC}" print( f"\tplugin name: {plugin_name}\n" - f"\tstate: id={component.state.id} label={component.state.label}\n" + f"\tstate: id={component.state.id} label={activity_color}{component.state.label}{bcolors.ENDC}\n" f"\tcommand interfaces" ) for cmd_interface in component.command_interfaces: From af4b48f75a0ff4c57f30df3477f9fa2786e07965 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 14 Aug 2024 16:24:23 +0100 Subject: [PATCH 10/10] Handle waiting in Spawner and align Hardware Spawner functionality (#1562) --- .../controller_manager_services.py | 37 +++++---- .../controller_manager/hardware_spawner.py | 77 ++++++------------- .../controller_manager/spawner.py | 76 ++++-------------- .../controller_manager/unspawner.py | 6 ++ controller_manager/doc/userdoc.rst | 20 +++++ .../test/test_spawner_unspawner.cpp | 22 +++--- 6 files changed, 98 insertions(+), 140 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 62b350f7d3..be11a4b782 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -28,15 +28,20 @@ import rclpy -def service_caller(node, service_name, service_type, request, service_timeout=10.0): +class ServiceNotFoundError(Exception): + pass + + +def service_caller(node, service_name, service_type, request, service_timeout=0.0): cli = node.create_client(service_type, service_name) - if not cli.service_is_ready(): - node.get_logger().debug( - f"waiting {service_timeout} seconds for service {service_name} to become available..." - ) - if not cli.wait_for_service(service_timeout): - raise RuntimeError(f"Could not contact service {service_name}") + while not cli.service_is_ready(): + node.get_logger().info(f"waiting for service {service_name} to become available...") + if service_timeout: + if not cli.wait_for_service(service_timeout): + raise ServiceNotFoundError(f"Could not contact service {service_name}") + elif not cli.wait_for_service(10.0): + node.get_logger().warn(f"Could not contact service {service_name}") node.get_logger().debug(f"requester: making request: {request}\n") future = cli.call_async(request) @@ -47,7 +52,7 @@ def service_caller(node, service_name, service_type, request, service_timeout=10 raise RuntimeError(f"Exception while calling service: {future.exception()}") -def configure_controller(node, controller_manager_name, controller_name, service_timeout=10.0): +def configure_controller(node, controller_manager_name, controller_name, service_timeout=0.0): request = ConfigureController.Request() request.name = controller_name return service_caller( @@ -59,7 +64,7 @@ def configure_controller(node, controller_manager_name, controller_name, service ) -def list_controllers(node, controller_manager_name, service_timeout=10.0): +def list_controllers(node, controller_manager_name, service_timeout=0.0): request = ListControllers.Request() return service_caller( node, @@ -70,7 +75,7 @@ def list_controllers(node, controller_manager_name, service_timeout=10.0): ) -def list_controller_types(node, controller_manager_name, service_timeout=10.0): +def list_controller_types(node, controller_manager_name, service_timeout=0.0): request = ListControllerTypes.Request() return service_caller( node, @@ -81,7 +86,7 @@ def list_controller_types(node, controller_manager_name, service_timeout=10.0): ) -def list_hardware_components(node, controller_manager_name, service_timeout=10.0): +def list_hardware_components(node, controller_manager_name, service_timeout=0.0): request = ListHardwareComponents.Request() return service_caller( node, @@ -92,7 +97,7 @@ def list_hardware_components(node, controller_manager_name, service_timeout=10.0 ) -def list_hardware_interfaces(node, controller_manager_name, service_timeout=10.0): +def list_hardware_interfaces(node, controller_manager_name, service_timeout=0.0): request = ListHardwareInterfaces.Request() return service_caller( node, @@ -103,7 +108,7 @@ def list_hardware_interfaces(node, controller_manager_name, service_timeout=10.0 ) -def load_controller(node, controller_manager_name, controller_name, service_timeout=10.0): +def load_controller(node, controller_manager_name, controller_name, service_timeout=0.0): request = LoadController.Request() request.name = controller_name return service_caller( @@ -115,7 +120,7 @@ def load_controller(node, controller_manager_name, controller_name, service_time ) -def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=10.0): +def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=0.0): request = ReloadControllerLibraries.Request() request.force_kill = force_kill return service_caller( @@ -128,7 +133,7 @@ def reload_controller_libraries(node, controller_manager_name, force_kill, servi def set_hardware_component_state( - node, controller_manager_name, component_name, lifecyle_state, service_timeout=10.0 + node, controller_manager_name, component_name, lifecyle_state, service_timeout=0.0 ): request = SetHardwareComponentState.Request() request.name = component_name @@ -165,7 +170,7 @@ def switch_controllers( ) -def unload_controller(node, controller_manager_name, controller_name, service_timeout=10.0): +def unload_controller(node, controller_manager_name, controller_name, service_timeout=0.0): request = UnloadController.Request() request.name = controller_name return service_caller( diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index c95fb6181e..3e3a487c6a 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -15,13 +15,15 @@ import argparse import sys -import time -from controller_manager import set_hardware_component_state +from controller_manager import ( + list_hardware_components, + set_hardware_component_state, +) +from controller_manager.controller_manager_services import ServiceNotFoundError from lifecycle_msgs.msg import State import rclpy -from rclpy.duration import Duration from rclpy.node import Node from rclpy.signals import SignalHandlerOptions @@ -43,17 +45,6 @@ def first_match(iterable, predicate): return next((n for n in iterable if predicate(n)), None) -def wait_for_value_or(function, node, timeout, default, description): - while node.get_clock().now() < timeout: - if result := function(): - return result - node.get_logger().info( - f"Waiting for {description}", throttle_duration_sec=2, skip_first=True - ) - time.sleep(0.2) - return default - - def combine_name_and_namespace(name_and_namespace): node_name, namespace = name_and_namespace return namespace + ("" if namespace.endswith("/") else "/") + node_name @@ -75,35 +66,11 @@ def has_service_names(node, node_name, node_namespace, service_names): return all(service in client_names for service in service_names) -def wait_for_controller_manager(node, controller_manager, timeout_duration): - # List of service names from controller_manager we wait for - service_names = ( - f"{controller_manager}/list_hardware_components", - f"{controller_manager}/set_hardware_component_state", - ) - - # Wait for controller_manager - timeout = node.get_clock().now() + Duration(seconds=timeout_duration) - node_and_namespace = wait_for_value_or( - lambda: find_node_and_namespace(node, controller_manager), - node, - timeout, - None, - f"'{controller_manager}' node to exist", - ) - - # Wait for the services if the node was found - if node_and_namespace: - node_name, namespace = node_and_namespace - return wait_for_value_or( - lambda: has_service_names(node, node_name, namespace, service_names), - node, - timeout, - False, - f"'{controller_manager}' services to be available", - ) - - return False +def is_hardware_component_loaded( + node, controller_manager, hardware_component, service_timeout=0.0 +): + components = list_hardware_components(node, hardware_component, service_timeout).component + return any(c.name == hardware_component for c in components) def handle_set_component_state_service_call( @@ -167,10 +134,9 @@ def main(args=None): "--controller-manager-timeout", help="Time to wait for the controller manager", required=False, - default=10, - type=int, + default=0, + type=float, ) - # add arguments which are mutually exclusive activate_or_confiigure_grp.add_argument( "--activate", @@ -202,13 +168,15 @@ def main(args=None): controller_manager_name = f"/{controller_manager_name}" try: - if not wait_for_controller_manager( - node, controller_manager_name, controller_manager_timeout + if not is_hardware_component_loaded( + node, controller_manager_name, hardware_component, controller_manager_timeout ): - node.get_logger().error("Controller manager not available") - return 1 - - if activate: + 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) @@ -218,6 +186,11 @@ def main(args=None): ) parser.print_help() return 0 + except KeyboardInterrupt: + pass + except ServiceNotFoundError as err: + node.get_logger().fatal(str(err)) + return 1 finally: rclpy.shutdown() diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index a8b5d25eff..0e83eb9d07 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -28,10 +28,10 @@ switch_controllers, unload_controller, ) +from controller_manager.controller_manager_services import ServiceNotFoundError import rclpy from rcl_interfaces.msg import Parameter -from rclpy.duration import Duration from rclpy.node import Node # @note: The versions conditioning is added here to support the source-compatibility with Humble @@ -62,17 +62,6 @@ def first_match(iterable, predicate): return next((n for n in iterable if predicate(n)), None) -def wait_for_value_or(function, node, timeout, default, description): - while node.get_clock().now() < timeout: - if result := function(): - return result - node.get_logger().info( - f"Waiting for {description}", throttle_duration_sec=2, skip_first=True - ) - time.sleep(0.2) - return default - - def combine_name_and_namespace(name_and_namespace): node_name, namespace = name_and_namespace return namespace + ("" if namespace.endswith("/") else "/") + node_name @@ -93,46 +82,8 @@ def has_service_names(node, node_name, node_namespace, service_names): return all(service in client_names for service in service_names) -def wait_for_controller_manager(node, controller_manager, timeout_duration): - # List of service names from controller_manager we wait for - service_names = ( - f"{controller_manager}/configure_controller", - f"{controller_manager}/list_controllers", - f"{controller_manager}/list_controller_types", - f"{controller_manager}/list_hardware_components", - f"{controller_manager}/list_hardware_interfaces", - f"{controller_manager}/load_controller", - f"{controller_manager}/reload_controller_libraries", - f"{controller_manager}/switch_controller", - f"{controller_manager}/unload_controller", - ) - - # Wait for controller_manager - timeout = node.get_clock().now() + Duration(seconds=timeout_duration) - node_and_namespace = wait_for_value_or( - lambda: find_node_and_namespace(node, controller_manager), - node, - timeout, - None, - f"'{controller_manager}' node to exist", - ) - - # Wait for the services if the node was found - if node_and_namespace: - node_name, namespace = node_and_namespace - return wait_for_value_or( - lambda: has_service_names(node, node_name, namespace, service_names), - node, - timeout, - False, - f"'{controller_manager}' services to be available", - ) - - return False - - -def is_controller_loaded(node, controller_manager, controller_name): - controllers = list_controllers(node, controller_manager).controller +def is_controller_loaded(node, controller_manager, controller_name, service_timeout=0.0): + controllers = list_controllers(node, controller_manager, service_timeout).controller return any(c.name == controller_name for c in controllers) @@ -201,8 +152,8 @@ def main(args=None): "--controller-manager-timeout", help="Time to wait for the controller manager", required=False, - default=10, - type=int, + default=0, + type=float, ) parser.add_argument( "--activate-as-group", @@ -258,18 +209,12 @@ def main(args=None): controller_manager_name = f"/{controller_manager_name}" try: - if not wait_for_controller_manager( - node, controller_manager_name, controller_manager_timeout - ): - node.get_logger().error( - bcolors.FAIL + "Controller manager not available" + bcolors.ENDC - ) - return 1 - for controller_name in controller_names: fallback_controllers = args.fallback_controllers - if is_controller_loaded(node, controller_manager_name, controller_name): + if is_controller_loaded( + node, controller_manager_name, controller_name, controller_manager_timeout + ): node.get_logger().warn( bcolors.WARNING + "Controller already loaded, skipping load_controller" @@ -471,6 +416,11 @@ def main(args=None): node.get_logger().info("Unloaded controller") return 0 + except KeyboardInterrupt: + pass + except ServiceNotFoundError as err: + node.get_logger().fatal(str(err)) + return 1 finally: rclpy.shutdown() diff --git a/controller_manager/controller_manager/unspawner.py b/controller_manager/controller_manager/unspawner.py index 5acf05e65d..e42d85aee9 100644 --- a/controller_manager/controller_manager/unspawner.py +++ b/controller_manager/controller_manager/unspawner.py @@ -19,6 +19,7 @@ import warnings from controller_manager import switch_controllers, unload_controller +from controller_manager.controller_manager_services import ServiceNotFoundError import rclpy from rclpy.node import Node @@ -57,6 +58,11 @@ def main(args=None): node.get_logger().info("Unloaded controller") return 0 + except KeyboardInterrupt: + pass + except ServiceNotFoundError as err: + node.get_logger().fatal(str(err)) + return 1 finally: rclpy.shutdown() diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 6338211665..bc9f75384e 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -136,6 +136,7 @@ There are two scripts to interact with controller manager from launch files: 1. ``spawner`` - loads, configures and start a controller on startup. 2. ``unspawner`` - stops and unloads a controller. + 3. ``hardware_spawner`` - activates and configures a hardware component. ``spawner`` @@ -186,6 +187,25 @@ There are two scripts to interact with controller manager from launch files: -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node +``hardware_spawner`` +^^^^^^^^^^^^^^^^^^^^^^ + +.. code-block:: console + + $ ros2 run controller_manager hardware_spawner -h + usage: hardware_spawner [-h] [-c CONTROLLER_MANAGER] (--activate | --configure) hardware_component_name + + positional arguments: + hardware_component_name + The name of the hardware component 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 + --activate Activates the given components. Note: Components are by default configured before activated. + --configure Configures the given components. + rqt_controller_manager ---------------------- A GUI tool to interact with the controller manager services to be able to switch the lifecycle states of the controllers as well as the hardware components. diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index fa27ee2c97..3edf770436 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -88,9 +88,10 @@ TEST_F(TestLoadController, spawner_with_no_arguments_errors) EXPECT_NE(call_spawner(""), 0) << "Missing mandatory arguments"; } -TEST_F(TestLoadController, spawner_without_manager_errors) +TEST_F(TestLoadController, spawner_without_manager_errors_with_given_timeout) { - EXPECT_NE(call_spawner("ctrl_1"), 0) << "Wrong controller manager name"; + EXPECT_NE(call_spawner("ctrl_1 --controller-manager-timeout 1.0"), 0) + << "Wrong controller manager name"; } TEST_F(TestLoadController, spawner_without_type_parameter_or_arg_errors) @@ -283,7 +284,9 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path), + "ctrl_with_parameters_and_no_type -c test_controller_manager --controller-manager-timeout " + "1.0 -p " + + test_file_path), 256); // Will still be same as the current call will fail ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); @@ -422,7 +425,7 @@ TEST_F(TestLoadControllerWithoutRobotDescription, when_no_robot_description_spaw cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); ControllerManagerRunner cm_runner(this); - EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 256) + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager --controller-manager-timeout 1.0"), 256) << "could not spawn controller because not robot description and not services for controller " "manager are active"; } @@ -504,7 +507,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, multi_ctrls_test_type_in_param) cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); ControllerManagerRunner cm_runner(this); - EXPECT_EQ(call_spawner("ctrl_1 ctrl_2 -c test_controller_manager"), 256) + EXPECT_EQ( + call_spawner("ctrl_1 ctrl_2 -c test_controller_manager --controller-manager-timeout 1.0"), 256) << "Should fail without defining the namespace"; EXPECT_EQ( call_spawner("ctrl_1 ctrl_2 -c test_controller_manager --ros-args -r __ns:=/foo_namespace"), 0); @@ -603,7 +607,7 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) EXPECT_EQ( call_spawner( "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " - "test_controller_manager -p " + + "test_controller_manager --controller-manager-timeout 1.0 -p " + test_file_path), 256) << "Should fail without the namespacing it"; @@ -663,21 +667,21 @@ TEST_F( EXPECT_EQ( call_spawner( "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " - "test_controller_manager -p " + + "test_controller_manager --controller-manager-timeout 1.0 -p " + test_file_path), 256) << "Should fail without the namespacing it"; EXPECT_EQ( call_spawner( "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " - "test_controller_manager --namespace foo_namespace -p " + + "test_controller_manager --namespace foo_namespace --controller-manager-timeout 1.0 -p " + test_file_path + " --ros-args -r __ns:=/random_namespace"), 256) << "Should fail when parsed namespace through both way with different namespaces"; EXPECT_EQ( call_spawner( "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " - "test_controller_manager --namespace foo_namespace -p " + + "test_controller_manager --namespace foo_namespace --controller-manager-timeout 1.0 -p" + test_file_path + " --ros-args -r __ns:=/foo_namespace"), 256) << "Should fail when parsed namespace through both ways even with same namespacing name";