Allow the InterfaceManager class to register other InterfaceManagers.

hardware_interface/include/hardware_interface/internal/interface_manager.h

@@ -26,20 +26,60 @@
 // POSSIBILITY OF SUCH DAMAGE.
 //////////////////////////////////////////////////////////////////////////////
 
-/// \author Wim Meussen, Adolfo Rodriguez Tsouroukdissian
+/// \author Wim Meussen, Adolfo Rodriguez Tsouroukdissian, Kelsey P. Hawkins
 
 #ifndef HARDWARE_INTERFACE_INTERFACE_MANAGER_H
 #define HARDWARE_INTERFACE_INTERFACE_MANAGER_H
 
 #include <map>
 #include <string>
+#include <vector>
+#include <boost/ptr_container/ptr_vector.hpp>
 
 #include <ros/console.h>
 
 #include <hardware_interface/internal/demangle_symbol.h>
+#include <hardware_interface/internal/resource_manager.h>
 
 namespace hardware_interface
 {
+// SFINAE workaround, so that we have reflection inside the template functions
+template <typename T>
+struct CheckIsResourceManager {
+  // variable definitions for compiler-time logic
+  typedef char yes[1];
+  typedef char no[2];
+
+  // method called if C is a ResourceManager
+  template <typename C>
+  static yes& testRM(typename C::resource_manager_type*);
+
+  // method called if C is not a ResourceManager
+  template <typename>
+  static no& testRM(...);
+
+  // CheckIsResourceManager<T>::value == true when T is a ResourceManager
+  static const bool value = (sizeof(testRM<T>(0)) == sizeof(yes));
+
+  // method called if C is a ResourceManager
+  template <typename C>
+  static yes& callCM(typename std::vector<C*>& managers, C* result, typename C::resource_manager_type*)
+  {
+    std::vector<typename C::resource_manager_type*> managers_in;
+    // we have to typecase back to base class
+    for(typename std::vector<C*>::iterator it = managers.begin(); it != managers.end(); ++it)
+      managers_in.push_back(static_cast<typename C::resource_manager_type*>(*it));
+    C::concatManagers(managers_in, result);
+  }
+
+  // method called if C is not a ResourceManager
+  template <typename C>
+  static no& callCM(typename std::vector<C*>& managers, C* result, ...) {}
+
+  // calls ResourceManager::concatManagers if C is a ResourceManager
+  static const void callConcatManagers(typename std::vector<T*>& managers, T* result)
+  { callCM<T>(managers, result, 0); }
+};
 
 class InterfaceManager
 {
@@ -64,6 +104,11 @@ class InterfaceManager
     interfaces_[internal::demangledTypeName<T>()] = iface;
   }
 
+  void registerInterfaceManager(InterfaceManager* iface_man)
+  {
+    interface_managers_.push_back(iface_man);
+  }
+
   /**
    * \brief Get an interface.
    *
@@ -77,18 +122,68 @@ class InterfaceManager
   template<class T>
   T* get()
   {
-    InterfaceMap::iterator it = interfaces_.find(internal::demangledTypeName<T>());
-    if (it == interfaces_.end())
-      return NULL;
+    std::string type_name = internal::demangledTypeName<T>();
+    std::vector<T*> iface_list;
 
-    T* iface = static_cast<T*>(it->second);
-    if (!iface)
-    {
-      ROS_ERROR_STREAM("Failed reconstructing type T = '" << internal::demangledTypeName<T>().c_str() <<
-                       "'. This should never happen");
+    // look for interfaces registered here
+    InterfaceMap::iterator it = interfaces_.find(type_name);
+    if (it != interfaces_.end()) {
+      T* iface = static_cast<T*>(it->second);
+      if (!iface) {
+        ROS_ERROR_STREAM("Failed reconstructing type T = '" << type_name.c_str() <<
+                         "'. This should never happen");
+        return NULL;
+      }
+      iface_list.push_back(iface);
+    }
+
+    // look for interfaces registered in the registered hardware
+    for(InterfaceManagerVector::iterator it = interface_managers_.begin();
+        it != interface_managers_.end(); ++it) {
+      T* iface = (*it)->get<T>();
+      if (iface)
+        iface_list.push_back(iface);
+    }
+
+    if(iface_list.size() == 0)
       return NULL;
+
+    if(iface_list.size() == 1)
+      return iface_list.front();
+
+    // if we're here, we have multiple interfaces, and thus we must construct a new
+    // combined interface, or return one already constructed
+    T* iface_combo;
+    InterfaceMap::iterator it_combo = interfaces_combo_.find(type_name);
+    if(it_combo != interfaces_combo_.end() &&
+        num_ifaces_registered_[type_name] == iface_list.size()) {
+      // there exists a combined interface with the same number of interfaces combined
+      // (since you cannot unregister interfaces, this will be guaranteed to be the
+      //  same interfaces from previous calls)
+      iface_combo = static_cast<T*>(it_combo->second);
+    } else {
+      // no existing combined interface
+      if(CheckIsResourceManager<T>::value) {
+        // it is a ResourceManager
+
+        // create a new combined interface
+        iface_combo = new T;
+        // save the new interface pointer to allow for its correct destruction
+        interface_destruction_list_.push_back(reinterpret_cast<ResourceManagerBase*>(iface_combo));
+        // concat all of the resource managers together
+        CheckIsResourceManager<T>::callConcatManagers(iface_list, iface_combo);
+        // save the combined interface for if this is called again
+        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.");
+        iface_combo = NULL;
+      }
     }
-    return iface;
+    return iface_combo;
   }
 
   /** \return Vector of interface names registered to this instance. */
@@ -105,7 +200,14 @@ class InterfaceManager
 
 protected:
   typedef std::map<std::string, void*> InterfaceMap;
+  typedef std::vector<InterfaceManager*> InterfaceManagerVector;
+  typedef std::map<std::string, size_t> SizeMap;
+
   InterfaceMap interfaces_;
+  InterfaceMap interfaces_combo_;
+  InterfaceManagerVector interface_managers_;
+  SizeMap num_ifaces_registered_;
+  boost::ptr_vector<ResourceManagerBase> interface_destruction_list_;
 };
 
 } // namespace

hardware_interface/include/hardware_interface/internal/resource_manager.h

@@ -44,6 +44,17 @@
 namespace hardware_interface
 {
 
+/**
+ * \brief Non-templated Base Class that contains a virtual destructor.
+ *
+ * This will allow to destroy the templated children without having to know the template type.
+ */
+class ResourceManagerBase
+{
+public:
+  virtual ~ResourceManagerBase() {}
+};
+
 /**
  * \brief Class for handling named resources.
  *
@@ -55,9 +66,10 @@ namespace hardware_interface
  *  \endcode
  */
 template <class ResourceHandle>
-class ResourceManager
+class ResourceManager : public ResourceManagerBase
 {
 public:
+  typedef ResourceManager<ResourceHandle> resource_manager_type;
   /** \name Non Real-Time Safe Functions
    *\{*/
 
@@ -113,6 +125,27 @@ class ResourceManager
     return it->second;
   }
 
+  /**
+   * \brief Combine a list of interfaces into one.
+   *
+   * Every registered handle in each of the managers is registered into the result interface
+   * \param managers The list of resource managers to be combined.
+   * \param result The interface where all the handles will be registered.
+   * \return Resource associated to \e name. If the resource name is not found, an exception is thrown.
+   */
+  static void concatManagers(std::vector<resource_manager_type*>& managers,
+                             resource_manager_type* result)
+  {
+    for(typename std::vector<resource_manager_type*>::iterator it_man = managers.begin();
+        it_man != managers.end(); ++it_man) {
+      std::vector<std::string> handle_names = (*it_man)->getNames();
+      for(std::vector<std::string>::iterator it_nms = handle_names.begin();
+          it_nms != handle_names.end(); ++it_nms) {
+        result->registerHandle((*it_man)->getHandle(*it_nms));
+      }
+    }
+  }
+
   /*\}*/
 
 protected:

hardware_interface/test/robot_hw_test.cpp

@@ -225,6 +225,135 @@ TEST_F(RobotHWTest, ConflictChecking)
   }
 }
 
+TEST_F(RobotHWTest, CombineDifferentInterfaces)
+{
+  // Populate hardware interfaces
+  JointStateInterface state_iface;
+  state_iface.registerHandle(hs1);
+  state_iface.registerHandle(hs2);
+
+  EffortJointInterface eff_cmd_iface;
+  eff_cmd_iface.registerHandle(hc1);
+  eff_cmd_iface.registerHandle(hc2);
+
+  PositionJointInterface pos_cmd_iface;
+  pos_cmd_iface.registerHandle(hc1);
+  pos_cmd_iface.registerHandle(hc2);
+
+  // Register them to different RobotHW instances
+  RobotHW hw1, hw2;
+  RobotHW hw_grp;
+  hw1.registerInterface(&state_iface);
+  hw2.registerInterface(&eff_cmd_iface);
+  hw_grp.registerInterface(&pos_cmd_iface);
+
+  hw_grp.registerInterfaceManager(&hw1);
+  hw_grp.registerInterfaceManager(&hw2);
+
+  // Get interfaces
+  EXPECT_TRUE(&state_iface   == hw_grp.get<JointStateInterface>());
+  EXPECT_TRUE(&eff_cmd_iface == hw_grp.get<EffortJointInterface>());
+  EXPECT_TRUE(&pos_cmd_iface == hw_grp.get<PositionJointInterface>());
+  EXPECT_FALSE(hw_grp.get<VelocityJointInterface>());
+}
+
+TEST_F(RobotHWTest, CombineSameInterfaces)
+{
+  // Populate hardware interfaces
+  JointStateInterface state_iface1;
+  state_iface1.registerHandle(hs1);
+  EffortJointInterface eff_cmd_iface1;
+  eff_cmd_iface1.registerHandle(hc1);
+
+  JointStateInterface state_iface2;
+  state_iface2.registerHandle(hs2);
+  EffortJointInterface eff_cmd_iface2;
+  eff_cmd_iface2.registerHandle(hc2);
+
+  // Register them to different RobotHW instances
+  RobotHW hw1, hw2;
+  RobotHW hw_grp;
+  hw1.registerInterface(&state_iface1);
+  hw1.registerInterface(&eff_cmd_iface1);
+  hw2.registerInterface(&state_iface2);
+  hw2.registerInterface(&eff_cmd_iface2);
+
+  hw_grp.registerInterfaceManager(&hw1);
+  hw_grp.registerInterfaceManager(&hw2);
+
+  // Get interfaces
+  JointStateInterface* js_combo = hw_grp.get<JointStateInterface>();
+  EffortJointInterface* ej_combo = hw_grp.get<EffortJointInterface>();
+
+  // confirm that the combined interfaces are different from the originals
+  EXPECT_FALSE(&state_iface1   == js_combo);
+  EXPECT_FALSE(&state_iface2   == js_combo);
+  EXPECT_FALSE(&eff_cmd_iface1 == ej_combo);
+  EXPECT_FALSE(&eff_cmd_iface2 == ej_combo);
+  EXPECT_FALSE(hw_grp.get<VelocityJointInterface>());
+
+  // confirm that each RobotHW is still working properly independently
+  EXPECT_TRUE(&state_iface1   == hw1.get<JointStateInterface>());
+  EXPECT_TRUE(&state_iface2   == hw2.get<JointStateInterface>());
+  EXPECT_TRUE(&eff_cmd_iface1 == hw1.get<EffortJointInterface>());
+  EXPECT_TRUE(&eff_cmd_iface2 == hw2.get<EffortJointInterface>());
+
+  // Retrieve all the handles from the combined interfaces
+  JointStateHandle hs1_ret = js_combo->getHandle(name1);
+  JointStateHandle hs2_ret = js_combo->getHandle(name2);
+  JointHandle hc1_ret = ej_combo->getHandle(name1);
+  JointHandle hc2_ret = ej_combo->getHandle(name2);
+
+  // confirm the handles are proper copies
+  EXPECT_TRUE(hs1.getPosition() == hs1_ret.getPosition());
+  EXPECT_TRUE(hs2.getPosition() == hs2_ret.getPosition());
+  hc1.setCommand(3.14);
+  EXPECT_EQ(3.14, hc1_ret.getCommand());
+  hc2.setCommand(6.28);
+  EXPECT_EQ(6.28, hc2_ret.getCommand());
+
+  // check to make sure further calls return the same combined interface objects
+  JointStateInterface* js_combo2 = hw_grp.get<JointStateInterface>();
+  EffortJointInterface* ej_combo2 = hw_grp.get<EffortJointInterface>();
+  EXPECT_TRUE(js_combo == js_combo2);
+  EXPECT_TRUE(ej_combo == ej_combo2);
+}
+
+TEST_F(RobotHWTest, IncrementalSameInterfaces)
+{
+  // Populate hardware interfaces
+  JointStateInterface state_iface1;
+  state_iface1.registerHandle(hs1);
+
+  JointStateInterface state_iface2;
+  state_iface2.registerHandle(hs2);
+
+  // Register them to different RobotHW instances
+  RobotHW hw1, hw2;
+  hw1.registerInterface(&state_iface1);
+  hw2.registerInterface(&state_iface2);
+
+  RobotHW hw_grp;
+  hw_grp.registerInterfaceManager(&hw1);
+  JointStateInterface* js_combo1 = hw_grp.get<JointStateInterface>();
+  // only one interface exists, so the combined should be exactly the registered interface object
+  EXPECT_TRUE(&state_iface1 == js_combo1);
+  // check that it contains hs1 handle
+  JointStateHandle hs1_ret1 = js_combo1->getHandle(name1);
+  EXPECT_TRUE(hs1.getPosition() == hs1_ret1.getPosition());
+
+  hw_grp.registerInterfaceManager(&hw2);
+  JointStateInterface* js_combo2 = hw_grp.get<JointStateInterface>();
+  EXPECT_FALSE(&state_iface1 == js_combo2);
+  EXPECT_FALSE(&state_iface2 == js_combo2);
+
+  // check to see if both joint handles are here
+  JointStateHandle hs1_ret2 = js_combo2->getHandle(name1);
+  JointStateHandle hs2_ret = js_combo2->getHandle(name2);
+  EXPECT_TRUE(hs1.getPosition() == hs1_ret2.getPosition());
+  EXPECT_TRUE(hs2.getPosition() == hs2_ret.getPosition());
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);