Adjust interface between CM and RM for managing controllers' reference interfaces.

Author: destogl

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -119,20 +119,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<std::string> import_controller_reference_interfaces(
-    std::vector<CommandInterface> & interfaces);
+  void import_controller_reference_interfaces(
+    const std::string & controller_name, std::vector<CommandInterface> & 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<std::string> 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<std::string> & interface_names);
+  void remove_controller_reference_interfaces(const std::string & controller_name);
 
   /// Checks whether a command interface is already claimed.
   /**

hardware_interface/src/resource_manager.cpp

@@ -449,16 +449,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());
-      }
     }
   }
 
@@ -523,6 +513,8 @@ class ResourceStorage
 
   std::unordered_map<std::string, HardwareComponentInfo> hardware_info_map_;
 
+  std::unordered_map<std::string, std::vector<std::string>> controllers_reference_interfaces_map_;
+
   /// Storage of all available state interfaces
   std::map<std::string, StateInterface> state_interface_map_;
   /// Storage of all available command interfaces
@@ -616,6 +608,7 @@ std::vector<std::string> ResourceManager::state_interface_keys() const
 
 std::vector<std::string> ResourceManager::available_state_interfaces() const
 {
+  std::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
   return resource_storage_->available_state_interfaces_;
 }
 
@@ -635,19 +628,62 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con
            name) != resource_storage_->available_state_interfaces_.end();
 }
 
-std::vector<std::string> ResourceManager::import_controller_reference_interfaces(
-  std::vector<CommandInterface> & interfaces)
+void ResourceManager::import_controller_reference_interfaces(
+  const std::string & controller_name, std::vector<CommandInterface> & interfaces)
 {
+  std::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
+  std::lock_guard<std::recursive_mutex> 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<std::string> 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<std::recursive_mutex> 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<std::string> & 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<std::recursive_mutex> 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<std::recursive_mutex> guard(resource_interfaces_lock_);
+  std::lock_guard<std::recursive_mutex> guard_claimed(claimed_command_interfaces_lock_);
   resource_storage_->remove_command_interfaces(interface_names);
 }
 

hardware_interface/test/test_resource_manager.cpp

@@ -1202,31 +1202,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<std::string> REFERENCE_INTERFACE_NAMES = {"input1", "input2", "input3"};
   std::vector<std::string> 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<hardware_interface::CommandInterface> reference_interfaces;
   std::vector<double> reference_interface_values = {1.0, 2.0, 3.0};
-  std::vector<std::string> 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]);
@@ -1263,18 +1281,35 @@ 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)
   {
     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);
 }