Extend resource manager to manage reference interfaces from controllers.

Author: destogl

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -112,6 +112,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<std::string> import_controller_reference_interfaces(
+    std::vector<CommandInterface> & 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<std::string> & interface_names);
+
   /// Checks whether a command interface is already claimed.
   /**
    * Any command interface can only be claimed by a single instance.

hardware_interface/src/resource_manager.cpp

@@ -213,7 +213,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
@@ -267,6 +268,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;
   }
@@ -421,6 +423,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<std::string> add_command_interfaces(std::vector<CommandInterface> & interfaces)
+  {
     std::vector<std::string> interface_names;
     interface_names.reserve(interfaces.size());
     for (auto & interface : interfaces)
@@ -430,9 +437,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<std::string> & 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
@@ -608,7 +635,23 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con
            name) != resource_storage_->available_state_interfaces_.end();
 }
 
-// CM API
+std::vector<std::string> ResourceManager::import_controller_reference_interfaces(
+  std::vector<CommandInterface> & 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<std::string> & 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))

hardware_interface/test/test_resource_manager.cpp

@@ -1197,3 +1197,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<std::string> FULL_REFERENCE_INTERFACE_NAMES = {
+    "test_controller/input1", "test_controller/input2", "test_controller/input3"};
+
+  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)
+  {
+    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));
+  }
+}