diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 2850b2be80..9d34620e43 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -206,6 +206,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. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c13eb74a80..7c1be104cd 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1970,8 +1970,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(); @@ -2002,6 +2002,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) { @@ -2018,6 +2019,16 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & 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()); + } + } } controller_interface::return_type ControllerManager::update( @@ -2048,7 +2059,15 @@ 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_factor != 1u) 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 diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 73daddd23d..528698c4ad 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1261,9 +1261,13 @@ 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()); + } } } };