diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index bff6f7f23e..38608395cc 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: @@ -176,6 +182,27 @@ 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. @@ -355,7 +382,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. /** @@ -364,7 +391,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. /** @@ -383,9 +410,13 @@ 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_; bool is_urdf_loaded__ = false; + + // 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/actuator.cpp b/hardware_interface/src/actuator.cpp index 3766c03d79..694e92355e 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -218,6 +218,7 @@ const rclcpp_lifecycle::State & Actuator::get_state() const { return impl_->get_ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) { + // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED" return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || @@ -234,6 +235,7 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & period) { + // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED" return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index da3176e953..2fd1578535 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; } @@ -629,6 +635,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 @@ -699,6 +709,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()); } bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded__; } @@ -807,6 +822,51 @@ void ResourceManager::remove_controller_reference_interfaces(const std::string & resource_storage_->remove_command_interfaces(interface_names); } +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; + } + } + } +} + +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 { @@ -895,19 +955,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 @@ -1089,32 +1161,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( diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 036237221c..ade9b8781a 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -195,6 +195,7 @@ const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_st return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period) { + // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED" return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index a6643b3e3b..f8703a47bc 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -214,6 +214,7 @@ const rclcpp_lifecycle::State & System::get_state() const { return impl_->get_st return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) { + // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED" return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || @@ -230,6 +231,7 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & period) { + // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED" return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 47dd9f0d32..5f9c09e95e 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -75,6 +75,13 @@ class TestActuator : public ActuatorInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + // simulate error on read + if (velocity_command_ == 28282828.0) + { + // reset value to get out from error on the next call - simplifies CM tests + velocity_command_ = 0.0; + return return_type::ERROR; + } // The next line is for the testing purposes. We need value to be changed to be sure that // the feedback from hardware to controllers in the chain is working as it should. // This makes value checks clearer and confirms there is no "state = command" line or some @@ -85,6 +92,13 @@ class TestActuator : public ActuatorInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + // simulate error on write + if (velocity_command_ == 23232323.0) + { + // reset value to get out from error on the next call - simplifies CM tests + velocity_command_ = 0.0; + return return_type::ERROR; + } return return_type::OK; } diff --git a/hardware_interface/test/test_components/test_system.cpp b/hardware_interface/test/test_components/test_system.cpp index f198e057da..6ed9254393 100644 --- a/hardware_interface/test/test_components/test_system.cpp +++ b/hardware_interface/test/test_components/test_system.cpp @@ -83,11 +83,25 @@ class TestSystem : public SystemInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + // simulate error on read + if (velocity_command_[0] == 28282828) + { + // reset value to get out from error on the next call - simplifies CM tests + velocity_command_[0] = 0.0; + return return_type::ERROR; + } return return_type::OK; } return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + // simulate error on write + if (velocity_command_[0] == 23232323) + { + // reset value to get out from error on the next call - simplifies CM tests + velocity_command_[0] = 0.0; + return return_type::ERROR; + } return return_type::OK; } diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 12cc597dc7..0230972d2d 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -1388,3 +1388,263 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) EXPECT_THROW( rm.make_controller_reference_interfaces_unavailable("unknown_controller"), std::out_of_range); } + +class TestResourceManagerReadWriteError : public ResourceManagerTest +{ +public: + void setup_resource_manager_and_do_initial_checks() + { + rm = std::make_shared( + ros2_control_test_assets::minimal_robot_urdf, false); + activate_components(*rm); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + claimed_itfs.push_back( + rm->claim_command_interface(TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES[0])); + claimed_itfs.push_back(rm->claim_command_interface(TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES[0])); + + check_if_interface_available(true, true); + // with default values read and write should run without any problems + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + { + auto [ok, failed_hardware_names] = rm->write(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + check_if_interface_available(true, true); + } + + // check if all interfaces are available + void check_if_interface_available(const bool actuator_interfaces, const bool system_interfaces) + { + for (const auto & interface : TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), system_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), system_interfaces); + } + }; + + using FunctionT = + std::function; + + void check_read_or_write_failure( + FunctionT method_that_fails, FunctionT other_method, const double fail_value) + { + // define state to set components to + rclcpp_lifecycle::State state_active( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + // read failure for TEST_ACTUATOR_HARDWARE_NAME + claimed_itfs[0].set_value(fail_value); + claimed_itfs[1].set_value(fail_value - 10.0); + { + auto [ok, failed_hardware_names] = method_that_fails(time, duration); + EXPECT_FALSE(ok); + EXPECT_FALSE(failed_hardware_names.empty()); + ASSERT_THAT( + failed_hardware_names, + testing::ElementsAreArray(std::vector({TEST_ACTUATOR_HARDWARE_NAME}))); + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(false, true); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active); + status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(true, true); + } + // write is sill OK + { + auto [ok, failed_hardware_names] = other_method(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + check_if_interface_available(true, true); + } + + // read failure for TEST_SYSTEM_HARDWARE_NAME + claimed_itfs[0].set_value(fail_value - 10.0); + claimed_itfs[1].set_value(fail_value); + { + auto [ok, failed_hardware_names] = method_that_fails(time, duration); + EXPECT_FALSE(ok); + EXPECT_FALSE(failed_hardware_names.empty()); + ASSERT_THAT( + failed_hardware_names, + testing::ElementsAreArray(std::vector({TEST_SYSTEM_HARDWARE_NAME}))); + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + check_if_interface_available(true, false); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active); + status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(true, true); + } + // write is sill OK + { + auto [ok, failed_hardware_names] = other_method(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + check_if_interface_available(true, true); + } + + // read failure for both, TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME + claimed_itfs[0].set_value(fail_value); + claimed_itfs[1].set_value(fail_value); + { + auto [ok, failed_hardware_names] = method_that_fails(time, duration); + EXPECT_FALSE(ok); + EXPECT_FALSE(failed_hardware_names.empty()); + ASSERT_THAT( + failed_hardware_names, testing::ElementsAreArray(std::vector( + {TEST_ACTUATOR_HARDWARE_NAME, TEST_SYSTEM_HARDWARE_NAME}))); + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + check_if_interface_available(false, false); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active); + status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(true, true); + } + // write is sill OK + { + auto [ok, failed_hardware_names] = other_method(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + check_if_interface_available(true, true); + } + } + +public: + std::shared_ptr rm; + std::vector claimed_itfs; + + const rclcpp::Time time = rclcpp::Time(0); + const rclcpp::Duration duration = rclcpp::Duration::from_seconds(0.01); + + // 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; +}; + +TEST_F(TestResourceManagerReadWriteError, handle_error_on_hardware_read) +{ + setup_resource_manager_and_do_initial_checks(); + + using namespace std::placeholders; + // check read methods failures + check_read_or_write_failure( + std::bind(&hardware_interface::ResourceManager::read, rm, _1, _2), + std::bind(&hardware_interface::ResourceManager::write, rm, _1, _2), READ_FAIL_VALUE); +} + +TEST_F(TestResourceManagerReadWriteError, handle_error_on_hardware_write) +{ + setup_resource_manager_and_do_initial_checks(); + + using namespace std::placeholders; + // check write methods failures + check_read_or_write_failure( + std::bind(&hardware_interface::ResourceManager::write, rm, _1, _2), + std::bind(&hardware_interface::ResourceManager::read, rm, _1, _2), WRITE_FAIL_VALUE); +} + +TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware) +{ + hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); + activate_components(rm); + + static const std::string TEST_CONTROLLER_ACTUATOR_NAME = "test_controller_actuator"; + static const std::string TEST_CONTROLLER_SYSTEM_NAME = "test_controller_system"; + static const std::string TEST_BROADCASTER_ALL_NAME = "test_broadcaster_all"; + static const std::string TEST_BROADCASTER_SENSOR_NAME = "test_broadcaster_sensor"; + + rm.cache_controller_to_hardware( + TEST_CONTROLLER_ACTUATOR_NAME, TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES); + rm.cache_controller_to_hardware( + TEST_BROADCASTER_ALL_NAME, TEST_ACTUATOR_HARDWARE_STATE_INTERFACES); + + rm.cache_controller_to_hardware( + TEST_CONTROLLER_SYSTEM_NAME, TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES); + rm.cache_controller_to_hardware(TEST_BROADCASTER_ALL_NAME, TEST_SYSTEM_HARDWARE_STATE_INTERFACES); + + rm.cache_controller_to_hardware( + TEST_BROADCASTER_SENSOR_NAME, TEST_SENSOR_HARDWARE_STATE_INTERFACES); + rm.cache_controller_to_hardware(TEST_BROADCASTER_ALL_NAME, TEST_SENSOR_HARDWARE_STATE_INTERFACES); + + { + auto controllers = rm.get_cached_controllers_to_hardware(TEST_ACTUATOR_HARDWARE_NAME); + ASSERT_THAT( + controllers, testing::ElementsAreArray(std::vector( + {TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}))); + } + + { + auto controllers = rm.get_cached_controllers_to_hardware(TEST_SYSTEM_HARDWARE_NAME); + ASSERT_THAT( + controllers, testing::ElementsAreArray(std::vector( + {TEST_CONTROLLER_SYSTEM_NAME, TEST_BROADCASTER_ALL_NAME}))); + } + + { + auto controllers = rm.get_cached_controllers_to_hardware(TEST_SENSOR_HARDWARE_NAME); + ASSERT_THAT( + controllers, testing::ElementsAreArray(std::vector( + {TEST_BROADCASTER_SENSOR_NAME, TEST_BROADCASTER_ALL_NAME}))); + } +}