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..dad907f683 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 33236afd1c..2b3a10362f 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -167,48 +167,28 @@ class ResourceStorage } } - // add command interfaces to available list - for (const auto & interface : hardware_info_map_[hardware.get_name()].command_interfaces) - { - // TODO(destogl): check if interface should be available on configure - auto found_it = std::find( - available_command_interfaces_.begin(), available_command_interfaces_.end(), interface); - - if (found_it == available_command_interfaces_.end()) - { - available_command_interfaces_.emplace_back(interface); - RCUTILS_LOG_DEBUG_NAMED( - "resource_manager", "(hardware '%s'): '%s' command interface added into 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 already in available list." - " This can happen due to multiple calls to 'configure'", - hardware.get_name().c_str(), interface.c_str()); - } - } } 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) + remove_all_state_interfaces_from_available_list(hardware_name); + remove_all_command_interfaces_from_available_list(hardware_name); + } + + void remove_all_state_interfaces_from_available_list(const std::string & hardware_name) + { + for (const auto & interface : hardware_info_map_[hardware_name].state_interfaces) { auto found_it = std::find( - available_command_interfaces_.begin(), available_command_interfaces_.end(), interface); + available_state_interfaces_.begin(), available_state_interfaces_.end(), interface); - if (found_it != available_command_interfaces_.end()) + if (found_it != available_state_interfaces_.end()) { - available_command_interfaces_.erase(found_it); + available_state_interfaces_.erase(found_it); RCUTILS_LOG_DEBUG_NAMED( - "resource_manager", "(hardware '%s'): '%s' command interface removed from available list", + "resource_manager", "(hardware '%s'): '%s' state interface removed from available list", hardware_name.c_str(), interface.c_str()); } else @@ -217,22 +197,25 @@ class ResourceStorage // "available" list - this should never be the case! RCUTILS_LOG_WARN_NAMED( "resource_manager", - "(hardware '%s'): '%s' command interface not in available list. " + "(hardware '%s'): '%s' state 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) + } + + void remove_all_command_interfaces_from_available_list(const std::string & hardware_name) + { + for (const auto & interface : hardware_info_map_[hardware_name].command_interfaces) { auto found_it = std::find( - available_state_interfaces_.begin(), available_state_interfaces_.end(), interface); + available_command_interfaces_.begin(), available_command_interfaces_.end(), interface); - if (found_it != available_state_interfaces_.end()) + if (found_it != available_command_interfaces_.end()) { - available_state_interfaces_.erase(found_it); + available_command_interfaces_.erase(found_it); RCUTILS_LOG_DEBUG_NAMED( - "resource_manager", "(hardware '%s'): '%s' state interface removed from available list", + "resource_manager", "(hardware '%s'): '%s' command interface removed from available list", hardware_name.c_str(), interface.c_str()); } else @@ -241,7 +224,7 @@ class ResourceStorage // "available" list - this should never be the case! RCUTILS_LOG_WARN_NAMED( "resource_manager", - "(hardware '%s'): '%s' state interface not in available list. " + "(hardware '%s'): '%s' command interface not in available list. " "This should not happen (hint: multiple cleanup calls).", hardware_name.c_str(), interface.c_str()); } @@ -288,7 +271,31 @@ class ResourceStorage if (result) { - // TODO(destogl): make all command interfaces available (currently are all available) + // add command interfaces to available list + for (const auto & interface : hardware_info_map_[hardware.get_name()].command_interfaces) + { + // TODO(destogl): check if interface should be available on configure + auto found_it = std::find( + available_command_interfaces_.begin(), available_command_interfaces_.end(), interface); + + if (found_it == available_command_interfaces_.end()) + { + available_command_interfaces_.emplace_back(interface); + RCUTILS_LOG_DEBUG_NAMED( + "resource_manager", "(hardware '%s'): '%s' command interface added into 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 already in available list." + " This can happen due to multiple calls to 'configure'", + hardware.get_name().c_str(), interface.c_str()); + } + } } return result; @@ -303,6 +310,8 @@ class ResourceStorage if (result) { + RCUTILS_LOG_INFO_NAMED("resource_manager", "Hardware '%s' has been deactivated", hardware.get_name().c_str()); + remove_all_command_interfaces_from_available_list(hardware.get_name()); // TODO(destogl): make all command interfaces unavailable that should be present only // when active (currently are all available) also at inactive } @@ -1072,12 +1081,25 @@ 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); + } + 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 +1122,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); + } } };