Make handling of registered InterfaceManagers in InterfaceManager transparent

hardware_interface/include/hardware_interface/internal/interface_manager.h

@@ -110,6 +110,15 @@ struct CheckIsResourceManager {
 
 } // namespace internal
 
+/**
+ * \brief Manager for hardware interface registrations.
+ *
+ * This class enables the registration of interfaces based on their class type,
+ * handling all the required demangling and storage. The registration ensures
+ * the presence of at most one interface instance per type. Accessors for
+ * interface listing are provided. Additionally, combinations of interfaces as
+ * required in \c CombinedRobotHW are handled transparently.
+ */
 class InterfaceManager
 {
 public:
@@ -119,6 +128,9 @@ class InterfaceManager
    * This associates the name of the type of interface to be registered with
    * the given pointer.
    *
+   * \note The registration of an interface will replace previously registered
+   * instances of its type
+   *
    * \tparam T The interface type
    * \param iface A pointer to the interface to store
    */
@@ -134,6 +146,14 @@ class InterfaceManager
     internal::CheckIsResourceManager<T>::callGetResources(resources_[iface_name], iface);
   }
 
+  /**
+   * \brief Register another interface manager.
+   *
+   * This manager will be integrated transparently into all further access
+   * methods.
+   *
+   * \param iface_man A pointer to the interface manager to store
+   */
   void registerInterfaceManager(InterfaceManager* iface_man)
   {
     interface_managers_.push_back(iface_man);
@@ -142,12 +162,22 @@ class InterfaceManager
   /**
    * \brief Get an interface.
    *
-   * Since this class only stores one interface per type, this returns a
-   * pointer to the requested interface type. If the interface type is not
-   * registered, it will return \c NULL.
+   * If this class and its registered sub-managers only have one registered
+   * instance of the requested interface type, this will be returned. If
+   * multiple instances of the interface type were registered and the type is a
+   * \c ResourceManager, this call will try to combine them into a single
+   * handle. In all other cases, \c nullptr will be returned.
+   *
+   * \note As this class can only store one instance of each interface type,
+   * the only way multiple instances of the same interface type can be
+   * registered  is through the registration of sub-managers.
+   *
+   * \note If there are multiple registered interfaces of the requested type
+   * and they are not \c ResourceManager, this method will not be able to
+   * combine them and will return \c nullptr.
    *
    * \tparam T The interface type
-   * \return A pointer to the stored interface of type \c T or \c NULL
+   * \return A handle for the stored interfaces of type \c T or \c nullptr
    */
   template<class T>
   T* get()
@@ -200,17 +230,23 @@ class InterfaceManager
         interfaces_combo_[type_name] = iface_combo;
         num_ifaces_registered_[type_name] = iface_list.size();
       } else {
-        // it is not a ResourceManager
-        ROS_ERROR("You cannot register multiple interfaces of the same type which are "
-                  "not of type ResourceManager. There is no established protocol "
-                  "for combining them.");
+        // Multiple interfaces of the same type which are not ResourceManager cannot be combined, so return
+        // nullptr
         iface_combo = nullptr;
       }
     }
     return iface_combo;
   }
 
-  /** \return Vector of interface names registered to this instance. */
+  /**
+   * \brief Lists the demangled names of all registered interfaces.
+   *
+   * This includes the interfaces directly registered to this instance and
+   * the interfaces registered to registered managers. As multiple interfaces
+   * of the same type will get merged on access, duplicates are omitted.
+   *
+   * \return Vector of demangled interface names of registered interfaces.
+   **/
   std::vector<std::string> getNames() const
   {
     std::vector<std::string> out;
@@ -219,14 +255,29 @@ class InterfaceManager
     {
       out.push_back(interface.first);
     }
+
+    for (const auto& interface_manager : interface_managers_)
+    {
+      // Make sure interfaces appear only once, as they may have been combined
+      for (const auto& interface_name : interface_manager->getNames())
+      {
+        if (std::find(out.begin(), out.end(), interface_name) == out.end())
+        {
+          out.push_back(interface_name);
+        }
+      }
+    }
+
     return out;
   }
 
   /**
-   * \brief Get the resource names registered to an interface, specified by type
-   * (as this class only stores one interface per type)
+   * \brief Get the resource names registered to an interface, specified by type.
    *
-   * \param iface_type A string with the demangled type name of the interface
+   * This will return the list of all registered resources for
+   * \c ResourceManager interfaces and an empty list for all others.
+   *
+   * \param iface_type The demangled type name of an interface
    * \return A vector of resource names registered to this interface
    */
   std::vector<std::string> getInterfaceResources(std::string iface_type) const
@@ -237,6 +288,13 @@ class InterfaceManager
     {
       out = it->second;
     }
+
+    for (const auto& interface_manager : interface_managers_)
+    {
+      std::vector<std::string> resources = interface_manager->getInterfaceResources(iface_type);
+      out.insert(out.end(), resources.begin(), resources.end());
+    }
+
     return out;
   }
 

hardware_interface/test/interface_manager_test.cpp

@@ -48,6 +48,22 @@ struct BazInterface
   double baz;
 };
 
+class TestCombinationHandle
+{
+public:
+  TestCombinationHandle(const std::string& name) : name_(name) {}
+
+  std::string getName() const
+  {
+    return name_;
+  }
+
+private:
+  std::string name_;
+};
+
+class TestCombinationInterface : public ResourceManager<TestCombinationHandle> {};
+
 TEST(InterfaceManagerTest, InterfaceRegistration)
 {
   // Register interfaces
@@ -84,6 +100,83 @@ TEST(InterfaceManagerTest, InterfaceRewriting)
   EXPECT_EQ(2, foo_iface_ptr->foo);
 }
 
+TEST(InterfaceManagerTest, InterfaceCombination)
+{
+  // Create two InterfaceManagers, each containing a simple interface and a ResourceManager interface,
+  // and an additional simple interface for the second one
+  InterfaceManager root_mgr;
+  InterfaceManager leaf_mgr;
+
+  FooInterface foo_root_iface(1);
+  root_mgr.registerInterface(&foo_root_iface);
+  FooInterface foo_leaf_iface(2);
+  leaf_mgr.registerInterface(&foo_leaf_iface);
+
+  BazInterface baz_leaf_iface;
+  baz_leaf_iface.baz = 4.2;
+  leaf_mgr.registerInterface(&baz_leaf_iface);
+
+  TestCombinationInterface combi_root_iface;
+  TestCombinationHandle combi_root_handle("combi_root_handle");
+  combi_root_iface.registerHandle(combi_root_handle);
+  root_mgr.registerInterface(&combi_root_iface);
+
+  TestCombinationInterface combi_leaf_iface;
+  TestCombinationHandle combi_leaf_handle("combi_leaf_handle");
+  combi_leaf_iface.registerHandle(combi_leaf_handle);
+  leaf_mgr.registerInterface(&combi_leaf_iface);
+
+  // Now register leaf_mgr to root_mgr
+  root_mgr.registerInterfaceManager(&leaf_mgr);
+
+  // Querying FooInterface should not work anymore, as this would require combining the interfaces
+  FooInterface* foo_iface_ptr = root_mgr.get<FooInterface>();
+  EXPECT_EQ(nullptr, foo_iface_ptr);
+
+  // BazInterface should still work, as there is only one interface of this type
+  BazInterface* baz_iface_ptr = root_mgr.get<BazInterface>();
+  EXPECT_NE(nullptr, baz_iface_ptr);
+  if (baz_iface_ptr != nullptr) // Don't crash on error
+  {
+    EXPECT_EQ(4.2, baz_iface_ptr->baz);
+  }
+
+  // Check presence of handles in combined interface
+  TestCombinationInterface* combi_iface_ptr = root_mgr.get<TestCombinationInterface>();
+  EXPECT_NE(nullptr, combi_iface_ptr);
+  if (combi_iface_ptr != nullptr) // Don't crash on error
+  {
+    std::vector<std::string> combi_handle_names = combi_iface_ptr->getNames();
+    EXPECT_EQ(2, combi_handle_names.size());
+    EXPECT_NE(combi_handle_names.end(), std::find(combi_handle_names.begin(), combi_handle_names.end(), "combi_root_handle"))
+      << "Did not find handle 'combi_root_handle' in combined interface";
+    EXPECT_NE(combi_handle_names.end(), std::find(combi_handle_names.begin(), combi_handle_names.end(), "combi_leaf_handle"))
+      << "Did not find handle 'combi_leaf_handle' in combined interface";
+  }
+
+  // Check presence of interfaces in combined interface
+  std::vector<std::string> iface_names = root_mgr.getNames();
+  EXPECT_EQ(3, iface_names.size());
+  EXPECT_NE(iface_names.end(), std::find(iface_names.begin(), iface_names.end(), "FooInterface"))
+    << "Did not find interface 'FooInterface' in combined interface";
+  EXPECT_NE(iface_names.end(), std::find(iface_names.begin(), iface_names.end(), "BazInterface"))
+    << "Did not find interface 'BazInterface' in combined interface";
+  EXPECT_NE(iface_names.end(), std::find(iface_names.begin(), iface_names.end(), "TestCombinationInterface"))
+    << "Did not find interface 'TestCombinationInterface' in combined interface";
+
+  // Check presence of resources in combined interface
+  std::vector<std::string> iface_res = root_mgr.getInterfaceResources("FooInterface");
+  EXPECT_EQ(0, iface_res.size());
+  iface_res = root_mgr.getInterfaceResources("BazInterface");
+  EXPECT_EQ(0, iface_res.size());
+  iface_res = root_mgr.getInterfaceResources("TestCombinationInterface");
+  EXPECT_EQ(2, iface_res.size());
+  EXPECT_NE(iface_res.end(), std::find(iface_res.begin(), iface_res.end(), "combi_root_handle"))
+    << "Did not find interface resource 'combi_root_handle' for interface 'TestCombinationInterface' in combined interface";
+  EXPECT_NE(iface_res.end(), std::find(iface_res.begin(), iface_res.end(), "combi_leaf_handle"))
+    << "Did not find interface resource 'combi_leaf_handle' for interface 'TestCombinationInterface' in combined interface";
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);