From db81a207d2d4cb459802e02d8777eb0c79ea7378 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 28 Sep 2022 20:16:12 +0200 Subject: [PATCH 1/3] Handle HW errors on read and write in CM by stopping controllers (#742) Add code for deactivating controller when hardware gets an error on read and write. Fix misleading variable name in the tests. Remove all interface from available list for hardware when an error happens. Do some more variable renaming to the new nomenclature. (cherry picked from commit ecf6c697e35a35a0a0e68052e20ab6f4bb973e2b) --- controller_manager/CMakeLists.txt | 8 + .../controller_manager/controller_manager.hpp | 53 +- controller_manager/src/controller_manager.cpp | 125 +++- .../test/controller_manager_test_common.hpp | 1 - .../test/test_controller/test_controller.cpp | 22 +- .../test/test_controller/test_controller.hpp | 3 + ...roller_manager_hardware_error_handling.cpp | 607 ++++++++++++++++++ 7 files changed, 786 insertions(+), 33 deletions(-) create mode 100644 controller_manager/test/test_controller_manager_hardware_error_handling.cpp diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index b0db9990b0..fe21e37027 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -104,6 +104,14 @@ if(BUILD_TESTING) target_link_libraries(test_controller_manager_with_namespace.cpp ${PROJECT_NAME} test_controller) ament_target_dependencies(test_controller_manager_with_namespace.cpp ros2_control_test_assets) + ament_add_gmock( + test_controller_manager_hardware_error_handling + test/test_controller_manager_hardware_error_handling.cpp + ) + target_include_directories(test_controller_manager_hardware_error_handling PRIVATE include) + target_link_libraries(test_controller_manager_hardware_error_handling ${PROJECT_NAME} test_controller) + ament_target_dependencies(test_controller_manager_hardware_error_handling ros2_control_test_assets) + ament_add_gmock( test_load_controller test/test_load_controller.cpp diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 6482248e3e..8741df8470 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -128,17 +128,17 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC controller_interface::return_type configure_controller(const std::string & controller_name); - /// switch_controller Stops some controllers and start others. + /// switch_controller Deactivates some controllers and activates others. /** - * \param[in] start_controllers is a list of controllers to start - * \param[in] stop_controllers is a list of controllers to stop + * \param[in] activate_controllers is a list of controllers to activate. + * \param[in] deactivate_controllers is a list of controllers to deactivate. * \param[in] set level of strictness (BEST_EFFORT or STRICT) * \see Documentation in controller_manager_msgs/SwitchController.srv */ CONTROLLER_MANAGER_PUBLIC controller_interface::return_type switch_controller( - const std::vector & start_controllers, - const std::vector & stop_controllers, int strictness, + const std::vector & activate_controllers, + const std::vector & deactivate_controllers, int strictness, bool activate_asap = kWaitForAllResources, const rclcpp::Duration & timeout = rclcpp::Duration::from_nanoseconds(kInfiniteTimeout)); @@ -200,8 +200,18 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC void manage_switch(); + /// Deactivate chosen controllers from real-time controller list. + /** + * Deactivate controllers with names \p controllers_to_deactivate from list \p rt_controller_list. + * The controller list will be iterated as many times as there are controller names. + * + * \param[in] rt_controller_list controllers in the real-time list. + * \param[in] controllers_to_deactivate names of the controller that have to be deactivated. + */ CONTROLLER_MANAGER_PUBLIC - void deactivate_controllers(); + void deactivate_controllers( + const std::vector & rt_controller_list, + const std::vector controllers_to_deactivate); /** * Switch chained mode for all the controllers with respect to the following cases: @@ -215,11 +225,34 @@ class ControllerManager : public rclcpp::Node void switch_chained_mode( const std::vector & chained_mode_switch_list, bool to_chained_mode); + /// Activate chosen controllers from real-time controller list. + /** + * Activate controllers with names \p controllers_to_activate from list \p rt_controller_list. + * The controller list will be iterated as many times as there are controller names. + * + * \param[in] rt_controller_list controllers in the real-time list. + * \param[in] controllers_to_activate names of the controller that have to be activated. + */ CONTROLLER_MANAGER_PUBLIC - void activate_controllers(); + void activate_controllers( + const std::vector & rt_controller_list, + const std::vector controllers_to_activate); + /// Activate chosen controllers from real-time controller list. + /** + * Activate controllers with names \p controllers_to_activate from list \p rt_controller_list. + * The controller list will be iterated as many times as there are controller names. + * + * *NOTE*: There is currently not difference to `activate_controllers` method. + * Check https://github.com/ros-controls/ros2_control/issues/263 for more information. + * + * \param[in] rt_controller_list controllers in the real-time list. + * \param[in] controllers_to_activate names of the controller that have to be activated. + */ CONTROLLER_MANAGER_PUBLIC - void activate_controllers_asap(); + void activate_controllers_asap( + const std::vector & rt_controller_list, + const std::vector controllers_to_activate); CONTROLLER_MANAGER_PUBLIC void list_controllers_srv_cb( @@ -309,7 +342,7 @@ class ControllerManager : public rclcpp::Node * * For each controller the whole chain of following controllers is checked. * - * NOTE: The automatically adding of following controller into starting list is not implemented + * NOTE: The automatically adding of following controller into activate list is not implemented * yet. * * \param[in] controllers list with controllers. @@ -333,7 +366,7 @@ class ControllerManager : public rclcpp::Node * - will be deactivated, * - and will not be activated. * - * NOTE: The automatically adding of preceding controllers into stopping list is not implemented + * NOTE: The automatically adding of preceding controllers into deactivate list is not implemented * yet. * * \param[in] controllers list with controllers. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 9f9b836f55..6806f226dc 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -870,6 +870,45 @@ controller_interface::return_type ControllerManager::switch_controller( { extract_interfaces_for_controller(controller, deactivate_command_interface_request_); } + + // cache mapping between hardware and controllers for stopping when read/write error happens + // TODO(destogl): This caching approach is suboptimal because the cache can fast become + // outdated. Keeping it up to date is not easy because of stopping controllers from multiple + // threads maybe we should not at all cache this but always search for the related controllers + // to a hardware when error in hardware happens + if (in_activate_list) + { + std::vector interface_names = {}; + + auto command_interface_config = controller.c->command_interface_configuration(); + if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) + { + interface_names = resource_manager_->available_command_interfaces(); + } + if ( + command_interface_config.type == + controller_interface::interface_configuration_type::INDIVIDUAL) + { + interface_names = command_interface_config.names; + } + + std::vector interfaces = {}; + auto state_interface_config = controller.c->state_interface_configuration(); + if (state_interface_config.type == controller_interface::interface_configuration_type::ALL) + { + interfaces = resource_manager_->available_state_interfaces(); + } + if ( + state_interface_config.type == + controller_interface::interface_configuration_type::INDIVIDUAL) + { + interfaces = state_interface_config.names; + } + + interface_names.insert(interface_names.end(), interfaces.begin(), interfaces.end()); + + resource_manager_->cache_controller_to_hardware(controller.info.name, interface_names); + } } if (activate_request_.empty() && deactivate_request_.empty()) @@ -892,6 +931,7 @@ controller_interface::return_type ControllerManager::switch_controller( return controller_interface::return_type::ERROR; } } + // start the atomic controller switching switch_params_.strictness = strictness; switch_params_.activate_asap = activate_asap; @@ -1016,7 +1056,10 @@ void ControllerManager::manage_switch() RCLCPP_ERROR(get_logger(), "Error while performing mode switch."); } - deactivate_controllers(); + std::vector & rt_controller_list = + rt_controllers_wrapper_.update_and_get_used_by_rt_list(); + + deactivate_controllers(rt_controller_list, deactivate_request_); switch_chained_mode(to_chained_mode_request_, true); switch_chained_mode(from_chained_mode_request_, false); @@ -1024,23 +1067,23 @@ void ControllerManager::manage_switch() // activate controllers once the switch is fully complete if (!switch_params_.activate_asap) { - activate_controllers(); + activate_controllers(rt_controller_list, activate_request_); } else { // activate controllers as soon as their required joints are done switching - activate_controllers_asap(); + activate_controllers_asap(rt_controller_list, activate_request_); } // TODO(destogl): move here "do_switch = false" } -void ControllerManager::deactivate_controllers() +void ControllerManager::deactivate_controllers( + const std::vector & rt_controller_list, + const std::vector controllers_to_deactivate) { - std::vector & rt_controller_list = - rt_controllers_wrapper_.update_and_get_used_by_rt_list(); - // stop controllers - for (const auto & request : deactivate_request_) + // deactivate controllers + for (const auto & request : controllers_to_deactivate) { auto found_it = std::find_if( rt_controller_list.begin(), rt_controller_list.end(), @@ -1049,7 +1092,7 @@ void ControllerManager::deactivate_controllers() { RCLCPP_ERROR( get_logger(), - "Got request to stop controller '%s' but it is not in the realtime controller list", + "Got request to deactivate controller '%s' but it is not in the realtime controller list", request.c_str()); continue; } @@ -1125,11 +1168,11 @@ void ControllerManager::switch_chained_mode( } } -void ControllerManager::activate_controllers() +void ControllerManager::activate_controllers( + const std::vector & rt_controller_list, + const std::vector controllers_to_activate) { - std::vector & rt_controller_list = - rt_controllers_wrapper_.update_and_get_used_by_rt_list(); - for (const auto & request : activate_request_) + for (const auto & request : controllers_to_activate) { auto found_it = std::find_if( rt_controller_list.begin(), rt_controller_list.end(), @@ -1239,10 +1282,12 @@ void ControllerManager::activate_controllers() switch_params_.do_switch = false; } -void ControllerManager::activate_controllers_asap() +void ControllerManager::activate_controllers_asap( + const std::vector & rt_controller_list, + const std::vector controllers_to_activate) { // https://github.com/ros-controls/ros2_control/issues/263 - activate_controllers(); + activate_controllers(rt_controller_list, controllers_to_activate); } void ControllerManager::list_controllers_srv_cb( @@ -1590,6 +1635,20 @@ void ControllerManager::list_hardware_components_srv_cb( hwi.name = interface; hwi.is_available = resource_manager_->command_interface_is_available(interface); hwi.is_claimed = resource_manager_->command_interface_is_claimed(interface); + // TODO(destogl): Add here mapping to controller that has claimed or + // can be claiming this interface + // Those should be two variables + // if (hwi.is_claimed) + // { + // for (const auto & controller : controllers_that_use_interface(interface)) + // { + // if (is_controller_active(controller)) + // { + // hwi.is_claimed_by = controller; + // } + // } + // } + // hwi.is_used_by = controllers_that_use_interface(interface); component.command_interfaces.push_back(hwi); } @@ -1688,7 +1747,23 @@ std::vector ControllerManager::get_controller_names() void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & period) { - resource_manager_->read(time, period); + auto [ok, failed_hardware_names] = resource_manager_->read(time, period); + + if (!ok) + { + std::vector stop_request = {}; + // Determine controllers to stop + for (const auto & hardware_name : failed_hardware_names) + { + auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name); + stop_request.insert(stop_request.end(), controllers.begin(), controllers.end()); + } + + std::vector & rt_controller_list = + rt_controllers_wrapper_.update_and_get_used_by_rt_list(); + deactivate_controllers(rt_controller_list, stop_request); + // TODO(destogl): do auto-start of broadcasters + } } controller_interface::return_type ControllerManager::update( @@ -1745,7 +1820,23 @@ controller_interface::return_type ControllerManager::update( void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration & period) { - resource_manager_->write(time, period); + auto [ok, failed_hardware_names] = resource_manager_->write(time, period); + + if (!ok) + { + std::vector stop_request = {}; + // Determine controllers to stop + for (const auto & hardware_name : failed_hardware_names) + { + auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name); + stop_request.insert(stop_request.end(), controllers.begin(), controllers.end()); + } + + std::vector & rt_controller_list = + rt_controllers_wrapper_.update_and_get_used_by_rt_list(); + deactivate_controllers(rt_controller_list, stop_request); + // TODO(destogl): do auto-start of broadcasters + } } std::vector & diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 11154e1022..b6e6acac0e 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -105,7 +105,6 @@ class ControllerManagerFixture : public ::testing::Test const std::future_status expected_future_status = std::future_status::timeout, const controller_interface::return_type expected_return = controller_interface::return_type::OK) { - // First activation not possible because controller not configured auto switch_future = std::async( std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index b6eac9b689..06f76bd044 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -14,6 +14,7 @@ #include "test_controller.hpp" +#include #include #include @@ -25,6 +26,7 @@ TestController::TestController() : controller_interface::ControllerInterface(), cmd_iface_cfg_{controller_interface::interface_configuration_type::NONE} { + set_first_command_interface_value_to = std::numeric_limits::quiet_NaN(); } controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const @@ -62,12 +64,22 @@ controller_interface::return_type TestController::update( { ++internal_counter; - for (size_t i = 0; i < command_interfaces_.size(); ++i) + // set value to hardware to produce and test different behaviors there + if (!std::isnan(set_first_command_interface_value_to)) { - RCLCPP_INFO( - get_node()->get_logger(), "Setting value of command interface '%s' to %f", - command_interfaces_[i].get_name().c_str(), external_commands_for_testing_[i]); - command_interfaces_[i].set_value(external_commands_for_testing_[i]); + command_interfaces_[0].set_value(set_first_command_interface_value_to); + // reset to be easier to test + set_first_command_interface_value_to = std::numeric_limits::quiet_NaN(); + } + else + { + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + RCLCPP_INFO( + get_node()->get_logger(), "Setting value of command interface '%s' to %f", + command_interfaces_[i].get_name().c_str(), external_commands_for_testing_[i]); + command_interfaces_[i].set_value(external_commands_for_testing_[i]); + } } return controller_interface::return_type::OK; diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index c88c11050d..f73f592037 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -74,6 +74,9 @@ class TestController : public controller_interface::ControllerInterface controller_interface::InterfaceConfiguration state_iface_cfg_; std::vector external_commands_for_testing_; + // enables external setting of values to command interfaces - used for simulation of hardware + // errors + double set_first_command_interface_value_to; }; } // namespace test_controller diff --git a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp new file mode 100644 index 0000000000..1ec725f8c5 --- /dev/null +++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp @@ -0,0 +1,607 @@ +// Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "test_controller/test_controller.hpp" + +using ::testing::_; +using ::testing::Return; + +using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES; +using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME; +using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES; +using ros2_control_test_assets::TEST_SENSOR_HARDWARE_COMMAND_INTERFACES; +using ros2_control_test_assets::TEST_SENSOR_HARDWARE_NAME; +using ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES; +using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES; +using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME; +using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_STATE_INTERFACES; + +class TestControllerManagerWithTestableCM; + +class TestableControllerManager : public controller_manager::ControllerManager +{ + friend TestControllerManagerWithTestableCM; + + FRIEND_TEST(TestControllerManagerWithTestableCM, check_cached_controllers_for_hardware); + FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_error); + FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_error); + FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_multiple_hardware_error); + +public: + TestableControllerManager( + std::unique_ptr resource_manager, + std::shared_ptr executor, + const std::string & manager_node_name = "controller_manager", + const std::string & namespace_ = "") + : controller_manager::ControllerManager( + std::move(resource_manager), executor, manager_node_name, namespace_) + { + } +}; + +class TestControllerManagerWithTestableCM +: public ControllerManagerFixture, + public testing::WithParamInterface +{ +public: + void SetupAndConfigureControllers(int strictness) + { + test_controller_actuator = std::make_shared(); + cm_->add_controller( + test_controller_actuator, TEST_CONTROLLER_ACTUATOR_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + controller_interface::InterfaceConfiguration test_controller_actuator_cmd_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES}; + controller_interface::InterfaceConfiguration test_controller_actuator_state_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + TEST_ACTUATOR_HARDWARE_STATE_INTERFACES}; + test_controller_actuator->set_command_interface_configuration( + test_controller_actuator_cmd_ifs_cfg); + test_controller_actuator->set_state_interface_configuration( + test_controller_actuator_state_ifs_cfg); + + test_controller_system = std::make_shared(); + cm_->add_controller( + test_controller_system, TEST_CONTROLLER_SYSTEM_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + controller_interface::InterfaceConfiguration test_system_controller_cmd_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES}; + controller_interface::InterfaceConfiguration test_system_controller_state_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + TEST_SYSTEM_HARDWARE_STATE_INTERFACES}; + test_controller_system->set_command_interface_configuration(test_system_controller_cmd_ifs_cfg); + test_controller_system->set_state_interface_configuration(test_system_controller_state_ifs_cfg); + + test_broadcaster_all = std::make_shared(); + cm_->add_controller( + test_broadcaster_all, TEST_BROADCASTER_ALL_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); + controller_interface::InterfaceConfiguration test_broadcaster_all_cmd_ifs_cfg = { + controller_interface::interface_configuration_type::NONE, {}}; + controller_interface::InterfaceConfiguration test_broadcaster_all_state_ifs_cfg = { + controller_interface::interface_configuration_type::ALL, {}}; + test_broadcaster_all->set_command_interface_configuration(test_broadcaster_all_cmd_ifs_cfg); + test_broadcaster_all->set_state_interface_configuration(test_broadcaster_all_state_ifs_cfg); + + test_broadcaster_sensor = std::make_shared(); + cm_->add_controller( + test_broadcaster_sensor, TEST_BROADCASTER_SENSOR_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + controller_interface::InterfaceConfiguration test_broadcaster_sensor_cmd_ifs_cfg = { + controller_interface::interface_configuration_type::NONE, {}}; + controller_interface::InterfaceConfiguration test_broadcaster_sensor_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + TEST_SENSOR_HARDWARE_STATE_INTERFACES}; + test_broadcaster_sensor->set_command_interface_configuration( + test_broadcaster_sensor_cmd_ifs_cfg); + test_broadcaster_sensor->set_state_interface_configuration(test_broadcaster_sensor_ifs_cfg); + + // check if all controllers are added correctly + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_actuator->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_system->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_broadcaster_all->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_broadcaster_sensor->get_state().id()); + + // configure controllers + cm_->configure_controller(TEST_CONTROLLER_ACTUATOR_NAME); + cm_->configure_controller(TEST_CONTROLLER_SYSTEM_NAME); + cm_->configure_controller(TEST_BROADCASTER_ALL_NAME); + cm_->configure_controller(TEST_BROADCASTER_SENSOR_NAME); + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(0u, test_controller_actuator->internal_counter) << "Controller is not started"; + EXPECT_EQ(0u, test_controller_system->internal_counter) << "Controller is not started"; + EXPECT_EQ(0u, test_broadcaster_all->internal_counter) << "Controller is not started"; + EXPECT_EQ(0u, test_broadcaster_sensor->internal_counter) << "Controller is not started"; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_actuator->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_sensor->get_state().id()); + + // Start controller, will take effect at the end of the update function + switch_test_controllers( + {TEST_CONTROLLER_ACTUATOR_NAME, TEST_CONTROLLER_SYSTEM_NAME, TEST_BROADCASTER_ALL_NAME, + TEST_BROADCASTER_SENSOR_NAME}, + {}, strictness); + } + + // values to set to hardware to simulate failure on read and write + static constexpr double READ_FAIL_VALUE = 28282828.0; + static constexpr double WRITE_FAIL_VALUE = 23232323.0; + + static constexpr char TEST_CONTROLLER_ACTUATOR_NAME[] = "test_controller_actuator"; + static constexpr char TEST_CONTROLLER_SYSTEM_NAME[] = "test_controller_system"; + static constexpr char TEST_BROADCASTER_ALL_NAME[] = "test_broadcaster_all"; + static constexpr char TEST_BROADCASTER_SENSOR_NAME[] = "test_broadcaster_sensor"; + + std::shared_ptr test_controller_actuator; + std::shared_ptr test_controller_system; + std::shared_ptr test_broadcaster_all; + std::shared_ptr test_broadcaster_sensor; +}; + +TEST_P(TestControllerManagerWithTestableCM, check_cached_controllers_for_hardware) +{ + auto strictness = GetParam().strictness; + SetupAndConfigureControllers(strictness); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_actuator->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + + { + auto controllers = + cm_->resource_manager_->get_cached_controllers_to_hardware(TEST_ACTUATOR_HARDWARE_NAME); + ASSERT_THAT( + controllers, testing::UnorderedElementsAreArray(std::vector( + {TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}))); + } + + { + auto controllers = + cm_->resource_manager_->get_cached_controllers_to_hardware(TEST_SYSTEM_HARDWARE_NAME); + ASSERT_THAT( + controllers, testing::UnorderedElementsAreArray(std::vector( + {TEST_CONTROLLER_SYSTEM_NAME, TEST_BROADCASTER_ALL_NAME}))); + } + + { + auto controllers = + cm_->resource_manager_->get_cached_controllers_to_hardware(TEST_SENSOR_HARDWARE_NAME); + ASSERT_THAT( + controllers, testing::UnorderedElementsAreArray(std::vector( + {TEST_BROADCASTER_SENSOR_NAME, TEST_BROADCASTER_ALL_NAME}))); + } +} + +TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_error) +{ + auto strictness = GetParam().strictness; + SetupAndConfigureControllers(strictness); + + rclcpp_lifecycle::State state_active( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + { + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_GE(test_controller_actuator->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_controller_system->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_broadcaster_all->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_broadcaster_sensor->internal_counter, 1u) + << "Controller is started at the end of update"; + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_actuator->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + + // Execute first time without any errors + { + auto new_counter = test_controller_actuator->internal_counter + 1; + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) << "Execute without errors"; + } + + // Simulate error in read() on TEST_ACTUATOR_HARDWARE_NAME by setting first command interface to + // READ_FAIL_VALUE + test_controller_actuator->set_first_command_interface_value_to = READ_FAIL_VALUE; + { + auto new_counter = test_controller_actuator->internal_counter + 1; + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + { + auto previous_counter = test_controller_actuator->internal_counter; + auto new_counter = test_controller_system->internal_counter + 1; + + // here happens error in hardware and + // "actuator controller" and "broadcaster all" are deactivated + EXPECT_NO_THROW(cm_->read(TIME, PERIOD)); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_actuator->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) + << "Execute has read error and it is not updated"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter) + << "Broadcaster for all interfaces is not updated"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + // Recover hardware and activate again all controllers + { + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + auto status_map = cm_->resource_manager_->get_components_status(); + ASSERT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter_higher = test_controller_system->internal_counter; + + switch_test_controllers( + {TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}, {}, strictness); + + EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower); + EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher); + EXPECT_GT(test_controller_system->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_lower); + EXPECT_LE(test_broadcaster_all->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher); + } + + // Simulate error in read() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME + // by setting first command interface to READ_FAIL_VALUE + test_controller_actuator->set_first_command_interface_value_to = READ_FAIL_VALUE; + test_controller_system->set_first_command_interface_value_to = READ_FAIL_VALUE; + { + auto previous_counter_lower = test_controller_actuator->internal_counter + 1; + auto previous_counter_higher = test_controller_system->internal_counter + 1; + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) + << "Execute without errors to write value"; + EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_lower) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, previous_counter_higher) + << "Execute without errors to write value"; + } + + { + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter_higher = test_controller_system->internal_counter; + auto new_counter = test_broadcaster_sensor->internal_counter + 1; + + EXPECT_NO_THROW(cm_->read(TIME, PERIOD)); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_actuator->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) + << "Execute has read error and it is not updated"; + EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) + << "Execute has read error and it is not updated"; + EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_lower) + << "Broadcaster for all interfaces is not updated"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + // Recover hardware and activate again all controllers + { + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + auto status_map = cm_->resource_manager_->get_components_status(); + ASSERT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter = test_controller_system->internal_counter; + auto previous_counter_higher = test_broadcaster_sensor->internal_counter; + + switch_test_controllers( + {TEST_CONTROLLER_SYSTEM_NAME, TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}, {}, + strictness); + + EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower); + EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher); + EXPECT_GT(test_controller_system->internal_counter, previous_counter); + EXPECT_LE(test_controller_system->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_lower); + EXPECT_LE(test_broadcaster_all->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher); + } +} + +TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_error) +{ + auto strictness = GetParam().strictness; + SetupAndConfigureControllers(strictness); + + rclcpp_lifecycle::State state_active( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + { + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_GE(test_controller_actuator->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_controller_system->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_broadcaster_all->internal_counter, 1u) + << "Controller is started at the end of update"; + EXPECT_GE(test_broadcaster_sensor->internal_counter, 1u) + << "Controller is started at the end of update"; + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_actuator->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + + // Execute first time without any errors + { + auto new_counter = test_controller_actuator->internal_counter + 1; + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) << "Execute without errors"; + } + + // Simulate error in write() on TEST_ACTUATOR_HARDWARE_NAME by setting first command interface to + // WRITE_FAIL_VALUE + test_controller_actuator->set_first_command_interface_value_to = WRITE_FAIL_VALUE; + { + auto new_counter = test_controller_actuator->internal_counter + 1; + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + { + auto previous_counter = test_controller_actuator->internal_counter; + auto new_counter = test_controller_system->internal_counter + 1; + + // here happens error in hardware and + // "actuator controller" and "broadcaster all" are deactivated + EXPECT_NO_THROW(cm_->write(TIME, PERIOD)); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_actuator->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_controller_system->internal_counter, new_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + // Recover hardware and activate again all controllers + { + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + auto status_map = cm_->resource_manager_->get_components_status(); + ASSERT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter_higher = test_controller_system->internal_counter; + + switch_test_controllers( + {TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}, {}, strictness); + + EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower); + EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher); + EXPECT_GT(test_controller_system->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_lower); + EXPECT_LE(test_broadcaster_all->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher); + } + + // Simulate error in write() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME + // by setting first command interface to WRITE_FAIL_VALUE + test_controller_actuator->set_first_command_interface_value_to = WRITE_FAIL_VALUE; + test_controller_system->set_first_command_interface_value_to = WRITE_FAIL_VALUE; + { + auto previous_counter_lower = test_controller_actuator->internal_counter + 1; + auto previous_counter_higher = test_controller_system->internal_counter + 1; + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) + << "Execute without errors to write value"; + EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_lower) + << "Execute without errors to write value"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, previous_counter_higher) + << "Execute without errors to write value"; + } + + { + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter_higher = test_controller_system->internal_counter; + auto new_counter = test_broadcaster_sensor->internal_counter + 1; + + // here happens error in hardware and + // "actuator controller" and "broadcaster all" are deactivated + EXPECT_NO_THROW(cm_->write(TIME, PERIOD)); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_actuator->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) + << "Execute has write error and it is not updated"; + EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) + << "Execute has write error and it is not updated"; + EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_lower) + << "Broadcaster for all interfaces is not updated"; + EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) + << "Execute without errors to write value"; + } + + // Recover hardware and activate again all controllers + { + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + ASSERT_EQ( + cm_->resource_manager_->set_component_state( + ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME, state_active), + hardware_interface::return_type::OK); + auto status_map = cm_->resource_manager_->get_components_status(); + ASSERT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + auto previous_counter_lower = test_controller_actuator->internal_counter; + auto previous_counter = test_controller_system->internal_counter; + auto previous_counter_higher = test_broadcaster_sensor->internal_counter; + + switch_test_controllers( + {TEST_CONTROLLER_SYSTEM_NAME, TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}, {}, + strictness); + + EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower); + EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher); + EXPECT_GT(test_controller_system->internal_counter, previous_counter); + EXPECT_LE(test_controller_system->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_lower); + EXPECT_LE(test_broadcaster_all->internal_counter, previous_counter_higher); + EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher); + } +} + +INSTANTIATE_TEST_SUITE_P( + test_strict_best_effort, TestControllerManagerWithTestableCM, + testing::Values(strict, best_effort)); From 1be93907ca3c1a1860eab07f209ca9a550fe2e00 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A1rk=20Szitanics?= Date: Thu, 16 Mar 2023 16:48:10 +0100 Subject: [PATCH 2/3] copy unavailable functions from master (#969) --- .../hardware_interface/resource_manager.hpp | 35 ++- hardware_interface/src/resource_manager.cpp | 237 +++++++++++++----- 2 files changed, 201 insertions(+), 71 deletions(-) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 5a919289b8..8b2914a381 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -36,6 +36,12 @@ class SensorInterface; class SystemInterface; class ResourceStorage; +struct HardwareReadWriteStatus +{ + bool ok; + std::vector failed_hardware_names; +}; + class HARDWARE_INTERFACE_PUBLIC ResourceManager { public: @@ -166,6 +172,26 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ void remove_controller_reference_interfaces(const std::string & controller_name); + /// Cache mapping between hardware and controllers using it + /** + * Find mapping between controller and hardware based on interfaces controller with + * \p controller_name is using and cache those for later usage. + * + * \param[in] controller_name name of the controller which interfaces are provided. + * \param[in] interfaces list of interfaces controller with \p controller_name is using. + */ + void cache_controller_to_hardware( + const std::string & controller_name, const std::vector & interfaces); + + /// Return cached controllers for a specific hardware. + /** + * Return list of cached controller names that use the hardware with name \p hardware_name. + * + * \param[in] hardware_name the name of the hardware for which cached controllers should be returned. + * \returns list of cached controller names that depend on hardware with name \p hardware_name. + */ + std::vector get_cached_controllers_to_hardware(const std::string & hardware_name); + /// Checks whether a command interface is already claimed. /** * Any command interface can only be claimed by a single instance. @@ -345,7 +371,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * Part of the real-time critical update loop. * It is realtime-safe if used hadware interfaces are implemented adequately. */ - void read(const rclcpp::Time & time, const rclcpp::Duration & period); + HardwareReadWriteStatus read(const rclcpp::Time & time, const rclcpp::Duration & period); /// Write all loaded hardware components. /** @@ -354,7 +380,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * Part of the real-time critical update loop. * It is realtime-safe if used hadware interfaces are implemented adequately. */ - void write(const rclcpp::Time & time, const rclcpp::Duration & period); + HardwareReadWriteStatus write(const rclcpp::Time & time, const rclcpp::Duration & period); /// Activates all available hardware components in the system. /** @@ -373,7 +399,12 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager mutable std::recursive_mutex resource_interfaces_lock_; mutable std::recursive_mutex claimed_command_interfaces_lock_; + mutable std::recursive_mutex resources_lock_; + std::unique_ptr resource_storage_; + + // Structure to store read and write status so it is not initialized in the real-time loop + HardwareReadWriteStatus read_write_status; }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 7082b7ea23..7efb7e2cd1 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -101,6 +101,8 @@ class ResourceStorage component_info.class_type = hardware_info.hardware_class_type; hardware_info_map_.insert(std::make_pair(component_info.name, component_info)); + hardware_used_by_controllers_.insert( + std::make_pair(component_info.name, std::vector())); } template @@ -195,6 +197,58 @@ class ResourceStorage return result; } + void remove_all_hardware_interfaces_from_available_list(const std::string & hardware_name) + { + // remove all command interfaces from available list + for (const auto & interface : hardware_info_map_[hardware_name].command_interfaces) + { + auto found_it = std::find( + available_command_interfaces_.begin(), available_command_interfaces_.end(), interface); + + if (found_it != available_command_interfaces_.end()) + { + available_command_interfaces_.erase(found_it); + RCUTILS_LOG_DEBUG_NAMED( + "resource_manager", "(hardware '%s'): '%s' command interface removed from available list", + hardware_name.c_str(), interface.c_str()); + } + else + { + // TODO(destogl): do here error management if interfaces are only partially added into + // "available" list - this should never be the case! + RCUTILS_LOG_WARN_NAMED( + "resource_manager", + "(hardware '%s'): '%s' command interface not in available list. " + "This should not happen (hint: multiple cleanup calls).", + hardware_name.c_str(), interface.c_str()); + } + } + // remove all state interfaces from available list + for (const auto & interface : hardware_info_map_[hardware_name].state_interfaces) + { + auto found_it = std::find( + available_state_interfaces_.begin(), available_state_interfaces_.end(), interface); + + if (found_it != available_state_interfaces_.end()) + { + available_state_interfaces_.erase(found_it); + RCUTILS_LOG_DEBUG_NAMED( + "resource_manager", "(hardware '%s'): '%s' state interface removed from available list", + hardware_name.c_str(), interface.c_str()); + } + else + { + // TODO(destogl): do here error management if interfaces are only partially added into + // "available" list - this should never be the case! + RCUTILS_LOG_WARN_NAMED( + "resource_manager", + "(hardware '%s'): '%s' state interface not in available list. " + "This should not happen (hint: multiple cleanup calls).", + hardware_name.c_str(), interface.c_str()); + } + } + } + template bool cleanup_hardware(HardwareT & hardware) { @@ -204,55 +258,7 @@ class ResourceStorage if (result) { - // remove all command interfaces from available list - for (const auto & interface : hardware_info_map_[hardware.get_name()].command_interfaces) - { - auto found_it = std::find( - available_command_interfaces_.begin(), available_command_interfaces_.end(), interface); - - if (found_it != available_command_interfaces_.end()) - { - available_command_interfaces_.erase(found_it); - RCUTILS_LOG_DEBUG_NAMED( - "resource_manager", - "(hardware '%s'): '%s' command interface removed from available list", - hardware.get_name().c_str(), interface.c_str()); - } - else - { - // TODO(destogl): do here error management if interfaces are only partially added into - // "available" list - this should never be the case! - RCUTILS_LOG_WARN_NAMED( - "resource_manager", - "(hardware '%s'): '%s' command interface not in available list." - " This can happen due to multiple calls to 'cleanup'", - hardware.get_name().c_str(), interface.c_str()); - } - } - // remove all state interfaces from available list - for (const auto & interface : hardware_info_map_[hardware.get_name()].state_interfaces) - { - auto found_it = std::find( - available_state_interfaces_.begin(), available_state_interfaces_.end(), interface); - - if (found_it != available_state_interfaces_.end()) - { - available_state_interfaces_.erase(found_it); - RCUTILS_LOG_DEBUG_NAMED( - "resource_manager", "(hardware '%s'): '%s' state interface removed from available list", - hardware.get_name().c_str(), interface.c_str()); - } - else - { - // TODO(destogl): do here error management if interfaces are only partially added into - // "available" list - this should never be the case! - RCUTILS_LOG_WARN_NAMED( - "resource_manager", - "(hardware '%s'): '%s' state interface not in available list. " - "This can happen due to multiple calls to 'cleanup'", - hardware.get_name().c_str(), interface.c_str()); - } - } + remove_all_hardware_interfaces_from_available_list(hardware.get_name()); } return result; } @@ -545,6 +551,10 @@ class ResourceStorage std::unordered_map hardware_info_map_; + /// Mapping between hardware and controllers that are using it (accessing data from it) + std::unordered_map> hardware_used_by_controllers_; + + /// Mapping between controllers and list of reference interfaces they are using std::unordered_map> controllers_reference_interfaces_map_; /// Storage of all available state interfaces @@ -614,6 +624,11 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac { validate_storage(hardware_info); } + + std::lock_guard guard(resources_lock_); + read_write_status.failed_hardware_names.reserve( + resource_storage_->actuators_.size() + resource_storage_->sensors_.size() + + resource_storage_->systems_.size()); } LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key) @@ -719,6 +734,53 @@ void ResourceManager::remove_controller_reference_interfaces(const std::string & resource_storage_->remove_command_interfaces(interface_names); } +// CM API: Called in "callback/slow"-thread +void ResourceManager::cache_controller_to_hardware( + const std::string & controller_name, const std::vector & interfaces) +{ + for (const auto & interface : interfaces) + { + bool found = false; + for (const auto & [hw_name, hw_info] : resource_storage_->hardware_info_map_) + { + auto cmd_itf_it = + std::find(hw_info.command_interfaces.begin(), hw_info.command_interfaces.end(), interface); + if (cmd_itf_it != hw_info.command_interfaces.end()) + { + found = true; + } + auto state_itf_it = + std::find(hw_info.state_interfaces.begin(), hw_info.state_interfaces.end(), interface); + if (state_itf_it != hw_info.state_interfaces.end()) + { + found = true; + } + + if (found) + { + // check if controller exist already in the list and if not add it + auto controllers = resource_storage_->hardware_used_by_controllers_[hw_name]; + auto ctrl_it = std::find(controllers.begin(), controllers.end(), controller_name); + if (ctrl_it == controllers.end()) + { + // add because it does not exist + controllers.reserve(controllers.size() + 1); + controllers.push_back(controller_name); + } + resource_storage_->hardware_used_by_controllers_[hw_name] = controllers; + break; + } + } + } +} + +// CM API: Called in "update"-thread +std::vector ResourceManager::get_cached_controllers_to_hardware( + const std::string & hardware_name) +{ + return resource_storage_->hardware_used_by_controllers_[hardware_name]; +} + // CM API: Called in "update"-thread bool ResourceManager::command_interface_is_claimed(const std::string & key) const { @@ -807,19 +869,31 @@ size_t ResourceManager::sensor_components_size() const void ResourceManager::import_component( std::unique_ptr actuator, const HardwareInfo & hardware_info) { + std::lock_guard guard(resources_lock_); resource_storage_->initialize_actuator(std::move(actuator), hardware_info); + read_write_status.failed_hardware_names.reserve( + resource_storage_->actuators_.size() + resource_storage_->sensors_.size() + + resource_storage_->systems_.size()); } void ResourceManager::import_component( std::unique_ptr sensor, const HardwareInfo & hardware_info) { + std::lock_guard guard(resources_lock_); resource_storage_->initialize_sensor(std::move(sensor), hardware_info); + read_write_status.failed_hardware_names.reserve( + resource_storage_->actuators_.size() + resource_storage_->sensors_.size() + + resource_storage_->systems_.size()); } void ResourceManager::import_component( std::unique_ptr system, const HardwareInfo & hardware_info) { + std::lock_guard guard(resources_lock_); resource_storage_->initialize_system(std::move(system), hardware_info); + read_write_status.failed_hardware_names.reserve( + resource_storage_->actuators_.size() + resource_storage_->sensors_.size() + + resource_storage_->systems_.size()); } size_t ResourceManager::system_components_size() const @@ -1001,32 +1075,57 @@ return_type ResourceManager::set_component_state( return result; } -void ResourceManager::read(const rclcpp::Time & time, const rclcpp::Duration & period) +HardwareReadWriteStatus ResourceManager::read( + const rclcpp::Time & time, const rclcpp::Duration & period) { - for (auto & component : resource_storage_->actuators_) - { - component.read(time, period); - } - for (auto & component : resource_storage_->sensors_) - { - component.read(time, period); - } - for (auto & component : resource_storage_->systems_) + std::lock_guard guard(resources_lock_); + read_write_status.ok = true; + read_write_status.failed_hardware_names.clear(); + + auto read_components = [&](auto & components) { - component.read(time, period); - } + for (auto & component : components) + { + if (component.read(time, period) != return_type::OK) + { + read_write_status.ok = false; + read_write_status.failed_hardware_names.push_back(component.get_name()); + resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name()); + } + } + }; + + read_components(resource_storage_->actuators_); + read_components(resource_storage_->sensors_); + read_components(resource_storage_->systems_); + + return read_write_status; } -void ResourceManager::write(const rclcpp::Time & time, const rclcpp::Duration & period) +HardwareReadWriteStatus ResourceManager::write( + const rclcpp::Time & time, const rclcpp::Duration & period) { - for (auto & component : resource_storage_->actuators_) - { - component.write(time, period); - } - for (auto & component : resource_storage_->systems_) + std::lock_guard guard(resources_lock_); + read_write_status.ok = true; + read_write_status.failed_hardware_names.clear(); + + auto write_components = [&](auto & components) { - component.write(time, period); - } + for (auto & component : components) + { + if (component.write(time, period) != return_type::OK) + { + read_write_status.ok = false; + read_write_status.failed_hardware_names.push_back(component.get_name()); + resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name()); + } + } + }; + + write_components(resource_storage_->actuators_); + write_components(resource_storage_->systems_); + + return read_write_status; } void ResourceManager::validate_storage( From 72d51f482dc99cab26ebf36a4c5a1492705221fc Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 11 Dec 2023 19:23:08 +0000 Subject: [PATCH 3/3] Remove duplicate function definition --- hardware_interface/src/resource_manager.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index dd43850b4c..75ff60fc6f 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -971,11 +971,6 @@ void ResourceManager::import_component( resource_storage_->systems_.size()); } -size_t ResourceManager::system_components_size() const -{ - return resource_storage_->systems_.size(); -} - // CM API: Called in "callback/slow"-thread std::unordered_map ResourceManager::get_components_status() {