diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 5a919289b8..8b2914a381 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: @@ -166,6 +172,26 @@ 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. @@ -345,7 +371,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. /** @@ -354,7 +380,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. /** @@ -373,7 +399,12 @@ 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_; + + // 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/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 4a5c82e0f2..75700fb575 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; } @@ -545,6 +551,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 @@ -614,6 +624,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()); } LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key) @@ -719,6 +734,53 @@ void ResourceManager::remove_controller_reference_interfaces(const std::string & resource_storage_->remove_command_interfaces(interface_names); } +// CM API: Called in "callback/slow"-thread +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; + } + } + } +} + +// CM API: Called in "update"-thread +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 { @@ -807,19 +869,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 @@ -1001,32 +1075,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(