From 0ba1428654cbbc69050f8d58eefc39c56b098cb6 Mon Sep 17 00:00:00 2001 From: Santosh Govindaraj <75157452+SantoshGovindaraj@users.noreply.github.com> Date: Tue, 15 Oct 2024 22:42:33 +0100 Subject: [PATCH 01/40] fix: call configure_controller on 'unconfigured' state instead load_controller (#1794) --- .../rqt_controller_manager/controller_manager.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index 4cb6c01e09..20b2f47a1a 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -260,7 +260,7 @@ def _on_ctrl_menu(self, pos): if action is action_configure: configure_controller(self._node, self._cm_name, ctrl.name) elif action is action_spawn: - load_controller(self._node, self._cm_name, ctrl.name) + configure_controller(self._node, self._cm_name, ctrl.name) self._activate_controller(ctrl.name) else: # Assume controller isn't loaded From 0a2838a57638e97d474b3891cbe582e08c9e9f1f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 16 Oct 2024 16:45:03 +0200 Subject: [PATCH 02/40] [ros2controlcli] Fix the missing exported state interface printing (#1800) --- .../ros2controlcli/verb/list_controllers.py | 23 ++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/ros2controlcli/ros2controlcli/verb/list_controllers.py b/ros2controlcli/ros2controlcli/verb/list_controllers.py index 6b6a42e3ce..593df643a7 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/list_controllers.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import warnings from controller_manager import list_controllers, bcolors from ros2cli.node.direct import add_arguments @@ -50,10 +51,14 @@ def print_controller_state(c, args, col_width_name, col_width_state, col_width_t for connection in c.chain_connections: for reference in connection.reference_interfaces: print(f"\t\t{reference:20s}") - if args.reference_interfaces or args.verbose: + if args.reference_interfaces or args.exported_interfaces or args.verbose: print("\texported reference interfaces:") - for reference_interfaces in c.reference_interfaces: - print(f"\t\t{reference_interfaces}") + for reference_interface in c.reference_interfaces: + print(f"\t\t{reference_interface}") + if args.reference_interfaces or args.exported_interfaces or args.verbose: + print("\texported state interfaces:") + for exported_state_interface in c.exported_state_interfaces: + print(f"\t\t{exported_state_interface}") class ListControllersVerb(VerbExtension): @@ -86,6 +91,11 @@ def add_arguments(self, parser, cli_name): action="store_true", help="List controller's exported references", ) + parser.add_argument( + "--exported-interfaces", + action="store_true", + help="List controller's exported state and reference interfaces", + ) parser.add_argument( "--verbose", "-v", @@ -98,6 +108,13 @@ def main(self, *, args): with NodeStrategy(args).direct_node as node: response = list_controllers(node, args.controller_manager) + if args.reference_interfaces: + warnings.filterwarnings("always") + warnings.warn( + "The '--reference-interfaces' argument is deprecated and will be removed in future releases. Use '--exported-interfaces' instead.", + DeprecationWarning, + ) + if not response.controller: print("No controllers are currently loaded!") return 0 From 1d2d6173d3929e3ee5187ec55cfa19665fc001d1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 16 Oct 2024 17:20:38 +0200 Subject: [PATCH 03/40] Check the update_rate set to the controllers to be a valid one (#1788) --- .../src/controller_interface_base.cpp | 22 ++++++++++- .../test/test_controller_interface.cpp | 37 ++++++++++++++++--- .../test/test_controller_manager.cpp | 2 +- .../test/test_load_controller.cpp | 4 +- 4 files changed, 55 insertions(+), 10 deletions(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 61d2c780b8..e2c1fa480a 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -28,13 +28,14 @@ return_type ControllerInterfaceBase::init( const std::string & node_namespace, const rclcpp::NodeOptions & node_options) { urdf_ = urdf; + update_rate_ = cm_update_rate; node_ = std::make_shared( controller_name, node_namespace, node_options, false); // disable LifecycleNode service interfaces try { - auto_declare("update_rate", cm_update_rate); + auto_declare("update_rate", update_rate_); auto_declare("is_async", false); } catch (const std::exception & e) @@ -85,7 +86,24 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() // Other solution is to add check into the LifecycleNode if a transition is valid to trigger if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { - update_rate_ = static_cast(get_node()->get_parameter("update_rate").as_int()); + const auto update_rate = get_node()->get_parameter("update_rate").as_int(); + if (update_rate < 0) + { + RCLCPP_ERROR(get_node()->get_logger(), "Update rate cannot be a negative value!"); + return get_lifecycle_state(); + } + if (update_rate_ != 0u && update_rate > update_rate_) + { + RCLCPP_WARN( + get_node()->get_logger(), + "The update rate of the controller : '%ld Hz' cannot be higher than the update rate of the " + "controller manager : '%d Hz'. Setting it to the update rate of the controller manager.", + update_rate, update_rate_); + } + else + { + update_rate_ = static_cast(update_rate); + } is_async_ = get_node()->get_parameter("is_async").as_bool(); } diff --git a/controller_interface/test/test_controller_interface.cpp b/controller_interface/test/test_controller_interface.cpp index f5c0a6b3de..4204d32f45 100644 --- a/controller_interface/test/test_controller_interface.cpp +++ b/controller_interface/test/test_controller_interface.cpp @@ -19,6 +19,7 @@ #include #include +#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/executor_options.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" @@ -46,8 +47,8 @@ TEST(TestableControllerInterface, init) controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); - // update_rate is set to default 0 - ASSERT_EQ(controller.get_update_rate(), 0u); + // update_rate is set to controller_manager's rate + ASSERT_EQ(controller.get_update_rate(), 10u); // Even after configure is 10 controller.configure(); @@ -56,6 +57,32 @@ TEST(TestableControllerInterface, init) rclcpp::shutdown(); } +TEST(TestableControllerInterface, setting_negative_update_rate_in_configure) +{ + // mocks the declaration of overrides parameters in a yaml file + rclcpp::init(0, nullptr); + + TestableControllerInterface controller; + // initialize, create node + auto node_options = controller.define_custom_node_options(); + std::vector node_options_arguments = node_options.arguments(); + node_options_arguments.push_back("--ros-args"); + node_options_arguments.push_back("-p"); + node_options_arguments.push_back("update_rate:=-100"); + node_options = node_options.arguments(node_options_arguments); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 1000.0, "", node_options), + controller_interface::return_type::OK); + + // update_rate is set to controller_manager's rate + ASSERT_EQ(controller.get_update_rate(), 1000u); + + // The configure should fail and the update rate should stay the same + ASSERT_EQ(controller.configure().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ(controller.get_update_rate(), 1000u); + rclcpp::shutdown(); +} + TEST(TestableControllerInterface, setting_update_rate_in_configure) { // mocks the declaration of overrides parameters in a yaml file @@ -70,7 +97,7 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure) node_options_arguments.push_back("update_rate:=2812"); node_options = node_options.arguments(node_options_arguments); ASSERT_EQ( - controller.init(TEST_CONTROLLER_NAME, "", 1.0, "", node_options), + controller.init(TEST_CONTROLLER_NAME, "", 5000.0, "", node_options), controller_interface::return_type::OK); // initialize executor to be able to get parameter update @@ -85,8 +112,8 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure) using namespace std::chrono_literals; std::this_thread::sleep_for(50ms); - // update_rate is set to default 0 because it is set on configure - ASSERT_EQ(controller.get_update_rate(), 0u); + // update_rate is set to controller_manager's rate + ASSERT_EQ(controller.get_update_rate(), 5000u); // Even after configure is 0 controller.configure(); diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 28e9f0477c..3bf56dd900 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -398,7 +398,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd // if we do 2 times of the controller_manager update rate, the internal counter should be // similarly incremented EXPECT_EQ(test_controller->internal_counter, pre_internal_counter + (2 * cm_->get_update_rate())); - EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate); + EXPECT_EQ(test_controller->get_update_rate(), cm_->get_update_rate()); auto deactivate_future = std::async( std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index 3645c06c24..ce7285fc49 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -66,13 +66,13 @@ TEST_F(TestLoadController, can_set_and_get_non_default_update_rate) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_lifecycle_state().id()); - controller_if->get_node()->set_parameter({"update_rate", 1337}); + controller_if->get_node()->set_parameter({"update_rate", 50}); cm_->configure_controller("test_controller_01"); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); - EXPECT_EQ(1337u, controller_if->get_update_rate()); + EXPECT_EQ(50u, controller_if->get_update_rate()); } class TestLoadedController : public TestLoadController From 4eabd255827741dc4ab8aa0fa7b80e3360b893d2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 16 Oct 2024 20:25:59 +0200 Subject: [PATCH 04/40] [Feature] Fallback controllers (#1789) Co-authored-by: Bence Magyar --- controller_manager/CMakeLists.txt | 1 + controller_manager/doc/userdoc.rst | 12 + .../controller_manager/controller_manager.hpp | 18 + controller_manager/src/controller_manager.cpp | 269 ++++- .../test/test_controller/test_controller.cpp | 5 + .../test/test_controller/test_controller.hpp | 2 + .../test/test_controller_manager.cpp | 919 ++++++++++++++++++ doc/release_notes.rst | 1 + 8 files changed, 1221 insertions(+), 6 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 5dea15c0d1..e44fb5fb9b 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -91,6 +91,7 @@ if(BUILD_TESTING) target_link_libraries(test_controller_manager controller_manager test_controller + test_chainable_controller ros2_control_test_assets::ros2_control_test_assets ) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index bc9f75384e..d3dd5e90c8 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -87,6 +87,18 @@ update_rate (mandatory; integer) Name of a plugin exported using ``pluginlib`` for a controller. This is a class from which controller's instance with name "``controller_name``" is created. +.params_file + The absolute path to the YAML file with parameters for the controller. + The file should contain the parameters for the controller in the standard ROS 2 YAML format. + +.fallback_controllers + List of controllers that are activated as a fallback strategy, when the spawned controllers fail by returning ``return_type::ERROR`` during the ``update`` cycle. + It is recommended to add all the controllers needed for the fallback strategy to the list, including the chainable controllers whose interfaces are used by the main fallback controllers. + +.. warning:: + The fallback controllers activation is subject to the availability of the state and command interfaces at the time of activation. + It is recommended to test the fallback strategy in simulation before deploying it on the real robot. + Handling Multiple Controller Managers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index b8c05efcc0..273b48b022 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -128,6 +128,12 @@ class ControllerManager : public rclcpp::Node return add_controller_impl(controller_spec); } + controller_interface::ControllerInterfaceBaseSharedPtr add_controller( + const ControllerSpec & controller_spec) + { + return add_controller_impl(controller_spec); + } + /// configure_controller Configure controller by name calling their "configure" method. /** * \param[in] controller_name as a string. @@ -418,6 +424,18 @@ class ControllerManager : public rclcpp::Node const std::vector & controllers, int strictness, const ControllersListIterator controller_it); + /// Checks if the fallback controllers of the given controllers are in the right + /// state, so they can be activated immediately + /** + * \param[in] controllers is a list of controllers to activate. + * \param[in] controller_it is the iterator pointing to the controller to be activated. + * \return return_type::OK if all fallback controllers are in the right state, otherwise + * return_type::ERROR. + */ + CONTROLLER_MANAGER_PUBLIC + controller_interface::return_type check_fallback_controllers_state_pre_activation( + const std::vector & controllers, const ControllersListIterator controller_it); + /** * @brief Inserts a controller into an ordered list based on dependencies to compute the * controller chain. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index b8a587f92e..72cebaa1e5 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -170,6 +170,41 @@ void controller_chain_spec_cleanup( } ctrl_chain_spec.erase(controller); } + +// Gets the list of active controllers that use the command interface of the given controller +void get_active_controllers_using_command_interfaces_of_controller( + const std::string & controller_name, + const std::vector & controllers, + std::vector & controllers_using_command_interfaces) +{ + auto it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller_name)); + if (it == controllers.end()) + { + RCLCPP_ERROR( + rclcpp::get_logger("ControllerManager::utils"), + "Controller '%s' not found in the list of controllers.", controller_name.c_str()); + return; + } + const auto cmd_itfs = it->c->command_interface_configuration().names; + for (const auto & cmd_itf : cmd_itfs) + { + for (const auto & controller : controllers) + { + const auto ctrl_cmd_itfs = controller.c->command_interface_configuration().names; + // check if the controller is active and has the command interface and make sure that it + // doesn't exist in the list already + if ( + is_controller_active(controller.c) && + std::find(ctrl_cmd_itfs.begin(), ctrl_cmd_itfs.end(), cmd_itf) != ctrl_cmd_itfs.end()) + { + add_element_to_list(controllers_using_command_interfaces, controller.info.name); + } + } + } +} + } // namespace namespace controller_manager @@ -536,6 +571,15 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c } if (get_parameter(fallback_ctrl_param, fallback_controllers) && !fallback_controllers.empty()) { + if ( + std::find(fallback_controllers.begin(), fallback_controllers.end(), controller_name) != + fallback_controllers.end()) + { + RCLCPP_ERROR( + get_logger(), "Controller '%s' cannot be a fallback controller for itself.", + controller_name.c_str()); + return nullptr; + } controller_spec.info.fallback_controllers_names = fallback_controllers; } @@ -1082,6 +1126,11 @@ controller_interface::return_type ControllerManager::switch_controller( status = check_following_controllers_for_activate(controllers, strictness, controller_it); } + if (status == controller_interface::return_type::OK) + { + status = check_fallback_controllers_state_pre_activation(controllers, controller_it); + } + if (status != controller_interface::return_type::OK) { RCLCPP_WARN( @@ -2360,16 +2409,68 @@ controller_interface::return_type ControllerManager::update( } if (!failed_controllers_list.empty()) { - std::string failed_controllers; + const auto FALLBACK_STACK_MAX_SIZE = 500; + std::vector active_controllers_using_interfaces(failed_controllers_list); + active_controllers_using_interfaces.reserve(FALLBACK_STACK_MAX_SIZE); + std::vector cumulative_fallback_controllers; + cumulative_fallback_controllers.reserve(FALLBACK_STACK_MAX_SIZE); + + for (const auto & failed_ctrl : failed_controllers_list) + { + auto ctrl_it = std::find_if( + rt_controller_list.begin(), rt_controller_list.end(), + std::bind(controller_name_compare, std::placeholders::_1, failed_ctrl)); + if (ctrl_it != rt_controller_list.end()) + { + for (const auto & fallback_controller : ctrl_it->info.fallback_controllers_names) + { + cumulative_fallback_controllers.push_back(fallback_controller); + get_active_controllers_using_command_interfaces_of_controller( + fallback_controller, rt_controller_list, active_controllers_using_interfaces); + } + } + } + std::string controllers_string; + controllers_string.reserve(500); for (const auto & controller : failed_controllers_list) { - failed_controllers += "\n\t- " + controller; + controllers_string.append(controller); + controllers_string.append(" "); } RCLCPP_ERROR( - get_logger(), "Deactivating following controllers as their update resulted in an error :%s", - failed_controllers.c_str()); - - deactivate_controllers(rt_controller_list, failed_controllers_list); + get_logger(), "Deactivating controllers : [ %s] as their update resulted in an error!", + controllers_string.c_str()); + if (active_controllers_using_interfaces.size() > failed_controllers_list.size()) + { + controllers_string.clear(); + for (size_t i = failed_controllers_list.size(); + i < active_controllers_using_interfaces.size(); i++) + { + controllers_string.append(active_controllers_using_interfaces[i]); + controllers_string.append(" "); + } + RCLCPP_ERROR_EXPRESSION( + get_logger(), !controllers_string.empty(), + "Deactivating controllers : [ %s] using the command interfaces needed for the fallback " + "controllers to activate.", + controllers_string.c_str()); + } + if (!cumulative_fallback_controllers.empty()) + { + controllers_string.clear(); + for (const auto & controller : cumulative_fallback_controllers) + { + controllers_string.append(controller); + controllers_string.append(" "); + } + RCLCPP_ERROR( + get_logger(), "Activating fallback controllers : [ %s]", controllers_string.c_str()); + } + deactivate_controllers(rt_controller_list, active_controllers_using_interfaces); + if (!cumulative_fallback_controllers.empty()) + { + activate_controllers(rt_controller_list, cumulative_fallback_controllers); + } } // there are controllers to (de)activate @@ -2765,6 +2866,162 @@ controller_interface::return_type ControllerManager::check_preceeding_controller return controller_interface::return_type::OK; } +controller_interface::return_type +ControllerManager::check_fallback_controllers_state_pre_activation( + const std::vector & controllers, const ControllersListIterator controller_it) +{ + for (const auto & fb_ctrl : controller_it->info.fallback_controllers_names) + { + auto fb_ctrl_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, fb_ctrl)); + if (fb_ctrl_it == controllers.end()) + { + RCLCPP_ERROR( + get_logger(), + "Unable to find the fallback controller : '%s' of the controller : '%s' " + "within the controller list", + fb_ctrl.c_str(), controller_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + else + { + if (!(is_controller_inactive(fb_ctrl_it->c) || is_controller_active(fb_ctrl_it->c))) + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as it's fallback controller : '%s'" + " need to be configured and be in inactive/active state!", + controller_it->info.name.c_str(), fb_ctrl.c_str()); + return controller_interface::return_type::ERROR; + } + for (const auto & fb_cmd_itf : fb_ctrl_it->c->command_interface_configuration().names) + { + if (!resource_manager_->command_interface_is_available(fb_cmd_itf)) + { + ControllersListIterator following_ctrl_it; + if (is_interface_a_chained_interface(fb_cmd_itf, controllers, following_ctrl_it)) + { + // if following_ctrl_it is inactive and it is in the fallback list of the + // controller_it and then check it it's exported reference interface names list if + // it's available + if (is_controller_inactive(following_ctrl_it->c)) + { + if ( + std::find( + controller_it->info.fallback_controllers_names.begin(), + controller_it->info.fallback_controllers_names.end(), + following_ctrl_it->info.name) != + controller_it->info.fallback_controllers_names.end()) + { + const auto exported_ref_itfs = + resource_manager_->get_controller_reference_interface_names( + following_ctrl_it->info.name); + if ( + std::find(exported_ref_itfs.begin(), exported_ref_itfs.end(), fb_cmd_itf) == + exported_ref_itfs.end()) + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as the command interface : " + "'%s' required by its fallback controller : '%s' is not exported by the " + "controller : '%s' in the current fallback list!", + controller_it->info.name.c_str(), fb_cmd_itf.c_str(), fb_ctrl.c_str(), + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as the command interface : " + "'%s' required by its fallback controller : '%s' is not available as the " + "controller is not in active state!. May be consider adding this controller to " + "the fallback list of the controller : '%s' or already have it activated.", + controller_it->info.name.c_str(), fb_cmd_itf.c_str(), fb_ctrl.c_str(), + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as not all of its fallback " + "controller's : '%s' command interfaces are currently available!", + controller_it->info.name.c_str(), fb_ctrl.c_str()); + return controller_interface::return_type::ERROR; + } + } + } + for (const auto & fb_state_itf : fb_ctrl_it->c->state_interface_configuration().names) + { + if (!resource_manager_->state_interface_is_available(fb_state_itf)) + { + ControllersListIterator following_ctrl_it; + if (is_interface_a_chained_interface(fb_state_itf, controllers, following_ctrl_it)) + { + // if following_ctrl_it is inactive and it is in the fallback list of the + // controller_it and then check it it's exported reference interface names list if + // it's available + if (is_controller_inactive(following_ctrl_it->c)) + { + if ( + std::find( + controller_it->info.fallback_controllers_names.begin(), + controller_it->info.fallback_controllers_names.end(), + following_ctrl_it->info.name) != + controller_it->info.fallback_controllers_names.end()) + { + const auto exported_state_itfs = + resource_manager_->get_controller_exported_state_interface_names( + following_ctrl_it->info.name); + if ( + std::find(exported_state_itfs.begin(), exported_state_itfs.end(), fb_state_itf) == + exported_state_itfs.end()) + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as the state interface : " + "'%s' required by its fallback controller : '%s' is not exported by the " + "controller : '%s' in the current fallback list!", + controller_it->info.name.c_str(), fb_state_itf.c_str(), fb_ctrl.c_str(), + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as the state interface : " + "'%s' required by its fallback controller : '%s' is not available as the " + "controller is not in active state!. May be consider adding this controller to " + "the fallback list of the controller : '%s' or already have it activated.", + controller_it->info.name.c_str(), fb_state_itf.c_str(), fb_ctrl.c_str(), + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as not all of its fallback " + "controller's : '%s' state interfaces are currently available!", + controller_it->info.name.c_str(), fb_ctrl.c_str()); + return controller_interface::return_type::ERROR; + } + } + } + } + } + return controller_interface::return_type::OK; +} + void ControllerManager::controller_activity_diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 7b9af6ecfc..ac89239e09 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -137,6 +137,11 @@ std::vector TestController::get_state_interface_data() const return state_intr_data; } +void TestController::set_external_commands_for_testing(const std::vector & commands) +{ + external_commands_for_testing_ = commands; +} + } // namespace test_controller #include "pluginlib/class_list_macros.hpp" diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index 4e5f827cd0..d57fd9ddd9 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -70,6 +70,8 @@ class TestController : public controller_interface::ControllerInterface const std::string & getRobotDescription() const; + void set_external_commands_for_testing(const std::vector & commands); + unsigned int internal_counter = 0; bool simulate_cleanup_failure = false; // Variable where we store when cleanup was called, pointer because the controller diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 3bf56dd900..a016b20440 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -20,6 +20,7 @@ #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::_; @@ -531,3 +532,921 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) INSTANTIATE_TEST_SUITE_P( per_controller_update_rate_check, TestControllerUpdateRates, testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 98)); + +class TestControllerManagerFallbackControllers +: public ControllerManagerFixture, + public testing::WithParamInterface +{ +}; + +TEST_F(TestControllerManagerFallbackControllers, test_failure_on_same_controller_in_fallback_list) +{ + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + + const std::vector fallback_controllers = {test_controller_1_name, "random_ctrl2"}; + rclcpp::Parameter fallback_ctrls_parameter( + test_controller_1_name + std::string(".fallback_controllers"), fallback_controllers); + cm_->set_parameter(fallback_ctrls_parameter); + { + ControllerManagerRunner cm_runner(this); + ASSERT_EQ( + nullptr, + cm_->load_controller(test_controller_1_name, test_controller::TEST_CONTROLLER_CLASS_NAME)); + } +} + +TEST_F(TestControllerManagerFallbackControllers, test_failure_on_fallback_controller_not_configured) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + 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); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + 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); + cm_->add_controller(controller_spec); // add controller_2 + } + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller_1_name); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); +} + +TEST_F(TestControllerManagerFallbackControllers, test_fallback_controllers_activation_simple_case) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + 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); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + 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); + cm_->add_controller(controller_spec); // add controller_2 + } + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller_1_name); + cm_->configure_controller(test_controller_2_name); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + test_controller_1->set_external_commands_for_testing({std::numeric_limits::quiet_NaN()}); + EXPECT_EQ( + controller_interface::return_type::ERROR, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); +} + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_failed_activation_on_missing_command_interface) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + auto test_controller_2 = std::make_shared(); + cmd_itfs_cfg.names = {"random_non_existing_interface/position"}; + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + 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); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + 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); + cm_->add_controller(controller_spec); // add controller_2 + } + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller_1_name); + cm_->configure_controller(test_controller_2_name); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); +} + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_failed_activation_on_missing_state_interface) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + auto test_controller_2 = std::make_shared(); + controller_interface::InterfaceConfiguration state_itfs_cfg; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_itfs_cfg.names = {"non_existing_state_interface/position"}; + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_2->set_state_interface_configuration(state_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + 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); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + 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); + cm_->add_controller(controller_spec); // add controller_2 + } + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller_1_name); + cm_->configure_controller(test_controller_2_name); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); +} + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_valid_activation_if_one_or_more_fallback_controllers_are_already_active) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + // controller 1 + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + // controller 2 + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + // controller 3 + auto test_controller_3 = std::make_shared(); + controller_interface::InterfaceConfiguration itfs_cfg; + itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + itfs_cfg.names = {"joint2/velocity"}; + test_controller_3->set_command_interface_configuration(itfs_cfg); + test_controller_3->set_state_interface_configuration(itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + const std::string test_controller_3_name = "test_controller_3"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + 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, test_controller_3_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + 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); + 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); + cm_->add_controller(controller_spec); // add controller_3 + } + + EXPECT_EQ(3u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ(2, test_controller_3.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_3->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_2_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_3_name)); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + + /// @note Here the order is important do not change the starting order + for (const auto & start_controller : {test_controller_3_name, test_controller_1_name}) + { + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {start_controller}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_3->get_lifecycle_state().id()); + + test_controller_1->set_external_commands_for_testing({std::numeric_limits::quiet_NaN()}); + EXPECT_EQ( + controller_interface::return_type::ERROR, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_3->get_lifecycle_state().id()); +} + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_with_chainable_controllers_multiple_checks) +{ + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + const std::string test_controller_3_name = "test_controller_3"; + const std::string test_controller_4_name = "test_chainable_controller_2"; + + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + // controller 1 + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + // controller 2 + cmd_itfs_cfg.names = {test_controller_4_name + "/joint1/position"}; + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + // controller 3 + auto test_controller_3 = std::make_shared(); + controller_interface::InterfaceConfiguration itfs_cfg; + itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint2/velocity"}; + itfs_cfg.names = {test_controller_4_name + "/joint2/velocity"}; + test_controller_3->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_3->set_state_interface_configuration(itfs_cfg); + + // controller 4 + auto test_controller_4 = std::make_shared(); + cmd_itfs_cfg.names = {"joint1/position"}; + itfs_cfg.names = {"joint2/velocity"}; + test_controller_4->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_4->set_state_interface_configuration(itfs_cfg); + test_controller_4->set_reference_interface_names({"joint1/position"}); + test_controller_4->set_exported_state_interface_names({"joint2/velocity"}); + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + 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, test_controller_3_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + 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); + 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); + 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); + cm_->add_controller(controller_spec); // add controller_4 + } + + EXPECT_EQ(4u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ(2, test_controller_3.use_count()); + EXPECT_EQ(2, test_controller_4.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_4->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_2_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_3_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_4_name)); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_4->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } + + // Now unload the controller_1 and set it up again with test_controller_3 alone and it should fail + // as it doesn't have the chained state interface + { + ControllerManagerRunner cm_runner(this); + cm_->unload_controller(test_controller_1_name); + + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + 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); + cm_->add_controller(controller_spec); // add controller_1 + + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + + switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } + + // Now unload the controller_1 and set it up again with test_controller_2, test_controller_3 and + // test_controller_4, and now it should work as all the controllers are in the list + { + ControllerManagerRunner cm_runner(this); + cm_->unload_controller(test_controller_1_name); + + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + // It is expected to place all the chainable interfaces first, so they can make the interfaces + // 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); + cm_->add_controller(controller_spec); // add controller_1 + + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + + switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_4->get_lifecycle_state().id()); + + test_controller_1->set_external_commands_for_testing( + {std::numeric_limits::quiet_NaN()}); + EXPECT_EQ( + controller_interface::return_type::ERROR, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_4->get_lifecycle_state().id()); + } +} + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_with_chainable_controllers_other_failing_checks) +{ + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + const std::string test_controller_3_name = "test_controller_3"; + const std::string test_controller_4_name = "test_chainable_controller_2"; + + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + // controller 1 + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + // controller 2 + cmd_itfs_cfg.names = {test_controller_4_name + "/joint1/position"}; + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + // controller 3 + auto test_controller_3 = std::make_shared(); + controller_interface::InterfaceConfiguration itfs_cfg; + itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint2/velocity"}; + itfs_cfg.names = {test_controller_4_name + "/joint2/velocity"}; + test_controller_3->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_3->set_state_interface_configuration(itfs_cfg); + + // controller 4 + auto test_controller_4 = std::make_shared(); + cmd_itfs_cfg.names = {"joint1/position"}; + itfs_cfg.names = {"joint2/velocity"}; + test_controller_4->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_4->set_state_interface_configuration(itfs_cfg); + test_controller_4->set_reference_interface_names({"modified_joint1/position"}); + test_controller_4->set_exported_state_interface_names({"modified_joint2/velocity"}); + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + 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, test_controller_4_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + 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); + 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); + 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); + cm_->add_controller(controller_spec); // add controller_4 + } + + EXPECT_EQ(4u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ(2, test_controller_3.use_count()); + EXPECT_EQ(2, test_controller_4.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_4->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_2_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_3_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_4_name)); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_4->get_lifecycle_state().id()); + + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + { + // Now the controller_1 is set with test_controller_2 and test_controller_4_name and it should + // fail as it has an non existing state interface Start controller, will take effect at the end + // of the update function + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } + } + + // Now unload the controller_1 and set it up again with test_controller_3 and + // test_controller_4_name and it should fail as it has an non existing state interface + { + ControllerManagerRunner cm_runner(this); + cm_->unload_controller(test_controller_1_name); + cm_->unload_controller(test_controller_4_name); + + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + 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, test_controller_4_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_1 + + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } +} diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 7d28b4390d..7fa7ae7f29 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -26,6 +26,7 @@ For details see the controller_manager section. * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. * The ``assign_interfaces`` and ``release_interfaces`` methods are now virtual, so that the user can override them to store the interfaces into custom variable types, so that the user can have the flexibility to take the ownership of the loaned interfaces to the controller (`#1743 `_) * The new ``PoseSensor`` semantic component provides a standard interface for hardware providing cartesian poses (`#1775 `_) +* The controllers now support the fallback controllers (a list of controllers that will be activated, when the spawned controllers fails by throwing an exception or returning ``return_type::ERROR`` during the ``update`` cycle) (`#1789 `_) controller_manager ****************** From 83fff77e15631ff3982d427d16150c73eb8764bd Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 17 Oct 2024 11:40:10 +0200 Subject: [PATCH 05/40] [Spawner] Add support for wildcard entries in the controller param files (#1724) --- .../controller_manager_services.py | 61 +++++-- controller_manager/doc/userdoc.rst | 54 ++++++ .../test_controller_spawner_with_type.yaml | 29 +++- .../test/test_spawner_unspawner.cpp | 160 +++++++++++++++--- doc/release_notes.rst | 1 + 5 files changed, 256 insertions(+), 49 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index a1d3c0f5ad..102531f8e8 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -253,24 +253,57 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti ) -def get_parameter_from_param_file(controller_name, namespace, parameter_file, parameter_name): +def get_parameter_from_param_file( + node, controller_name, namespace, parameter_file, parameter_name +): with open(parameter_file) as f: namespaced_controller = ( - controller_name if namespace == "/" else f"{namespace}/{controller_name}" + f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}" ) + WILDCARD_KEY = "/**" + ROS_PARAMS_KEY = "ros__parameters" parameters = yaml.safe_load(f) - if namespaced_controller in parameters: - value = parameters[namespaced_controller] - if not isinstance(value, dict) or "ros__parameters" not in value: + controller_param_dict = None + # check for the parameter in 'controller_name' or 'namespaced_controller' or '/**/namespaced_controller' or '/**/controller_name' + for key in [ + controller_name, + namespaced_controller, + f"{WILDCARD_KEY}/{controller_name}", + f"{WILDCARD_KEY}{namespaced_controller}", + ]: + if key in parameters: + if key == controller_name and namespace != "/": + node.get_logger().fatal( + f"{bcolors.FAIL}Missing namespace : {namespace} or wildcard in parameter file for controller : {controller_name}{bcolors.ENDC}" + ) + break + controller_param_dict = parameters[key] + + if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]: + controller_param_dict = parameters[WILDCARD_KEY][key] + + if controller_param_dict and ( + not isinstance(controller_param_dict, dict) + or ROS_PARAMS_KEY not in controller_param_dict + ): raise RuntimeError( - f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}" + f"YAML file : {parameter_file} is not a valid ROS parameter file for controller node : {namespaced_controller}" ) - if parameter_name in parameters[namespaced_controller]["ros__parameters"]: - return parameters[namespaced_controller]["ros__parameters"][parameter_name] - else: - return None - else: - return None + if ( + controller_param_dict + and ROS_PARAMS_KEY in controller_param_dict + and parameter_name in controller_param_dict[ROS_PARAMS_KEY] + ): + break + + if controller_param_dict is None: + node.get_logger().fatal( + f"{bcolors.FAIL}Controller : {namespaced_controller} parameters not found in parameter file : {parameter_file}{bcolors.ENDC}" + ) + if parameter_name in controller_param_dict[ROS_PARAMS_KEY]: + return controller_param_dict[ROS_PARAMS_KEY][parameter_name] + + return None def set_controller_parameters( @@ -324,7 +357,7 @@ def set_controller_parameters_from_param_file( ) controller_type = get_parameter_from_param_file( - controller_name, spawner_namespace, parameter_file, "type" + node, controller_name, spawner_namespace, parameter_file, "type" ) if controller_type: if not set_controller_parameters( @@ -333,7 +366,7 @@ def set_controller_parameters_from_param_file( return False fallback_controllers = get_parameter_from_param_file( - controller_name, spawner_namespace, parameter_file, "fallback_controllers" + node, controller_name, spawner_namespace, parameter_file, "fallback_controllers" ) if fallback_controllers: if not set_controller_parameters( diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index d3dd5e90c8..312e7c68ef 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -183,6 +183,60 @@ There are two scripts to interact with controller manager from launch files: param file +The parsed controller config file can follow the same conventions as the typical ROS 2 parameter file format. Now, the spawner can handle config files with wildcard entries and also the controller name in the absolute namespace. See the following examples on the config files: + + .. code-block:: yaml + + /**/position_trajectory_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 + + command_interfaces: + - position + ..... + + .. code-block:: yaml + + /position_trajectory_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 + + command_interfaces: + - position + ..... + + .. code-block:: yaml + + position_trajectory_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 + + command_interfaces: + - position + ..... + + .. code-block:: yaml + + /rrbot_1/position_trajectory_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 + + command_interfaces: + - position + ..... + ``unspawner`` ^^^^^^^^^^^^^^^^ diff --git a/controller_manager/test/test_controller_spawner_with_type.yaml b/controller_manager/test/test_controller_spawner_with_type.yaml index 892427bab7..087994bd23 100644 --- a/controller_manager/test/test_controller_spawner_with_type.yaml +++ b/controller_manager/test/test_controller_spawner_with_type.yaml @@ -3,25 +3,40 @@ ctrl_with_parameters_and_type: type: "controller_manager/test_controller" joint_names: ["joint0"] -chainable_ctrl_with_parameters_and_type: - ros__parameters: - type: "controller_manager/test_chainable_controller" - joint_names: ["joint1"] +/**: + chainable_ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_chainable_controller" + joint_names: ["joint1"] + + wildcard_ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_controller" + joint_names: ["joint1"] ctrl_with_parameters_and_no_type: ros__parameters: joint_names: ["joint2"] -/foo_namespace/ctrl_with_parameters_and_type: +/foo_namespace/ns_ctrl_with_parameters_and_type: ros__parameters: type: "controller_manager/test_controller" joint_names: ["joint1"] -/foo_namespace/chainable_ctrl_with_parameters_and_type: +/foo_namespace/ns_chainable_ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_chainable_controller" + joint_names: ["joint1"] + +/foo_namespace/ns_ctrl_with_parameters_and_no_type: + ros__parameters: + joint_names: ["joint2"] + +/**/wildcard_chainable_ctrl_with_parameters_and_type: ros__parameters: type: "controller_manager/test_chainable_controller" joint_names: ["joint1"] -/foo_namespace/ctrl_with_parameters_and_no_type: +/**/wildcard_ctrl_with_parameters_and_no_type: ros__parameters: joint_names: ["joint2"] diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index cba832f8bf..74e1efeeed 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -674,14 +674,14 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) // Provide controller type via the parsed file EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "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 " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "test_controller_manager -p " + test_file_path + " --ros-args -r __ns:=/foo_namespace"), 0); @@ -689,17 +689,18 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ns_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), + test_file_path); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; ASSERT_EQ( - chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type"); + chain_ctrl_with_parameters_and_type.info.name, "ns_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ( chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); @@ -707,12 +708,12 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), + cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), test_file_path); EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path + + "ns_ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path + " --ros-args -r __ns:=/foo_namespace"), 256) << "Should fail as no type is defined!"; @@ -720,21 +721,18 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); auto ctrl_1 = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_1.info.name, "ns_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ( - cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + ASSERT_EQ(cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string(), test_file_path); auto ctrl_2 = cm_->get_loaded_controllers()[1]; - ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_2.info.name, "ns_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ( - cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), - test_file_path); + ASSERT_EQ(cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string(), test_file_path); } TEST_F( @@ -747,28 +745,28 @@ TEST_F( // Provide controller type via the parsed file EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "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 " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "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 " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "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"; EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "test_controller_manager --namespace foo_namespace -p " + test_file_path), 0) @@ -777,17 +775,18 @@ TEST_F( ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ns_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), + test_file_path); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; ASSERT_EQ( - chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type"); + chain_ctrl_with_parameters_and_type.info.name, "ns_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ( chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); @@ -795,12 +794,13 @@ TEST_F( chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), + cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), test_file_path); EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_no_type -c test_controller_manager --namespace foo_namespace -p " + + "ns_ctrl_with_parameters_and_no_type -c test_controller_manager --namespace foo_namespace " + "-p " + test_file_path), 256) << "Should fail as no type is defined!"; @@ -808,19 +808,123 @@ TEST_F( ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); auto ctrl_1 = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_1.info.name, "ns_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ(cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string(), test_file_path); + + auto ctrl_2 = cm_->get_loaded_controllers()[1]; + ASSERT_EQ(ctrl_2.info.name, "ns_chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( - cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ(cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string(), test_file_path); +} + +TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_with_wildcard_entries_in_params_file) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "wildcard_ctrl_with_parameters_and_type wildcard_chainable_ctrl_with_parameters_and_type " + "--load-only -c " + "test_controller_manager --controller-manager-timeout 1.0 -p " + + test_file_path), + 256) + << "Should fail without the namespacing it due to timeout but can find the parameters"; + EXPECT_EQ( + call_spawner( + "wildcard_ctrl_with_parameters_and_type wildcard_chainable_ctrl_with_parameters_and_type " + "--load-only -c " + "test_controller_manager -p " + + test_file_path + " --ros-args -r __ns:=/foo_namespace"), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "wildcard_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.name, + "wildcard_chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.type, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + EXPECT_EQ( + call_spawner( + "wildcard_ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path + + " --ros-args -r __ns:=/foo_namespace"), + 256) + << "Should fail as no type is defined!"; + // Will still be same as the current call will fail + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "wildcard_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_2 = cm_->get_loaded_controllers()[1]; - ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_2.info.name, "wildcard_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +} + +TEST_F( + TestLoadControllerWithNamespacedCM, + spawner_test_fail_namespaced_controllers_with_non_wildcard_entries) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type --load-only -c " + "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 --load-only -c " + "test_controller_manager --namespace foo_namespace -p " + + test_file_path), + 256) + << "Should fail even namespacing it as ctrl_with_parameters_and_type is not a wildcard entry"; + EXPECT_EQ( + call_spawner( + "chainable_ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager --namespace foo_namespace -p " + + test_file_path), + 0) + << "Should work as chainable_ctrl_with_parameters_and_type is a wildcard entry"; + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type"); ASSERT_EQ( - cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), - test_file_path); + ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 7fa7ae7f29..611e8d8176 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -75,6 +75,7 @@ controller_manager * The support for the ``description`` parameter for loading the URDF was removed (`#1358 `_). * 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 `_). hardware_interface ****************** From d55def1923fe15b3b7a6e6dd3ffb3a90cfe67997 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 17 Oct 2024 17:03:10 +0200 Subject: [PATCH 06/40] [CM] Async Function Handler for Controllers (#1489) --------- Co-authored-by: Jordan Palacios Co-authored-by: atzaros <128592691+atzaros@users.noreply.github.com> Co-authored-by: Bence Magyar Co-authored-by: Dr. Denis --- controller_interface/CMakeLists.txt | 1 + .../controller_interface/async_controller.hpp | 112 ----------- .../controller_interface_base.hpp | 45 +++++ controller_interface/package.xml | 4 + .../src/controller_interface_base.cpp | 56 +++++- .../controller_manager/controller_manager.hpp | 4 - controller_manager/src/controller_manager.cpp | 51 ++--- .../test/test_controller/test_controller.cpp | 4 + .../test/test_controller_manager.cpp | 185 ++++++++++++++++++ 9 files changed, 320 insertions(+), 142 deletions(-) delete mode 100644 controller_interface/include/controller_interface/async_controller.hpp diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index cad5810ee5..85294c68d1 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -9,6 +9,7 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS hardware_interface rclcpp_lifecycle + realtime_tools ) find_package(ament_cmake REQUIRED) diff --git a/controller_interface/include/controller_interface/async_controller.hpp b/controller_interface/include/controller_interface/async_controller.hpp deleted file mode 100644 index 357b3a2ce3..0000000000 --- a/controller_interface/include/controller_interface/async_controller.hpp +++ /dev/null @@ -1,112 +0,0 @@ -// Copyright 2024 ros2_control development team -// -// 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. - -#ifndef CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_ -#define CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_ - -#include -#include -#include - -#include "controller_interface_base.hpp" -#include "lifecycle_msgs/msg/state.hpp" - -namespace controller_interface -{ - -class AsyncControllerThread -{ -public: - /// Constructor for the AsyncControllerThread object. - /** - * - * \param[in] controller shared pointer to a controller. - * \param[in] cm_update_rate the controller manager's update rate. - */ - AsyncControllerThread( - std::shared_ptr & controller, int cm_update_rate) - : terminated_(false), controller_(controller), thread_{}, cm_update_rate_(cm_update_rate) - { - } - - AsyncControllerThread(const AsyncControllerThread & t) = delete; - AsyncControllerThread(AsyncControllerThread && t) = delete; - - // Destructor, called when the component is erased from its map. - ~AsyncControllerThread() - { - terminated_.store(true, std::memory_order_seq_cst); - if (thread_.joinable()) - { - thread_.join(); - } - } - - /// Creates the controller's thread. - /** - * Called when the controller is activated. - * - */ - void activate() - { - thread_ = std::thread(&AsyncControllerThread::controller_update_callback, this); - } - - /// Periodically execute the controller's update method. - /** - * Callback of the async controller's thread. - * **Not synchronized with the controller manager's write and read currently** - * - */ - void controller_update_callback() - { - using TimePoint = std::chrono::system_clock::time_point; - unsigned int used_update_rate = - controller_->get_update_rate() == 0 ? cm_update_rate_ : controller_->get_update_rate(); - - auto previous_time = controller_->get_node()->now(); - while (!terminated_.load(std::memory_order_relaxed)) - { - auto const period = std::chrono::nanoseconds(1'000'000'000 / used_update_rate); - TimePoint next_iteration_time = - TimePoint(std::chrono::nanoseconds(controller_->get_node()->now().nanoseconds())); - - if ( - controller_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - auto const current_time = controller_->get_node()->now(); - auto const measured_period = current_time - previous_time; - previous_time = current_time; - controller_->update( - controller_->get_node()->now(), - (controller_->get_update_rate() != cm_update_rate_ && controller_->get_update_rate() != 0) - ? rclcpp::Duration::from_seconds(1.0 / controller_->get_update_rate()) - : measured_period); - } - - next_iteration_time += period; - std::this_thread::sleep_until(next_iteration_time); - } - } - -private: - std::atomic terminated_; - std::shared_ptr controller_; - std::thread thread_; - unsigned int cm_update_rate_; -}; - -} // namespace controller_interface - -#endif // CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_ diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 2bd01cc326..211716f301 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -17,9 +17,11 @@ #include #include +#include #include #include "controller_interface/visibility_control.h" +#include "realtime_tools/async_function_handler.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/loaned_command_interface.hpp" @@ -58,6 +60,17 @@ struct InterfaceConfiguration std::vector names = {}; }; +struct ControllerUpdateStats +{ + void reset() + { + total_triggers = 0; + failed_triggers = 0; + } + + unsigned int total_triggers; + unsigned int failed_triggers; +}; /** * Base interface class for an controller. The interface may not be used to implement a controller. * The class provides definitions for `ControllerInterface` and `ChainableControllerInterface` @@ -153,6 +166,23 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; + /** + * Trigger update method. This method is used by the controller_manager to trigger the update + * method of the controller. + * The method is used to trigger the update method of the controller synchronously or + * asynchronously, based on the controller configuration. + * **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 + * \returns A pair with the first element being a boolean indicating if the async callback method + * was triggered and the second element being the last return value of the async function. For + * more details check the AsyncFunctionHandler implementation in `realtime_tools` package. + */ + CONTROLLER_INTERFACE_PUBLIC + std::pair trigger_update( + const rclcpp::Time & time, const rclcpp::Duration & period); + CONTROLLER_INTERFACE_PUBLIC std::shared_ptr get_node(); @@ -270,15 +300,30 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual bool is_in_chained_mode() const = 0; + /** + * Method to wait for any running async update cycle to finish after finishing the current cycle. + * This is needed to be called before deactivating the controller by the controller_manager, so + * that the interfaces still exist when the controller finishes its cycle and then it's exits. + * + * \note **The method is not real-time safe and shouldn't be called in the control loop.** + * + * If the controller is running in async mode, the method will wait for the current async update + * to finish. If the controller is not running in async mode, the method will do nothing. + */ + CONTROLLER_INTERFACE_PUBLIC + void wait_for_trigger_update_to_finish(); + protected: std::vector command_interfaces_; std::vector state_interfaces_; private: std::shared_ptr node_; + std::unique_ptr> async_handler_; unsigned int update_rate_ = 0; bool is_async_ = false; std::string urdf_ = ""; + ControllerUpdateStats trigger_stats_; }; using ControllerInterfaceBaseSharedPtr = std::shared_ptr; diff --git a/controller_interface/package.xml b/controller_interface/package.xml index dce1d79f86..09955c46c9 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -12,12 +12,16 @@ ament_cmake_gen_version_h hardware_interface + realtime_tools rclcpp_lifecycle sensor_msgs hardware_interface + realtime_tools rclcpp_lifecycle + realtime_tools + ament_cmake_gmock geometry_msgs sensor_msgs diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index e2c1fa480a..a6e0da988f 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -16,7 +16,6 @@ #include #include -#include #include #include "lifecycle_msgs/msg/state.hpp" @@ -37,6 +36,7 @@ return_type ControllerInterfaceBase::init( { auto_declare("update_rate", update_rate_); auto_declare("is_async", false); + auto_declare("thread_priority", 50); } catch (const std::exception & e) { @@ -57,7 +57,14 @@ return_type ControllerInterfaceBase::init( std::bind(&ControllerInterfaceBase::on_configure, this, std::placeholders::_1)); node_->register_on_cleanup( - std::bind(&ControllerInterfaceBase::on_cleanup, this, std::placeholders::_1)); + [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn + { + if (is_async() && async_handler_ && async_handler_->is_running()) + { + async_handler_->stop_thread(); + } + return on_cleanup(previous_state); + }); node_->register_on_activate( std::bind(&ControllerInterfaceBase::on_activate, this, std::placeholders::_1)); @@ -106,6 +113,21 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() } is_async_ = get_node()->get_parameter("is_async").as_bool(); } + if (is_async_) + { + const unsigned int thread_priority = + static_cast(get_node()->get_parameter("thread_priority").as_int()); + RCLCPP_INFO( + get_node()->get_logger(), "Starting async handler with scheduler priority: %d", + thread_priority); + async_handler_ = std::make_unique>(); + async_handler_->init( + std::bind( + &ControllerInterfaceBase::update, this, std::placeholders::_1, std::placeholders::_2), + thread_priority); + async_handler_->start_thread(); + } + trigger_stats_.reset(); return get_node()->configure(); } @@ -129,6 +151,29 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() c return node_->get_current_state(); } +std::pair ControllerInterfaceBase::trigger_update( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + trigger_stats_.total_triggers++; + if (is_async()) + { + const auto result = async_handler_->trigger_async_callback(time, period); + if (!result.first) + { + trigger_stats_.failed_triggers++; + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 20000, + "The controller missed %u update cycles out of %u total triggers.", + trigger_stats_.failed_triggers, trigger_stats_.total_triggers); + } + return result; + } + else + { + return std::make_pair(true, update(time, period)); + } +} + std::shared_ptr ControllerInterfaceBase::get_node() { if (!node_.get()) @@ -153,4 +198,11 @@ bool ControllerInterfaceBase::is_async() const { return is_async_; } const std::string & ControllerInterfaceBase::get_robot_description() const { return urdf_; } +void ControllerInterfaceBase::wait_for_trigger_update_to_finish() +{ + if (is_async() && async_handler_ && async_handler_->is_running()) + { + async_handler_->wait_for_trigger_cycle_to_finish(); + } +} } // namespace controller_interface diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 273b48b022..6c0b4fde9b 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -22,7 +22,6 @@ #include #include -#include "controller_interface/async_controller.hpp" #include "controller_interface/chainable_controller_interface.hpp" #include "controller_interface/controller_interface.hpp" #include "controller_interface/controller_interface_base.hpp" @@ -628,9 +627,6 @@ class ControllerManager : public rclcpp::Node }; SwitchParams switch_params_; - - std::unordered_map> - async_controller_threads_; }; } // namespace controller_manager diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 72cebaa1e5..a8668f7f1b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -649,13 +649,6 @@ controller_interface::return_type ControllerManager::unload_controller( controller_name.c_str()); return controller_interface::return_type::ERROR; } - if (controller.c->is_async()) - { - RCLCPP_DEBUG( - get_logger(), "Removing controller '%s' from the list of async controllers", - controller_name.c_str()); - async_controller_threads_.erase(controller_name); - } RCLCPP_DEBUG(get_logger(), "Cleanup controller"); controller_chain_spec_cleanup(controller_chain_spec_, controller_name); @@ -798,14 +791,6 @@ controller_interface::return_type ControllerManager::configure_controller( return controller_interface::return_type::ERROR; } - // ASYNCHRONOUS CONTROLLERS: Start background thread for update - if (controller->is_async()) - { - async_controller_threads_.emplace( - controller_name, - std::make_unique(controller, update_rate_)); - } - const auto controller_update_rate = controller->get_update_rate(); const auto cm_update_rate = get_update_rate(); if (controller_update_rate > cm_update_rate) @@ -1393,6 +1378,18 @@ controller_interface::return_type ControllerManager::switch_controller( RCLCPP_DEBUG(get_logger(), " - %s", interface.c_str()); } + // wait for deactivating async controllers to finish their current cycle + for (const auto & controller : deactivate_request_) + { + auto controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller)); + if (controller_it != controllers.end()) + { + controller_it->c->wait_for_trigger_update_to_finish(); + } + } + if ( !activate_command_interface_request_.empty() || !deactivate_command_interface_request_.empty()) { @@ -1798,11 +1795,6 @@ void ControllerManager::activate_controllers( resource_manager_->make_controller_exported_state_interfaces_available(controller_name); resource_manager_->make_controller_reference_interfaces_available(controller_name); } - - if (controller->is_async()) - { - async_controller_threads_.at(controller_name)->activate(); - } } } @@ -2343,8 +2335,19 @@ controller_interface::return_type ControllerManager::update( { // TODO(v-lopez) we could cache this information // https://github.com/ros-controls/ros2_control/issues/153 - if (!loaded_controller.c->is_async() && is_controller_active(*loaded_controller.c)) + if (is_controller_active(*loaded_controller.c)) { + if ( + switch_params_.do_switch && loaded_controller.c->is_async() && + std::find( + deactivate_request_.begin(), deactivate_request_.end(), loaded_controller.info.name) != + deactivate_request_.end()) + { + RCLCPP_DEBUG( + get_logger(), "Skipping update for async controller '%s' as it is being deactivated", + loaded_controller.info.name.c_str()); + continue; + } const auto controller_update_rate = loaded_controller.c->get_update_rate(); const bool run_controller_at_cm_rate = (controller_update_rate >= update_rate_); const auto controller_period = @@ -2377,10 +2380,12 @@ controller_interface::return_type ControllerManager::update( 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 try { - controller_ret = loaded_controller.c->update(time, controller_actual_period); + std::tie(trigger_status, controller_ret) = + loaded_controller.c->trigger_update(time, controller_actual_period); } catch (const std::exception & e) { @@ -2601,8 +2606,6 @@ unsigned int ControllerManager::get_update_rate() const { return update_rate_; } void ControllerManager::shutdown_async_controllers_and_components() { - async_controller_threads_.erase( - async_controller_threads_.begin(), async_controller_threads_.end()); resource_manager_->shutdown_async_components(); } diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index ac89239e09..04ae8c02c2 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -61,6 +61,10 @@ controller_interface::InterfaceConfiguration TestController::state_interface_con controller_interface::return_type TestController::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { + if (is_async()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1000 / (2 * get_update_rate()))); + } update_period_ = period; ++internal_counter; diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index a016b20440..8ff844adc2 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -233,6 +233,191 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ(1, test_controller.use_count()); } +TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle) +{ + const auto test_param = GetParam(); + auto test_controller = std::make_shared(); + auto test_controller2 = std::make_shared(); + constexpr char TEST_CONTROLLER2_NAME[] = "test_controller2_name"; + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller2, TEST_CONTROLLER2_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller.use_count()); + + // setup interface to claim from controllers + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + cmd_itfs_cfg.names.push_back(interface); + } + test_controller->set_command_interface_configuration(cmd_itfs_cfg); + + controller_interface::InterfaceConfiguration state_itfs_cfg; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + test_controller->set_state_interface_configuration(state_itfs_cfg); + + controller_interface::InterfaceConfiguration cmd_itfs_cfg2; + cmd_itfs_cfg2.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES) + { + cmd_itfs_cfg2.names.push_back(interface); + } + test_controller2->set_command_interface_configuration(cmd_itfs_cfg2); + + controller_interface::InterfaceConfiguration state_itfs_cfg2; + state_itfs_cfg2.type = controller_interface::interface_configuration_type::ALL; + test_controller2->set_state_interface_configuration(state_itfs_cfg2); + + // Check if namespace is set correctly + RCLCPP_INFO( + rclcpp::get_logger("test_controller_manager"), "Controller Manager namespace is '%s'", + cm_->get_namespace()); + EXPECT_STREQ(cm_->get_namespace(), "/"); + RCLCPP_INFO( + rclcpp::get_logger("test_controller_manager"), "Controller 1 namespace is '%s'", + test_controller->get_node()->get_namespace()); + EXPECT_STREQ(test_controller->get_node()->get_namespace(), "/"); + RCLCPP_INFO( + rclcpp::get_logger("test_controller_manager"), "Controller 2 namespace is '%s'", + test_controller2->get_node()->get_namespace()); + EXPECT_STREQ(test_controller2->get_node()->get_namespace(), "/"); + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller->internal_counter) + << "Update should not reach an unconfigured controller"; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); + + // configure controller + rclcpp::Parameter update_rate_parameter("update_rate", static_cast(20)); + rclcpp::Parameter is_async_parameter("is_async", rclcpp::ParameterValue(true)); + test_controller->get_node()->set_parameter(update_rate_parameter); + test_controller->get_node()->set_parameter(is_async_parameter); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + cm_->configure_controller(TEST_CONTROLLER2_NAME); + } + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; + EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is not started"; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {"fake_controller", TEST_CONTROLLER2_NAME}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0)); + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is started at the end of update"; + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(test_param.expected_return, switch_future.get()); + } + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_GE(test_controller2->internal_counter, test_param.expected_counter); + + // Start the real test controller, will take effect at the end of the update function + start_controllers = {test_controller::TEST_CONTROLLER_NAME}; + stop_controllers = {}; + switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(test_controller->internal_counter, 0u); + std::this_thread::sleep_for( + std::chrono::milliseconds(1000 / (test_controller->get_update_rate()))); + EXPECT_EQ(test_controller->internal_counter, 1u); + size_t last_internal_counter = test_controller->internal_counter; + + // Stop controller, will take effect at the end of the update function + start_controllers = {}; + stop_controllers = {test_controller::TEST_CONTROLLER_NAME}; + switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(last_internal_counter, test_controller->internal_counter) + << "This shouldn't have updated as this is async and in the controller it is waiting before " + "updating the counter"; + std::this_thread::sleep_for( + std::chrono::milliseconds(1000 / (test_controller->get_update_rate()))); + EXPECT_EQ(last_internal_counter, test_controller->internal_counter) + << "Controller is stopped at the end of update, so it should have done one more update"; + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + auto unload_future = std::async( + std::launch::async, &controller_manager::ControllerManager::unload_controller, cm_, + test_controller::TEST_CONTROLLER_NAME); + + ASSERT_EQ(std::future_status::timeout, unload_future.wait_for(std::chrono::milliseconds(100))) + << "unload_controller should be blocking until next update cycle"; + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, unload_future.get()); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); + EXPECT_EQ(1, test_controller.use_count()); +} + TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) { auto strictness = GetParam().strictness; From cb91599f8f66aaf39b7485a2f7e131157f633474 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Fri, 25 Oct 2024 15:18:23 +0200 Subject: [PATCH 07/40] 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 08/40] 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 09/40] 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 10/40] 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 11/40] 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 12/40] 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 13/40] 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 14/40] 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 15/40] 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 16/40] [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 17/40] [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 18/40] [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( From 033a6bcd5ece6a34140106f2ebadeafa2e932376 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 6 Nov 2024 19:46:51 +0100 Subject: [PATCH 19/40] [HW_IF] Prepare the handles for async operations (#1750) --- .../include/hardware_interface/handle.hpp | 85 +++++++++++++++++-- .../loaned_command_interface.hpp | 85 ++++++++++++++++++- .../loaned_state_interface.hpp | 56 +++++++++++- 3 files changed, 217 insertions(+), 9 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 6fe4f25663..1dfd499c2c 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -17,7 +17,10 @@ #include #include +#include +#include #include +#include #include #include "hardware_interface/hardware_info.hpp" @@ -69,13 +72,57 @@ class Handle { } - Handle(const Handle & other) = default; + Handle(const Handle & other) noexcept + { + std::unique_lock lock(other.handle_mutex_); + std::unique_lock lock_this(handle_mutex_); + prefix_name_ = other.prefix_name_; + interface_name_ = other.interface_name_; + handle_name_ = other.handle_name_; + value_ = other.value_; + value_ptr_ = other.value_ptr_; + } - Handle(Handle && other) = default; + Handle(Handle && other) noexcept + { + std::unique_lock lock(other.handle_mutex_); + std::unique_lock lock_this(handle_mutex_); + prefix_name_ = std::move(other.prefix_name_); + interface_name_ = std::move(other.interface_name_); + handle_name_ = std::move(other.handle_name_); + value_ = std::move(other.value_); + value_ptr_ = std::move(other.value_ptr_); + } - Handle & operator=(const Handle & other) = default; + Handle & operator=(const Handle & other) + { + if (this != &other) + { + std::unique_lock lock(other.handle_mutex_); + std::unique_lock lock_this(handle_mutex_); + prefix_name_ = other.prefix_name_; + interface_name_ = other.interface_name_; + handle_name_ = other.handle_name_; + value_ = other.value_; + value_ptr_ = other.value_ptr_; + } + return *this; + } - Handle & operator=(Handle && other) = default; + Handle & operator=(Handle && other) + { + if (this != &other) + { + std::unique_lock lock(other.handle_mutex_); + std::unique_lock lock_this(handle_mutex_); + prefix_name_ = std::move(other.prefix_name_); + interface_name_ = std::move(other.interface_name_); + handle_name_ = std::move(other.handle_name_); + value_ = std::move(other.value_); + value_ptr_ = std::move(other.value_ptr_); + } + return *this; + } virtual ~Handle() = default; @@ -95,8 +142,14 @@ class Handle const std::string & get_prefix_name() const { return prefix_name_; } + [[deprecated("Use bool get_value(double & value) instead to retrieve the value.")]] double get_value() const { + std::shared_lock lock(handle_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + return std::numeric_limits::quiet_NaN(); + } // BEGIN (Handle export change): for backward compatibility // TODO(Manuel) return value_ if old functionality is removed THROW_ON_NULLPTR(value_ptr_); @@ -104,12 +157,33 @@ class Handle // END } - void set_value(double value) + [[nodiscard]] bool get_value(double & value) const + { + std::shared_lock lock(handle_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + return false; + } + // BEGIN (Handle export change): for backward compatibility + // TODO(Manuel) set value directly if old functionality is removed + THROW_ON_NULLPTR(value_ptr_); + value = *value_ptr_; + return true; + // END + } + + [[nodiscard]] bool set_value(double value) { + std::unique_lock lock(handle_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + return false; + } // BEGIN (Handle export change): for backward compatibility // TODO(Manuel) set value_ directly if old functionality is removed THROW_ON_NULLPTR(this->value_ptr_); *this->value_ptr_ = value; + return true; // END } @@ -122,6 +196,7 @@ class Handle // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed double * value_ptr_; // END + mutable std::shared_mutex handle_mutex_; }; class StateInterface : public Handle diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index aa306870a1..6013dea778 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -16,10 +16,13 @@ #define HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_ #include +#include #include +#include #include #include "hardware_interface/handle.hpp" +#include "rclcpp/logging.hpp" namespace hardware_interface { @@ -51,6 +54,27 @@ class LoanedCommandInterface virtual ~LoanedCommandInterface() { + auto logger = rclcpp::get_logger(command_interface_.get_name()); + RCLCPP_WARN_EXPRESSION( + rclcpp::get_logger(get_name()), + (get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0), + "LoanedCommandInterface %s has %u (%.4f %%) timeouts and %u (~ %.4f %%) missed calls out of " + "%u get_value calls", + get_name().c_str(), get_value_statistics_.timeout_counter, + (get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter, + get_value_statistics_.failed_counter, + (get_value_statistics_.failed_counter * 10.0) / get_value_statistics_.total_counter, + get_value_statistics_.total_counter); + RCLCPP_WARN_EXPRESSION( + rclcpp::get_logger(get_name()), + (set_value_statistics_.failed_counter > 0 || set_value_statistics_.timeout_counter > 0), + "LoanedCommandInterface %s has %u (%.4f %%) timeouts and %u (~ %.4f %%) missed calls out of " + "%u set_value calls", + get_name().c_str(), set_value_statistics_.timeout_counter, + (set_value_statistics_.timeout_counter * 100.0) / set_value_statistics_.total_counter, + set_value_statistics_.failed_counter, + (set_value_statistics_.failed_counter * 10.0) / set_value_statistics_.total_counter, + set_value_statistics_.total_counter); if (deleter_) { deleter_(); @@ -70,13 +94,70 @@ class LoanedCommandInterface const std::string & get_prefix_name() const { return command_interface_.get_prefix_name(); } - void set_value(double val) { command_interface_.set_value(val); } + template + [[nodiscard]] bool set_value(T value, unsigned int max_tries = 10) + { + unsigned int nr_tries = 0; + ++set_value_statistics_.total_counter; + while (!command_interface_.set_value(value)) + { + ++set_value_statistics_.failed_counter; + ++nr_tries; + if (nr_tries == max_tries) + { + ++set_value_statistics_.timeout_counter; + return false; + } + std::this_thread::yield(); + } + return true; + } - double get_value() const { return command_interface_.get_value(); } + double get_value() const + { + double value; + if (get_value(value)) + { + return value; + } + else + { + return std::numeric_limits::quiet_NaN(); + } + } + + template + [[nodiscard]] bool get_value(T & value, unsigned int max_tries = 10) const + { + unsigned int nr_tries = 0; + ++get_value_statistics_.total_counter; + while (!command_interface_.get_value(value)) + { + ++get_value_statistics_.failed_counter; + ++nr_tries; + if (nr_tries == max_tries) + { + ++get_value_statistics_.timeout_counter; + return false; + } + std::this_thread::yield(); + } + return true; + } protected: CommandInterface & command_interface_; Deleter deleter_; + +private: + struct HandleRTStatistics + { + unsigned int total_counter = 0; + unsigned int failed_counter = 0; + unsigned int timeout_counter = 0; + }; + mutable HandleRTStatistics get_value_statistics_; + HandleRTStatistics set_value_statistics_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 96cc3e89df..3ebc8c7ca0 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -16,11 +16,13 @@ #define HARDWARE_INTERFACE__LOANED_STATE_INTERFACE_HPP_ #include +#include #include +#include #include #include "hardware_interface/handle.hpp" - +#include "rclcpp/logging.hpp" namespace hardware_interface { class LoanedStateInterface @@ -56,6 +58,17 @@ class LoanedStateInterface virtual ~LoanedStateInterface() { + auto logger = rclcpp::get_logger(state_interface_.get_name()); + RCLCPP_WARN_EXPRESSION( + logger, + (get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0), + "LoanedStateInterface %s has %u (%.4f %%) timeouts and %u (%.4f %%) missed calls out of %u " + "get_value calls", + state_interface_.get_name().c_str(), get_value_statistics_.timeout_counter, + (get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter, + get_value_statistics_.failed_counter, + (get_value_statistics_.failed_counter * 10.0) / get_value_statistics_.total_counter, + get_value_statistics_.total_counter); if (deleter_) { deleter_(); @@ -75,11 +88,50 @@ class LoanedStateInterface const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); } - double get_value() const { return state_interface_.get_value(); } + double get_value() const + { + double value; + if (get_value(value)) + { + return value; + } + else + { + return std::numeric_limits::quiet_NaN(); + } + } + + template + [[nodiscard]] bool get_value(T & value, unsigned int max_tries = 10) const + { + unsigned int nr_tries = 0; + ++get_value_statistics_.total_counter; + while (!state_interface_.get_value(value)) + { + ++get_value_statistics_.failed_counter; + ++nr_tries; + if (nr_tries == max_tries) + { + ++get_value_statistics_.timeout_counter; + return false; + } + std::this_thread::yield(); + } + return true; + } protected: const StateInterface & state_interface_; Deleter deleter_; + +private: + struct HandleRTStatistics + { + unsigned int total_counter = 0; + unsigned int failed_counter = 0; + unsigned int timeout_counter = 0; + }; + mutable HandleRTStatistics get_value_statistics_; }; } // namespace hardware_interface From d714e8bfe521c653d82e60ec2b47e5a0d5083863 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 6 Nov 2024 20:10:39 +0100 Subject: [PATCH 20/40] [ros2_control_node] Handle simulation environment clocks (#1810) --- controller_manager/src/ros2_control_node.cpp | 14 ++++++++++++-- doc/release_notes.rst | 1 + 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 51e7e83d23..0ed68d9765 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -57,6 +57,9 @@ int main(int argc, char ** argv) auto cm = std::make_shared( executor, manager_node_name, "", cm_node_options); + const bool use_sim_time = cm->get_parameter_or("use_sim_time", false); + rclcpp::Rate rate(cm->get_update_rate(), cm->get_clock()); + const bool lock_memory = cm->get_parameter_or("lock_memory", true); std::string message; if (lock_memory && !realtime_tools::lock_memory(message)) @@ -82,7 +85,7 @@ int main(int argc, char ** argv) thread_priority); std::thread cm_thread( - [cm, thread_priority]() + [cm, thread_priority, use_sim_time, &rate]() { if (!realtime_tools::configure_sched_fifo(thread_priority)) { @@ -123,7 +126,14 @@ int main(int argc, char ** argv) // wait until we hit the end of the period next_iteration_time += period; - std::this_thread::sleep_until(next_iteration_time); + if (use_sim_time) + { + rate.sleep(); + } + else + { + std::this_thread::sleep_until(next_iteration_time); + } } cm->shutdown_async_controllers_and_components(); diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 12ded009dc..b11a1351fe 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 `_). +* ``ros2_control_node`` can now handle the sim time used by different simulators, when ``use_sim_time`` is set to true (`#1810 `_). * 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 `_). From 93a2a68f6ab4110b7e5816bce134437780ba3bf3 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Fri, 8 Nov 2024 00:53:02 +0530 Subject: [PATCH 21/40] Add Support for SDF (#1763) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --------- Signed-off-by: Aarav Gupta <134804732+Amronos@users.noreply.github.com> Co-authored-by: Christoph Fröhlich Co-authored-by: Bence Magyar --- doc/release_notes.rst | 1 + hardware_interface/package.xml | 1 + hardware_interface/src/component_parser.cpp | 21 +- .../test/test_component_parser.cpp | 35 +++ .../ros2_control_test_assets/descriptions.hpp | 219 ++++++++++++++++++ 5 files changed, 273 insertions(+), 4 deletions(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index b11a1351fe..a6ebb16e5f 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -113,6 +113,7 @@ hardware_interface * Added ``get_hardware_info`` method to the hardware components interface to access the ``HardwareInfo`` instead of accessing the variable ``info_`` directly (`#1643 `_) * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. * With (`#1421 `_) a key-value storage is added to InterfaceInfo. This allows to define extra params with per Command-/StateInterface in the ``.ros2_control.xacro`` file. +* With (`#1763 `_) parsing for SDF published to ``robot_description`` topic is now also supported. joint_limits ************ diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 96d593e8f6..05cff62957 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -18,6 +18,7 @@ tinyxml2_vendor joint_limits urdf + sdformat_urdf rcutils rcutils diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index d2ec0f9d53..a49b83b964 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -31,6 +31,8 @@ namespace { constexpr const auto kRobotTag = "robot"; +constexpr const auto kSDFTag = "sdf"; +constexpr const auto kModelTag = "model"; constexpr const auto kROS2ControlTag = "ros2_control"; constexpr const auto kHardwareTag = "hardware"; constexpr const auto kPluginNameTag = "plugin"; @@ -812,15 +814,26 @@ std::vector parse_control_resources_from_urdf(const std::string & "invalid URDF passed in to robot parser: " + std::string(doc.ErrorStr())); } - // Find robot tag + // Find robot or sdf tag const tinyxml2::XMLElement * robot_it = doc.RootElement(); + const tinyxml2::XMLElement * ros2_control_it; - if (std::string(kRobotTag) != robot_it->Name()) + if (std::string(kRobotTag) == robot_it->Name()) { - throw std::runtime_error("the robot tag is not root element in URDF"); + ros2_control_it = robot_it->FirstChildElement(kROS2ControlTag); + } + else if (std::string(kSDFTag) == robot_it->Name()) + { + // find model tag in sdf tag + const tinyxml2::XMLElement * model_it = robot_it->FirstChildElement(kModelTag); + ros2_control_it = model_it->FirstChildElement(kROS2ControlTag); + } + else + { + throw std::runtime_error( + "the robot tag is not root element in URDF or sdf tag is not root element in SDF"); } - const tinyxml2::XMLElement * ros2_control_it = robot_it->FirstChildElement(kROS2ControlTag); if (!ros2_control_it) { throw std::runtime_error("no " + std::string(kROS2ControlTag) + " tag"); diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 20e098b62e..3c24b0cc2a 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -1616,3 +1616,38 @@ TEST_F(TestComponentParser, parse_gpio_command_interface_descriptions_from_hardw EXPECT_EQ(gpio_state_descriptions[1].get_interface_name(), "vacuum"); EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_vacuum/vacuum"); } + +TEST_F(TestComponentParser, successfully_parse_valid_sdf) +{ + std::string sdf_to_test = ros2_control_test_assets::diff_drive_robot_sdf; + const auto control_hardware = parse_control_resources_from_urdf(sdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + const auto hardware_info = control_hardware.front(); + + EXPECT_EQ(hardware_info.name, "GazeboSimSystem"); + EXPECT_EQ(hardware_info.type, "system"); + ASSERT_THAT(hardware_info.group, IsEmpty()); + EXPECT_EQ(hardware_info.hardware_plugin_name, "gz_ros2_control/GazeboSimSystem"); + + ASSERT_THAT(hardware_info.joints, SizeIs(2)); + + EXPECT_EQ(hardware_info.joints[0].name, "left_wheel_joint"); + EXPECT_EQ(hardware_info.joints[0].type, "joint"); + ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "-10"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "10"); + ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(2)); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].name, HW_IF_POSITION); + + EXPECT_EQ(hardware_info.joints[1].name, "right_wheel_joint"); + EXPECT_EQ(hardware_info.joints[1].type, "joint"); + ASSERT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].min, "-10"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].max, "10"); + ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(2)); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[1].name, HW_IF_POSITION); +} diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index cc2b1798d4..e94d4e6736 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -1715,6 +1715,225 @@ const auto gripper_hardware_resources_mimic_false_command_if = )"; +const auto diff_drive_robot_sdf = + R"( + + + + + + true + + + + base_link + chassis_link + 0 0 0.075 0 0 0 + + + + + + + + 0.3 0.3 0.15 + + + + + 1 1 1 1 + 1 1 1 1 + + + + + + + 0.3 0.3 0.15 + + + + + + 0.5 + + 0.0046875 + 0.0 + 0.0 + 0.0046875 + 0.0 + 0.0075 + + + + + + chassis_link + left_wheel_link + 0.09 0.16999999999999998 -0.075 -1.5707963267948966 0 0 + + 0 0 1 + + -inf + inf + + + + + + + + + 0.05 + 0.04 + + + + 0 0 1 + 0 0 1 + + + + + + 0.05 + 0.04 + + + + + 0.1 + + 7.583333333333335e-05 + 0.0 + 0.0 + 7.583333333333335e-05 + 0.0 + 0.00012500000000000003 + + + + + + chassis_link + right_wheel_link + 0.09 -0.16999999999999998 -0.075 -1.5707963267948966 0 0 + + 0 0 1 + + + -inf + inf + + + + + + + + + 0.05 + 0.04 + + + + 0 0 1 + 0 0 1 + + + + + + 0.05 + 0.04 + + + + + 0.1 + + 7.583333333333335e-05 + 0.0 + 0.0 + 7.583333333333335e-05 + 0.0 + 0.00012500000000000003 + + + + + + chassis_link + caster_link + -0.09 0 -0.075 0 0 0 + + 1 1 1 + + -inf + inf + + + + + + + + + 0.05 + + + + 0 0 1 + 0 0 1 + + + + + + 0.05 + + + + + 0.1 + + 0.00010000000000000002 + 0.0 + 0.0 + 0.00010000000000000002 + 0.0 + 0.00010000000000000002 + + + + + + gz_ros2_control/GazeboSimSystem + + + + -10 + 10 + + + + + + + -10 + 10 + + + + + + + /path/to/config.yml + + + +)"; + const auto minimal_robot_urdf = std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail); const auto minimal_async_robot_urdf = From 5d36d99c97dfb54e5265b08777931d933f64e266 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Nov 2024 11:17:52 +0100 Subject: [PATCH 22/40] Use Clock instead of Rate for backward compatibility of rolling (#1864) --- controller_manager/src/ros2_control_node.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 0ed68d9765..dcf508d13c 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -58,7 +58,6 @@ int main(int argc, char ** argv) executor, manager_node_name, "", cm_node_options); const bool use_sim_time = cm->get_parameter_or("use_sim_time", false); - rclcpp::Rate rate(cm->get_update_rate(), cm->get_clock()); const bool lock_memory = cm->get_parameter_or("lock_memory", true); std::string message; @@ -85,7 +84,7 @@ int main(int argc, char ** argv) thread_priority); std::thread cm_thread( - [cm, thread_priority, use_sim_time, &rate]() + [cm, thread_priority, use_sim_time]() { if (!realtime_tools::configure_sched_fifo(thread_priority)) { @@ -128,7 +127,7 @@ int main(int argc, char ** argv) next_iteration_time += period; if (use_sim_time) { - rate.sleep(); + cm->get_clock()->sleep_until(current_time + period); } else { From 569fb58b8520645445fe3993e5240d3350d227b3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Nov 2024 11:18:14 +0100 Subject: [PATCH 23/40] reset the async variables upon activation to work post exceptions (#1860) --- controller_interface/src/controller_interface_base.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index a6e0da988f..0713ec3c25 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -67,7 +67,15 @@ return_type ControllerInterfaceBase::init( }); node_->register_on_activate( - std::bind(&ControllerInterfaceBase::on_activate, this, std::placeholders::_1)); + [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn + { + if (is_async() && async_handler_ && async_handler_->is_running()) + { + // This is needed if it is disabled due to a thrown exception in the async callback thread + async_handler_->reset_variables(); + } + return on_activate(previous_state); + }); node_->register_on_deactivate( std::bind(&ControllerInterfaceBase::on_deactivate, this, std::placeholders::_1)); From 8fa60dba374563077d221d2395867ce1906c41fe Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Nov 2024 11:18:44 +0100 Subject: [PATCH 24/40] change from thread_priority.hpp to realtime_helpers.hpp (#1829) --- 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 dcf508d13c..f8347df952 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -20,7 +20,7 @@ #include "controller_manager/controller_manager.hpp" #include "rclcpp/executors.hpp" -#include "realtime_tools/thread_priority.hpp" +#include "realtime_tools/realtime_helpers.hpp" using namespace std::chrono_literals; From 8853a188813671609d017fefb8c04ff6b2246185 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 8 Nov 2024 11:02:53 +0000 Subject: [PATCH 25/40] Update changelogs --- controller_interface/CHANGELOG.rst | 6 ++++++ controller_manager/CHANGELOG.rst | 14 ++++++++++++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 6 ++++++ hardware_interface_testing/CHANGELOG.rst | 3 +++ joint_limits/CHANGELOG.rst | 3 +++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 5 +++++ ros2controlcli/CHANGELOG.rst | 3 +++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 3 +++ 11 files changed, 52 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index be2de417d6..c3f845e1ee 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* reset the async variables upon activation to work post exceptions (`#1860 `_) +* [CM] Fix controller missing update cycles in a real setup (`#1774 `_) +* Contributors: Sai Kishor Kothakota + 4.19.0 (2024-10-26) ------------------- * [CM] Async Function Handler for Controllers (`#1489 `_) diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 7d9fb12a0d..51a4e47855 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* change from thread_priority.hpp to realtime_helpers.hpp (`#1829 `_) +* Use Clock instead of Rate for backward compatibility of rolling (`#1864 `_) +* [ros2_control_node] Handle simulation environment clocks (`#1810 `_) +* [CM] Fix controller missing update cycles in a real setup (`#1774 `_) +* [ros2_control_node] Add option to set the CPU affinity (`#1852 `_) +* [ros2_control_node] Add the realtime_tools lock_memory method to prevent page faults (`#1822 `_) +* Fix CMP0115 (`#1830 `_) +* fix: typo use thread_priority (`#1844 `_) +* Fix Hardware spawner and add tests for it (`#1759 `_) +* add thread_priority option to the ros2_control_node (`#1820 `_) +* Contributors: Baris Yazici, Christoph Fröhlich, Felix Exner (fexner), Sai Kishor Kothakota + 4.19.0 (2024-10-26) ------------------- * Fix timeout value in std output (`#1807 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 48802d740a..86f1bdead9 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.19.0 (2024-10-26) ------------------- diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 913d4cc36b..42e9c66514 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add Support for SDF (`#1763 `_) +* [HW_IF] Prepare the handles for async operations (`#1750 `_) +* Contributors: Aarav Gupta, Sai Kishor Kothakota + 4.19.0 (2024-10-26) ------------------- * [RM/HW] Constify the exported state interfaces using ConstSharedPtr (`#1767 `_) diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 8cab703114..900e855979 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.19.0 (2024-10-26) ------------------- diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 7c1e88b2f9..de4f2c0579 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.19.0 (2024-10-26) ------------------- diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 771506022e..280375752f 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.19.0 (2024-10-26) ------------------- diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 8a2406ca4e..e23d33f756 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add Support for SDF (`#1763 `_) +* Contributors: Aarav Gupta + 4.19.0 (2024-10-26) ------------------- diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 546356417a..4ad90c790c 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.19.0 (2024-10-26) ------------------- * [ros2controlcli] Fix the missing exported state interface printing (`#1800 `_) diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index da13697e81..fc1bc6fd73 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.19.0 (2024-10-26) ------------------- * fix: call configure_controller on 'unconfigured' state instead load_controller (`#1794 `_) diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 68b6ec5054..185ab62c20 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.19.0 (2024-10-26) ------------------- From 0031ada55856f61782f74796aa3ba9ccee08b392 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 8 Nov 2024 11:03:36 +0000 Subject: [PATCH 26/40] 4.20.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 c3f845e1ee..665b0175f6 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2024-11-08) +------------------- * reset the async variables upon activation to work post exceptions (`#1860 `_) * [CM] Fix controller missing update cycles in a real setup (`#1774 `_) * Contributors: Sai Kishor Kothakota diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 55c47473f0..ca3fc2af49 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.19.0 + 4.20.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 51a4e47855..376b15014d 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2024-11-08) +------------------- * change from thread_priority.hpp to realtime_helpers.hpp (`#1829 `_) * Use Clock instead of Rate for backward compatibility of rolling (`#1864 `_) * [ros2_control_node] Handle simulation environment clocks (`#1810 `_) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 69f5f6e4ce..18189d5d16 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.19.0 + 4.20.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 86f1bdead9..46ecc7ab6e 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.20.0 (2024-11-08) +------------------- 4.19.0 (2024-10-26) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index fbd4859323..78a9f99591 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.19.0 + 4.20.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 42e9c66514..17e276bc93 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2024-11-08) +------------------- * Add Support for SDF (`#1763 `_) * [HW_IF] Prepare the handles for async operations (`#1750 `_) * Contributors: Aarav Gupta, Sai Kishor Kothakota diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 05cff62957..dbe9ad5750 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.19.0 + 4.20.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 900e855979..8daf6bab93 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.20.0 (2024-11-08) +------------------- 4.19.0 (2024-10-26) ------------------- diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index 1c4f2f1b73..3966cbc993 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,7 @@ hardware_interface_testing - 4.19.0 + 4.20.0 ros2_control hardware interface testing Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index de4f2c0579..6b00437dd1 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2024-11-08) +------------------- 4.19.0 (2024-10-26) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index a945aa4710..c7fca5f2af 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.19.0 + 4.20.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 280375752f..5b57ddc2e2 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2024-11-08) +------------------- 4.19.0 (2024-10-26) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 71ec807544..925ad6e23f 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.19.0 + 4.20.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 e23d33f756..cad5325b2e 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.20.0 (2024-11-08) +------------------- * Add Support for SDF (`#1763 `_) * Contributors: Aarav Gupta diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 9752b87fca..bb818063ab 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 4.19.0 + 4.20.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 4ad90c790c..894bbb8ea3 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2024-11-08) +------------------- 4.19.0 (2024-10-26) ------------------- diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 67b651e3ee..ddb904c432 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.19.0 + 4.20.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 244359601e..949445ec59 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.19.0", + version="4.20.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 fc1bc6fd73..1f9ad52f22 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.20.0 (2024-11-08) +------------------- 4.19.0 (2024-10-26) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index ca6e74373c..1611ce05a3 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.19.0 + 4.20.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 70cf0f8984..76d8c706c2 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.19.0", + version="4.20.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 185ab62c20..842cc76488 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2024-11-08) +------------------- 4.19.0 (2024-10-26) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 22d0c90ba1..7ae2f2eda7 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.19.0 + 4.20.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 ec70ae1dc66ff39ce9a2fddf83a04c4261897dcb Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 9 Nov 2024 09:58:23 +0100 Subject: [PATCH 27/40] Fix the missing bcolors.ENDC in hardware_spawner log prints (#1870) --- .../controller_manager/hardware_spawner.py | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index 323e02584a..4f7afe714c 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -67,18 +67,15 @@ def handle_set_component_state_service_call( response = set_hardware_component_state(node, controller_manager_name, component, target_state) if response.ok and response.state == target_state: node.get_logger().info( - bcolors.OKGREEN - + f"{action} component '{component}'. Hardware now in state: {response.state}." + f"{bcolors.OKGREEN}{action} component '{component}'. Hardware now in state: {response.state}.{bcolors.ENDC}" ) elif response.ok and not response.state == target_state: node.get_logger().warn( - bcolors.WARNING - + f"Could not {action} component '{component}'. Service call returned ok=True, but state: {response.state} is not equal to target state '{target_state}'." + f"{bcolors.WARNING}Could not {action} component '{component}'. Service call returned ok=True, but state: {response.state} is not equal to target state '{target_state}'.{bcolors.ENDC}" ) else: node.get_logger().warn( - bcolors.WARNING - + f"Could not {action} component '{component}'. Service call failed. Wrong component name?" + f"{bcolors.WARNING}Could not {action} component '{component}'. Service call failed. Wrong component name?{bcolors.ENDC}" ) @@ -160,9 +157,7 @@ def main(args=None): 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 + f"{bcolors.WARNING}Hardware Component is not loaded - state can not be changed.{bcolors.ENDC}" ) elif activate: activate_component(node, controller_manager_name, hardware_component) @@ -170,14 +165,14 @@ def main(args=None): 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' + f'{bcolors.FAIL}You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag{bcolors.ENDC}' ) parser.print_help() return 0 except KeyboardInterrupt: pass except ServiceNotFoundError as err: - node.get_logger().fatal(str(err)) + node.get_logger().fatal(f"{bcolors.FAIL}{str(err)}{bcolors.ENDC}") return 1 finally: rclpy.shutdown() From 30d51cd7d04a93886cadc7d60275c9c1c529079f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 13 Nov 2024 21:37:50 +0100 Subject: [PATCH 28/40] Add compatibility build for humble+jazzy distro (#1872) --- ...ling-compatibility-humble-binary-build.yml | 47 +++++++++++++++++++ ...lling-compatibility-jazzy-binary-build.yml | 47 +++++++++++++++++++ 2 files changed, 94 insertions(+) create mode 100644 .github/workflows/rolling-compatibility-humble-binary-build.yml create mode 100644 .github/workflows/rolling-compatibility-jazzy-binary-build.yml diff --git a/.github/workflows/rolling-compatibility-humble-binary-build.yml b/.github/workflows/rolling-compatibility-humble-binary-build.yml new file mode 100644 index 0000000000..0c25f2313d --- /dev/null +++ b/.github/workflows/rolling-compatibility-humble-binary-build.yml @@ -0,0 +1,47 @@ +name: Check Rolling Compatibility on Humble +# author: Christoph Froehlich +# description: 'Build & test the rolling version on Humble distro.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '.github/workflows/rolling-compatibility-humble-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_control.rolling.repos' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '.github/workflows/rolling-compatibility-humble-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_control.rolling.repos' + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +jobs: + build-on-humble: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [humble] + ROS_REPO: [main, testing] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_control.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml new file mode 100644 index 0000000000..cddb806eee --- /dev/null +++ b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml @@ -0,0 +1,47 @@ +name: Check Rolling Compatibility on Jazzy +# author: Christoph Froehlich +# description: 'Build & test the rolling version on Jazzy distro.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '.github/workflows/rolling-compatibility-jazzy-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_control.rolling.repos' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '.github/workflows/rolling-compatibility-jazzy-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_control.rolling.repos' + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +jobs: + build-on-jazzy: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [jazzy] + ROS_REPO: [main, testing] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_control.rolling.repos + ref_for_scheduled_build: master From bab2d303903970f32ec530030b0cade2ca0254bb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 14 Nov 2024 11:04:55 +0100 Subject: [PATCH 29/40] Reduce number of CI jobs (#1876) --- .github/workflows/humble-semi-binary-build.yml | 2 +- .github/workflows/iron-semi-binary-build.yml | 2 +- .github/workflows/jazzy-semi-binary-build.yml | 2 +- .../workflows/rolling-compatibility-humble-binary-build.yml | 5 +---- .../workflows/rolling-compatibility-jazzy-binary-build.yml | 5 +---- .github/workflows/rolling-semi-binary-build.yml | 2 +- 6 files changed, 6 insertions(+), 12 deletions(-) diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml index 560ac37cff..96938467f9 100644 --- a/.github/workflows/humble-semi-binary-build.yml +++ b/.github/workflows/humble-semi-binary-build.yml @@ -39,7 +39,7 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [humble] - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml index 30a83e5367..c2d137bde0 100644 --- a/.github/workflows/iron-semi-binary-build.yml +++ b/.github/workflows/iron-semi-binary-build.yml @@ -39,7 +39,7 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [iron] - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml index 9634732cf9..12769eb740 100644 --- a/.github/workflows/jazzy-semi-binary-build.yml +++ b/.github/workflows/jazzy-semi-binary-build.yml @@ -39,7 +39,7 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [jazzy] - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/rolling-compatibility-humble-binary-build.yml b/.github/workflows/rolling-compatibility-humble-binary-build.yml index 0c25f2313d..b55091521e 100644 --- a/.github/workflows/rolling-compatibility-humble-binary-build.yml +++ b/.github/workflows/rolling-compatibility-humble-binary-build.yml @@ -28,9 +28,6 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.rolling.repos' - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' jobs: build-on-humble: @@ -39,7 +36,7 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [humble] - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml index cddb806eee..621ffb6a26 100644 --- a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml +++ b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml @@ -28,9 +28,6 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.rolling.repos' - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' jobs: build-on-jazzy: @@ -39,7 +36,7 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [jazzy] - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index 4cdb7ab585..492da45ab9 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -39,7 +39,7 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [rolling] - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: ${{ matrix.ROS_REPO }} From da167e79ff90b250f3d4b9686e46a77c8b9ba4ab Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 16 Nov 2024 17:07:47 +0100 Subject: [PATCH 30/40] Refactor: add parse_state_interface_descriptions and parse_command_interface_descriptions to import the components (#1768) --- .../hardware_interface/actuator_interface.hpp | 32 +---------- .../hardware_interface/component_parser.hpp | 20 +++++++ .../hardware_interface/sensor_interface.hpp | 16 +----- .../hardware_interface/system_interface.hpp | 53 ++----------------- hardware_interface/src/component_parser.cpp | 32 +++++++++++ 5 files changed, 60 insertions(+), 93 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 4cdd81b60f..8aa214e728 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -123,39 +123,11 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; - import_state_interface_descriptions(info_); - import_command_interface_descriptions(info_); + parse_state_interface_descriptions(info_.joints, joint_state_interfaces_); + parse_command_interface_descriptions(info_.joints, joint_command_interfaces_); return CallbackReturn::SUCCESS; }; - /** - * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo. - * Separate them into the possible types: Joint and store them. - */ - virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info) - { - auto joint_state_interface_descriptions = - parse_state_interface_descriptions(hardware_info.joints); - for (const auto & description : joint_state_interface_descriptions) - { - joint_state_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - } - - /** - * Import the InterfaceDescription for the CommandInterfaces from the HardwareInfo. - * Separate them into the possible types: Joint and store them. - */ - virtual void import_command_interface_descriptions(const HardwareInfo & hardware_info) - { - auto joint_command_interface_descriptions = - parse_command_interface_descriptions(hardware_info.joints); - for (const auto & description : joint_command_interface_descriptions) - { - joint_command_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - } - /// Exports all state interfaces for this hardware interface. /** * Old way of exporting the StateInterfaces. If a empty vector is returned then diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index 2d0c067606..fc195d0a65 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -16,6 +16,7 @@ #define HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ #include +#include #include #include "hardware_interface/hardware_info.hpp" @@ -41,6 +42,16 @@ HARDWARE_INTERFACE_PUBLIC std::vector parse_state_interface_descriptions( const std::vector & component_info); +/** + * \param[in] component_info information about a component (gpio, joint, sensor) + * \param[out] state_interfaces_map unordered_map filled with information about hardware's + * StateInterfaces for the component which are exported + */ +HARDWARE_INTERFACE_PUBLIC +void parse_state_interface_descriptions( + const std::vector & component_info, + std::unordered_map & state_interfaces_map); + /** * \param[in] component_info information about a component (gpio, joint, sensor) * \return vector filled with information about hardware's CommandInterfaces for the component @@ -50,5 +61,14 @@ HARDWARE_INTERFACE_PUBLIC std::vector parse_command_interface_descriptions( const std::vector & component_info); +/** + * \param[in] component_info information about a component (gpio, joint, sensor) + * \param[out] command_interfaces_map unordered_map filled with information about hardware's + * CommandInterfaces for the component which are exported + */ +HARDWARE_INTERFACE_PUBLIC +void parse_command_interface_descriptions( + const std::vector & component_info, + std::unordered_map & command_interfaces_map); } // namespace hardware_interface #endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 50d79d1a45..74c1d04bd7 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -123,24 +123,10 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; - import_state_interface_descriptions(info_); + parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_); return CallbackReturn::SUCCESS; }; - /** - * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo. - * Separate them into the possible types: Sensor and store them. - */ - virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info) - { - auto sensor_state_interface_descriptions = - parse_state_interface_descriptions(hardware_info.sensors); - for (const auto & description : sensor_state_interface_descriptions) - { - sensor_state_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - } - /// Exports all state interfaces for this hardware interface. /** * Old way of exporting the StateInterfaces. If a empty vector is returned then diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 6b0652d3fe..18da0e4012 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -126,57 +126,14 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; - import_state_interface_descriptions(info_); - import_command_interface_descriptions(info_); + parse_state_interface_descriptions(info_.joints, joint_state_interfaces_); + parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_); + parse_state_interface_descriptions(info_.gpios, gpio_state_interfaces_); + parse_command_interface_descriptions(info_.joints, joint_command_interfaces_); + parse_command_interface_descriptions(info_.gpios, gpio_command_interfaces_); return CallbackReturn::SUCCESS; }; - /** - * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo. - * Separate them into the possible types: Joint, GPIO, Sensor and store them. - */ - void import_state_interface_descriptions(const HardwareInfo & hardware_info) - { - auto joint_state_interface_descriptions = - parse_state_interface_descriptions(hardware_info.joints); - for (const auto & description : joint_state_interface_descriptions) - { - joint_state_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - auto sensor_state_interface_descriptions = - parse_state_interface_descriptions(hardware_info.sensors); - for (const auto & description : sensor_state_interface_descriptions) - { - sensor_state_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - auto gpio_state_interface_descriptions = - parse_state_interface_descriptions(hardware_info.gpios); - for (const auto & description : gpio_state_interface_descriptions) - { - gpio_state_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - } - - /** - * Import the InterfaceDescription for the CommandInterfaces from the HardwareInfo. - * Separate them into the possible types: Joint and GPIO and store them. - */ - void import_command_interface_descriptions(const HardwareInfo & hardware_info) - { - auto joint_command_interface_descriptions = - parse_command_interface_descriptions(hardware_info.joints); - for (const auto & description : joint_command_interface_descriptions) - { - joint_command_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - auto gpio_command_interface_descriptions = - parse_command_interface_descriptions(hardware_info.gpios); - for (const auto & description : gpio_command_interface_descriptions) - { - gpio_command_interfaces_.insert(std::make_pair(description.get_name(), description)); - } - } - /// Exports all state interfaces for this hardware interface. /** * Old way of exporting the StateInterfaces. If a empty vector is returned then diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index a49b83b964..0f186531e9 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -942,6 +942,22 @@ std::vector parse_state_interface_descriptions( return component_state_interface_descriptions; } +void parse_state_interface_descriptions( + const std::vector & component_info, + std::unordered_map & state_interfaces_map) +{ + state_interfaces_map.reserve(state_interfaces_map.size() + component_info.size()); + + for (const auto & component : component_info) + { + for (const auto & state_interface : component.state_interfaces) + { + InterfaceDescription description(component.name, state_interface); + state_interfaces_map.insert(std::make_pair(description.get_name(), description)); + } + } +} + std::vector parse_command_interface_descriptions( const std::vector & component_info) { @@ -959,4 +975,20 @@ std::vector parse_command_interface_descriptions( return component_command_interface_descriptions; } +void parse_command_interface_descriptions( + const std::vector & component_info, + std::unordered_map & command_interfaces_map) +{ + command_interfaces_map.reserve(command_interfaces_map.size() + component_info.size()); + + for (const auto & component : component_info) + { + for (const auto & command_interface : component.command_interfaces) + { + InterfaceDescription description(component.name, command_interface); + command_interfaces_map.insert(std::make_pair(description.get_name(), description)); + } + } +} + } // namespace hardware_interface From 23bd1c3c06c30d706f010628d85133a7198e226d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 18 Nov 2024 14:33:18 +0100 Subject: [PATCH 31/40] Add CM `switch_controller` service timeout as parameter to spawner.py (#1790) Co-authored-by: Felix Exner (fexner) Co-authored-by: Sai Kishor Kothakota --- .../controller_manager/spawner.py | 36 ++++++++++++++++--- .../controller_manager/unspawner.py | 18 +++++++++- controller_manager/doc/userdoc.rst | 21 ++++++----- doc/release_notes.rst | 1 + 4 files changed, 62 insertions(+), 14 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index d6df3be01f..7e9fe3443b 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -112,7 +112,16 @@ def main(args=None): "--controller-manager-timeout", help="Time to wait for the controller manager", required=False, - default=0, + default=0.0, + type=float, + ) + parser.add_argument( + "--switch-timeout", + help="Time to wait for a successful state switch of controllers." + " Useful when switching cannot be performed immediately, e.g.," + " paused simulations at startup", + required=False, + default=5.0, type=float, ) parser.add_argument( @@ -129,6 +138,7 @@ def main(args=None): controller_manager_name = args.controller_manager param_file = args.param_file controller_manager_timeout = args.controller_manager_timeout + switch_timeout = args.switch_timeout if param_file and not os.path.isfile(param_file): raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file) @@ -206,7 +216,13 @@ def main(args=None): if not args.inactive and not args.activate_as_group: ret = switch_controllers( - node, controller_manager_name, [], [controller_name], True, True, 5.0 + node, + controller_manager_name, + [], + [controller_name], + True, + True, + switch_timeout, ) if not ret.ok: node.get_logger().error( @@ -224,7 +240,13 @@ def main(args=None): if not args.inactive and args.activate_as_group: ret = switch_controllers( - node, controller_manager_name, [], controller_names, True, True, 5.0 + node, + controller_manager_name, + [], + controller_names, + True, + True, + switch_timeout, ) if not ret.ok: node.get_logger().error( @@ -250,7 +272,13 @@ def main(args=None): node.get_logger().info("Interrupt captured, deactivating and unloading controller") # TODO(saikishor) we might have an issue in future, if any of these controllers is in chained mode ret = switch_controllers( - node, controller_manager_name, controller_names, [], True, True, 5.0 + node, + controller_manager_name, + controller_names, + [], + True, + True, + switch_timeout, ) if not ret.ok: node.get_logger().error( diff --git a/controller_manager/controller_manager/unspawner.py b/controller_manager/controller_manager/unspawner.py index e42d85aee9..9e380f5086 100644 --- a/controller_manager/controller_manager/unspawner.py +++ b/controller_manager/controller_manager/unspawner.py @@ -36,17 +36,33 @@ def main(args=None): default="/controller_manager", required=False, ) + parser.add_argument( + "--switch-timeout", + help="Time to wait for a successful state switch of controllers." + " Useful when switching cannot be performed immediately, e.g.," + " paused simulations at startup", + required=False, + default=5.0, + type=float, + ) command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] args = parser.parse_args(command_line_args) controller_names = args.controller_names controller_manager_name = args.controller_manager + switch_timeout = args.switch_timeout node = Node("unspawner_" + controller_names[0]) try: # Ignore returncode, because message is already printed and we'll try to unload anyway ret = switch_controllers( - node, controller_manager_name, controller_names, [], True, True, 5.0 + node, + controller_manager_name, + controller_names, + [], + True, + True, + switch_timeout, ) node.get_logger().info("Deactivated controller") diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index fa673cff0a..ca222d68c0 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -157,9 +157,9 @@ There are two scripts to interact with controller manager from launch files: .. code-block:: console $ ros2 run controller_manager spawner -h - usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--inactive] [-t CONTROLLER_TYPE] [-u] - [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] - controller_name + usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--inactive] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] + [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] + controller_names [controller_names ...] positional arguments: controller_names List of controllers @@ -177,10 +177,10 @@ There are two scripts to interact with controller manager from launch files: -u, --unload-on-kill Wait until this application is interrupted and unload controller --controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT Time to wait for the controller manager + --switch-timeout SWITCH_TIMEOUT + Time to wait for a successful state switch of controllers. Useful if controllers cannot be switched immediately, e.g., paused + simulations at startup --activate-as-group Activates all the parsed controllers list together instead of one by one. Useful for activating all chainable controllers altogether - --fallback_controllers FALLBACK_CONTROLLERS [FALLBACK_CONTROLLERS ...] - Fallback controllers list are activated as a fallback strategy when the spawned controllers fail. When the argument is provided, it takes precedence over the fallback_controllers list in the - param file The parsed controller config file can follow the same conventions as the typical ROS 2 parameter file format. Now, the spawner can handle config files with wildcard entries and also the controller name in the absolute namespace. See the following examples on the config files: @@ -243,15 +243,18 @@ The parsed controller config file can follow the same conventions as the typical .. code-block:: console $ ros2 run controller_manager unspawner -h - usage: unspawner [-h] [-c CONTROLLER_MANAGER] controller_name + usage: unspawner [-h] [-c CONTROLLER_MANAGER] [--switch-timeout SWITCH_TIMEOUT] controller_names [controller_names ...] positional arguments: - controller_name Name of the controller + controller_names Name of the controller - optional arguments: + options: -h, --help show this help message and exit -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node + --switch-timeout SWITCH_TIMEOUT + Time to wait for a successful state switch of controllers. Useful if controllers cannot be switched immediately, e.g., paused + simulations at startup ``hardware_spawner`` ^^^^^^^^^^^^^^^^^^^^^^ diff --git a/doc/release_notes.rst b/doc/release_notes.rst index a6ebb16e5f..09a9236d79 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 `_). +* ``--switch-timeout`` was added as parameter to the helper scripts ``spawner.py`` and ``unspawner.py``. Useful if controllers cannot be switched immediately, e.g., paused simulations at startup (`#1790 `_). * ``ros2_control_node`` can now handle the sim time used by different simulators, when ``use_sim_time`` is set to true (`#1810 `_). * 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 `_). From 9997023ff7266a652a36cf753619205d73af9199 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 22 Nov 2024 13:05:27 +0100 Subject: [PATCH 32/40] Remove iron workflows (#1834) --- .github/dependabot.yml | 7 --- .github/workflows/README.md | 1 - .github/workflows/iron-abi-compatibility.yml | 27 ----------- .github/workflows/iron-binary-build.yml | 47 -------------------- .github/workflows/iron-check-docs.yml | 18 -------- .github/workflows/iron-coverage-build.yml | 36 --------------- .github/workflows/iron-debian-build.yml | 33 -------------- .github/workflows/iron-pre-commit.yml | 13 ------ .github/workflows/iron-rhel-binary-build.yml | 31 ------------- .github/workflows/iron-semi-binary-build.yml | 47 -------------------- .github/workflows/iron-source-build.yml | 27 ----------- README.md | 1 - ros2_control-not-released.iron.repos | 1 - ros2_control.iron.repos | 9 ---- 14 files changed, 298 deletions(-) delete mode 100644 .github/workflows/iron-abi-compatibility.yml delete mode 100644 .github/workflows/iron-binary-build.yml delete mode 100644 .github/workflows/iron-check-docs.yml delete mode 100644 .github/workflows/iron-coverage-build.yml delete mode 100644 .github/workflows/iron-debian-build.yml delete mode 100644 .github/workflows/iron-pre-commit.yml delete mode 100644 .github/workflows/iron-rhel-binary-build.yml delete mode 100644 .github/workflows/iron-semi-binary-build.yml delete mode 100644 .github/workflows/iron-source-build.yml delete mode 100644 ros2_control-not-released.iron.repos delete mode 100644 ros2_control.iron.repos diff --git a/.github/dependabot.yml b/.github/dependabot.yml index aafd67c236..f5e9921f23 100644 --- a/.github/dependabot.yml +++ b/.github/dependabot.yml @@ -18,10 +18,3 @@ updates: schedule: interval: "weekly" target-branch: "humble" - - package-ecosystem: "github-actions" - # Workflow files stored in the - # default location of `.github/workflows` - directory: "/" - schedule: - interval: "weekly" - target-branch: "iron" diff --git a/.github/workflows/README.md b/.github/workflows/README.md index 62007ffc2d..2ad6a249fe 100644 --- a/.github/workflows/README.md +++ b/.github/workflows/README.md @@ -3,5 +3,4 @@ ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: **Rolling** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build.yml?branch=master)
[![Rolling Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-source-build.yml?branch=master)
[![Debian Rolling Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-debian-build.yml/badge.svg)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-debian-build.yml)
[![RHEL Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-rhel-binary-build.yml/badge.svg)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-rhel-binary-build.yml) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling) **Jazzy** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [![Jazzy Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-binary-build.yml?branch=master)
[![Jazzy Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-semi-binary-build.yml?branch=master)
[![Jazzy Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-source-build.yml?branch=master)
[![Debian Jazzy Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-debian-build.yml/badge.svg)](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-debian-build.yml)
[![RHEL Jazzy Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-rhel-binary-build.yml/badge.svg)](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-rhel-binary-build.yml) | [Documentation](https://control.ros.org/jazzy/index.html)
[API Reference](https://control.ros.org/jazzy/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#jazzy) -**Iron** | [`iron`](https://github.com/ros-controls/ros2_control/tree/master) | [![Iron Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/iron-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/iron-binary-build.yml?branch=master)
[![Iron Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/iron-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/iron-semi-binary-build.yml?branch=master)
[![Iron Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/iron-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/iron-source-build.yml?branch=master)
[![Debian Iron Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/iron-debian-build.yml/badge.svg)](https://github.com/ros-controls/ros2_control/actions/workflows/iron-debian-build.yml)
[![RHEL Iron Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/iron-rhel-binary-build.yml/badge.svg)](https://github.com/ros-controls/ros2_control/actions/workflows/iron-rhel-binary-build.yml) | [Documentation](https://control.ros.org/iron/index.html)
[API Reference](https://control.ros.org/iron/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#iron) **Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build.yml?branch=master)
[![Humble Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-source-build.yml?branch=master)
[![Debian Humble Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-debian-build.yml/badge.svg)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-debian-build.yml)
[![RHEL Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-rhel-binary-build.yml/badge.svg)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-rhel-binary-build.yml) | [Documentation](https://control.ros.org/humble/index.html)
[API Reference](https://control.ros.org/humble/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#humble) diff --git a/.github/workflows/iron-abi-compatibility.yml b/.github/workflows/iron-abi-compatibility.yml deleted file mode 100644 index c2d9c19110..0000000000 --- a/.github/workflows/iron-abi-compatibility.yml +++ /dev/null @@ -1,27 +0,0 @@ -name: Iron - ABI Compatibility Check -on: - workflow_dispatch: - pull_request: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '.github/workflows/iron-abi-compatibility.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_control-not-released.iron.repos' - -jobs: - abi_check: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: iron - ROS_REPO: testing - ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} - NOT_TEST_BUILD: true diff --git a/.github/workflows/iron-binary-build.yml b/.github/workflows/iron-binary-build.yml deleted file mode 100644 index ef90e256a0..0000000000 --- a/.github/workflows/iron-binary-build.yml +++ /dev/null @@ -1,47 +0,0 @@ -name: Iron Binary Build -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - pull_request: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '.github/workflows/iron-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_control-not-released.iron.repos' - push: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '.github/workflows/iron-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_control-not-released.iron.repos' - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' - -jobs: - binary: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [iron] - ROS_REPO: [main, testing] - with: - ros_distro: ${{ matrix.ROS_DISTRO }} - ros_repo: ${{ matrix.ROS_REPO }} - upstream_workspace: ros2_control-not-released.iron.repos - ref_for_scheduled_build: iron diff --git a/.github/workflows/iron-check-docs.yml b/.github/workflows/iron-check-docs.yml deleted file mode 100644 index e9295dad44..0000000000 --- a/.github/workflows/iron-check-docs.yml +++ /dev/null @@ -1,18 +0,0 @@ -name: Iron Check Docs - -on: - workflow_dispatch: - pull_request: - branches: - - iron - paths: - - '**.rst' - - '**.md' - - '**.yaml' - -jobs: - check-docs: - name: Check Docs - uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@iron - with: - ROS2_CONTROL_PR: ${{ github.ref }} diff --git a/.github/workflows/iron-coverage-build.yml b/.github/workflows/iron-coverage-build.yml deleted file mode 100644 index ff5be81d7d..0000000000 --- a/.github/workflows/iron-coverage-build.yml +++ /dev/null @@ -1,36 +0,0 @@ -name: Coverage Build - Iron -on: - workflow_dispatch: - push: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '.github/workflows/iron-coverage-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_control.iron.repos' - - 'codecov.yml' - pull_request: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '.github/workflows/iron-coverage-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_control.iron.repos' - - 'codecov.yml' - -jobs: - coverage_iron: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master - secrets: inherit - with: - ros_distro: iron diff --git a/.github/workflows/iron-debian-build.yml b/.github/workflows/iron-debian-build.yml deleted file mode 100644 index 3cbe0c5127..0000000000 --- a/.github/workflows/iron-debian-build.yml +++ /dev/null @@ -1,33 +0,0 @@ -name: Debian Iron Source Build -on: - workflow_dispatch: - pull_request: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '.github/workflows/iron-debian-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_control.iron.repos' - schedule: - # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' - - -jobs: - debian_source_build: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [iron] - with: - ros_distro: ${{ matrix.ROS_DISTRO }} - upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos - ref_for_scheduled_build: master - skip_packages: rqt_controller_manager - skip_packages_test: controller_manager_msgs diff --git a/.github/workflows/iron-pre-commit.yml b/.github/workflows/iron-pre-commit.yml deleted file mode 100644 index a128958031..0000000000 --- a/.github/workflows/iron-pre-commit.yml +++ /dev/null @@ -1,13 +0,0 @@ -name: Pre-Commit - Iron - -on: - workflow_dispatch: - pull_request: - branches: - - iron - -jobs: - pre-commit: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master - with: - ros_distro: iron diff --git a/.github/workflows/iron-rhel-binary-build.yml b/.github/workflows/iron-rhel-binary-build.yml deleted file mode 100644 index f308c495f3..0000000000 --- a/.github/workflows/iron-rhel-binary-build.yml +++ /dev/null @@ -1,31 +0,0 @@ -name: RHEL Iron Semi-Binary Build -on: - workflow_dispatch: - pull_request: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '.github/workflows/iron-rhel-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_control.iron.repos' - schedule: - # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' - -jobs: - rhel_semi_binary_build: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [iron] - with: - ros_distro: ${{ matrix.ROS_DISTRO }} - upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos - ref_for_scheduled_build: iron - skip_packages: rqt_controller_manager diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml deleted file mode 100644 index c2d137bde0..0000000000 --- a/.github/workflows/iron-semi-binary-build.yml +++ /dev/null @@ -1,47 +0,0 @@ -name: Iron Semi-Binary Build -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - pull_request: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '.github/workflows/iron-semi-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_control.iron.repos' - push: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '.github/workflows/iron-semi-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_control.iron.repos' - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' - -jobs: - binary: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [iron] - ROS_REPO: [testing] - with: - ros_distro: ${{ matrix.ROS_DISTRO }} - ros_repo: ${{ matrix.ROS_REPO }} - upstream_workspace: ros2_control.iron.repos - ref_for_scheduled_build: iron diff --git a/.github/workflows/iron-source-build.yml b/.github/workflows/iron-source-build.yml deleted file mode 100644 index 3b7c53f6ff..0000000000 --- a/.github/workflows/iron-source-build.yml +++ /dev/null @@ -1,27 +0,0 @@ -name: Iron Source Build -on: - workflow_dispatch: - push: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '.github/workflows/iron-source-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_control.iron.repos' - schedule: - # Run every day to detect flakiness and broken dependencies - - cron: '03 3 * * *' - -jobs: - source: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master - with: - ros_distro: iron - ref: iron - ros2_repo_branch: iron - os_name: ubuntu-22.04 diff --git a/README.md b/README.md index 40d2a3c189..47c6ad1523 100644 --- a/README.md +++ b/README.md @@ -13,7 +13,6 @@ ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: **Rolling** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling) **Jazzy** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-semi-binary-build.yml?branch=master) | [Documentation](https://control.ros.org/jazzy/index.html)
[API Reference](https://control.ros.org/jazzy/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#jazzy) -**Iron** | [`iron`](https://github.com/ros-controls/ros2_control/tree/iron) | [![Iron Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/iron-binary-build.yml/badge.svg?branch=iron)](https://github.com/ros-controls/ros2_control/actions/workflows/iron-binary-build.yml?branch=iron)
[![Iron Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/iron-semi-binary-build.yml/badge.svg?branch=iron)](https://github.com/ros-controls/ros2_control/actions/workflows/iron-semi-binary-build.yml?branch=iron) | [Documentation](https://control.ros.org/iron/index.html)
[API Reference](https://control.ros.org/iron/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#iron) **Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build.yml?branch=master) | [Documentation](https://control.ros.org/humble/index.html)
[API Reference](https://control.ros.org/humble/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#humble) [Detailed build status](.github/workflows/README.md) diff --git a/ros2_control-not-released.iron.repos b/ros2_control-not-released.iron.repos deleted file mode 100644 index 56f46b6f79..0000000000 --- a/ros2_control-not-released.iron.repos +++ /dev/null @@ -1 +0,0 @@ -repositories: diff --git a/ros2_control.iron.repos b/ros2_control.iron.repos deleted file mode 100644 index c93d8f4ef6..0000000000 --- a/ros2_control.iron.repos +++ /dev/null @@ -1,9 +0,0 @@ -repositories: - ros-controls/realtime_tools: - type: git - url: https://github.com/ros-controls/realtime_tools.git - version: master - ros-controls/control_msgs: - type: git - url: https://github.com/ros-controls/control_msgs.git - version: master From 41d73939597a9332779ebdab82a72f4d13125328 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 25 Nov 2024 21:17:09 +0100 Subject: [PATCH 33/40] Add service call timeout argument in spawner (#1808) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --------- Signed-off-by: Angsa Deployment Team Signed-off-by: Tony Najjar Co-authored-by: Angsa Deployment Team Co-authored-by: Christoph Fröhlich Co-authored-by: Sai Kishor Kothakota --- .../controller_manager_services.py | 51 +++++++++++++++---- .../controller_manager/spawner.py | 35 +++++++++++-- controller_manager/doc/userdoc.rst | 6 ++- doc/release_notes.rst | 1 + 4 files changed, 76 insertions(+), 17 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 909e681ce6..7d958920f3 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -123,7 +123,9 @@ def service_caller( ) -def configure_controller(node, controller_manager_name, controller_name, service_timeout=0.0): +def configure_controller( + node, controller_manager_name, controller_name, service_timeout=0.0, call_timeout=10.0 +): request = ConfigureController.Request() request.name = controller_name return service_caller( @@ -132,10 +134,11 @@ def configure_controller(node, controller_manager_name, controller_name, service ConfigureController, request, service_timeout, + call_timeout, ) -def list_controllers(node, controller_manager_name, service_timeout=0.0): +def list_controllers(node, controller_manager_name, service_timeout=0.0, call_timeout=10.0): request = ListControllers.Request() return service_caller( node, @@ -143,10 +146,11 @@ def list_controllers(node, controller_manager_name, service_timeout=0.0): ListControllers, request, service_timeout, + call_timeout, ) -def list_controller_types(node, controller_manager_name, service_timeout=0.0): +def list_controller_types(node, controller_manager_name, service_timeout=0.0, call_timeout=10.0): request = ListControllerTypes.Request() return service_caller( node, @@ -154,10 +158,13 @@ def list_controller_types(node, controller_manager_name, service_timeout=0.0): ListControllerTypes, request, service_timeout, + call_timeout, ) -def list_hardware_components(node, controller_manager_name, service_timeout=0.0): +def list_hardware_components( + node, controller_manager_name, service_timeout=0.0, call_timeout=10.0 +): request = ListHardwareComponents.Request() return service_caller( node, @@ -165,10 +172,13 @@ def list_hardware_components(node, controller_manager_name, service_timeout=0.0) ListHardwareComponents, request, service_timeout, + call_timeout, ) -def list_hardware_interfaces(node, controller_manager_name, service_timeout=0.0): +def list_hardware_interfaces( + node, controller_manager_name, service_timeout=0.0, call_timeout=10.0 +): request = ListHardwareInterfaces.Request() return service_caller( node, @@ -176,10 +186,13 @@ def list_hardware_interfaces(node, controller_manager_name, service_timeout=0.0) ListHardwareInterfaces, request, service_timeout, + call_timeout, ) -def load_controller(node, controller_manager_name, controller_name, service_timeout=0.0): +def load_controller( + node, controller_manager_name, controller_name, service_timeout=0.0, call_timeout=10.0 +): request = LoadController.Request() request.name = controller_name return service_caller( @@ -188,10 +201,13 @@ def load_controller(node, controller_manager_name, controller_name, service_time LoadController, request, service_timeout, + call_timeout, ) -def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=0.0): +def reload_controller_libraries( + node, controller_manager_name, force_kill, service_timeout=0.0, call_timeout=10.0 +): request = ReloadControllerLibraries.Request() request.force_kill = force_kill return service_caller( @@ -200,11 +216,17 @@ def reload_controller_libraries(node, controller_manager_name, force_kill, servi ReloadControllerLibraries, request, service_timeout, + call_timeout, ) def set_hardware_component_state( - node, controller_manager_name, component_name, lifecyle_state, service_timeout=0.0 + node, + controller_manager_name, + component_name, + lifecyle_state, + service_timeout=0.0, + call_timeout=10.0, ): request = SetHardwareComponentState.Request() request.name = component_name @@ -215,6 +237,7 @@ def set_hardware_component_state( SetHardwareComponentState, request, service_timeout, + call_timeout, ) @@ -226,6 +249,7 @@ def switch_controllers( strict, activate_asap, timeout, + call_timeout=10.0, ): request = SwitchController.Request() request.activate_controllers = activate_controllers @@ -237,11 +261,17 @@ def switch_controllers( request.activate_asap = activate_asap request.timeout = rclpy.duration.Duration(seconds=timeout).to_msg() return service_caller( - node, f"{controller_manager_name}/switch_controller", SwitchController, request + node, + f"{controller_manager_name}/switch_controller", + SwitchController, + request, + call_timeout=call_timeout, ) -def unload_controller(node, controller_manager_name, controller_name, service_timeout=0.0): +def unload_controller( + node, controller_manager_name, controller_name, service_timeout=0.0, call_timeout=10.0 +): request = UnloadController.Request() request.name = controller_name return service_caller( @@ -250,6 +280,7 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti UnloadController, request, service_timeout, + call_timeout, ) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 7e9fe3443b..5d5e34e2a4 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -60,8 +60,12 @@ def has_service_names(node, node_name, node_namespace, service_names): return all(service in client_names for service in service_names) -def is_controller_loaded(node, controller_manager, controller_name, service_timeout=0.0): - controllers = list_controllers(node, controller_manager, service_timeout).controller +def is_controller_loaded( + node, controller_manager, controller_name, service_timeout=0.0, call_timeout=10.0 +): + controllers = list_controllers( + node, controller_manager, service_timeout, call_timeout + ).controller return any(c.name == controller_name for c in controllers) @@ -110,7 +114,7 @@ def main(args=None): ) parser.add_argument( "--controller-manager-timeout", - help="Time to wait for the controller manager", + help="Time to wait for the controller manager service to be available", required=False, default=0.0, type=float, @@ -124,6 +128,13 @@ def main(args=None): default=5.0, type=float, ) + parser.add_argument( + "--service-call-timeout", + help="Time to wait for the service response from the controller manager", + required=False, + default=10.0, + type=float, + ) parser.add_argument( "--activate-as-group", help="Activates all the parsed controllers list together instead of one by one." @@ -138,6 +149,7 @@ def main(args=None): controller_manager_name = args.controller_manager param_file = args.param_file controller_manager_timeout = args.controller_manager_timeout + service_call_timeout = args.service_call_timeout switch_timeout = args.switch_timeout if param_file and not os.path.isfile(param_file): @@ -174,7 +186,11 @@ def main(args=None): for controller_name in controller_names: if is_controller_loaded( - node, controller_manager_name, controller_name, controller_manager_timeout + node, + controller_manager_name, + controller_name, + controller_manager_timeout, + service_call_timeout, ): node.get_logger().warn( bcolors.WARNING @@ -207,7 +223,13 @@ def main(args=None): ) if not args.load_only: - ret = configure_controller(node, controller_manager_name, controller_name) + ret = configure_controller( + node, + controller_manager_name, + controller_name, + controller_manager_timeout, + service_call_timeout, + ) if not ret.ok: node.get_logger().error( bcolors.FAIL + "Failed to configure controller" + bcolors.ENDC @@ -223,6 +245,7 @@ def main(args=None): True, True, switch_timeout, + service_call_timeout, ) if not ret.ok: node.get_logger().error( @@ -247,6 +270,7 @@ def main(args=None): True, True, switch_timeout, + service_call_timeout, ) if not ret.ok: node.get_logger().error( @@ -279,6 +303,7 @@ def main(args=None): True, True, switch_timeout, + service_call_timeout, ) if not ret.ok: node.get_logger().error( diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index ca222d68c0..1ac867fa9b 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -158,7 +158,7 @@ There are two scripts to interact with controller manager from launch files: $ ros2 run controller_manager spawner -h usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--inactive] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] - [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] + [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] [--service-call-timeout SERVICE_CALL_TIMEOUT] controller_names [controller_names ...] positional arguments: @@ -176,7 +176,9 @@ There are two scripts to interact with controller manager from launch files: --inactive Load and configure the controller, however do not activate them -u, --unload-on-kill Wait until this application is interrupted and unload controller --controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT - Time to wait for the controller manager + Time to wait for the controller manager service to be available + --service-call-timeout SERVICE_CALL_TIMEOUT + Time to wait for the service response from the controller manager --switch-timeout SWITCH_TIMEOUT Time to wait for a successful state switch of controllers. Useful if controllers cannot be switched immediately, e.g., paused simulations at startup diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 09a9236d79..52fcf7cc20 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -81,6 +81,7 @@ controller_manager * The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 `_). * 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 `_). +* The ``--service-call-timeout`` was added as parameter to the helper scripts ``spawner.py``. Useful when the CPU load is high at startup and the service call does not return immediately (`#1808 `_). hardware_interface ****************** From e284af2171087fa6d2fca58fadcbd404dc7e0fb0 Mon Sep 17 00:00:00 2001 From: Takashi Sato Date: Thu, 28 Nov 2024 01:07:34 +0900 Subject: [PATCH 34/40] Fix missing virtual of on_export_[state|command]_interfaces methods (#1888) --- .../include/hardware_interface/system_interface.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 18da0e4012..32d0b8a48a 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -178,7 +178,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of shared pointers to the created and stored StateInterfaces */ - std::vector on_export_state_interfaces() + virtual std::vector on_export_state_interfaces() { // import the unlisted interfaces std::vector unlisted_interface_descriptions = @@ -270,7 +270,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of shared pointers to the created and stored CommandInterfaces */ - std::vector on_export_command_interfaces() + virtual std::vector on_export_command_interfaces() { // import the unlisted interfaces std::vector unlisted_interface_descriptions = From abb4c688bdfd6b47a659a884f71ae8736c4a19bc Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 27 Nov 2024 18:08:45 +0100 Subject: [PATCH 35/40] Add documentation on `ros2_control_node` and make lock_memory false by default (#1890) --- controller_manager/doc/userdoc.rst | 32 ++++++++++++++++++++ controller_manager/src/ros2_control_node.cpp | 2 +- 2 files changed, 33 insertions(+), 1 deletion(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 1ac867fa9b..494d164741 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -325,6 +325,38 @@ The workaround for this is to specify another node name remap rule in the ``Node auto cm = std::make_shared( executor, "_target_node_name", "some_optional_namespace", options); +Launching controller_manager with ros2_control_node +--------------------------------------------------- + +The controller_manager can be launched with the ros2_control_node executable. The following example shows how to launch the controller_manager with the ros2_control_node executable: + +.. code-block:: python + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controllers], + output="both", + ) + +The ros2_control_node executable uses the following parameters from the ``controller_manager`` node: + +lock_memory (optional; bool; default: false) + Locks the memory of the ``controller_manager`` node at startup to physical RAM in order to avoid page faults + and to prevent the node from being swapped out to disk. + Find more information about the setup for memory locking in the following link : `How to set ulimit values `_ + The following command can be used to set the memory locking limit temporarily : ``ulimit -l unlimited``. + +cpu_affinity (optional; int; default: -1) + Sets the CPU affinity of the ``controller_manager`` node to the specified CPU core. + The value of -1 means that the CPU affinity is not set. + +thread_priority (optional; int; default: 50) + Sets the thread priority of the ``controller_manager`` node to the specified value. The value must be between 0 and 99. + +use_sim_time (optional; bool; default: false) + Enables the use of simulation time in the ``controller_manager`` node. + Concepts ----------- diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index f8347df952..5dace36d34 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -59,7 +59,7 @@ int main(int argc, char ** argv) const bool use_sim_time = cm->get_parameter_or("use_sim_time", false); - const bool lock_memory = cm->get_parameter_or("lock_memory", true); + const bool lock_memory = cm->get_parameter_or("lock_memory", false); std::string message; if (lock_memory && !realtime_tools::lock_memory(message)) { From 73fe227419738a27f1829015664f084426b6237b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 28 Nov 2024 12:58:40 +0100 Subject: [PATCH 36/40] [Spawner] Accept parsing multiple `--param-file` arguments to spawner (#1805) --- .../controller_manager/__init__.py | 8 +- .../controller_manager_services.py | 165 ++++++++++------ .../controller_manager/launch_utils.py | 54 +++++- .../controller_manager/spawner.py | 21 ++- controller_manager/doc/userdoc.rst | 2 +- controller_manager/src/controller_manager.cpp | 21 ++- .../test/test_spawner_unspawner.cpp | 177 ++++++++++++++++-- doc/release_notes.rst | 1 + .../hardware_interface/controller_info.hpp | 2 +- .../ros2controlcli/verb/load_controller.py | 6 +- 10 files changed, 348 insertions(+), 109 deletions(-) diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py index 4a8d7daee5..638a28ce86 100644 --- a/controller_manager/controller_manager/__init__.py +++ b/controller_manager/controller_manager/__init__.py @@ -23,9 +23,9 @@ set_hardware_component_state, switch_controllers, unload_controller, - get_parameter_from_param_file, + get_parameter_from_param_files, set_controller_parameters, - set_controller_parameters_from_param_file, + set_controller_parameters_from_param_files, bcolors, ) @@ -40,8 +40,8 @@ "set_hardware_component_state", "switch_controllers", "unload_controller", - "get_parameter_from_param_file", + "get_parameter_from_param_files", "set_controller_parameters", - "set_controller_parameters_from_param_file", + "set_controller_parameters_from_param_files", "bcolors", ] diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 7d958920f3..16dee623e7 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -284,57 +284,90 @@ def unload_controller( ) -def get_parameter_from_param_file( - node, controller_name, namespace, parameter_file, parameter_name +def get_params_files_with_controller_parameters( + node, controller_name: str, namespace: str, parameter_files: list ): - with open(parameter_file) as f: - namespaced_controller = ( - f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}" - ) - WILDCARD_KEY = "/**" - ROS_PARAMS_KEY = "ros__parameters" - parameters = yaml.safe_load(f) - controller_param_dict = None - # check for the parameter in 'controller_name' or 'namespaced_controller' or '/**/namespaced_controller' or '/**/controller_name' - for key in [ - controller_name, - namespaced_controller, - f"{WILDCARD_KEY}/{controller_name}", - f"{WILDCARD_KEY}{namespaced_controller}", - ]: - if key in parameters: - if key == controller_name and namespace != "/": - node.get_logger().fatal( - f"{bcolors.FAIL}Missing namespace : {namespace} or wildcard in parameter file for controller : {controller_name}{bcolors.ENDC}" + controller_parameter_files = [] + for parameter_file in parameter_files: + if parameter_file in controller_parameter_files: + continue + with open(parameter_file) as f: + namespaced_controller = ( + f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}" + ) + WILDCARD_KEY = "/**" + parameters = yaml.safe_load(f) + # check for the parameter in 'controller_name' or 'namespaced_controller' or '/**/namespaced_controller' or '/**/controller_name' + for key in [ + controller_name, + namespaced_controller, + f"{WILDCARD_KEY}/{controller_name}", + f"{WILDCARD_KEY}{namespaced_controller}", + ]: + if key in parameters: + if key == controller_name and namespace != "/": + node.get_logger().fatal( + f"{bcolors.FAIL}Missing namespace : {namespace} or wildcard in parameter file for controller : {controller_name}{bcolors.ENDC}" + ) + break + controller_parameter_files.append(parameter_file) + + if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]: + controller_parameter_files.append(parameter_file) + return controller_parameter_files + + +def get_parameter_from_param_files( + node, controller_name: str, namespace: str, parameter_files: list, parameter_name: str +): + for parameter_file in parameter_files: + with open(parameter_file) as f: + namespaced_controller = ( + f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}" + ) + WILDCARD_KEY = "/**" + ROS_PARAMS_KEY = "ros__parameters" + parameters = yaml.safe_load(f) + controller_param_dict = None + # check for the parameter in 'controller_name' or 'namespaced_controller' or '/**/namespaced_controller' or '/**/controller_name' + for key in [ + controller_name, + namespaced_controller, + f"{WILDCARD_KEY}/{controller_name}", + f"{WILDCARD_KEY}{namespaced_controller}", + ]: + if key in parameters: + if key == controller_name and namespace != "/": + node.get_logger().fatal( + f"{bcolors.FAIL}Missing namespace : {namespace} or wildcard in parameter file for controller : {controller_name}{bcolors.ENDC}" + ) + break + controller_param_dict = parameters[key] + + if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]: + controller_param_dict = parameters[WILDCARD_KEY][key] + + if controller_param_dict and ( + not isinstance(controller_param_dict, dict) + or ROS_PARAMS_KEY not in controller_param_dict + ): + raise RuntimeError( + f"YAML file : {parameter_file} is not a valid ROS parameter file for controller node : {namespaced_controller}" ) + if ( + controller_param_dict + and ROS_PARAMS_KEY in controller_param_dict + and parameter_name in controller_param_dict[ROS_PARAMS_KEY] + ): break - controller_param_dict = parameters[key] - - if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]: - controller_param_dict = parameters[WILDCARD_KEY][key] - - if controller_param_dict and ( - not isinstance(controller_param_dict, dict) - or ROS_PARAMS_KEY not in controller_param_dict - ): - raise RuntimeError( - f"YAML file : {parameter_file} is not a valid ROS parameter file for controller node : {namespaced_controller}" - ) - if ( - controller_param_dict - and ROS_PARAMS_KEY in controller_param_dict - and parameter_name in controller_param_dict[ROS_PARAMS_KEY] - ): - break - - if controller_param_dict is None: - node.get_logger().fatal( - f"{bcolors.FAIL}Controller : {namespaced_controller} parameters not found in parameter file : {parameter_file}{bcolors.ENDC}" - ) - if parameter_name in controller_param_dict[ROS_PARAMS_KEY]: - return controller_param_dict[ROS_PARAMS_KEY][parameter_name] - return None + if controller_param_dict and parameter_name in controller_param_dict[ROS_PARAMS_KEY]: + return controller_param_dict[ROS_PARAMS_KEY][parameter_name] + if controller_param_dict is None: + node.get_logger().fatal( + f"{bcolors.FAIL}Controller : {namespaced_controller} parameters not found in parameter files : {parameter_files}{bcolors.ENDC}" + ) + return None def set_controller_parameters( @@ -378,26 +411,36 @@ def set_controller_parameters( return True -def set_controller_parameters_from_param_file( - node, controller_manager_name, controller_name, parameter_file, namespace=None +def set_controller_parameters_from_param_files( + node, controller_manager_name: str, controller_name: str, parameter_files: list, namespace=None ): - if parameter_file: - spawner_namespace = namespace if namespace else node.get_namespace() + spawner_namespace = namespace if namespace else node.get_namespace() + controller_parameter_files = get_params_files_with_controller_parameters( + node, controller_name, spawner_namespace, parameter_files + ) + if controller_parameter_files: set_controller_parameters( - node, controller_manager_name, controller_name, "params_file", parameter_file + node, + controller_manager_name, + controller_name, + "params_file", + controller_parameter_files, ) - controller_type = get_parameter_from_param_file( - node, controller_name, spawner_namespace, parameter_file, "type" + controller_type = get_parameter_from_param_files( + node, controller_name, spawner_namespace, controller_parameter_files, "type" ) - if controller_type: - if not set_controller_parameters( - node, controller_manager_name, controller_name, "type", controller_type - ): - return False + if controller_type and not set_controller_parameters( + node, controller_manager_name, controller_name, "type", controller_type + ): + return False - fallback_controllers = get_parameter_from_param_file( - node, controller_name, spawner_namespace, parameter_file, "fallback_controllers" + fallback_controllers = get_parameter_from_param_files( + node, + controller_name, + spawner_namespace, + controller_parameter_files, + "fallback_controllers", ) if fallback_controllers: if not set_controller_parameters( diff --git a/controller_manager/controller_manager/launch_utils.py b/controller_manager/controller_manager/launch_utils.py index c64b893156..e10096b675 100644 --- a/controller_manager/controller_manager/launch_utils.py +++ b/controller_manager/controller_manager/launch_utils.py @@ -20,7 +20,7 @@ def generate_controllers_spawner_launch_description( - controller_names: list, controller_params_file=None, extra_spawner_args=[] + controller_names: list, controller_params_files=None, extra_spawner_args=[] ): """ Generate launch description for loading a controller using spawner. @@ -37,8 +37,8 @@ def generate_controllers_spawner_launch_description( # Passing controller parameter file to load the controller (Controller type is retrieved from config file) generate_controllers_spawner_launch_description( ['joint_state_broadcaster'], - controller_params_file=os.path.join(get_package_share_directory('my_pkg'), - 'config', 'controller_params.yaml'), + controller_params_files=[os.path.join(get_package_share_directory('my_pkg'), + 'config', 'controller_params.yaml')], extra_spawner_args=[--load-only] ) @@ -62,8 +62,10 @@ def generate_controllers_spawner_launch_description( ] ) - if controller_params_file: - spawner_arguments += ["--param-file", controller_params_file] + if controller_params_files: + for controller_params_file in controller_params_files: + if controller_params_file: + spawner_arguments += ["--param-file", controller_params_file] # Setting --unload-on-kill if launch arg unload_on_kill is "true" # See https://github.com/ros2/launch/issues/290 @@ -98,11 +100,51 @@ def generate_controllers_spawner_launch_description( ) +def generate_controllers_spawner_launch_description_from_dict( + controller_info_dict: dict, extra_spawner_args=[] +): + """ + Generate launch description for loading a controller using spawner. + + controller_info_dict: dict + A dictionary with the following info: + - controller_name: str + The name of the controller to load as the key + - controller_params_file: str or list or None + The path to the controller parameter file or a list of paths to multiple parameter files + or None if no parameter file is needed as the value of the key + If a list is passed, the controller parameters will be overloaded in same order + extra_spawner_args: list + A list of extra arguments to pass to the controller spawner + """ + if not type(controller_info_dict) is dict: + raise ValueError(f"Invalid controller_info_dict type parsed {controller_info_dict}") + controller_names = controller_info_dict.keys() + controller_params_files = [] + for controller_name in controller_names: + controller_params_file = controller_info_dict[controller_name] + if controller_params_file: + if type(controller_params_file) is list: + controller_params_files.extend(controller_params_file) + elif type(controller_params_file) is str: + controller_params_files.append(controller_params_file) + else: + raise ValueError( + f"Invalid controller_params_file type parsed in the dict {controller_params_file}" + ) + return generate_controllers_spawner_launch_description( + controller_names=controller_names, + controller_params_files=controller_params_files, + extra_spawner_args=extra_spawner_args, + ) + + def generate_load_controller_launch_description( controller_name: str, controller_params_file=None, extra_spawner_args=[] ): + controller_params_files = [controller_params_file] if controller_params_file else None return generate_controllers_spawner_launch_description( controller_names=[controller_name], - controller_params_file=controller_params_file, + controller_params_file=controller_params_files, extra_spawner_args=extra_spawner_args, ) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 5d5e34e2a4..c5a23defe4 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -26,7 +26,7 @@ load_controller, switch_controllers, unload_controller, - set_controller_parameters_from_param_file, + set_controller_parameters_from_param_files, bcolors, ) from controller_manager.controller_manager_services import ServiceNotFoundError @@ -83,8 +83,11 @@ def main(args=None): parser.add_argument( "-p", "--param-file", - help="Controller param file to be loaded into controller node before configure", + help="Controller param file to be loaded into controller node before configure. " + "Pass multiple times to load different files for different controllers or to " + "override the parameters of the same controller.", default=None, + action="append", required=False, ) parser.add_argument( @@ -147,13 +150,15 @@ def main(args=None): args = parser.parse_args(command_line_args) controller_names = args.controller_names controller_manager_name = args.controller_manager - param_file = args.param_file + param_files = args.param_file controller_manager_timeout = args.controller_manager_timeout service_call_timeout = args.service_call_timeout switch_timeout = args.switch_timeout - if param_file and not os.path.isfile(param_file): - raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file) + if param_files: + for param_file in param_files: + if not os.path.isfile(param_file): + raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file) node = Node("spawner_" + controller_names[0]) @@ -198,12 +203,12 @@ def main(args=None): + bcolors.ENDC ) else: - if param_file: - if not set_controller_parameters_from_param_file( + if param_files: + if not set_controller_parameters_from_param_files( node, controller_manager_name, controller_name, - param_file, + param_files, spawner_namespace, ): return 1 diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 494d164741..5a86c3373b 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -169,7 +169,7 @@ 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 -p PARAM_FILE, --param-file PARAM_FILE - Controller param file to be loaded into controller node before configure + Controller param file to be loaded into controller node before configure. Pass multiple times to load different files for different controllers or to override the parameters of the same controller. -n NAMESPACE, --namespace NAMESPACE DEPRECATED Namespace for the controller_manager and the controller(s) --load-only Only load the controller and leave unconfigured. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d7e924a863..ebab7a2674 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -551,16 +551,16 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c // read_only params, dynamic maps lists etc // Now check if the parameters_file parameter exist const std::string param_name = controller_name + ".params_file"; - std::string parameters_file; + std::vector parameters_files; // Check if parameter has been declared if (!has_parameter(param_name)) { - declare_parameter(param_name, rclcpp::ParameterType::PARAMETER_STRING); + declare_parameter(param_name, rclcpp::ParameterType::PARAMETER_STRING_ARRAY); } - if (get_parameter(param_name, parameters_file) && !parameters_file.empty()) + if (get_parameter(param_name, parameters_files) && !parameters_files.empty()) { - controller_spec.info.parameters_file = parameters_file; + controller_spec.info.parameters_files = parameters_files; } const std::string fallback_ctrl_param = controller_name + ".fallback_controllers"; @@ -3250,14 +3250,17 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( node_options_arguments.push_back(arg); } - if (controller.info.parameters_file.has_value()) + if (controller.info.parameters_files.has_value()) { - if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG)) + for (const auto & parameters_file : controller.info.parameters_files.value()) { - node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); + if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG)) + { + node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); + } + node_options_arguments.push_back(RCL_PARAM_FILE_FLAG); + node_options_arguments.push_back(parameters_file); } - node_options_arguments.push_back(RCL_PARAM_FILE_FLAG); - node_options_arguments.push_back(controller.info.parameters_file.value()); } // ensure controller's `use_sim_time` parameter matches controller_manager's diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 74e1efeeed..ca580b1130 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -278,7 +278,8 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array()[0], + test_file_path); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; ASSERT_EQ( @@ -290,7 +291,7 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), + cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string_array()[0], test_file_path); EXPECT_EQ( @@ -308,7 +309,8 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) ASSERT_EQ( ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array()[0], + test_file_path); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); @@ -316,7 +318,7 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), + cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string_array()[0], test_file_path); } @@ -377,7 +379,7 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); ASSERT_EQ( ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ(cm_->get_parameter("ctrl_1.params_file").as_string(), test_file_path); + ASSERT_EQ(cm_->get_parameter("ctrl_1.params_file").as_string_array()[0], test_file_path); } // Try to spawn now the controller with fallback controllers inside the yaml @@ -392,7 +394,7 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); ASSERT_EQ( ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ(cm_->get_parameter("ctrl_1.params_file").as_string(), test_file_path); + ASSERT_EQ(cm_->get_parameter("ctrl_1.params_file").as_string_array()[0], test_file_path); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); @@ -401,7 +403,7 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) ctrl_2.info.fallback_controllers_names, testing::ElementsAre("ctrl_6", "ctrl_7", "ctrl_8")); ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ(cm_->get_parameter("ctrl_2.params_file").as_string(), test_file_path); + ASSERT_EQ(cm_->get_parameter("ctrl_2.params_file").as_string_array()[0], test_file_path); auto ctrl_3 = cm_->get_loaded_controllers()[2]; ASSERT_EQ(ctrl_3.info.name, "ctrl_3"); @@ -409,7 +411,7 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) ASSERT_THAT(ctrl_3.info.fallback_controllers_names, testing::ElementsAre("ctrl_9")); ASSERT_EQ( ctrl_3.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ(cm_->get_parameter("ctrl_3.params_file").as_string(), test_file_path); + ASSERT_EQ(cm_->get_parameter("ctrl_3.params_file").as_string_array()[0], test_file_path); } } @@ -695,7 +697,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), + cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file") + .as_string_array()[0], test_file_path); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; @@ -708,7 +711,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), + cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file") + .as_string_array()[0], test_file_path); EXPECT_EQ( @@ -725,14 +729,16 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ(cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string(), test_file_path); + ASSERT_EQ( + cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string_array()[0], test_file_path); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "ns_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ(cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string(), test_file_path); + ASSERT_EQ( + cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string_array()[0], test_file_path); } TEST_F( @@ -781,7 +787,8 @@ TEST_F( ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), + cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file") + .as_string_array()[0], test_file_path); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; @@ -794,7 +801,8 @@ TEST_F( chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), + cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file") + .as_string_array()[0], test_file_path); EXPECT_EQ( @@ -812,14 +820,16 @@ TEST_F( ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ(cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string(), test_file_path); + ASSERT_EQ( + cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string_array()[0], test_file_path); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "ns_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ(cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string(), test_file_path); + ASSERT_EQ( + cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string_array()[0], test_file_path); } TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_with_wildcard_entries_in_params_file) @@ -928,3 +938,138 @@ TEST_F( ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } + +TEST_F(TestLoadController, spawner_test_parsing_multiple_params_file) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + const std::string fallback_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_fallback_controllers.yaml"; + + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type ctrl_2 ctrl_1 " + "--load-only -c " + "test_controller_manager -p " + + test_file_path + " -p" + fallback_test_file_path), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 4ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + auto params_file_info = + cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], test_file_path); + + auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.type, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + params_file_info = + cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], test_file_path); + + auto ctrl_2 = cm_->get_loaded_controllers()[2]; + ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); + ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + params_file_info = cm_->get_parameter("ctrl_2.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], fallback_test_file_path); + + auto ctrl_1 = cm_->get_loaded_controllers()[3]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + params_file_info = cm_->get_parameter("ctrl_1.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], fallback_test_file_path); +} + +TEST_F(TestLoadController, spawner_test_parsing_same_params_file_multiple_times) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + const std::string fallback_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_fallback_controllers.yaml"; + + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type ctrl_2 ctrl_1 " + "--load-only -c " + "test_controller_manager -p " + + test_file_path + " -p" + fallback_test_file_path + " -p" + fallback_test_file_path + " -p" + + test_file_path), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 4ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + auto params_file_info = + cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], test_file_path); + + auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.type, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + params_file_info = + cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], test_file_path); + + auto ctrl_2 = cm_->get_loaded_controllers()[2]; + ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); + ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + params_file_info = cm_->get_parameter("ctrl_2.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], fallback_test_file_path); + + auto ctrl_1 = cm_->get_loaded_controllers()[3]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + params_file_info = cm_->get_parameter("ctrl_1.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], fallback_test_file_path); +} diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 52fcf7cc20..b7c4b934c4 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 spawner now supports parsing multiple ``-p`` or ``--param-file`` arguments, this should help in loading multiple parameter files for a controller or for multiple controllers (`#1805 `_). * ``--switch-timeout`` was added as parameter to the helper scripts ``spawner.py`` and ``unspawner.py``. Useful if controllers cannot be switched immediately, e.g., paused simulations at startup (`#1790 `_). * ``ros2_control_node`` can now handle the sim time used by different simulators, when ``use_sim_time`` is set to true (`#1810 `_). * The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 `_). diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp index a38fb99cb3..4563e2dc75 100644 --- a/hardware_interface/include/hardware_interface/controller_info.hpp +++ b/hardware_interface/include/hardware_interface/controller_info.hpp @@ -34,7 +34,7 @@ struct ControllerInfo std::string type; /// Controller param file - std::optional parameters_file; + std::optional> parameters_files; /// List of claimed interfaces by the controller. std::vector claimed_interfaces; diff --git a/ros2controlcli/ros2controlcli/verb/load_controller.py b/ros2controlcli/ros2controlcli/verb/load_controller.py index e47540a1df..5e29058a31 100644 --- a/ros2controlcli/ros2controlcli/verb/load_controller.py +++ b/ros2controlcli/ros2controlcli/verb/load_controller.py @@ -17,7 +17,7 @@ load_controller, list_controllers, switch_controllers, - set_controller_parameters_from_param_file, + set_controller_parameters_from_param_files, bcolors, ) @@ -68,11 +68,11 @@ def main(self, *, args): if not os.path.isabs(args.param_file): args.param_file = os.path.join(os.getcwd(), args.param_file) - if not set_controller_parameters_from_param_file( + if not set_controller_parameters_from_param_files( node, args.controller_manager, args.controller_name, - args.param_file, + [args.param_file], node.get_namespace(), ): return 1 From c9a0ab9cc2eb2f6825d8233c5d469a2095e54c53 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 29 Nov 2024 10:41:45 +0100 Subject: [PATCH 37/40] add logic for 'params_file' to handle both string and string_array (#1898) --- controller_manager/src/controller_manager.cpp | 41 +++++++++++-------- .../test/test_spawner_unspawner.cpp | 38 +++++++++++++++++ .../hardware_interface/controller_info.hpp | 2 +- 3 files changed, 64 insertions(+), 17 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index ebab7a2674..82546aaeda 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -551,16 +551,28 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c // read_only params, dynamic maps lists etc // Now check if the parameters_file parameter exist const std::string param_name = controller_name + ".params_file"; - std::vector parameters_files; + controller_spec.info.parameters_files.clear(); - // Check if parameter has been declared - if (!has_parameter(param_name)) + // get_parameter checks if parameter has been declared/set + rclcpp::Parameter params_files_parameter; + if (get_parameter(param_name, params_files_parameter)) { - declare_parameter(param_name, rclcpp::ParameterType::PARAMETER_STRING_ARRAY); - } - if (get_parameter(param_name, parameters_files) && !parameters_files.empty()) - { - controller_spec.info.parameters_files = parameters_files; + if (params_files_parameter.get_type() == rclcpp::ParameterType::PARAMETER_STRING_ARRAY) + { + controller_spec.info.parameters_files = params_files_parameter.as_string_array(); + } + else if (params_files_parameter.get_type() == rclcpp::ParameterType::PARAMETER_STRING) + { + controller_spec.info.parameters_files.push_back(params_files_parameter.as_string()); + } + else + { + RCLCPP_ERROR( + get_logger(), + "The 'params_file' param needs to be a string or a string array for '%s', but it is of " + "type %s", + controller_name.c_str(), params_files_parameter.get_type_name().c_str()); + } } const std::string fallback_ctrl_param = controller_name + ".fallback_controllers"; @@ -3250,17 +3262,14 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( node_options_arguments.push_back(arg); } - if (controller.info.parameters_files.has_value()) + for (const auto & parameters_file : controller.info.parameters_files) { - for (const auto & parameters_file : controller.info.parameters_files.value()) + if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG)) { - if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG)) - { - node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); - } - node_options_arguments.push_back(RCL_PARAM_FILE_FLAG); - node_options_arguments.push_back(parameters_file); + node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); } + node_options_arguments.push_back(RCL_PARAM_FILE_FLAG); + node_options_arguments.push_back(parameters_file); } // ensure controller's `use_sim_time` parameter matches controller_manager's diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index ca580b1130..cd2eee31f8 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -255,6 +255,44 @@ TEST_F(TestLoadController, multi_ctrls_test_type_in_param) } } +TEST_F(TestLoadController, spawner_test_with_params_file_string_parameter) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + cm_->set_parameter(rclcpp::Parameter( + "ctrl_with_parameters_and_type.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter( + rclcpp::Parameter("ctrl_with_parameters_and_type.params_file", test_file_path)); + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner("ctrl_with_parameters_and_type --load-only -c test_controller_manager"), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + auto ctrl_node = ctrl_with_parameters_and_type.c->get_node(); + ASSERT_THAT( + ctrl_with_parameters_and_type.info.parameters_files, + std::vector({test_file_path})); + if (!ctrl_node->has_parameter("joint_names")) + { + ctrl_node->declare_parameter("joint_names", std::vector({"random_joint"})); + } + ASSERT_THAT( + ctrl_node->get_parameter("joint_names").as_string_array(), + std::vector({"joint0"})); +} + TEST_F(TestLoadController, spawner_test_type_in_params_file) { const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp index 4563e2dc75..3ad89551d5 100644 --- a/hardware_interface/include/hardware_interface/controller_info.hpp +++ b/hardware_interface/include/hardware_interface/controller_info.hpp @@ -34,7 +34,7 @@ struct ControllerInfo std::string type; /// Controller param file - std::optional> parameters_files; + std::vector parameters_files; /// List of claimed interfaces by the controller. std::vector claimed_interfaces; From 2b22d0cdf28412fc4fde7200c367473dfba754af Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 29 Nov 2024 13:12:46 +0100 Subject: [PATCH 38/40] Add more parameter overriding tests by parsing multiple parameter files (#1899) --- controller_manager/CMakeLists.txt | 3 + ...test_controller_overriding_parameters.yaml | 5 + .../test_controller_spawner_with_type.yaml | 1 + .../test/test_spawner_unspawner.cpp | 114 ++++++++++++++++++ 4 files changed, 123 insertions(+) create mode 100644 controller_manager/test/test_controller_overriding_parameters.yaml diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 1bb84eb32c..ea5fc176cd 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -210,6 +210,9 @@ if(BUILD_TESTING) install(FILES test/test_controller_spawner_with_type.yaml DESTINATION test) + install(FILES test/test_controller_overriding_parameters.yaml + DESTINATION test) + ament_add_gmock(test_hardware_management_srvs test/test_hardware_management_srvs.cpp ) diff --git a/controller_manager/test/test_controller_overriding_parameters.yaml b/controller_manager/test/test_controller_overriding_parameters.yaml new file mode 100644 index 0000000000..115d0ed993 --- /dev/null +++ b/controller_manager/test/test_controller_overriding_parameters.yaml @@ -0,0 +1,5 @@ +ctrl_with_parameters_and_type: + ros__parameters: + interface_name: "impedance" + joint_offset: 0.2 + joint_names: ["joint10"] diff --git a/controller_manager/test/test_controller_spawner_with_type.yaml b/controller_manager/test/test_controller_spawner_with_type.yaml index 087994bd23..23fd69b216 100644 --- a/controller_manager/test/test_controller_spawner_with_type.yaml +++ b/controller_manager/test/test_controller_spawner_with_type.yaml @@ -2,6 +2,7 @@ ctrl_with_parameters_and_type: ros__parameters: type: "controller_manager/test_controller" joint_names: ["joint0"] + interface_name: "position" /**: chainable_ctrl_with_parameters_and_type: diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index cd2eee31f8..70a5480a06 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -291,6 +291,12 @@ TEST_F(TestLoadController, spawner_test_with_params_file_string_parameter) ASSERT_THAT( ctrl_node->get_parameter("joint_names").as_string_array(), std::vector({"joint0"})); + + if (!ctrl_node->has_parameter("interface_name")) + { + ctrl_node->declare_parameter("interface_name", "invalid_interface"); + } + ASSERT_EQ(ctrl_node->get_parameter("interface_name").as_string(), "position"); } TEST_F(TestLoadController, spawner_test_type_in_params_file) @@ -397,6 +403,114 @@ TEST_F(TestLoadController, unload_on_kill_activate_as_group) ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul); } +TEST_F(TestLoadController, spawner_test_to_check_parameter_overriding) +{ + const std::string main_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + const std::string overriding_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_overriding_parameters.yaml"; + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager -p " + + main_test_file_path + " -p " + overriding_test_file_path), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_THAT( + cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array(), + std::vector({main_test_file_path, overriding_test_file_path})); + auto ctrl_node = ctrl_with_parameters_and_type.c->get_node(); + ASSERT_THAT( + ctrl_with_parameters_and_type.info.parameters_files, + std::vector({main_test_file_path, overriding_test_file_path})); + if (!ctrl_node->has_parameter("joint_names")) + { + ctrl_node->declare_parameter("joint_names", std::vector({"random_joint"})); + } + ASSERT_THAT( + ctrl_node->get_parameter("joint_names").as_string_array(), + std::vector({"joint10"})); + + if (!ctrl_node->has_parameter("interface_name")) + { + ctrl_node->declare_parameter("interface_name", "invalid_interface"); + } + ASSERT_EQ(ctrl_node->get_parameter("interface_name").as_string(), "impedance") + << "The parameter should be overridden"; + + if (!ctrl_node->has_parameter("joint_offset")) + { + ctrl_node->declare_parameter("joint_offset", -M_PI); + } + ASSERT_EQ(ctrl_node->get_parameter("joint_offset").as_double(), 0.2); +} + +TEST_F(TestLoadController, spawner_test_to_check_parameter_overriding_reverse) +{ + const std::string main_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_overriding_parameters.yaml"; + const std::string overriding_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager -p " + + main_test_file_path + " -p " + overriding_test_file_path), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_THAT( + cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array(), + std::vector({main_test_file_path, overriding_test_file_path})); + auto ctrl_node = ctrl_with_parameters_and_type.c->get_node(); + ASSERT_THAT( + ctrl_with_parameters_and_type.info.parameters_files, + std::vector({main_test_file_path, overriding_test_file_path})); + if (!ctrl_node->has_parameter("joint_names")) + { + ctrl_node->declare_parameter("joint_names", std::vector({"random_joint"})); + } + ASSERT_THAT( + ctrl_node->get_parameter("joint_names").as_string_array(), + std::vector({"joint0"})); + + if (!ctrl_node->has_parameter("interface_name")) + { + ctrl_node->declare_parameter("interface_name", "invalid_interface"); + } + ASSERT_EQ(ctrl_node->get_parameter("interface_name").as_string(), "position") + << "The parameter should be overridden"; + + if (!ctrl_node->has_parameter("joint_offset")) + { + ctrl_node->declare_parameter("joint_offset", -M_PI); + } + ASSERT_EQ(ctrl_node->get_parameter("joint_offset").as_double(), 0.2); +} + TEST_F(TestLoadController, spawner_test_fallback_controllers) { const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + From dd8441417cb36a880f947feac43d9ee1eb200dfa Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 1 Dec 2024 11:10:20 +0100 Subject: [PATCH 39/40] Bump version of pre-commit hooks (#1902) Co-authored-by: christophfroehlich <3367244+christophfroehlich@users.noreply.github.com> --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 205e0f63ab..75c5402ffe 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -63,7 +63,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v19.1.3 + rev: v19.1.4 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.4 + rev: 0.30.0 hooks: - id: check-github-workflows args: ["--verbose"] From 89fd4ef05e1fc107c9d1b2b122292665ccca6f36 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 3 Dec 2024 16:28:40 +0100 Subject: [PATCH 40/40] [Diagnostics] Add diagnostics of execution time and periodicity of the controllers and controller_manager (#1871) --- .../controller_interface_base.hpp | 27 +- .../src/controller_interface_base.cpp | 24 +- controller_manager/CMakeLists.txt | 11 +- .../doc/parameters_context.yaml | 21 ++ controller_manager/doc/userdoc.rst | 36 +-- .../controller_manager/controller_manager.hpp | 10 +- .../controller_manager/controller_spec.hpp | 13 + controller_manager/package.xml | 2 + controller_manager/src/controller_manager.cpp | 286 ++++++++++++++---- .../src/controller_manager_parameters.yaml | 136 +++++++++ controller_manager/src/ros2_control_node.cpp | 2 +- .../test/test_controller_manager.cpp | 242 ++++++++++++++- .../test/test_hardware_management_srvs.cpp | 4 +- 13 files changed, 703 insertions(+), 111 deletions(-) create mode 100644 controller_manager/doc/parameters_context.yaml create mode 100644 controller_manager/src/controller_manager_parameters.yaml diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index ae3804919c..e41779cf7b 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -71,6 +71,25 @@ struct ControllerUpdateStats unsigned int total_triggers; unsigned int failed_triggers; }; + +/** + * Struct to store the status of the controller update method. + * The status contains information if the update was triggered successfully, the result of the + * update method and the execution duration of the update method. The status is used to provide + * feedback to the controller_manager. + * @var successful: true if the update was triggered successfully, false if not. + * @var result: return_type::OK if update is successfully, otherwise return_type::ERROR. + * @var execution_time: duration of the execution of the update method. + * @var period: period of the update method. + */ +struct ControllerUpdateStatus +{ + bool successful = true; + return_type result = return_type::OK; + std::optional execution_time = std::nullopt; + std::optional period = std::nullopt; +}; + /** * Base interface class for an controller. The interface may not be used to implement a controller. * The class provides definitions for `ControllerInterface` and `ChainableControllerInterface` @@ -175,13 +194,11 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * * \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 - * \returns A pair with the first element being a boolean indicating if the async callback method - * was triggered and the second element being the last return value of the async function. For - * more details check the AsyncFunctionHandler implementation in `realtime_tools` package. + * \returns ControllerUpdateStatus. The status contains information if the update was triggered + * successfully, the result of the update method and the execution duration of the update method. */ CONTROLLER_INTERFACE_PUBLIC - std::pair trigger_update( - const rclcpp::Time & time, const rclcpp::Duration & period); + ControllerUpdateStatus trigger_update(const rclcpp::Time & time, const rclcpp::Duration & period); CONTROLLER_INTERFACE_PUBLIC std::shared_ptr get_node(); diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 0713ec3c25..065fe77061 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -159,12 +159,14 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() c return node_->get_current_state(); } -std::pair ControllerInterfaceBase::trigger_update( +ControllerUpdateStatus ControllerInterfaceBase::trigger_update( const rclcpp::Time & time, const rclcpp::Duration & period) { + ControllerUpdateStatus status; trigger_stats_.total_triggers++; if (is_async()) { + const rclcpp::Time last_trigger_time = async_handler_->get_current_callback_time(); const auto result = async_handler_->trigger_async_callback(time, period); if (!result.first) { @@ -174,12 +176,28 @@ std::pair ControllerInterfaceBase::trigger_update( "The controller missed %u update cycles out of %u total triggers.", trigger_stats_.failed_triggers, trigger_stats_.total_triggers); } - return result; + status.successful = result.first; + status.result = result.second; + const auto execution_time = async_handler_->get_last_execution_time(); + if (execution_time.count() > 0) + { + status.execution_time = execution_time; + } + if (last_trigger_time.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + status.period = time - last_trigger_time; + } } else { - return std::make_pair(true, update(time, period)); + const auto start_time = std::chrono::steady_clock::now(); + status.successful = true; + status.result = update(time, period); + status.execution_time = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start_time); + status.period = period; } + return status; } std::shared_ptr ControllerInterfaceBase::get_node() diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index ea5fc176cd..dba9531e45 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -16,6 +16,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp realtime_tools std_msgs + libstatistics_collector + generate_parameter_library ) find_package(ament_cmake REQUIRED) @@ -27,6 +29,10 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +generate_parameter_library(controller_manager_parameters + src/controller_manager_parameters.yaml +) + add_library(controller_manager SHARED src/controller_manager.cpp ) @@ -35,6 +41,9 @@ target_include_directories(controller_manager PUBLIC $ $ ) +target_link_libraries(controller_manager PUBLIC + controller_manager_parameters +) ament_target_dependencies(controller_manager PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, @@ -236,7 +245,7 @@ install( DESTINATION include/controller_manager ) install( - TARGETS controller_manager + TARGETS controller_manager controller_manager_parameters EXPORT export_controller_manager RUNTIME DESTINATION bin LIBRARY DESTINATION lib diff --git a/controller_manager/doc/parameters_context.yaml b/controller_manager/doc/parameters_context.yaml new file mode 100644 index 0000000000..a015765c79 --- /dev/null +++ b/controller_manager/doc/parameters_context.yaml @@ -0,0 +1,21 @@ +hardware_components_initial_state: | + Map of parameters for controlled lifecycle management of hardware components. + The names of the components are defined as attribute of ````-tag in ``robot_description``. + Hardware components found in ``robot_description``, but without explicit state definition will be immediately activated. + Detailed explanation of each parameter is given below. + The full structure of the map is given in the following example: + + .. code-block:: yaml + + hardware_components_initial_state: + unconfigured: + - "arm1" + - "arm2" + inactive: + - "base3" + +diagnostics.threshold.controllers.periodicity: | + The ``periodicity`` diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity. + +diagnostics.threshold.controllers.execution_time: | + The ``execution_time`` diagnostics will be published for all controllers. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle. diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 5a86c3373b..872a695d0f 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -57,32 +57,6 @@ robot_description [std_msgs::msg::String] Parameters ----------- -hardware_components_initial_state - Map of parameters for controlled lifecycle management of hardware components. - The names of the components are defined as attribute of ````-tag in ``robot_description``. - Hardware components found in ``robot_description``, but without explicit state definition will be immediately activated. - Detailed explanation of each parameter is given below. - The full structure of the map is given in the following example: - -.. code-block:: yaml - - hardware_components_initial_state: - unconfigured: - - "arm1" - - "arm2" - inactive: - - "base3" - -hardware_components_initial_state.unconfigured (optional; list; default: empty) - Defines which hardware components will be only loaded immediately when controller manager is started. - -hardware_components_initial_state.inactive (optional; list; default: empty) - Defines which hardware components will be configured immediately when controller manager is started. - -update_rate (mandatory; integer) - The frequency of controller manager's real-time update loop. - This loop reads states from hardware, updates controller and writes commands to hardware. - .type Name of a plugin exported using ``pluginlib`` for a controller. This is a class from which controller's instance with name "``controller_name``" is created. @@ -99,6 +73,16 @@ update_rate (mandatory; integer) The fallback controllers activation is subject to the availability of the state and command interfaces at the time of activation. It is recommended to test the fallback strategy in simulation before deploying it on the real robot. +.. generate_parameter_library_details:: + ../src/controller_manager_parameters.yaml + parameters_context.yaml + +**An example parameter file:** + +.. generate_parameter_library_default:: + ../src/controller_manager_parameters.yaml + + Handling Multiple Controller Managers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 068eefc1f9..332d565238 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -50,6 +50,8 @@ namespace controller_manager { +class ParamListener; +class Params; using ControllersListIterator = std::vector::const_iterator; CONTROLLER_MANAGER_PUBLIC rclcpp::NodeOptions get_cm_node_options(); @@ -346,7 +348,7 @@ class ControllerManager : public rclcpp::Node // Per controller update rate support unsigned int update_loop_counter_ = 0; - unsigned int update_rate_ = 100; + unsigned int update_rate_; std::vector> chained_controllers_configuration_; std::unique_ptr resource_manager_; @@ -357,6 +359,8 @@ class ControllerManager : public rclcpp::Node const std::string & command_interface); void init_controller_manager(); + void initialize_parameters(); + /** * Clear request lists used when switching controllers. The lists are shared between "callback" * and "control loop" threads. @@ -473,6 +477,8 @@ class ControllerManager : public rclcpp::Node */ rclcpp::NodeOptions determine_controller_node_options(const ControllerSpec & controller) const; + std::shared_ptr cm_param_listener_; + std::shared_ptr params_; diagnostic_updater::Updater diagnostics_updater_; std::shared_ptr executor_; @@ -603,6 +609,8 @@ class ControllerManager : public rclcpp::Node rclcpp::Subscription::SharedPtr robot_description_subscription_; rclcpp::TimerBase::SharedPtr robot_description_notification_timer_; + controller_manager::MovingAverageStatistics periodicity_stats_; + struct SwitchParams { void reset() diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 0f44867814..9cc508772d 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -24,9 +24,13 @@ #include #include "controller_interface/controller_interface_base.hpp" #include "hardware_interface/controller_info.hpp" +#include "libstatistics_collector/moving_average_statistics/moving_average.hpp" namespace controller_manager { + +using MovingAverageStatistics = + libstatistics_collector::moving_average_statistics::MovingAverageStatistics; /// Controller Specification /** * This struct contains both a pointer to a given controller, \ref c, as well @@ -35,9 +39,18 @@ namespace controller_manager */ struct ControllerSpec { + ControllerSpec() + { + last_update_cycle_time = std::make_shared(0, 0, RCL_CLOCK_UNINITIALIZED); + execution_time_statistics = std::make_shared(); + periodicity_statistics = std::make_shared(); + } + hardware_interface::ControllerInfo info; controller_interface::ControllerInterfaceBaseSharedPtr c; std::shared_ptr last_update_cycle_time; + std::shared_ptr execution_time_statistics; + std::shared_ptr periodicity_statistics; }; struct ControllerChainSpec diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 18189d5d16..42211e4e40 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -28,6 +28,8 @@ ros2param ros2run std_msgs + libstatistics_collector + generate_parameter_library ament_cmake_gmock ament_cmake_pytest diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 82546aaeda..56e95c9f7b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -28,6 +28,8 @@ #include "rclcpp/version.h" #include "rclcpp_lifecycle/state.hpp" +#include "controller_manager_parameters.hpp" + namespace // utility { static constexpr const char * kControllerInterfaceNamespace = "controller_interface"; @@ -237,6 +239,7 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), cm_node_options_(options) { + initialize_parameters(); init_controller_manager(); } @@ -245,10 +248,6 @@ ControllerManager::ControllerManager( bool activate_all_hw_components, const std::string & manager_node_name, const std::string & node_namespace, const rclcpp::NodeOptions & options) : rclcpp::Node(manager_node_name, node_namespace, options), - update_rate_(get_parameter_or("update_rate", 100)), - resource_manager_(std::make_unique( - urdf, this->get_node_clock_interface(), this->get_node_logging_interface(), - activate_all_hw_components, update_rate_)), diagnostics_updater_(this), executor_(executor), loader_(std::make_shared>( @@ -258,6 +257,10 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), cm_node_options_(options) { + initialize_parameters(); + resource_manager_ = std::make_unique( + urdf, this->get_node_clock_interface(), this->get_node_logging_interface(), + activate_all_hw_components, params_->update_rate); init_controller_manager(); } @@ -276,18 +279,13 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), cm_node_options_(options) { + initialize_parameters(); init_controller_manager(); } void ControllerManager::init_controller_manager() { // Get parameters needed for RT "update" loop to work - if (!get_parameter("update_rate", update_rate_)) - { - RCLCPP_WARN( - get_logger(), "'update_rate' parameter not set, using default value of %d Hz.", update_rate_); - } - if (is_resource_manager_initialized()) { init_services(); @@ -313,6 +311,7 @@ void ControllerManager::init_controller_manager() robot_description_subscription_->get_topic_name()); // Setup diagnostics + periodicity_stats_.Reset(); diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); @@ -324,6 +323,25 @@ void ControllerManager::init_controller_manager() &ControllerManager::controller_manager_diagnostic_callback); } +void ControllerManager::initialize_parameters() +{ + // Initialize parameters + try + { + cm_param_listener_ = std::make_shared( + this->get_node_parameters_interface(), this->get_logger()); + params_ = std::make_shared(cm_param_listener_->get_params()); + update_rate_ = static_cast(params_->update_rate); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + this->get_logger(), + "Exception thrown while initializing controller manager parameters: %s \n", e.what()); + throw e; + } +} + void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description) { RCLCPP_INFO(get_logger(), "Received robot description from topic."); @@ -365,51 +383,52 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript using lifecycle_msgs::msg::State; auto set_components_to_state = - [&](const std::string & parameter_name, rclcpp_lifecycle::State state) + [&](const std::vector & components_to_set, rclcpp_lifecycle::State state) { - std::vector components_to_set = std::vector({}); - if (get_parameter(parameter_name, components_to_set)) + for (const auto & component : components_to_set) { - for (const auto & component : components_to_set) + if (component.empty()) { - if (component.empty()) - { - continue; - } - if (components_to_activate.find(component) == components_to_activate.end()) - { - RCLCPP_WARN( - get_logger(), "Hardware component '%s' is unknown, therefore not set in '%s' state.", - component.c_str(), state.label().c_str()); - } - else + continue; + } + if (components_to_activate.find(component) == components_to_activate.end()) + { + RCLCPP_WARN( + get_logger(), "Hardware component '%s' is unknown, therefore not set in '%s' state.", + component.c_str(), state.label().c_str()); + } + else + { + RCLCPP_INFO( + get_logger(), "Setting component '%s' to '%s' state.", component.c_str(), + state.label().c_str()); + if ( + resource_manager_->set_component_state(component, state) == + hardware_interface::return_type::ERROR) { - RCLCPP_INFO( - get_logger(), "Setting component '%s' to '%s' state.", component.c_str(), - state.label().c_str()); - if ( - resource_manager_->set_component_state(component, state) == - hardware_interface::return_type::ERROR) - { - throw std::runtime_error( - "Failed to set the initial state of the component : " + component + " to " + - state.label()); - } - components_to_activate.erase(component); + throw std::runtime_error( + "Failed to set the initial state of the component : " + component + " to " + + state.label()); } + components_to_activate.erase(component); } } }; + if (cm_param_listener_->is_old(*params_)) + { + *params_ = cm_param_listener_->get_params(); + } + // unconfigured (loaded only) set_components_to_state( - "hardware_components_initial_state.unconfigured", + params_->hardware_components_initial_state.unconfigured, rclcpp_lifecycle::State( State::PRIMARY_STATE_UNCONFIGURED, hardware_interface::lifecycle_state_names::UNCONFIGURED)); // inactive (configured) set_components_to_state( - "hardware_components_initial_state.inactive", + params_->hardware_components_initial_state.inactive, rclcpp_lifecycle::State( State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE)); @@ -545,6 +564,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.info.type = controller_type; controller_spec.last_update_cycle_time = std::make_shared( 0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); + controller_spec.execution_time_statistics = std::make_shared(); + controller_spec.periodicity_statistics = std::make_shared(); // We have to fetch the parameters_file at the time of loading the controller, because this way we // can load them at the creation of the LifeCycleNode and this helps in using the features such as @@ -1774,6 +1795,8 @@ void ControllerManager::activate_controllers( try { + found_it->periodicity_statistics->Reset(); + found_it->execution_time_statistics->Reset(); const auto new_state = controller->get_node()->activate(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { @@ -2255,6 +2278,7 @@ std::vector ControllerManager::get_controller_names() void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & period) { + periodicity_stats_.AddMeasurement(1.0 / period.seconds()); auto [ok, failed_hardware_names] = resource_manager_->read(time, period); if (!ok) @@ -2367,12 +2391,14 @@ controller_interface::return_type ControllerManager::update( : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); const rclcpp::Time current_time = get_clock()->now(); + bool first_update_cycle = false; if ( *loaded_controller.last_update_cycle_time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) { + first_update_cycle = true; // it is zero after activation - *loaded_controller.last_update_cycle_time = current_time - controller_period; + *loaded_controller.last_update_cycle_time = current_time; RCLCPP_DEBUG( 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()); @@ -2390,7 +2416,7 @@ controller_interface::return_type ControllerManager::update( run_controller_at_cm_rate || (time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) || - (controller_actual_period.seconds() * controller_update_rate >= 0.99); + (controller_actual_period.seconds() * controller_update_rate >= 0.99) || first_update_cycle; RCLCPP_DEBUG( get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '", @@ -2404,8 +2430,20 @@ controller_interface::return_type ControllerManager::update( // Catch exceptions thrown by the controller update function try { - std::tie(trigger_status, controller_ret) = - loaded_controller.c->trigger_update(time, controller_actual_period); + const auto trigger_result = + loaded_controller.c->trigger_update(current_time, controller_actual_period); + trigger_status = trigger_result.successful; + controller_ret = trigger_result.result; + if (trigger_status && trigger_result.execution_time.has_value()) + { + loaded_controller.execution_time_statistics->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); + } + if (!first_update_cycle && trigger_status && trigger_result.period.has_value()) + { + loaded_controller.periodicity_statistics->AddMeasurement( + 1.0 / trigger_result.period.value().seconds()); + } } catch (const std::exception & e) { @@ -3062,34 +3100,140 @@ void ControllerManager::controller_activity_diagnostic_callback( std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); const std::vector & controllers = rt_controllers_wrapper_.get_updated_list(guard); bool all_active = true; + const std::string periodicity_suffix = ".periodicity"; + const std::string exec_time_suffix = ".execution_time"; + const std::string state_suffix = ".state"; + + if (cm_param_listener_->is_old(*params_)) + { + *params_ = cm_param_listener_->get_params(); + } + + auto make_stats_string = + [](const auto & statistics_data, const std::string & measurement_unit) -> std::string + { + std::ostringstream oss; + oss << std::fixed << std::setprecision(2); + oss << "Avg: " << statistics_data.average << " [" << statistics_data.min << " - " + << statistics_data.max << "] " << measurement_unit + << ", StdDev: " << statistics_data.standard_deviation; + return oss.str(); + }; + + // Variable to define the overall status of the controller diagnostics + auto level = diagnostic_msgs::msg::DiagnosticStatus::OK; + + std::vector high_exec_time_controllers; + std::vector bad_periodicity_async_controllers; for (size_t i = 0; i < controllers.size(); ++i) { + const bool is_async = controllers[i].c->is_async(); if (!is_controller_active(controllers[i].c)) { all_active = false; } - stat.add(controllers[i].info.name, controllers[i].c->get_lifecycle_state().label()); + stat.add( + controllers[i].info.name + state_suffix, controllers[i].c->get_lifecycle_state().label()); + if (is_controller_active(controllers[i].c)) + { + const auto periodicity_stats = controllers[i].periodicity_statistics->GetStatistics(); + const auto exec_time_stats = controllers[i].execution_time_statistics->GetStatistics(); + stat.add( + controllers[i].info.name + exec_time_suffix, make_stats_string(exec_time_stats, "us")); + if (is_async) + { + stat.add( + controllers[i].info.name + periodicity_suffix, + make_stats_string(periodicity_stats, "Hz") + + " -> Desired : " + std::to_string(controllers[i].c->get_update_rate()) + " Hz"); + const double periodicity_error = std::abs( + periodicity_stats.average - static_cast(controllers[i].c->get_update_rate())); + if ( + periodicity_error > + params_->diagnostics.threshold.controllers.periodicity.mean_error.error || + periodicity_stats.standard_deviation > + params_->diagnostics.threshold.controllers.periodicity.standard_deviation.error) + { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + add_element_to_list(bad_periodicity_async_controllers, controllers[i].info.name); + } + else if ( + periodicity_error > + params_->diagnostics.threshold.controllers.periodicity.mean_error.warn || + periodicity_stats.standard_deviation > + params_->diagnostics.threshold.controllers.periodicity.standard_deviation.warn) + { + if (level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) + { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } + add_element_to_list(bad_periodicity_async_controllers, controllers[i].info.name); + } + } + const double max_exp_exec_time = is_async ? 1.e6 / controllers[i].c->get_update_rate() : 0.0; + if ( + (exec_time_stats.average - max_exp_exec_time) > + params_->diagnostics.threshold.controllers.execution_time.mean_error.error || + exec_time_stats.standard_deviation > + params_->diagnostics.threshold.controllers.execution_time.standard_deviation.error) + { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + high_exec_time_controllers.push_back(controllers[i].info.name); + } + else if ( + (exec_time_stats.average - max_exp_exec_time) > + params_->diagnostics.threshold.controllers.execution_time.mean_error.warn || + exec_time_stats.standard_deviation > + params_->diagnostics.threshold.controllers.execution_time.standard_deviation.warn) + { + if (level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) + { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } + high_exec_time_controllers.push_back(controllers[i].info.name); + } + } + } + + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::OK, + all_active ? "All controllers are active" : "Not all controllers are active"); + + if (!high_exec_time_controllers.empty()) + { + std::string high_exec_time_controllers_string; + for (const auto & controller : high_exec_time_controllers) + { + high_exec_time_controllers_string.append(controller); + high_exec_time_controllers_string.append(" "); + } + stat.mergeSummary( + level, + "\nControllers with high execution time : [ " + high_exec_time_controllers_string + "]"); + } + if (!bad_periodicity_async_controllers.empty()) + { + std::string bad_periodicity_async_controllers_string; + for (const auto & controller : bad_periodicity_async_controllers) + { + bad_periodicity_async_controllers_string.append(controller); + bad_periodicity_async_controllers_string.append(" "); + } + stat.mergeSummary( + level, + "\nControllers with bad periodicity : [ " + bad_periodicity_async_controllers_string + "]"); } if (!atleast_one_hw_active) { - stat.summary( + stat.mergeSummary( diagnostic_msgs::msg::DiagnosticStatus::ERROR, "No hardware components are currently active to activate controllers"); } - else + else if (controllers.empty()) { - if (controllers.empty()) - { - stat.summary( - diagnostic_msgs::msg::DiagnosticStatus::WARN, "No controllers are currently loaded"); - } - else - { - stat.summary( - diagnostic_msgs::msg::DiagnosticStatus::OK, - all_active ? "All controllers are active" : "Not all controllers are active"); - } + stat.mergeSummary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "No controllers are currently loaded"); } } @@ -3142,7 +3286,14 @@ void ControllerManager::hardware_components_diagnostic_callback( void ControllerManager::controller_manager_diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { + const std::string periodicity_stat_name = "periodicity"; + const auto cm_stats = periodicity_stats_.GetStatistics(); stat.add("update_rate", std::to_string(get_update_rate())); + stat.add(periodicity_stat_name + ".average", std::to_string(cm_stats.average)); + stat.add( + periodicity_stat_name + ".standard_deviation", std::to_string(cm_stats.standard_deviation)); + stat.add(periodicity_stat_name + ".min", std::to_string(cm_stats.min)); + stat.add(periodicity_stat_name + ".max", std::to_string(cm_stats.max)); if (is_resource_manager_initialized()) { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Controller Manager is running"); @@ -3161,6 +3312,27 @@ void ControllerManager::controller_manager_diagnostic_callback( "Resource Manager is not initialized properly!"); } } + + const double periodicity_error = std::abs(cm_stats.average - get_update_rate()); + const std::string diag_summary = + "Controller Manager has bad periodicity : " + std::to_string(cm_stats.average) + + " Hz. Expected consistent " + std::to_string(get_update_rate()) + " Hz"; + if ( + periodicity_error > + params_->diagnostics.threshold.controller_manager.periodicity.mean_error.error || + cm_stats.standard_deviation > + params_->diagnostics.threshold.controller_manager.periodicity.standard_deviation.error) + { + stat.mergeSummary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, diag_summary); + } + else if ( + periodicity_error > + params_->diagnostics.threshold.controller_manager.periodicity.mean_error.warn || + cm_stats.standard_deviation > + params_->diagnostics.threshold.controller_manager.periodicity.standard_deviation.warn) + { + stat.mergeSummary(diagnostic_msgs::msg::DiagnosticStatus::WARN, diag_summary); + } } void ControllerManager::update_list_with_controller_chain( diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml new file mode 100644 index 0000000000..1bb9b152b1 --- /dev/null +++ b/controller_manager/src/controller_manager_parameters.yaml @@ -0,0 +1,136 @@ +controller_manager: + update_rate: { + type: int, + default_value: 100, + read_only: true, + description: "The frequency of controller manager's real-time update loop. This loop reads states from hardware, updates controllers and writes commands to hardware." + } + + hardware_components_initial_state: + unconfigured: { + type: string_array, + default_value: [], + description: "Defines which hardware components will be only loaded when controller manager is started. These hardware components will need to be configured and activated manually or via a hardware spawner.", + validation: { + unique<>: null, + } + } + + inactive: { + type: string_array, + default_value: [], + description: "Defines which hardware components will be configured when controller manager is started. These hardware components will need to be activated manually or via a hardware spawner.", + validation: { + unique<>: null, + } + } + + diagnostics: + threshold: + controller_manager: + periodicity: + mean_error: + warn: { + type: double, + default_value: 5.0, + description: "The warning threshold for the mean error of the controller manager's periodicity. If the mean error exceeds this threshold, a warning diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 10.0, + description: "The error threshold for the mean error of the controller manager's periodicity. If the mean error exceeds this threshold, an error diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + standard_deviation: + warn: { + type: double, + default_value: 5.0, + description: "The warning threshold for the standard deviation of the controller manager's periodicity. If the standard deviation exceeds this threshold, a warning diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 10.0, + description: "The error threshold for the standard deviation of the controller manager's periodicity. If the standard deviation exceeds this threshold, an error diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + controllers: + periodicity: + mean_error: + warn: { + type: double, + default_value: 5.0, + description: "The warning threshold for the mean error of the controller update loop. If the mean error exceeds this threshold, a warning diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 10.0, + description: "The error threshold for the mean error of the controller update loop. If the mean error exceeds this threshold, an error diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.", + validation: { + gt<>: 0.0, + } + } + standard_deviation: + warn: { + type: double, + default_value: 5.0, + description: "The warning threshold for the standard deviation of the controller update loop. If the standard deviation exceeds this threshold, a warning diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 10.0, + description: "The error threshold for the standard deviation of the controller update loop. If the standard deviation exceeds this threshold, an error diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.", + validation: { + gt<>: 0.0, + } + } + execution_time: + mean_error: + warn: { + type: double, + default_value: 1000.0, + description: "The warning threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, a warning diagnostic will be published. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 2000.0, + description: "The error threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, an error diagnostic will be published. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle", + validation: { + gt<>: 0.0, + } + } + standard_deviation: + warn: { + type: double, + default_value: 100.0, + description: "The warning threshold for the standard deviation of the controller's update cycle execution time. If the standard deviation exceeds this threshold, a warning diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 200.0, + description: "The error threshold for the standard deviation of the controller's update cycle execution time. If the standard deviation exceeds this threshold, an error diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 5dace36d34..da79de76a3 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -109,7 +109,7 @@ int main(int argc, char ** argv) next_iteration_time{cm_now}; // for calculating the measured period of the loop - rclcpp::Time previous_time = cm->now(); + rclcpp::Time previous_time = cm->now() - period; while (rclcpp::ok()) { diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 0c4e51985f..b715bc23dc 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -366,6 +366,7 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle) EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + time_ = cm_->get_clock()->now(); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -373,6 +374,21 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle) std::this_thread::sleep_for( std::chrono::milliseconds(1000 / (test_controller->get_update_rate()))); EXPECT_EQ(test_controller->internal_counter, 1u); + EXPECT_EQ(test_controller->update_period_.seconds(), 0.0) + << "The first trigger cycle should have zero period"; + + const double exp_period = (cm_->get_clock()->now() - time_).seconds(); + time_ = cm_->get_clock()->now(); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(test_controller->internal_counter, 1u); + std::this_thread::sleep_for( + std::chrono::milliseconds(1000 / (test_controller->get_update_rate()))); + EXPECT_EQ(test_controller->internal_counter, 2u); + EXPECT_THAT( + test_controller->update_period_.seconds(), + testing::AllOf(testing::Gt(0.6 * exp_period), testing::Lt((1.4 * exp_period)))); size_t last_internal_counter = test_controller->internal_counter; // Stop controller, will take effect at the end of the update function @@ -576,7 +592,26 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd // [cm_update_rate, 2*cm_update_rate) EXPECT_THAT( test_controller->update_period_.seconds(), - testing::AllOf(testing::Ge(0.9 / cm_update_rate), testing::Lt((1.1 / cm_update_rate)))); + testing::AllOf(testing::Ge(0.85 / cm_update_rate), testing::Lt((1.15 / cm_update_rate)))); + ASSERT_EQ( + test_controller->internal_counter, + cm_->get_loaded_controllers()[0].execution_time_statistics->GetCount()); + ASSERT_EQ( + test_controller->internal_counter - 1, + cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount()) + << "The first update is not counted in periodicity statistics"; + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Average(), + testing::AllOf( + testing::Ge(0.95 * cm_->get_update_rate()), testing::Lt((1.05 * cm_->get_update_rate())))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Min(), + testing::AllOf( + testing::Ge(0.85 * cm_->get_update_rate()), testing::Lt((1.2 * cm_->get_update_rate())))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Max(), + testing::AllOf( + testing::Ge(0.85 * cm_->get_update_rate()), testing::Lt((1.2 * cm_->get_update_rate())))); loop_rate.sleep(); } // if we do 2 times of the controller_manager update rate, the internal counter should be @@ -683,6 +718,8 @@ TEST_P(TestControllerUpdateRates, check_the_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_; + const auto exp_periodicity = + cm_update_rate / std::ceil(static_cast(cm_update_rate) / controller_update_rate); for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter) { rclcpp::Time old_time = time; @@ -691,19 +728,36 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) 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_.seconds(), - testing::AllOf( - 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(); - if (update_counter % cm_update_rate == 0) + if (test_controller->internal_counter - initial_counter > 0) + { + // 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_.seconds(), + testing::AllOf( + 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(); + } + else + { + // Check that the first cycle update period is zero + EXPECT_EQ(test_controller->update_period_.seconds(), 0.0); + } + + if (update_counter > 0 && update_counter % cm_update_rate == 0) { const double no_of_secs_passed = static_cast(update_counter) / cm_update_rate; + const auto actual_counter = test_controller->internal_counter - initial_counter; + const unsigned int exp_counter = + static_cast(exp_periodicity * no_of_secs_passed); + SCOPED_TRACE( + "The internal counter is : " + std::to_string(actual_counter) + " [" + + std::to_string(exp_counter - 1) + ", " + std::to_string(exp_counter + 1) + + "] and number of seconds passed : " + std::to_string(no_of_secs_passed)); // NOTE: here EXPECT_NEAR is used because it is observed that in the first iteration of whole // cycle of cm_update_rate counts, there is one count missing, but in rest of the 9 cycles it // is clearly tracking, so adding 1 here won't affect the final count. @@ -711,10 +765,26 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) // cycle and then on accumulating 37 on every other update cycle so at the end of the 10 // cycles it will have 369 instead of 370. EXPECT_THAT( - test_controller->internal_counter - initial_counter, - testing::AnyOf( - testing::Ge((controller_update_rate - 1) * no_of_secs_passed), - testing::Lt((controller_update_rate * no_of_secs_passed)))); + actual_counter, testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter + 1))); + ASSERT_EQ( + test_controller->internal_counter, + cm_->get_loaded_controllers()[0].execution_time_statistics->GetCount()); + ASSERT_EQ( + test_controller->internal_counter - 1, + cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount()) + << "The first update is not counted in periodicity statistics"; + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Average(), + testing::AllOf(testing::Ge(0.95 * exp_periodicity), testing::Lt((1.05 * exp_periodicity)))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Min(), + testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity)))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Max(), + testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity)))); + EXPECT_LT( + cm_->get_loaded_controllers()[0].execution_time_statistics->Average(), + 50.0); // 50 microseconds } } } @@ -723,6 +793,148 @@ INSTANTIATE_TEST_SUITE_P( per_controller_update_rate_check, TestControllerUpdateRates, testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 90)); +class TestAsyncControllerUpdateRates +: public ControllerManagerFixture +{ +}; + +TEST_F(TestAsyncControllerUpdateRates, check_the_async_controller_update_rate_and_stats) +{ + const unsigned int ctrl_update_rate = cm_->get_update_rate() / 2; + auto test_controller = std::make_shared(); + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller.use_count()); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); + + test_controller->get_node()->set_parameter({"update_rate", static_cast(ctrl_update_rate)}); + test_controller->get_node()->set_parameter({"is_async", true}); + // configure controller + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + } + ASSERT_TRUE(test_controller->is_async()); + 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))); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + + 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))); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + + 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_; + const auto exp_periodicity = + cm_update_rate / std::ceil(static_cast(cm_update_rate) / controller_update_rate); + 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))); + + // the async controllers will have to wait for one cycle to have the correct update period in + // the controller + if (test_controller->internal_counter - initial_counter > 1) + { + EXPECT_THAT( + test_controller->update_period_.seconds(), + testing::AllOf( + 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(); + } + // else + // { + // // Check that the first cycle update period is zero + // EXPECT_EQ(test_controller->update_period_.seconds(), 0.0); + // } + + if (update_counter > 0 && update_counter % cm_update_rate == 0) + { + const double no_of_secs_passed = static_cast(update_counter) / cm_update_rate; + const auto actual_counter = test_controller->internal_counter - initial_counter; + const unsigned int exp_counter = + static_cast(exp_periodicity * no_of_secs_passed); + SCOPED_TRACE( + "The internal counter is : " + std::to_string(actual_counter) + " [" + + std::to_string(exp_counter - 1) + ", " + std::to_string(exp_counter + 1) + + "] and number of seconds passed : " + std::to_string(no_of_secs_passed)); + // NOTE: here EXPECT_THAT is used because it is observed that in the first iteration of whole + // cycle of cm_update_rate counts, there is one count missing, but in rest of the 9 cycles it + // is clearly tracking, so adding 1 here won't affect the final count. + // For instance, a controller with update rate 37 Hz, seems to have 36 in the first update + // cycle and then on accumulating 37 on every other update cycle so at the end of the 10 + // cycles it will have 369 instead of 370. + EXPECT_THAT( + actual_counter, testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter + 1))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].execution_time_statistics->GetCount(), + testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount(), + testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Average(), + testing::AllOf(testing::Ge(0.95 * exp_periodicity), testing::Lt((1.05 * exp_periodicity)))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Min(), + testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity)))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Max(), + testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity)))); + EXPECT_LT( + cm_->get_loaded_controllers()[0].execution_time_statistics->Average(), + 12000); // more or less 12 milliseconds considering the waittime in the controller + } + } +} + class TestControllerManagerFallbackControllers : public ControllerManagerFixture, public testing::WithParamInterface diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index c7b67e0cfe..bdd48f15ae 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -65,6 +65,7 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs cm_ = std::make_shared(executor_, TEST_CM_NAME); run_updater_ = false; + SetUpSrvsCMExecutor(); cm_->set_parameter(rclcpp::Parameter( "hardware_components_initial_state.unconfigured", std::vector({TEST_SYSTEM_HARDWARE_NAME}))); @@ -72,11 +73,10 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs "hardware_components_initial_state.inactive", std::vector({TEST_SENSOR_HARDWARE_NAME}))); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); auto msg = std_msgs::msg::String(); msg.data = ros2_control_test_assets::minimal_robot_urdf; cm_->robot_description_callback(msg); - - SetUpSrvsCMExecutor(); } void check_component_fileds(