Resource Manager extension to support management of reference interfaces from chained controllers.

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -114,6 +114,58 @@ 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] controller_name name of the controller which reference interfaces are imported.
+   * \param[in] interfaces list of controller's reference interfaces as CommandInterfaces.
+   */
+  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] controller_name list of interface names that will be deleted from resource manager.
+   */
+  void remove_controller_reference_interfaces(const std::string & controller_name);
+
   /// 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

@@ -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,22 @@ 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);
+  }
+
+  /// 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<std::string> add_command_interfaces(std::vector<CommandInterface> & interfaces)
+  {
     std::vector<std::string> interface_names;
     interface_names.reserve(interfaces.size());
     for (auto & interface : interfaces)
@@ -438,9 +456,25 @@ 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;
+  }
+
+  /// 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<std::string> & interface_names)
+  {
+    for (const auto & interface : interface_names)
+    {
+      command_interface_map_.erase(interface);
+      claimed_command_interface_map_.erase(interface);
+    }
   }
 
   // TODO(destogl): Propagate "false" up, if happens in initialize_hardware
@@ -504,6 +538,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
@@ -597,6 +633,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_;
 }
 
@@ -616,7 +653,66 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con
            name) != resource_storage_->available_state_interfaces_.end();
 }
 
-// CM API
+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());
+}
+
+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);
+}
+
+// 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

@@ -1205,3 +1205,119 @@ 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::string CONTROLLER_NAME = "test_controller";
+  std::vector<std::string> REFERENCE_INTERFACE_NAMES = {"input1", "input2", "input3"};
+  std::vector<std::string> FULL_REFERENCE_INTERFACE_NAMES = {
+    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};
+
+  for (size_t i = 0; i < REFERENCE_INTERFACE_NAMES.size(); ++i)
+  {
+    reference_interfaces.push_back(hardware_interface::CommandInterface(
+      CONTROLLER_NAME, REFERENCE_INTERFACE_NAMES[i], &(reference_interface_values[i])));
+  }
+
+  rm.import_controller_reference_interfaces(CONTROLLER_NAME, reference_interfaces);
+
+  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]);
+    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));
+  }
+
+  // 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(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);
+}