Remove inconsistent InterfaceManager manager registering behavior

All InterfaceManager now handle registered InterfaceManagers transparently. This allows chains of multiple InterfaceManagers registered to each other to work corectly, mostly relevant for registering a manager from a combined_robot_hw RobotHW. Resolves #452
ros-controls · Dec 5, 2020 · 5e99e87 · 5e99e87
1 parent 64b1b5e
commit 5e99e87
Show file tree

Hide file tree

Showing 2 changed files with 113 additions and 0 deletions.
diff --git a/hardware_interface/include/hardware_interface/internal/interface_manager.h b/hardware_interface/include/hardware_interface/internal/interface_manager.h
@@ -219,6 +219,19 @@ 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;
   }
 
@@ -237,6 +250,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;
   }
 

diff --git a/hardware_interface/test/interface_manager_test.cpp b/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);