From c62e736f53f20ee2b3cadaddf3285f6e67eeaf5d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 4 Mar 2022 16:00:17 +0100 Subject: [PATCH 1/3] Extend resource manager to manage reference interfaces from controllers. --- .../hardware_interface/resource_manager.hpp | 22 +++++ hardware_interface/src/resource_manager.cpp | 49 ++++++++++- .../test/test_resource_manager.cpp | 81 +++++++++++++++++++ 3 files changed, 149 insertions(+), 3 deletions(-) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index ef7e3f5c4b..cefb70c6ee 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -114,6 +114,28 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool state_interface_is_available(const std::string & name) const; + /// Add controllers' reference interfaces to resource manager. + /** + * Interface for transferring management of reference interfaces to resource manager. + * When chaining controllers, reference interfaces are used as command interface of preceding + * controllers. + * Therefore, they should be managed in the same way as command interface of hardware. + * + * \param[in] interfaces list of controller's reference interfaces as CommandInterfaces. + * \return list of added reference interfaces + */ + std::vector import_controller_reference_interfaces( + std::vector & interfaces); + + /// Remove controllers reference interfaces from resource manager. + /** + * Remove reference interfaces from resource manager, i.e., resource storage. + * The interfaces will be deleted from all internal maps and lists. + * + * \param[in] interface_names list of interface names that will be deleted from resource manager. + */ + void remove_controller_reference_interfaces(const std::vector & interface_names); + /// Checks whether a command interface is already claimed. /** * Any command interface can only be claimed by a single instance. diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 2bc81a8795..71894b958b 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -221,7 +221,8 @@ class ResourceStorage { available_command_interfaces_.erase(found_it); RCUTILS_LOG_DEBUG_NAMED( - "resource_manager", "(hardware '%s'): '%s' command removed from available list", + "resource_manager", + "(hardware '%s'): '%s' command interface removed from available list", hardware.get_name().c_str(), interface.c_str()); } else @@ -275,6 +276,7 @@ class ResourceStorage // TODO(destogl): change this - deimport all things if there is there are interfaces there // deimport_non_movement_command_interfaces(hardware); // deimport_state_interfaces(hardware); + // use remove_command_interfaces(hardware); } return result; } @@ -429,6 +431,11 @@ class ResourceStorage void import_command_interfaces(HardwareT & hardware) { auto interfaces = hardware.export_command_interfaces(); + hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); + } + + std::vector add_command_interfaces(std::vector & interfaces) + { std::vector interface_names; interface_names.reserve(interfaces.size()); for (auto & interface : interfaces) @@ -438,9 +445,29 @@ class ResourceStorage claimed_command_interface_map_.emplace(std::make_pair(key, false)); interface_names.push_back(key); } - hardware_info_map_[hardware.get_name()].command_interfaces = interface_names; available_command_interfaces_.reserve( available_command_interfaces_.capacity() + interface_names.size()); + + return interface_names; + } + + void remove_command_interfaces(const std::vector & interface_names) + { + for (const auto & interface : interface_names) + { + command_interface_map_.erase(interface); + claimed_command_interface_map_.erase(interface); + + 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", "'%s' command interface removed from available list", + interface.c_str()); + } + } } // TODO(destogl): Propagate "false" up, if happens in initialize_hardware @@ -616,7 +643,23 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con name) != resource_storage_->available_state_interfaces_.end(); } -// CM API +std::vector ResourceManager::import_controller_reference_interfaces( + std::vector & interfaces) +{ + auto interface_names = resource_storage_->add_command_interfaces(interfaces); + resource_storage_->available_command_interfaces_.insert( + resource_storage_->available_command_interfaces_.end(), interface_names.begin(), + interface_names.end()); + return interface_names; +} + +void ResourceManager::remove_controller_reference_interfaces( + const std::vector & interface_names) +{ + resource_storage_->remove_command_interfaces(interface_names); +} + +// CM API: Called in "update"-thread bool ResourceManager::command_interface_is_claimed(const std::string & key) const { if (!command_interface_is_available(key)) diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 3f1a6edb3c..736314e222 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -1205,3 +1205,84 @@ TEST_F(TestResourceManager, resource_availability_and_claiming_in_lifecycle) std::bind(&hardware_interface::ResourceManager::state_interface_exists, &rm, _1), true); } } + +TEST_F(TestResourceManager, managing_controllers_reference_interfaces) +{ + hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + + std::vector FULL_REFERENCE_INTERFACE_NAMES = { + "test_controller/input1", "test_controller/input2", "test_controller/input3"}; + + std::vector reference_interfaces; + std::vector reference_interface_values = {1.0, 2.0, 3.0}; + std::vector reference_interface_names = {"input1", "input2", "input3"}; + + for (size_t i = 0; i < reference_interface_names.size(); ++i) + { + reference_interfaces.push_back(hardware_interface::CommandInterface( + "test_controller", reference_interface_names[i], &(reference_interface_values[i]))); + } + + auto ref_itf_names = rm.import_controller_reference_interfaces(reference_interfaces); + + ASSERT_THAT(ref_itf_names, testing::ElementsAreArray(FULL_REFERENCE_INTERFACE_NAMES)); + + // check that all interfaces are imported properly + for (const auto & interface : FULL_REFERENCE_INTERFACE_NAMES) + { + EXPECT_TRUE(rm.command_interface_exists(interface)); + EXPECT_TRUE(rm.command_interface_is_available(interface)); + EXPECT_FALSE(rm.command_interface_is_claimed(interface)); + } + + // claim interfaces in a scope that deletes them after + { + auto claimed_itf1 = rm.claim_command_interface(FULL_REFERENCE_INTERFACE_NAMES[0]); + auto claimed_itf3 = rm.claim_command_interface(FULL_REFERENCE_INTERFACE_NAMES[2]); + + for (const auto & interface : FULL_REFERENCE_INTERFACE_NAMES) + { + EXPECT_TRUE(rm.command_interface_exists(interface)); + EXPECT_TRUE(rm.command_interface_is_available(interface)); + } + EXPECT_TRUE(rm.command_interface_is_claimed(FULL_REFERENCE_INTERFACE_NAMES[0])); + EXPECT_FALSE(rm.command_interface_is_claimed(FULL_REFERENCE_INTERFACE_NAMES[1])); + EXPECT_TRUE(rm.command_interface_is_claimed(FULL_REFERENCE_INTERFACE_NAMES[2])); + + // access interface value + EXPECT_EQ(claimed_itf1.get_value(), 1.0); + EXPECT_EQ(claimed_itf3.get_value(), 3.0); + + claimed_itf1.set_value(11.1); + claimed_itf3.set_value(33.3); + EXPECT_EQ(claimed_itf1.get_value(), 11.1); + EXPECT_EQ(claimed_itf3.get_value(), 33.3); + + EXPECT_EQ(reference_interface_values[0], 11.1); + EXPECT_EQ(reference_interface_values[1], 2.0); + EXPECT_EQ(reference_interface_values[2], 33.3); + } + + // interfaces should be released now, but still managed by resource manager + for (const auto & interface : FULL_REFERENCE_INTERFACE_NAMES) + { + EXPECT_TRUE(rm.command_interface_exists(interface)); + EXPECT_TRUE(rm.command_interface_is_available(interface)); + EXPECT_FALSE(rm.command_interface_is_claimed(interface)); + } + + // Last written values should stay + EXPECT_EQ(reference_interface_values[0], 11.1); + EXPECT_EQ(reference_interface_values[1], 2.0); + EXPECT_EQ(reference_interface_values[2], 33.3); + + // remove reference interfaces from resource manager + rm.remove_controller_reference_interfaces(ref_itf_names); + + // they should not exist in resource manager + for (const auto & interface : FULL_REFERENCE_INTERFACE_NAMES) + { + EXPECT_FALSE(rm.command_interface_exists(interface)); + EXPECT_FALSE(rm.command_interface_is_available(interface)); + } +} From 767c247e742aa777e95b1e2d85529b67fc22c882 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 6 Mar 2022 10:50:58 +0100 Subject: [PATCH 2/3] Adjust interface between CM and RM for managing controllers' reference interfaces. --- .../hardware_interface/resource_manager.hpp | 40 +++++++++-- hardware_interface/src/resource_manager.cpp | 66 ++++++++++++++----- .../test/test_resource_manager.cpp | 49 ++++++++++++-- 3 files changed, 128 insertions(+), 27 deletions(-) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index cefb70c6ee..d77950103c 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -121,20 +121,50 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * controllers. * Therefore, they should be managed in the same way as command interface of hardware. * + * \param[in] controller_name name of the controller which reference interfaces are imported. * \param[in] interfaces list of controller's reference interfaces as CommandInterfaces. - * \return list of added reference interfaces */ - std::vector import_controller_reference_interfaces( - std::vector & interfaces); + void import_controller_reference_interfaces( + const std::string & controller_name, std::vector & interfaces); + + /// Get list of reference interface of a controller. + /** + * Returns lists of stored reference interfaces names for a controller. + * + * \param[in] controller_name for which list of reference interface names is returned. + * \returns list of reference interface names. + */ + std::vector get_controller_reference_interface_names( + const std::string & controller_name); + + /// Add controller's reference interface to available list. + /** + * Adds interfaces of a controller with given name to the available list. This method should be + * called when a controller gets activated with chained mode turned on. That means, the + * controller's reference interfaces can be used by another controller in chained architectures. + * + * \param[in] controller_name name of the controller which interfaces should become available. + */ + void make_controller_reference_interfaces_available(const std::string & controller_name); + + /// Remove controller's reference interface to available list. + /** + * Removes interfaces of a controller with given name from the available list. This method should + * be called when a controller gets deactivated and its reference interfaces cannot be used by + * another controller anymore. + * + * \param[in] controller_name name of the controller which interfaces should become unavailable. + */ + void make_controller_reference_interfaces_unavailable(const std::string & controller_name); /// Remove controllers reference interfaces from resource manager. /** * Remove reference interfaces from resource manager, i.e., resource storage. * The interfaces will be deleted from all internal maps and lists. * - * \param[in] interface_names list of interface names that will be deleted from resource manager. + * \param[in] controller_name list of interface names that will be deleted from resource manager. */ - void remove_controller_reference_interfaces(const std::vector & interface_names); + void remove_controller_reference_interfaces(const std::string & controller_name); /// Checks whether a command interface is already claimed. /** diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 71894b958b..f749d94b90 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -457,16 +457,6 @@ class ResourceStorage { command_interface_map_.erase(interface); claimed_command_interface_map_.erase(interface); - - 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", "'%s' command interface removed from available list", - interface.c_str()); - } } } @@ -531,6 +521,8 @@ class ResourceStorage std::unordered_map hardware_info_map_; + std::unordered_map> controllers_reference_interfaces_map_; + /// Storage of all available state interfaces std::map state_interface_map_; /// Storage of all available command interfaces @@ -624,6 +616,7 @@ std::vector ResourceManager::state_interface_keys() const std::vector ResourceManager::available_state_interfaces() const { + std::lock_guard guard(resource_interfaces_lock_); return resource_storage_->available_state_interfaces_; } @@ -643,19 +636,62 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con name) != resource_storage_->available_state_interfaces_.end(); } -std::vector ResourceManager::import_controller_reference_interfaces( - std::vector & interfaces) +void ResourceManager::import_controller_reference_interfaces( + const std::string & controller_name, std::vector & interfaces) { + std::lock_guard guard(resource_interfaces_lock_); + std::lock_guard guard_claimed(claimed_command_interfaces_lock_); auto interface_names = resource_storage_->add_command_interfaces(interfaces); + resource_storage_->controllers_reference_interfaces_map_[controller_name] = interface_names; +} + +std::vector ResourceManager::get_controller_reference_interface_names( + const std::string & controller_name) +{ + return resource_storage_->controllers_reference_interfaces_map_.at(controller_name); +} + +void ResourceManager::make_controller_reference_interfaces_available( + const std::string & controller_name) +{ + auto interface_names = + resource_storage_->controllers_reference_interfaces_map_.at(controller_name); + std::lock_guard guard(resource_interfaces_lock_); resource_storage_->available_command_interfaces_.insert( resource_storage_->available_command_interfaces_.end(), interface_names.begin(), interface_names.end()); - return interface_names; } -void ResourceManager::remove_controller_reference_interfaces( - const std::vector & interface_names) +void ResourceManager::make_controller_reference_interfaces_unavailable( + const std::string & controller_name) { + auto interface_names = + resource_storage_->controllers_reference_interfaces_map_.at(controller_name); + + std::lock_guard guard(resource_interfaces_lock_); + for (const auto & interface : interface_names) + { + auto found_it = std::find( + resource_storage_->available_command_interfaces_.begin(), + resource_storage_->available_command_interfaces_.end(), interface); + if (found_it != resource_storage_->available_command_interfaces_.end()) + { + resource_storage_->available_command_interfaces_.erase(found_it); + RCUTILS_LOG_DEBUG_NAMED( + "resource_manager", "'%s' command interface removed from available list", + interface.c_str()); + } + } +} + +void ResourceManager::remove_controller_reference_interfaces(const std::string & controller_name) +{ + auto interface_names = + resource_storage_->controllers_reference_interfaces_map_.at(controller_name); + resource_storage_->controllers_reference_interfaces_map_.erase(controller_name); + + std::lock_guard guard(resource_interfaces_lock_); + std::lock_guard guard_claimed(claimed_command_interfaces_lock_); resource_storage_->remove_command_interfaces(interface_names); } diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 736314e222..54f8165195 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -1210,31 +1210,49 @@ TEST_F(TestResourceManager, managing_controllers_reference_interfaces) { hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + std::string CONTROLLER_NAME = "test_controller"; + std::vector REFERENCE_INTERFACE_NAMES = {"input1", "input2", "input3"}; std::vector FULL_REFERENCE_INTERFACE_NAMES = { - "test_controller/input1", "test_controller/input2", "test_controller/input3"}; + CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[0], + CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[1], + CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[2]}; std::vector reference_interfaces; std::vector reference_interface_values = {1.0, 2.0, 3.0}; - std::vector reference_interface_names = {"input1", "input2", "input3"}; - for (size_t i = 0; i < reference_interface_names.size(); ++i) + for (size_t i = 0; i < REFERENCE_INTERFACE_NAMES.size(); ++i) { reference_interfaces.push_back(hardware_interface::CommandInterface( - "test_controller", reference_interface_names[i], &(reference_interface_values[i]))); + CONTROLLER_NAME, REFERENCE_INTERFACE_NAMES[i], &(reference_interface_values[i]))); } - auto ref_itf_names = rm.import_controller_reference_interfaces(reference_interfaces); + rm.import_controller_reference_interfaces(CONTROLLER_NAME, reference_interfaces); - ASSERT_THAT(ref_itf_names, testing::ElementsAreArray(FULL_REFERENCE_INTERFACE_NAMES)); + ASSERT_THAT( + rm.get_controller_reference_interface_names(CONTROLLER_NAME), + testing::ElementsAreArray(FULL_REFERENCE_INTERFACE_NAMES)); // check that all interfaces are imported properly for (const auto & interface : FULL_REFERENCE_INTERFACE_NAMES) + { + EXPECT_TRUE(rm.command_interface_exists(interface)); + EXPECT_FALSE(rm.command_interface_is_available(interface)); + EXPECT_FALSE(rm.command_interface_is_claimed(interface)); + } + + // make interface available + rm.make_controller_reference_interfaces_available(CONTROLLER_NAME); + for (const auto & interface : FULL_REFERENCE_INTERFACE_NAMES) { EXPECT_TRUE(rm.command_interface_exists(interface)); EXPECT_TRUE(rm.command_interface_is_available(interface)); EXPECT_FALSE(rm.command_interface_is_claimed(interface)); } + // try to make interfaces available from unknown controller + EXPECT_THROW( + rm.make_controller_reference_interfaces_available("unknown_controller"), std::out_of_range); + // claim interfaces in a scope that deletes them after { auto claimed_itf1 = rm.claim_command_interface(FULL_REFERENCE_INTERFACE_NAMES[0]); @@ -1271,13 +1289,26 @@ TEST_F(TestResourceManager, managing_controllers_reference_interfaces) EXPECT_FALSE(rm.command_interface_is_claimed(interface)); } + // make interfaces unavailable + rm.make_controller_reference_interfaces_unavailable(CONTROLLER_NAME); + for (const auto & interface : FULL_REFERENCE_INTERFACE_NAMES) + { + EXPECT_TRUE(rm.command_interface_exists(interface)); + EXPECT_FALSE(rm.command_interface_is_available(interface)); + EXPECT_FALSE(rm.command_interface_is_claimed(interface)); + } + + // try to make interfaces unavailable from unknown controller + EXPECT_THROW( + rm.make_controller_reference_interfaces_unavailable("unknown_controller"), std::out_of_range); + // Last written values should stay EXPECT_EQ(reference_interface_values[0], 11.1); EXPECT_EQ(reference_interface_values[1], 2.0); EXPECT_EQ(reference_interface_values[2], 33.3); // remove reference interfaces from resource manager - rm.remove_controller_reference_interfaces(ref_itf_names); + rm.remove_controller_reference_interfaces(CONTROLLER_NAME); // they should not exist in resource manager for (const auto & interface : FULL_REFERENCE_INTERFACE_NAMES) @@ -1285,4 +1316,8 @@ TEST_F(TestResourceManager, managing_controllers_reference_interfaces) EXPECT_FALSE(rm.command_interface_exists(interface)); EXPECT_FALSE(rm.command_interface_is_available(interface)); } + + // try to remove interfaces from unknown controller + EXPECT_THROW( + rm.make_controller_reference_interfaces_unavailable("unknown_controller"), std::out_of_range); } From 2c54f64cd8c922fa723687efe1b18335f97ba36f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 18 May 2022 11:28:46 +0200 Subject: [PATCH 3/3] Apply suggestions from code review --- hardware_interface/src/resource_manager.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index f749d94b90..760cab26d8 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -434,6 +434,17 @@ class ResourceStorage hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); } + /// Adds exported command interfaces into internal storage. + /** + * Add command interfaces to the internal storage. Command interfaces exported from hardware or + * chainable controllers are moved to the map with name-interface pairs, the interface names are + * added to the claimed map and available list's size is increased to reserve storage when + * interface change theirs status in real-time control loop. + * + * \param[interfaces] list of command interface to add into storage. + * \returns list of interface names that are added into internal storage. The output is used to + * avoid additional iterations to cache interface names, e.g., for initializing info structures. + */ std::vector add_command_interfaces(std::vector & interfaces) { std::vector interface_names; @@ -451,6 +462,12 @@ class ResourceStorage return interface_names; } + /// Removes command interfaces from internal storage. + /** + * Command interface are removed from the maps with theirs storage and their claimed status. + * + * \param[interface_names] list of command interface names to remove from storage. + */ void remove_command_interfaces(const std::vector & interface_names) { for (const auto & interface : interface_names)