From 23dd7620fe245b6e4ef02b2cd8c56412f26799dd Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 20 Dec 2022 09:01:42 +0000 Subject: [PATCH] First attempt to deactivate hardware from read/write return value This is the first step to deactivate a hw component when writing to that component stops working (e.g. because of some interpreter program stopped running). The idea is that a hw component can go back to "configured", where the communication is working, but commanding the robot is not possible --- .../hardware_interface_return_values.hpp | 1 + hardware_interface/src/resource_manager.cpp | 25 +++++++++++++++++-- 2 files changed, 24 insertions(+), 2 deletions(-) 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 5c3ea22ca05..dad907f6837 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, + INACTIVE = 2, }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 33236afd1c5..e1f12106408 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1072,12 +1072,26 @@ HardwareReadWriteStatus ResourceManager::read( { for (auto & component : components) { - if (component.read(time, period) != return_type::OK) + auto ret_val = component.read(time, period); + if (ret_val == return_type::ERROR) { 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()); } + else if (ret_val == return_type::INACTIVE) + { + using lifecycle_msgs::msg::State; + rclcpp_lifecycle::State state(State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); + set_component_state(component.get_name(), state); + } + // If desired: automatic re-activation. We could add a flag for this... + //else + //{ + //using lifecycle_msgs::msg::State; + //rclcpp_lifecycle::State state(State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + //set_component_state(component.get_name(), state); + //} } }; @@ -1100,12 +1114,19 @@ HardwareReadWriteStatus ResourceManager::write( { for (auto & component : components) { - if (component.write(time, period) != return_type::OK) + auto ret_val = component.write(time, period); + if (ret_val == return_type::ERROR) { 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()); } + else if (ret_val == return_type::INACTIVE) + { + using lifecycle_msgs::msg::State; + rclcpp_lifecycle::State state(State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE); + set_component_state(component.get_name(), state); + } } };