From ae96fb65c990ef3c6118215fbe534b1d397e94e4 Mon Sep 17 00:00:00 2001 From: Florent Chretien Date: Mon, 6 Feb 2023 10:41:24 +0100 Subject: [PATCH 1/5] add SKIPPED flag in return values --- .../types/hardware_interface_return_values.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp index 5c3ea22ca0..b066eb54a6 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp @@ -23,6 +23,7 @@ enum class return_type : std::uint8_t { OK = 0, ERROR = 1, + SKIPPED = 2, }; } // namespace hardware_interface From 75cba715fa4fbbb7585196a876aa9d193b1b3c92 Mon Sep 17 00:00:00 2001 From: Florent Chretien Date: Mon, 6 Feb 2023 10:44:27 +0100 Subject: [PATCH 2/5] change the read_write_status accordingly to OK or ERROR --- hardware_interface/src/resource_manager.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index fd874c806c..285013d170 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1074,9 +1074,11 @@ HardwareReadWriteStatus ResourceManager::read( { 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()); + if (component.read(time, period) == return_type::ERROR){ + read_write_status.ok = false; + resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name()); + } } } }; From fb267170d903e094cf26c16df9be8b8f6406d622 Mon Sep 17 00:00:00 2001 From: Florent Chretien Date: Mon, 6 Feb 2023 10:45:47 +0100 Subject: [PATCH 3/5] add a vector of string to skip controllers --- .../include/controller_manager/controller_manager.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index e6b377570a..a29ed7933e 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -197,6 +197,9 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC void manage_switch(); + CONTROLLER_MANAGER_LOCAL + std::vector controllers_to_skip; + /// Deactivate chosen controllers from real-time controller list. /** * Deactivate controllers with names \p controllers_to_deactivate from list \p rt_controller_list. From 57024f207a159934a9bc9852890ec2ab81b96dc4 Mon Sep 17 00:00:00 2001 From: Florent Chretien Date: Mon, 6 Feb 2023 10:49:07 +0100 Subject: [PATCH 4/5] Update set_component_state to not ok only if errors Update read and update function to work with skip --- controller_manager/src/controller_manager.cpp | 21 ++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 15ebc4492c..a70e97a2ad 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1697,8 +1697,8 @@ void ControllerManager::set_hardware_component_state_srv_cb( // the ternary operator is needed because label in State constructor cannot be an empty string request->target_state.label.empty() ? "-" : request->target_state.label); response->ok = - (resource_manager_->set_component_state(request->name, target_state) == - hardware_interface::return_type::OK); + (resource_manager_->set_component_state(request->name, target_state) != + hardware_interface::return_type::ERROR); hw_components_info = resource_manager_->get_components_status(); response->state.id = hw_components_info[request->name].state.id(); response->state.label = hw_components_info[request->name].state.label(); @@ -1729,6 +1729,7 @@ std::vector ControllerManager::get_controller_names() void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & period) { auto [ok, failed_hardware_names] = resource_manager_->read(time, period); + controllers_to_skip.clear(); if (!ok) { @@ -1744,6 +1745,13 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & rt_controllers_wrapper_.update_and_get_used_by_rt_list(); deactivate_controllers(rt_controller_list, stop_request); // TODO(destogl): do auto-start of broadcasters + } else if ( failed_hardware_names.size() > 0 ){ + // Status is ok but some hardware is not ok (SKIPPED) + // Determine controllers to skip + for (const auto & hardware_name : failed_hardware_names){ + auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name); + controllers_to_skip.insert(controllers_to_skip.end(), controllers.begin(), controllers.end()); + } } } @@ -1772,7 +1780,14 @@ controller_interface::return_type ControllerManager::update( update_loop_counter_, controller_go ? "True" : "False", loaded_controller.info.name.c_str()); - if (controller_go) + bool controller_skip = (std::find(controllers_to_skip.begin(), controllers_to_skip.end(), loaded_controller.info.name) != controllers_to_skip.end()); + RCLCPP_DEBUG( + get_logger(), "Skip ?: controller_skip: '%s' controller_name: '%s'", + controller_skip ? "True" : "False", + loaded_controller.info.name.c_str() + ); + + if (!controller_skip && controller_go) { auto controller_ret = loaded_controller.c->update( time, (controller_update_rate != update_rate_ && controller_update_rate != 0) From ce1b1f682a8bc7c9a8cf0a249268ecee1d1cf30d Mon Sep 17 00:00:00 2001 From: Florent Chretien Date: Mon, 6 Feb 2023 11:29:41 +0100 Subject: [PATCH 5/5] run pre-commit format --- controller_manager/src/controller_manager.cpp | 16 ++++++++++------ hardware_interface/src/resource_manager.cpp | 6 ++++-- 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a70e97a2ad..46dcaacccc 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1745,10 +1745,13 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & rt_controllers_wrapper_.update_and_get_used_by_rt_list(); deactivate_controllers(rt_controller_list, stop_request); // TODO(destogl): do auto-start of broadcasters - } else if ( failed_hardware_names.size() > 0 ){ + } + else if (failed_hardware_names.size() > 0) + { // Status is ok but some hardware is not ok (SKIPPED) // Determine controllers to skip - for (const auto & hardware_name : failed_hardware_names){ + for (const auto & hardware_name : failed_hardware_names) + { auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name); controllers_to_skip.insert(controllers_to_skip.end(), controllers.begin(), controllers.end()); } @@ -1780,12 +1783,13 @@ controller_interface::return_type ControllerManager::update( update_loop_counter_, controller_go ? "True" : "False", loaded_controller.info.name.c_str()); - bool controller_skip = (std::find(controllers_to_skip.begin(), controllers_to_skip.end(), loaded_controller.info.name) != controllers_to_skip.end()); + bool controller_skip = + (std::find( + controllers_to_skip.begin(), controllers_to_skip.end(), loaded_controller.info.name) != + controllers_to_skip.end()); RCLCPP_DEBUG( get_logger(), "Skip ?: controller_skip: '%s' controller_name: '%s'", - controller_skip ? "True" : "False", - loaded_controller.info.name.c_str() - ); + controller_skip ? "True" : "False", loaded_controller.info.name.c_str()); if (!controller_skip && controller_go) { diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 285013d170..a9e6792bb8 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1075,9 +1075,11 @@ HardwareReadWriteStatus ResourceManager::read( if (component.read(time, period) != return_type::OK) { read_write_status.failed_hardware_names.push_back(component.get_name()); - if (component.read(time, period) == return_type::ERROR){ + if (component.read(time, period) == return_type::ERROR) + { read_write_status.ok = false; - resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name()); + resource_storage_->remove_all_hardware_interfaces_from_available_list( + component.get_name()); } } }