diff --git a/hardware_interface/include/hardware_interface/internal/interface_manager.h b/hardware_interface/include/hardware_interface/internal/interface_manager.h index 753387241..1c55c3452 100644 --- a/hardware_interface/include/hardware_interface/internal/interface_manager.h +++ b/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 #include +#include +#include #include #include +#include namespace hardware_interface { +// SFINAE workaround, so that we have reflection inside the template functions +template +struct CheckIsResourceManager { + // variable definitions for compiler-time logic + typedef char yes[1]; + typedef char no[2]; + + // method called if C is a ResourceManager + template + static yes& testRM(typename C::resource_manager_type*); + + // method called if C is not a ResourceManager + template + static no& testRM(...); + + // CheckIsResourceManager::value == true when T is a ResourceManager + static const bool value = (sizeof(testRM(0)) == sizeof(yes)); + + // method called if C is a ResourceManager + template + static yes& callCM(typename std::vector& managers, C* result, typename C::resource_manager_type*) + { + std::vector managers_in; + // we have to typecase back to base class + for(typename std::vector::iterator it = managers.begin(); it != managers.end(); ++it) + managers_in.push_back(static_cast(*it)); + C::concatManagers(managers_in, result); + } + + // method called if C is not a ResourceManager + template + static no& callCM(typename std::vector& managers, C* result, ...) {} + + // calls ResourceManager::concatManagers if C is a ResourceManager + static const void callConcatManagers(typename std::vector& managers, T* result) + { callCM(managers, result, 0); } +}; class InterfaceManager { @@ -64,6 +104,11 @@ class InterfaceManager interfaces_[internal::demangledTypeName()] = iface; } + void registerInterfaceManager(InterfaceManager* iface_man) + { + interface_managers_.push_back(iface_man); + } + /** * \brief Get an interface. * @@ -77,18 +122,68 @@ class InterfaceManager template T* get() { - InterfaceMap::iterator it = interfaces_.find(internal::demangledTypeName()); - if (it == interfaces_.end()) - return NULL; + std::string type_name = internal::demangledTypeName(); + std::vector iface_list; - T* iface = static_cast(it->second); - if (!iface) - { - ROS_ERROR_STREAM("Failed reconstructing type T = '" << internal::demangledTypeName().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(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(); + 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(it_combo->second); + } else { + // no existing combined interface + if(CheckIsResourceManager::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(iface_combo)); + // concat all of the resource managers together + CheckIsResourceManager::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 InterfaceMap; + typedef std::vector InterfaceManagerVector; + typedef std::map SizeMap; + InterfaceMap interfaces_; + InterfaceMap interfaces_combo_; + InterfaceManagerVector interface_managers_; + SizeMap num_ifaces_registered_; + boost::ptr_vector interface_destruction_list_; }; } // namespace diff --git a/hardware_interface/include/hardware_interface/internal/resource_manager.h b/hardware_interface/include/hardware_interface/internal/resource_manager.h index 78585cbe8..2cb409153 100644 --- a/hardware_interface/include/hardware_interface/internal/resource_manager.h +++ b/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 ResourceManager +class ResourceManager : public ResourceManagerBase { public: + typedef ResourceManager 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& managers, + resource_manager_type* result) + { + for(typename std::vector::iterator it_man = managers.begin(); + it_man != managers.end(); ++it_man) { + std::vector handle_names = (*it_man)->getNames(); + for(std::vector::iterator it_nms = handle_names.begin(); + it_nms != handle_names.end(); ++it_nms) { + result->registerHandle((*it_man)->getHandle(*it_nms)); + } + } + } + /*\}*/ protected: diff --git a/hardware_interface/test/robot_hw_test.cpp b/hardware_interface/test/robot_hw_test.cpp index a050cd18c..0c7f9baf0 100644 --- a/hardware_interface/test/robot_hw_test.cpp +++ b/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()); + EXPECT_TRUE(&eff_cmd_iface == hw_grp.get()); + EXPECT_TRUE(&pos_cmd_iface == hw_grp.get()); + EXPECT_FALSE(hw_grp.get()); +} + +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(); + EffortJointInterface* ej_combo = hw_grp.get(); + + // 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()); + + // confirm that each RobotHW is still working properly independently + EXPECT_TRUE(&state_iface1 == hw1.get()); + EXPECT_TRUE(&state_iface2 == hw2.get()); + EXPECT_TRUE(&eff_cmd_iface1 == hw1.get()); + EXPECT_TRUE(&eff_cmd_iface2 == hw2.get()); + + // 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(); + EffortJointInterface* ej_combo2 = hw_grp.get(); + 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(); + // 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(); + 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);