From 6ecdde0d4798bc138840cf58a32c9364b7be7760 Mon Sep 17 00:00:00 2001 From: Kelsey Date: Wed, 8 Jan 2014 16:19:00 -0500 Subject: [PATCH 01/16] Adding RobotHWGroup, which maintains a group of RobotHW as if it were only one. --- .../hardware_interface/robot_hw_group.h | 88 +++++++++ .../test/robot_hw_group_test.cpp | 177 ++++++++++++++++++ 2 files changed, 265 insertions(+) create mode 100644 hardware_interface/include/hardware_interface/robot_hw_group.h create mode 100644 hardware_interface/test/robot_hw_group_test.cpp diff --git a/hardware_interface/include/hardware_interface/robot_hw_group.h b/hardware_interface/include/hardware_interface/robot_hw_group.h new file mode 100644 index 000000000..fbc00259a --- /dev/null +++ b/hardware_interface/include/hardware_interface/robot_hw_group.h @@ -0,0 +1,88 @@ +/////////////////////////////////////////////////////////////////////////////// +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of hiDOF, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/* + * Author: Kelsey Hawkins + */ + +#ifndef HARDWARE_INTERFACE_ROBOT_HW_GROUP_H +#define HARDWARE_INTERFACE_ROBOT_HW_GROUP_H + +#include +#include +#include + +namespace hardware_interface +{ + +/** \brief Robot Hardware Interface and Resource Manager + * + * This class provides a standardized interface to a set of robot hardware + * interfaces to the controller manager. It performs resource conflict checking + * for a given set of controllers and maintains a map of hardware interfaces. + * It is meant to be used as a base class for abstracting custom robot + * hardware. + * + */ +class RobotHWGroup : public RobotHW +{ +public: + RobotHWGroup() + { + + } + + void registerHardware(RobotHW* hw) + { + hardware_.push_back(hw); + } + + virtual void* findInterfaceData(std::string type_name) + { + // look for interfaces registered here + void* iface_data = InterfaceManager::findInterfaceData(type_name); + if(iface_data != NULL) + return iface_data; + + // look for interfaces registered in the registered hardware + for(HardwareVector::iterator it = hardware_.begin(); it != hardware_.end(); ++it) { + iface_data = (*it)->findInterfaceData(type_name); + if(iface_data != NULL) + return iface_data; + } + return NULL; + } + +protected: + typedef std::vector HardwareVector; + HardwareVector hardware_; + +}; + +} + +#endif + diff --git a/hardware_interface/test/robot_hw_group_test.cpp b/hardware_interface/test/robot_hw_group_test.cpp new file mode 100644 index 000000000..4b80dc55c --- /dev/null +++ b/hardware_interface/test/robot_hw_group_test.cpp @@ -0,0 +1,177 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of hiDOF, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Adolfo Rodriguez Tsouroukdissian +/// \author Kelsey Hawkins + +#include +#include +#include +#include +#include +#include +#include + +using std::list; +using std::string; +using namespace hardware_interface; + +class RobotHWGroupTest : public ::testing::Test +{ +public: + RobotHWGroupTest() + : pos1(1.0), vel1(2.0), eff1(3.0), cmd1(0.0), + pos2(4.0), vel2(5.0), eff2(6.0), cmd2(0.0), + name1("name_1"), + name2("name_2"), + hs1(name1, &pos1, &vel1, &eff1), + hs2(name2, &pos2, &vel2, &eff2), + hc1(hs1, &cmd1), + hc2(hs2, &cmd2) + {} + +protected: + double pos1, vel1, eff1, cmd1; + double pos2, vel2, eff2, cmd2; + string name1; + string name2; + JointStateHandle hs1, hs2; + JointHandle hc1, hc2; +}; + +TEST_F(RobotHWGroupTest, InterfaceRegistration) +{ + // 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 a RobotHW instance + RobotHW hw; + hw.registerInterface(&state_iface); + hw.registerInterface(&eff_cmd_iface); + hw.registerInterface(&pos_cmd_iface); + + RobotHWGroup hw_grp; + hw_grp.registerHardware(&hw); + + // 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(RobotHWGroupTest, InterfaceRewriting) +{ + // Two hardware interfaces of the same type, with different joints + JointStateInterface state1_iface; + state1_iface.registerHandle(hs1); + + JointStateInterface state2_iface; + state2_iface.registerHandle(hs1); + state2_iface.registerHandle(hs2); + + // Register first interface and validate it + RobotHW hw; + hw.registerInterface(&state1_iface); + + JointStateInterface* state_iface_ptr = hw.get(); + EXPECT_EQ(1, state_iface_ptr->getNames().size()); + + // Register second interface and verify that it has taken the place of the previously inserted one + hw.registerInterface(&state2_iface); + state_iface_ptr = hw.get(); + EXPECT_EQ(2, state_iface_ptr->getNames().size()); +} + +TEST_F(RobotHWGroupTest, ConflictChecking) +{ + ControllerInfo info1; + info1.name = "controller_1"; + info1.type = "type_1"; + info1.hardware_interface = "interface_1"; + info1.resources.insert("resource_1"); + + ControllerInfo info2; + info2.name = "controller_2"; + info2.type = "type_2"; + info2.hardware_interface = "interface_2"; + info2.resources.insert("resource_2"); + + ControllerInfo info12; + info12.name = "controller_12"; + info12.type = "type_12"; + info12.hardware_interface = "interface_12"; + info12.resources.insert("resource_1"); + info12.resources.insert("resource_2"); + + // No conflict + { + list info_list; + info_list.push_back(info1); + info_list.push_back(info2); + + RobotHW hw; + EXPECT_FALSE(hw.checkForConflict(info_list)); + } + + // Conflict + { + list info_list; + info_list.push_back(info1); + info_list.push_back(info12); + + RobotHW hw; + EXPECT_TRUE(hw.checkForConflict(info_list)); + } + + // Conflict + { + list info_list; + info_list.push_back(info2); + info_list.push_back(info12); + + RobotHW hw; + EXPECT_TRUE(hw.checkForConflict(info_list)); + } +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + From ecbc3d33643066cf9881aed934392e2c01a18f82 Mon Sep 17 00:00:00 2001 From: Kelsey Date: Wed, 8 Jan 2014 16:25:01 -0500 Subject: [PATCH 02/16] Updated tests, added changes to infrastructure. --- hardware_interface/CMakeLists.txt | 5 ++ .../internal/interface_manager.h | 14 +++-- .../test/robot_hw_group_test.cpp | 53 +++++++++++++++---- 3 files changed, 59 insertions(+), 13 deletions(-) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index fa001a1a4..23aec3757 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -17,6 +17,7 @@ if(USE_ROSBUILD) rosbuild_add_gtest(force_torque_sensor_interface_test test/force_torque_sensor_interface_test.cpp) rosbuild_add_gtest(imu_sensor_interface_test test/imu_sensor_interface_test.cpp) rosbuild_add_gtest(robot_hw_test test/robot_hw_test.cpp) + rosbuild_add_gtest(robot_hw_group_test test/robot_hw_group_test.cpp) rosbuild_add_gtest(interface_manager_test test/interface_manager_test.cpp) # TODO: why is it explicitly needed???, without it the linker fails. @@ -28,6 +29,7 @@ if(USE_ROSBUILD) target_link_libraries(force_torque_sensor_interface_test pthread) target_link_libraries(imu_sensor_interface_test pthread) target_link_libraries(robot_hw_test pthread) + target_link_libraries(robot_hw_group_test pthread) else() @@ -66,6 +68,9 @@ else() catkin_add_gtest(robot_hw_test test/robot_hw_test.cpp) target_link_libraries(robot_hw_test ${catkin_LIBRARIES}) + catkin_add_gtest(robot_hw_group_test test/robot_hw_group_test.cpp) + target_link_libraries(robot_hw_group_test ${catkin_LIBRARIES}) + catkin_add_gtest(interface_manager_test test/interface_manager_test.cpp) target_link_libraries(interface_manager_test ${catkin_LIBRARIES}) endif() diff --git a/hardware_interface/include/hardware_interface/internal/interface_manager.h b/hardware_interface/include/hardware_interface/internal/interface_manager.h index 46f5abf89..dab4fd8ec 100644 --- a/hardware_interface/include/hardware_interface/internal/interface_manager.h +++ b/hardware_interface/include/hardware_interface/internal/interface_manager.h @@ -77,11 +77,9 @@ class InterfaceManager template T* get() { - InterfaceMap::iterator it = interfaces_.find(internal::demangledTypeName()); - if (it == interfaces_.end()) - return NULL; + void* iface_data = findInterfaceData(internal::demangledTypeName()); - T* iface = static_cast(it->second); + T* iface = static_cast(iface_data); if (!iface) { ROS_ERROR_STREAM("Failed reconstructing type T = '" << internal::demangledTypeName().c_str() << @@ -91,6 +89,14 @@ class InterfaceManager return iface; } + virtual void* findInterfaceData(std::string type_name) + { + InterfaceMap::iterator it = interfaces_.find(type_name); + if (it == interfaces_.end()) + return NULL; + return it->second; + } + protected: typedef std::map InterfaceMap; InterfaceMap interfaces_; diff --git a/hardware_interface/test/robot_hw_group_test.cpp b/hardware_interface/test/robot_hw_group_test.cpp index 4b80dc55c..1aaa88895 100644 --- a/hardware_interface/test/robot_hw_group_test.cpp +++ b/hardware_interface/test/robot_hw_group_test.cpp @@ -63,6 +63,38 @@ class RobotHWGroupTest : public ::testing::Test JointHandle hc1, hc2; }; +TEST_F(RobotHWGroupTest, GroupTest) +{ + // 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; + RobotHWGroup hw_grp; + hw1.registerInterface(&state_iface); + hw2.registerInterface(&eff_cmd_iface); + hw_grp.registerInterface(&pos_cmd_iface); + + hw_grp.registerHardware(&hw1); + hw_grp.registerHardware(&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(RobotHWGroupTest, InterfaceRegistration) { // Populate hardware interfaces @@ -108,12 +140,15 @@ TEST_F(RobotHWGroupTest, InterfaceRewriting) RobotHW hw; hw.registerInterface(&state1_iface); - JointStateInterface* state_iface_ptr = hw.get(); + RobotHWGroup hw_grp; + hw_grp.registerHardware(&hw); + + JointStateInterface* state_iface_ptr = hw_grp.get(); EXPECT_EQ(1, state_iface_ptr->getNames().size()); // Register second interface and verify that it has taken the place of the previously inserted one - hw.registerInterface(&state2_iface); - state_iface_ptr = hw.get(); + hw_grp.registerInterface(&state2_iface); + state_iface_ptr = hw_grp.get(); EXPECT_EQ(2, state_iface_ptr->getNames().size()); } @@ -144,8 +179,8 @@ TEST_F(RobotHWGroupTest, ConflictChecking) info_list.push_back(info1); info_list.push_back(info2); - RobotHW hw; - EXPECT_FALSE(hw.checkForConflict(info_list)); + RobotHWGroup hw_grp; + EXPECT_FALSE(hw_grp.checkForConflict(info_list)); } // Conflict @@ -154,8 +189,8 @@ TEST_F(RobotHWGroupTest, ConflictChecking) info_list.push_back(info1); info_list.push_back(info12); - RobotHW hw; - EXPECT_TRUE(hw.checkForConflict(info_list)); + RobotHWGroup hw_grp; + EXPECT_TRUE(hw_grp.checkForConflict(info_list)); } // Conflict @@ -164,8 +199,8 @@ TEST_F(RobotHWGroupTest, ConflictChecking) info_list.push_back(info2); info_list.push_back(info12); - RobotHW hw; - EXPECT_TRUE(hw.checkForConflict(info_list)); + RobotHWGroup hw_grp; + EXPECT_TRUE(hw_grp.checkForConflict(info_list)); } } From 398e3a01a825861b4d6e85b8bacf06600c1e9d8d Mon Sep 17 00:00:00 2001 From: Kelsey Date: Wed, 8 Jan 2014 17:07:27 -0500 Subject: [PATCH 03/16] Updated documentation for RobotHWGroup. --- .../internal/interface_manager.h | 9 +++++++++ .../hardware_interface/robot_hw_group.h | 20 +++++++++++++------ 2 files changed, 23 insertions(+), 6 deletions(-) diff --git a/hardware_interface/include/hardware_interface/internal/interface_manager.h b/hardware_interface/include/hardware_interface/internal/interface_manager.h index dab4fd8ec..28959394e 100644 --- a/hardware_interface/include/hardware_interface/internal/interface_manager.h +++ b/hardware_interface/include/hardware_interface/internal/interface_manager.h @@ -89,6 +89,15 @@ class InterfaceManager return iface; } + /** + * \brief Get generic pointer to interface with type_name. + * + * This is used as a polymorphic lookup for the templated + * get() call, which can't be virtual. + * + * \param type_name The name of the interface type stored. + * \return Generic pointer to the interface. + */ virtual void* findInterfaceData(std::string type_name) { InterfaceMap::iterator it = interfaces_.find(type_name); diff --git a/hardware_interface/include/hardware_interface/robot_hw_group.h b/hardware_interface/include/hardware_interface/robot_hw_group.h index fbc00259a..c286343dd 100644 --- a/hardware_interface/include/hardware_interface/robot_hw_group.h +++ b/hardware_interface/include/hardware_interface/robot_hw_group.h @@ -38,13 +38,11 @@ namespace hardware_interface { -/** \brief Robot Hardware Interface and Resource Manager +/** \brief Robot Hardware Collection Manager * - * This class provides a standardized interface to a set of robot hardware - * interfaces to the controller manager. It performs resource conflict checking - * for a given set of controllers and maintains a map of hardware interfaces. - * It is meant to be used as a base class for abstracting custom robot - * hardware. + * This class maintains a group of RobotHW, along with its + * own registered interfaces, and exposes an interface + * identical to RobotHW. * */ class RobotHWGroup : public RobotHW @@ -60,6 +58,16 @@ class RobotHWGroup : public RobotHW hardware_.push_back(hw); } + /** + * \brief Get generic pointer to interface with type_name. + * + * This overloads the base implementation to look across both + * local interfaces and interfaces in all registered RobotHW. + * This is used by the get() call in the base class. + * + * \param type_name The name of the interface type stored. + * \return Generic pointer to the interface. + */ virtual void* findInterfaceData(std::string type_name) { // look for interfaces registered here From bc0d5861a0f945b731f3b2dd00653a0484c1316f Mon Sep 17 00:00:00 2001 From: Kelsey Date: Thu, 9 Jan 2014 18:21:08 -0500 Subject: [PATCH 04/16] Added NULL-checking for the new method added. --- .../include/hardware_interface/internal/interface_manager.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/hardware_interface/include/hardware_interface/internal/interface_manager.h b/hardware_interface/include/hardware_interface/internal/interface_manager.h index 28959394e..9e17c8cf1 100644 --- a/hardware_interface/include/hardware_interface/internal/interface_manager.h +++ b/hardware_interface/include/hardware_interface/internal/interface_manager.h @@ -78,6 +78,8 @@ class InterfaceManager T* get() { void* iface_data = findInterfaceData(internal::demangledTypeName()); + if(!iface_data) + return NULL; T* iface = static_cast(iface_data); if (!iface) From f5b54c0505e66e705ef1b2af5a767333a75ad4d1 Mon Sep 17 00:00:00 2001 From: Kelsey Date: Sun, 12 Jan 2014 21:44:31 -0500 Subject: [PATCH 05/16] Modified hardware_interface to allow multiple RobotHW registered in a RobotHWGroup to have identical interfaces. It accomplishes this by combining interfaces during a InterfaceManager::get() call. This new combined interface is saved internally, but distinct from other interface instances, so that while each interface keeps its integrity, the RobotHWGroup::get() call combines them all into a new interface from which all ResourceHandles may be obtained. --- .../internal/hardware_resource_manager.h | 23 ++++ .../internal/interface_manager.h | 111 +++++++++++++++--- .../internal/resource_manager.h | 25 ++++ .../hardware_interface/robot_hw_group.h | 31 ++--- .../test/robot_hw_group_test.cpp | 64 +++++++++- 5 files changed, 222 insertions(+), 32 deletions(-) diff --git a/hardware_interface/include/hardware_interface/internal/hardware_resource_manager.h b/hardware_interface/include/hardware_interface/internal/hardware_resource_manager.h index 55e61c014..35ca15801 100644 --- a/hardware_interface/include/hardware_interface/internal/hardware_resource_manager.h +++ b/hardware_interface/include/hardware_interface/internal/hardware_resource_manager.h @@ -79,6 +79,11 @@ template class HardwareResourceManager : public HardwareInterface, public ResourceManager { public: + // save template class types for reference + typedef ResourceHandle handle_type; + typedef ClaimPolicy claim_policy; + typedef HardwareResourceManager hw_resource_manager; + /** \name Non Real-Time Safe Functions *\{*/ @@ -108,6 +113,24 @@ class HardwareResourceManager : public HardwareInterface, public ResourceManager } } + /** + * \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 hardware interfaces 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, + hw_resource_manager* result) + { + for(typename std::vector::iterator it = managers.begin(); + it != managers.end(); ++it) { + std::vector handles = (*it)->getHandles(); + result->registerHandles(handles); + } + } + /*\}*/ }; diff --git a/hardware_interface/include/hardware_interface/internal/interface_manager.h b/hardware_interface/include/hardware_interface/internal/interface_manager.h index 9e17c8cf1..e11974ae5 100644 --- a/hardware_interface/include/hardware_interface/internal/interface_manager.h +++ b/hardware_interface/include/hardware_interface/internal/interface_manager.h @@ -32,6 +32,7 @@ #define HARDWARE_INTERFACE_INTERFACE_MANAGER_H #include +#include #include #include @@ -41,6 +42,44 @@ namespace hardware_interface { +// SFINAE workaround, so that we have reflection inside the template functions +template +struct check_is_hw_resource_manager { + // variable definitions for compiler-time logic + typedef struct {} yes; + typedef yes no[2]; + + // method called if C is a HardwareResourceManager + template + static yes& testHWRM(typename C::hw_resource_manager*); + + // method called if C is not a HardwareResourceManager + template + static no& testHWRM(...); + + // check_is_hw_resource_manager::value == true when T is a HardwareResourceManager + static const bool value = (sizeof(testHWRM(0)) == sizeof(yes)); + + // method called if C is a HardwareResourceManager + template + static yes& callCM(typename std::vector& managers, C* result, typename C::hw_resource_manager*) + { + 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 HardwareResourceManager + template + static no& callCM(typename std::vector& managers, C* result, ...) {} + + // calls HardwareResourceManager::concatManagers if C is a HardwareResourceManager + static const void callConcatManagers(typename std::vector& managers, T* result) + { callCM(managers, result, 0); } +}; + class InterfaceManager { public: @@ -77,40 +116,80 @@ class InterfaceManager template T* get() { - void* iface_data = findInterfaceData(internal::demangledTypeName()); - if(!iface_data) - return NULL; + std::string type_name = internal::demangledTypeName(); + std::vector iface_data = findInterfaceData(type_name); + std::vector iface_list; + for(std::vector::iterator it = iface_data.begin(); it != iface_data.end(); ++it) { + T* iface = static_cast(*it); + if (!iface) + { + ROS_ERROR_STREAM("Failed reconstructing type T = '" << type_name.c_str() << + "'. This should never happen"); + return NULL; + } + iface_list.push_back(iface); + } - T* iface = static_cast(iface_data); - if (!iface) - { - ROS_ERROR_STREAM("Failed reconstructing type T = '" << internal::demangledTypeName().c_str() << - "'. This should never happen"); + if(iface_data.size() == 0) return NULL; + + if(iface_data.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()) { + // there exists a combined interface + iface_combo = static_cast(it_combo->second); + } else { + // no existing combined interface + if(check_is_hw_resource_manager::value) { + // it is a HardwareResourceManager + + // create a new combined interface + iface_combo = new T; + // concat all of the hardware resource managers together + check_is_hw_resource_manager::callConcatManagers(iface_list, iface_combo); + // save the combined interface for if this is called again + interfaces_combo_[type_name] = iface_combo; + } else { + // it is not a HardwareResourceManager + ROS_ERROR("You cannot register multiple interfaces of the same type which are " + "not of type HardwareResourceManager. There is no established protocol " + "for combining them."); + iface_combo = NULL; + } } - return iface; + return iface_combo; } /** - * \brief Get generic pointer to interface with type_name. + * \brief Get generic pointers to interfaces with type_name. * * This is used as a polymorphic lookup for the templated * get() call, which can't be virtual. + * By default, this method returns a list with the only element the interface + * found in the internal variable interfaces_. + * Derived classes may return more interfaces. * - * \param type_name The name of the interface type stored. - * \return Generic pointer to the interface. + * \param type_name The name of the interface types stored. + * \return List of generic pointers to the interfaces found. */ - virtual void* findInterfaceData(std::string type_name) + virtual std::vector findInterfaceData(std::string type_name) { + std::vector iface_data; InterfaceMap::iterator it = interfaces_.find(type_name); - if (it == interfaces_.end()) - return NULL; - return it->second; + if (it != interfaces_.end()) + iface_data.push_back(it->second); + return iface_data; } protected: typedef std::map InterfaceMap; InterfaceMap interfaces_; + InterfaceMap interfaces_combo_; }; } // 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..a09b8fccd 100644 --- a/hardware_interface/include/hardware_interface/internal/resource_manager.h +++ b/hardware_interface/include/hardware_interface/internal/resource_manager.h @@ -113,6 +113,31 @@ class ResourceManager return it->second; } + /** + * \brief Get a list of all resource handles. + * \return Vector of all resource handles. + */ + std::vector getHandles() + { + std::vector ret; + for(typename ResourceMap::iterator it = resource_map_.begin(); it != resource_map_.end(); ++it) + ret.push_back(&(it->second)); + return ret; + } + + /** + * \brief Register a list of new resources. + * If the resource name already exists, the previously stored resource value will be replaced with \e val. + * \param handles Resource values. Each type should implement a std::string getName() method. + */ + void registerHandles(const std::vector& handles) + { + for(typename std::vector::const_iterator it = handles.begin(); + it != handles.end(); ++it) { + registerHandle(*(*it)); + } + } + /*\}*/ protected: diff --git a/hardware_interface/include/hardware_interface/robot_hw_group.h b/hardware_interface/include/hardware_interface/robot_hw_group.h index c286343dd..bb35d8b67 100644 --- a/hardware_interface/include/hardware_interface/robot_hw_group.h +++ b/hardware_interface/include/hardware_interface/robot_hw_group.h @@ -59,29 +59,30 @@ class RobotHWGroup : public RobotHW } /** - * \brief Get generic pointer to interface with type_name. + * \brief Get generic pointers to interfaces with type_name. * - * This overloads the base implementation to look across both - * local interfaces and interfaces in all registered RobotHW. - * This is used by the get() call in the base class. - * - * \param type_name The name of the interface type stored. - * \return Generic pointer to the interface. + * Returns a list of all interfaces registered internally or + * registered in one of the registered hardwares. + * \param type_name The name of the interface types stored. + * \return List of generic pointers to the interfaces found. */ - virtual void* findInterfaceData(std::string type_name) + virtual std::vector findInterfaceData(std::string type_name) { + std::vector iface_data; + // look for interfaces registered here - void* iface_data = InterfaceManager::findInterfaceData(type_name); - if(iface_data != NULL) - return iface_data; + std::vector iface_data_cur; + iface_data_cur = InterfaceManager::findInterfaceData(type_name); + if(iface_data_cur.size() > 0) + iface_data.insert(iface_data.end(), iface_data_cur.begin(), iface_data_cur.end()); // look for interfaces registered in the registered hardware for(HardwareVector::iterator it = hardware_.begin(); it != hardware_.end(); ++it) { - iface_data = (*it)->findInterfaceData(type_name); - if(iface_data != NULL) - return iface_data; + iface_data_cur = (*it)->findInterfaceData(type_name); + if(iface_data_cur.size() > 0) + iface_data.insert(iface_data.end(), iface_data_cur.begin(), iface_data_cur.end()); } - return NULL; + return iface_data; } protected: diff --git a/hardware_interface/test/robot_hw_group_test.cpp b/hardware_interface/test/robot_hw_group_test.cpp index 1aaa88895..2dcca7213 100644 --- a/hardware_interface/test/robot_hw_group_test.cpp +++ b/hardware_interface/test/robot_hw_group_test.cpp @@ -63,7 +63,7 @@ class RobotHWGroupTest : public ::testing::Test JointHandle hc1, hc2; }; -TEST_F(RobotHWGroupTest, GroupTest) +TEST_F(RobotHWGroupTest, CombineDifferentInterfaces) { // Populate hardware interfaces JointStateInterface state_iface; @@ -95,6 +95,68 @@ TEST_F(RobotHWGroupTest, GroupTest) EXPECT_FALSE(hw_grp.get()); } +TEST_F(RobotHWGroupTest, 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; + RobotHWGroup hw_grp; + hw1.registerInterface(&state_iface1); + hw1.registerInterface(&eff_cmd_iface1); + hw2.registerInterface(&state_iface2); + hw2.registerInterface(&eff_cmd_iface2); + + hw_grp.registerHardware(&hw1); + hw_grp.registerHardware(&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(RobotHWGroupTest, InterfaceRegistration) { // Populate hardware interfaces From d6b9daa2d666a460f179d08dc44c416453c92810 Mon Sep 17 00:00:00 2001 From: Kelsey Date: Fri, 17 Jan 2014 17:38:18 -0500 Subject: [PATCH 06/16] Removed getHandles/registerHandles and reimplemented in concatManagers: Adolfo: The new getHandles() and registerHandles(...) methods of ResourceManager are not strictly needed. You could get away with getNames() and a loop around getHandle(name) with no public API additions. --- .../internal/hardware_resource_manager.h | 11 +++++--- .../internal/resource_manager.h | 25 ------------------- 2 files changed, 7 insertions(+), 29 deletions(-) diff --git a/hardware_interface/include/hardware_interface/internal/hardware_resource_manager.h b/hardware_interface/include/hardware_interface/internal/hardware_resource_manager.h index 35ca15801..b17175907 100644 --- a/hardware_interface/include/hardware_interface/internal/hardware_resource_manager.h +++ b/hardware_interface/include/hardware_interface/internal/hardware_resource_manager.h @@ -124,10 +124,13 @@ class HardwareResourceManager : public HardwareInterface, public ResourceManager static void concatManagers(std::vector& managers, hw_resource_manager* result) { - for(typename std::vector::iterator it = managers.begin(); - it != managers.end(); ++it) { - std::vector handles = (*it)->getHandles(); - result->registerHandles(handles); + 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)); + } } } diff --git a/hardware_interface/include/hardware_interface/internal/resource_manager.h b/hardware_interface/include/hardware_interface/internal/resource_manager.h index a09b8fccd..78585cbe8 100644 --- a/hardware_interface/include/hardware_interface/internal/resource_manager.h +++ b/hardware_interface/include/hardware_interface/internal/resource_manager.h @@ -113,31 +113,6 @@ class ResourceManager return it->second; } - /** - * \brief Get a list of all resource handles. - * \return Vector of all resource handles. - */ - std::vector getHandles() - { - std::vector ret; - for(typename ResourceMap::iterator it = resource_map_.begin(); it != resource_map_.end(); ++it) - ret.push_back(&(it->second)); - return ret; - } - - /** - * \brief Register a list of new resources. - * If the resource name already exists, the previously stored resource value will be replaced with \e val. - * \param handles Resource values. Each type should implement a std::string getName() method. - */ - void registerHandles(const std::vector& handles) - { - for(typename std::vector::const_iterator it = handles.begin(); - it != handles.end(); ++it) { - registerHandle(*(*it)); - } - } - /*\}*/ protected: From 1d835cd3f31a7c3a16053f82b079b49c01afc7c5 Mon Sep 17 00:00:00 2001 From: Kelsey Date: Fri, 17 Jan 2014 19:32:48 -0500 Subject: [PATCH 07/16] Moved concatManagers to base class ResourceManager, updated InterfaceManager to this effect. --- .../internal/hardware_resource_manager.h | 26 ++------------ .../internal/interface_manager.h | 34 +++++++++---------- .../internal/resource_manager.h | 24 +++++++++++++ 3 files changed, 43 insertions(+), 41 deletions(-) diff --git a/hardware_interface/include/hardware_interface/internal/hardware_resource_manager.h b/hardware_interface/include/hardware_interface/internal/hardware_resource_manager.h index b17175907..c643a8db1 100644 --- a/hardware_interface/include/hardware_interface/internal/hardware_resource_manager.h +++ b/hardware_interface/include/hardware_interface/internal/hardware_resource_manager.h @@ -80,9 +80,8 @@ class HardwareResourceManager : public HardwareInterface, public ResourceManager { public: // save template class types for reference - typedef ResourceHandle handle_type; - typedef ClaimPolicy claim_policy; - typedef HardwareResourceManager hw_resource_manager; + typedef ClaimPolicy claim_policy_type; + typedef HardwareResourceManager hw_resource_manager_type; /** \name Non Real-Time Safe Functions *\{*/ @@ -113,27 +112,6 @@ class HardwareResourceManager : public HardwareInterface, public ResourceManager } } - /** - * \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 hardware interfaces 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, - hw_resource_manager* 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)); - } - } - } - /*\}*/ }; diff --git a/hardware_interface/include/hardware_interface/internal/interface_manager.h b/hardware_interface/include/hardware_interface/internal/interface_manager.h index e11974ae5..8fcef8738 100644 --- a/hardware_interface/include/hardware_interface/internal/interface_manager.h +++ b/hardware_interface/include/hardware_interface/internal/interface_manager.h @@ -44,30 +44,30 @@ namespace hardware_interface // SFINAE workaround, so that we have reflection inside the template functions template -struct check_is_hw_resource_manager { +struct CheckIsResourceManager { // variable definitions for compiler-time logic typedef struct {} yes; typedef yes no[2]; - // method called if C is a HardwareResourceManager + // method called if C is a ResourceManager template - static yes& testHWRM(typename C::hw_resource_manager*); + static yes& testHWRM(typename C::resource_manager_type*); - // method called if C is not a HardwareResourceManager + // method called if C is not a ResourceManager template static no& testHWRM(...); - // check_is_hw_resource_manager::value == true when T is a HardwareResourceManager + // CheckIsResourceManager::value == true when T is a ResourceManager static const bool value = (sizeof(testHWRM(0)) == sizeof(yes)); - // method called if C is a HardwareResourceManager + // method called if C is a ResourceManager template - static yes& callCM(typename std::vector& managers, C* result, typename C::hw_resource_manager*) + static yes& callCM(typename std::vector& managers, C* result, typename C::resource_manager_type*) { - std::vector managers_in; + 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)); + managers_in.push_back(static_cast(*it)); C::concatManagers(managers_in, result); } @@ -75,7 +75,7 @@ struct check_is_hw_resource_manager { template static no& callCM(typename std::vector& managers, C* result, ...) {} - // calls HardwareResourceManager::concatManagers if C is a HardwareResourceManager + // calls ResourceManager::concatManagers if C is a ResourceManager static const void callConcatManagers(typename std::vector& managers, T* result) { callCM(managers, result, 0); } }; @@ -145,19 +145,19 @@ class InterfaceManager iface_combo = static_cast(it_combo->second); } else { // no existing combined interface - if(check_is_hw_resource_manager::value) { - // it is a HardwareResourceManager + if(CheckIsResourceManager::value) { + // it is a ResourceManager - // create a new combined interface + // create a new combined interface (FIXME: never deallocated) iface_combo = new T; - // concat all of the hardware resource managers together - check_is_hw_resource_manager::callConcatManagers(iface_list, 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; } else { - // it is not a HardwareResourceManager + // it is not a ResourceManager ROS_ERROR("You cannot register multiple interfaces of the same type which are " - "not of type HardwareResourceManager. There is no established protocol " + "not of type ResourceManager. There is no established protocol " "for combining them."); iface_combo = NULL; } diff --git a/hardware_interface/include/hardware_interface/internal/resource_manager.h b/hardware_interface/include/hardware_interface/internal/resource_manager.h index 78585cbe8..a7de83e8d 100644 --- a/hardware_interface/include/hardware_interface/internal/resource_manager.h +++ b/hardware_interface/include/hardware_interface/internal/resource_manager.h @@ -58,6 +58,9 @@ template class ResourceManager { public: + typedef ResourceHandle resource_handle_type; + typedef ResourceManager resource_manager_type; + /** \name Non Real-Time Safe Functions *\{*/ @@ -113,6 +116,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: From 05bb7fb7aa1243807faae6c3e836562e66cecd29 Mon Sep 17 00:00:00 2001 From: Kelsey Date: Fri, 17 Jan 2014 20:32:16 -0500 Subject: [PATCH 08/16] Removed implementation of RobotHWGroup and moved its implementation to base class InterfaceManager. --- hardware_interface/CMakeLists.txt | 5 - .../internal/interface_manager.h | 54 ++-- .../hardware_interface/robot_hw_group.h | 97 ------- .../test/robot_hw_group_test.cpp | 274 ------------------ hardware_interface/test/robot_hw_test.cpp | 94 ++++++ 5 files changed, 119 insertions(+), 405 deletions(-) delete mode 100644 hardware_interface/include/hardware_interface/robot_hw_group.h delete mode 100644 hardware_interface/test/robot_hw_group_test.cpp diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 23aec3757..fa001a1a4 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -17,7 +17,6 @@ if(USE_ROSBUILD) rosbuild_add_gtest(force_torque_sensor_interface_test test/force_torque_sensor_interface_test.cpp) rosbuild_add_gtest(imu_sensor_interface_test test/imu_sensor_interface_test.cpp) rosbuild_add_gtest(robot_hw_test test/robot_hw_test.cpp) - rosbuild_add_gtest(robot_hw_group_test test/robot_hw_group_test.cpp) rosbuild_add_gtest(interface_manager_test test/interface_manager_test.cpp) # TODO: why is it explicitly needed???, without it the linker fails. @@ -29,7 +28,6 @@ if(USE_ROSBUILD) target_link_libraries(force_torque_sensor_interface_test pthread) target_link_libraries(imu_sensor_interface_test pthread) target_link_libraries(robot_hw_test pthread) - target_link_libraries(robot_hw_group_test pthread) else() @@ -68,9 +66,6 @@ else() catkin_add_gtest(robot_hw_test test/robot_hw_test.cpp) target_link_libraries(robot_hw_test ${catkin_LIBRARIES}) - catkin_add_gtest(robot_hw_group_test test/robot_hw_group_test.cpp) - target_link_libraries(robot_hw_group_test ${catkin_LIBRARIES}) - catkin_add_gtest(interface_manager_test test/interface_manager_test.cpp) target_link_libraries(interface_manager_test ${catkin_LIBRARIES}) endif() diff --git a/hardware_interface/include/hardware_interface/internal/interface_manager.h b/hardware_interface/include/hardware_interface/internal/interface_manager.h index 8fcef8738..573786860 100644 --- a/hardware_interface/include/hardware_interface/internal/interface_manager.h +++ b/hardware_interface/include/hardware_interface/internal/interface_manager.h @@ -26,7 +26,7 @@ // 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 @@ -103,6 +103,11 @@ class InterfaceManager interfaces_[internal::demangledTypeName()] = iface; } + void registerInterfaceManager(InterfaceManager* iface_man) + { + interface_managers_.push_back(iface_man); + } + /** * \brief Get an interface. * @@ -117,12 +122,13 @@ class InterfaceManager T* get() { std::string type_name = internal::demangledTypeName(); - std::vector iface_data = findInterfaceData(type_name); std::vector iface_list; - for(std::vector::iterator it = iface_data.begin(); it != iface_data.end(); ++it) { - T* iface = static_cast(*it); - if (!iface) - { + + // 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; @@ -130,10 +136,18 @@ class InterfaceManager iface_list.push_back(iface); } - if(iface_data.size() == 0) + // 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_data.size() == 1) + if(iface_list.size() == 1) return iface_list.front(); // if we're here, we have multiple interfaces, and thus we must construct a new @@ -165,31 +179,13 @@ class InterfaceManager return iface_combo; } - /** - * \brief Get generic pointers to interfaces with type_name. - * - * This is used as a polymorphic lookup for the templated - * get() call, which can't be virtual. - * By default, this method returns a list with the only element the interface - * found in the internal variable interfaces_. - * Derived classes may return more interfaces. - * - * \param type_name The name of the interface types stored. - * \return List of generic pointers to the interfaces found. - */ - virtual std::vector findInterfaceData(std::string type_name) - { - std::vector iface_data; - InterfaceMap::iterator it = interfaces_.find(type_name); - if (it != interfaces_.end()) - iface_data.push_back(it->second); - return iface_data; - } - protected: typedef std::map InterfaceMap; + typedef std::vector InterfaceManagerVector; + InterfaceMap interfaces_; InterfaceMap interfaces_combo_; + InterfaceManagerVector interface_managers_; }; } // namespace diff --git a/hardware_interface/include/hardware_interface/robot_hw_group.h b/hardware_interface/include/hardware_interface/robot_hw_group.h deleted file mode 100644 index bb35d8b67..000000000 --- a/hardware_interface/include/hardware_interface/robot_hw_group.h +++ /dev/null @@ -1,97 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// * Redistributions of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// * Neither the name of hiDOF, Inc. nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -////////////////////////////////////////////////////////////////////////////// - -/* - * Author: Kelsey Hawkins - */ - -#ifndef HARDWARE_INTERFACE_ROBOT_HW_GROUP_H -#define HARDWARE_INTERFACE_ROBOT_HW_GROUP_H - -#include -#include -#include - -namespace hardware_interface -{ - -/** \brief Robot Hardware Collection Manager - * - * This class maintains a group of RobotHW, along with its - * own registered interfaces, and exposes an interface - * identical to RobotHW. - * - */ -class RobotHWGroup : public RobotHW -{ -public: - RobotHWGroup() - { - - } - - void registerHardware(RobotHW* hw) - { - hardware_.push_back(hw); - } - - /** - * \brief Get generic pointers to interfaces with type_name. - * - * Returns a list of all interfaces registered internally or - * registered in one of the registered hardwares. - * \param type_name The name of the interface types stored. - * \return List of generic pointers to the interfaces found. - */ - virtual std::vector findInterfaceData(std::string type_name) - { - std::vector iface_data; - - // look for interfaces registered here - std::vector iface_data_cur; - iface_data_cur = InterfaceManager::findInterfaceData(type_name); - if(iface_data_cur.size() > 0) - iface_data.insert(iface_data.end(), iface_data_cur.begin(), iface_data_cur.end()); - - // look for interfaces registered in the registered hardware - for(HardwareVector::iterator it = hardware_.begin(); it != hardware_.end(); ++it) { - iface_data_cur = (*it)->findInterfaceData(type_name); - if(iface_data_cur.size() > 0) - iface_data.insert(iface_data.end(), iface_data_cur.begin(), iface_data_cur.end()); - } - return iface_data; - } - -protected: - typedef std::vector HardwareVector; - HardwareVector hardware_; - -}; - -} - -#endif - diff --git a/hardware_interface/test/robot_hw_group_test.cpp b/hardware_interface/test/robot_hw_group_test.cpp deleted file mode 100644 index 2dcca7213..000000000 --- a/hardware_interface/test/robot_hw_group_test.cpp +++ /dev/null @@ -1,274 +0,0 @@ -/////////////////////////////////////////////////////////////////////////////// -// Copyright (C) 2013, PAL Robotics S.L. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// * Redistributions of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// * Neither the name of hiDOF, Inc. nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -////////////////////////////////////////////////////////////////////////////// - -/// \author Adolfo Rodriguez Tsouroukdissian -/// \author Kelsey Hawkins - -#include -#include -#include -#include -#include -#include -#include - -using std::list; -using std::string; -using namespace hardware_interface; - -class RobotHWGroupTest : public ::testing::Test -{ -public: - RobotHWGroupTest() - : pos1(1.0), vel1(2.0), eff1(3.0), cmd1(0.0), - pos2(4.0), vel2(5.0), eff2(6.0), cmd2(0.0), - name1("name_1"), - name2("name_2"), - hs1(name1, &pos1, &vel1, &eff1), - hs2(name2, &pos2, &vel2, &eff2), - hc1(hs1, &cmd1), - hc2(hs2, &cmd2) - {} - -protected: - double pos1, vel1, eff1, cmd1; - double pos2, vel2, eff2, cmd2; - string name1; - string name2; - JointStateHandle hs1, hs2; - JointHandle hc1, hc2; -}; - -TEST_F(RobotHWGroupTest, 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; - RobotHWGroup hw_grp; - hw1.registerInterface(&state_iface); - hw2.registerInterface(&eff_cmd_iface); - hw_grp.registerInterface(&pos_cmd_iface); - - hw_grp.registerHardware(&hw1); - hw_grp.registerHardware(&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(RobotHWGroupTest, 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; - RobotHWGroup hw_grp; - hw1.registerInterface(&state_iface1); - hw1.registerInterface(&eff_cmd_iface1); - hw2.registerInterface(&state_iface2); - hw2.registerInterface(&eff_cmd_iface2); - - hw_grp.registerHardware(&hw1); - hw_grp.registerHardware(&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(RobotHWGroupTest, InterfaceRegistration) -{ - // 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 a RobotHW instance - RobotHW hw; - hw.registerInterface(&state_iface); - hw.registerInterface(&eff_cmd_iface); - hw.registerInterface(&pos_cmd_iface); - - RobotHWGroup hw_grp; - hw_grp.registerHardware(&hw); - - // 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(RobotHWGroupTest, InterfaceRewriting) -{ - // Two hardware interfaces of the same type, with different joints - JointStateInterface state1_iface; - state1_iface.registerHandle(hs1); - - JointStateInterface state2_iface; - state2_iface.registerHandle(hs1); - state2_iface.registerHandle(hs2); - - // Register first interface and validate it - RobotHW hw; - hw.registerInterface(&state1_iface); - - RobotHWGroup hw_grp; - hw_grp.registerHardware(&hw); - - JointStateInterface* state_iface_ptr = hw_grp.get(); - EXPECT_EQ(1, state_iface_ptr->getNames().size()); - - // Register second interface and verify that it has taken the place of the previously inserted one - hw_grp.registerInterface(&state2_iface); - state_iface_ptr = hw_grp.get(); - EXPECT_EQ(2, state_iface_ptr->getNames().size()); -} - -TEST_F(RobotHWGroupTest, ConflictChecking) -{ - ControllerInfo info1; - info1.name = "controller_1"; - info1.type = "type_1"; - info1.hardware_interface = "interface_1"; - info1.resources.insert("resource_1"); - - ControllerInfo info2; - info2.name = "controller_2"; - info2.type = "type_2"; - info2.hardware_interface = "interface_2"; - info2.resources.insert("resource_2"); - - ControllerInfo info12; - info12.name = "controller_12"; - info12.type = "type_12"; - info12.hardware_interface = "interface_12"; - info12.resources.insert("resource_1"); - info12.resources.insert("resource_2"); - - // No conflict - { - list info_list; - info_list.push_back(info1); - info_list.push_back(info2); - - RobotHWGroup hw_grp; - EXPECT_FALSE(hw_grp.checkForConflict(info_list)); - } - - // Conflict - { - list info_list; - info_list.push_back(info1); - info_list.push_back(info12); - - RobotHWGroup hw_grp; - EXPECT_TRUE(hw_grp.checkForConflict(info_list)); - } - - // Conflict - { - list info_list; - info_list.push_back(info2); - info_list.push_back(info12); - - RobotHWGroup hw_grp; - EXPECT_TRUE(hw_grp.checkForConflict(info_list)); - } -} - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/hardware_interface/test/robot_hw_test.cpp b/hardware_interface/test/robot_hw_test.cpp index 170dd4345..f4c8bb7cd 100644 --- a/hardware_interface/test/robot_hw_test.cpp +++ b/hardware_interface/test/robot_hw_test.cpp @@ -164,6 +164,100 @@ 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); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); From c0f9e0847c1ccedba4803f16c9d4ecc3a5056b19 Mon Sep 17 00:00:00 2001 From: Kelsey Date: Fri, 17 Jan 2014 20:52:47 -0500 Subject: [PATCH 09/16] InterfaceManager keeps track of how many interfaces are combined in an original get() call so that if new interfaces are registered in the meantime, you will make new combined interfaces for the new get calls to use. Thus, every get call will only have a ResourceManager with handles from only those interfaces registered up to that call. --- .../hardware_interface/internal/interface_manager.h | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/hardware_interface/include/hardware_interface/internal/interface_manager.h b/hardware_interface/include/hardware_interface/internal/interface_manager.h index 573786860..e8b12f844 100644 --- a/hardware_interface/include/hardware_interface/internal/interface_manager.h +++ b/hardware_interface/include/hardware_interface/internal/interface_manager.h @@ -154,8 +154,11 @@ class InterfaceManager // 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()) { - // there exists a combined interface + 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 @@ -168,6 +171,7 @@ class InterfaceManager 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 " @@ -182,10 +186,12 @@ 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_; }; } // namespace From 53b7a97241fba436bb0ac4a84d342704c54d92fa Mon Sep 17 00:00:00 2001 From: Kelsey Date: Fri, 17 Jan 2014 21:11:04 -0500 Subject: [PATCH 10/16] Added incremental InterfaceManager:get() call test, as per Adolfo's suggestion. --- hardware_interface/test/robot_hw_test.cpp | 35 +++++++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/hardware_interface/test/robot_hw_test.cpp b/hardware_interface/test/robot_hw_test.cpp index f4c8bb7cd..03b35f2b3 100644 --- a/hardware_interface/test/robot_hw_test.cpp +++ b/hardware_interface/test/robot_hw_test.cpp @@ -258,6 +258,41 @@ TEST_F(RobotHWTest, CombineSameInterfaces) 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); From 4ce12bd72f3a2e014072e779d0b9749b978758bf Mon Sep 17 00:00:00 2001 From: Kelsey Date: Sun, 19 Jan 2014 19:27:36 -0500 Subject: [PATCH 11/16] Adding multi_controller.h, an extension of controller.h which allows you to use 2 templated interfaces, instead of only 1. --- .../controller_interface/multi_controller.h | 151 ++++++++++++++++++ 1 file changed, 151 insertions(+) create mode 100644 controller_interface/include/controller_interface/multi_controller.h diff --git a/controller_interface/include/controller_interface/multi_controller.h b/controller_interface/include/controller_interface/multi_controller.h new file mode 100644 index 000000000..31370fc83 --- /dev/null +++ b/controller_interface/include/controller_interface/multi_controller.h @@ -0,0 +1,151 @@ +/////////////////////////////////////////////////////////////////////////////// +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of hiDOF, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/* + * Author: Kelsey Hawkins + */ + +#ifndef CONTROLLER_INTERFACE_MULTI_CONTROLLER_H +#define CONTROLLER_INTERFACE_MULTI_CONTROLLER_H + +#include +#include +#include +#include +#include + + +namespace controller_interface +{ + +/** \brief %Controller with a specific hardware interface + * + * \tparam T The hardware interface type used by this controller. This enforces + * semantic compatibility between the controller and the hardware it's meant to + * control. + */ +template +class Controller: public ControllerBase +{ +public: + Controller() {state_ = CONSTRUCTED;} + virtual ~Controller(){} + + /** \brief The init function is called to initialize the controller from a + * non-realtime thread with a pointer to the hardware interface, itself, + * instead of a pointer to a RobotHW. + * + * \param hw The specific hardware interface used by this controller. + * + * \param controller_nh A NodeHandle in the namespace from which the controller + * should read its configuration, and where it should set up its ROS + * interface. + * + * \returns True if initialization was successful and the controller + * is ready to be started. + */ + virtual bool init(T1* hw1, T2* hw2, ros::NodeHandle &controller_nh) {return true;}; + + /** \brief The init function is called to initialize the controller from a + * non-realtime thread with a pointer to the hardware interface, itself, + * instead of a pointer to a RobotHW. + * + * \param hw The specific hardware interface used by this controller. + * + * \param root_nh A NodeHandle in the root of the controller manager namespace. + * This is where the ROS interfaces are setup (publishers, subscribers, services). + * + * \param controller_nh A NodeHandle in the namespace of the controller. + * This is where the controller-specific configuration resides. + * + * \returns True if initialization was successful and the controller + * is ready to be started. + */ + virtual bool init(T1* hw1, T2* hw2, ros::NodeHandle& root_nh, ros::NodeHandle &controller_nh) {return true;}; + + +protected: + /** \brief Initialize the controller from a RobotHW pointer + * + * This calls \ref init with the hardware interface for this controller if it + * can extract the correct interface from \c robot_hw. + * + */ + virtual bool initRequest(hardware_interface::RobotHW* robot_hw, + ros::NodeHandle& root_nh, ros::NodeHandle &controller_nh, + std::set &claimed_resources) + { + // check if construction finished cleanly + if (state_ != CONSTRUCTED){ + ROS_ERROR("Cannot initialize this controller because it failed to be constructed"); + return false; + } + + // get a pointer to the hardware interface + T1* hw1 = robot_hw->get(); + T2* hw2 = robot_hw->get(); + if (!hw1 || !hw2) + { + ROS_ERROR("This controller requires a hardware interface of type '%s'." + " Make sure this is registered in the hardware_interface::RobotHW class.", + getHardwareInterfaceType().c_str()); + return false; + } + + // return which resources are claimed by this controller + hw1->clearClaims(); + hw2->clearClaims(); + if (!init(hw1, hw2, controller_nh) || !init(hw1, hw2, root_nh, controller_nh)) + { + ROS_ERROR("Failed to initialize the controller"); + return false; + } + claimed_resources = hw1->getClaims(); + std::set claimed_resources2 = hw2->getClaims(); + claimed_resources.insert(claimed_resources2.begin(), claimed_resources2.end()); + hw1->clearClaims(); + hw2->clearClaims(); + + // success + state_ = INITIALIZED; + return true; + } + + virtual std::string getHardwareInterfaceType() const + { + return "<" + hardware_interface::internal::demangledTypeName() + "," + + hardware_interface::internal::demangledTypeName() + ">"; + } + +private: + Controller(const Controller &c); + Controller& operator =(const Controller &c); + +}; + +} + +#endif From bcaad43e5d11bcb254c9055c79f40922e82627fa Mon Sep 17 00:00:00 2001 From: Kelsey Date: Sun, 19 Jan 2014 19:29:02 -0500 Subject: [PATCH 12/16] Adding new tests to test loading multiple different interfaces into a controller. (multi_controller.h) --- controller_manager_tests/CMakeLists.txt | 9 ++ .../controller_manager_tests/multi_robot_hw.h | 70 ++++++++++++++ .../controller_manager_tests/my_robot_hw.h | 10 +- .../vel_effort_test_controller.h | 65 +++++++++++++ .../src/multi_dummy_app.cpp | 70 ++++++++++++++ .../src/multi_robot_hw.cpp | 91 +++++++++++++++++++ controller_manager_tests/src/my_robot_hw.cpp | 6 +- .../src/vel_effort_test_controller.cpp | 85 +++++++++++++++++ .../test/multi_cm_test.cpp | 64 +++++++++++++ .../test/multi_cm_test.test | 9 ++ .../test_controllers_plugin.xml | 6 ++ 11 files changed, 481 insertions(+), 4 deletions(-) create mode 100644 controller_manager_tests/include/controller_manager_tests/multi_robot_hw.h create mode 100644 controller_manager_tests/include/controller_manager_tests/vel_effort_test_controller.h create mode 100644 controller_manager_tests/src/multi_dummy_app.cpp create mode 100644 controller_manager_tests/src/multi_robot_hw.cpp create mode 100644 controller_manager_tests/src/vel_effort_test_controller.cpp create mode 100644 controller_manager_tests/test/multi_cm_test.cpp create mode 100644 controller_manager_tests/test/multi_cm_test.test diff --git a/controller_manager_tests/CMakeLists.txt b/controller_manager_tests/CMakeLists.txt index 95c73ccdf..e643ed709 100644 --- a/controller_manager_tests/CMakeLists.txt +++ b/controller_manager_tests/CMakeLists.txt @@ -45,18 +45,27 @@ else() include/controller_manager_tests/my_robot_hw.h src/effort_test_controller.cpp include/controller_manager_tests/effort_test_controller.h + src/vel_effort_test_controller.cpp + include/controller_manager_tests/vel_effort_test_controller.h src/my_dummy_controller.cpp include/controller_manager_tests/my_dummy_controller.h ) add_executable(dummy_app src/dummy_app.cpp) target_link_libraries(dummy_app ${PROJECT_NAME} ${catkin_LIBRARIES}) + add_executable(multi_dummy_app src/multi_dummy_app.cpp) + target_link_libraries(multi_dummy_app ${PROJECT_NAME} ${catkin_LIBRARIES}) if(CATKIN_ENABLE_TESTING) add_executable(cm_test test/cm_test.cpp) add_dependencies(tests cm_test) target_link_libraries(cm_test ${GTEST_LIBRARIES} ${catkin_LIBRARIES}) add_rostest(test/cm_test.test) + + add_executable(multi_cm_test test/multi_cm_test.cpp) + add_dependencies(tests multi_cm_test) + target_link_libraries(multi_cm_test ${GTEST_LIBRARIES} ${catkin_LIBRARIES}) + add_rostest(test/multi_cm_test.test) endif() # Install diff --git a/controller_manager_tests/include/controller_manager_tests/multi_robot_hw.h b/controller_manager_tests/include/controller_manager_tests/multi_robot_hw.h new file mode 100644 index 000000000..8ce192536 --- /dev/null +++ b/controller_manager_tests/include/controller_manager_tests/multi_robot_hw.h @@ -0,0 +1,70 @@ +// Author: Kelsey Hawkins +// Based on code by: +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2012, hiDOF INC. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of hiDOF, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/* + * Author: Wim Meeussen, Kelsey Hawkins + */ + + +#ifndef CONTROLLER_MANAGER_TESTS_MULTI_ROBOT_HW_H +#define CONTROLLER_MANAGER_TESTS_MULTI_ROBOT_HW_H + +#include +#include + +namespace controller_manager_tests +{ + +// Used to test multiple robot hardware mixed together +class TypeARobotHW : public hardware_interface::RobotHW +{ +public: + TypeARobotHW(int id); + + void read(); + void write(); + +protected: + +private: + hardware_interface::JointStateInterface js_interface_; + hardware_interface::EffortJointInterface ej_interface_; + hardware_interface::VelocityJointInterface vj_interface_; + + std::vector joint_effort_command_; + std::vector joint_velocity_command_; + std::vector joint_position_; + std::vector joint_velocity_; + std::vector joint_effort_; + std::vector joint_name_; +}; +} + + +#endif diff --git a/controller_manager_tests/include/controller_manager_tests/my_robot_hw.h b/controller_manager_tests/include/controller_manager_tests/my_robot_hw.h index a939855c8..05042c487 100644 --- a/controller_manager_tests/include/controller_manager_tests/my_robot_hw.h +++ b/controller_manager_tests/include/controller_manager_tests/my_robot_hw.h @@ -42,7 +42,7 @@ namespace controller_manager_tests class MyRobotHW : public hardware_interface::RobotHW { public: - MyRobotHW(); + MyRobotHW(std::string joint_prefix = "hiDOF_"); void read(); void write(); @@ -61,6 +61,14 @@ class MyRobotHW : public hardware_interface::RobotHW std::vector joint_effort_; std::vector joint_name_; }; + +class DerivedMyRobotHW : public MyRobotHW +{ +public: + DerivedMyRobotHW(std::string joint_prefix = "hiDOF_") + : MyRobotHW(joint_prefix + "derived_") {} +}; + } diff --git a/controller_manager_tests/include/controller_manager_tests/vel_effort_test_controller.h b/controller_manager_tests/include/controller_manager_tests/vel_effort_test_controller.h new file mode 100644 index 000000000..6d673a326 --- /dev/null +++ b/controller_manager_tests/include/controller_manager_tests/vel_effort_test_controller.h @@ -0,0 +1,65 @@ +// Author: Kelsey Hawkins +// Based on code by: +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2012, hiDOF INC. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of hiDOF Inc nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +#ifndef CONTROLLER_MANAGER_TESTS_VEL_EFFORT_TEST_CONTROLLER_H +#define CONTROLLER_MANAGER_TESTS_VEL_EFFORT_TEST_CONTROLLER_H + + +#include +#include +#include + + +namespace controller_manager_tests +{ + + +class VelocityEffortTestController: + public controller_interface::Controller +{ +public: + VelocityEffortTestController(){} + + bool init(hardware_interface::VelocityJointInterface* hw_vel, + hardware_interface::EffortJointInterface* hw_eff, + ros::NodeHandle &n); + void starting(const ros::Time& time); + void update(const ros::Time& time, const ros::Duration& period); + void stopping(const ros::Time& time); + +private: + std::vector joint_velocity_commands_; + std::vector joint_effort_commands_; + +}; + +} + +#endif diff --git a/controller_manager_tests/src/multi_dummy_app.cpp b/controller_manager_tests/src/multi_dummy_app.cpp new file mode 100644 index 000000000..7fe6159df --- /dev/null +++ b/controller_manager_tests/src/multi_dummy_app.cpp @@ -0,0 +1,70 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2012, hiDOF INC. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of hiDOF Inc nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +#include +#include +#include + +using namespace controller_manager_tests; + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "MultiDummyApp"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + MyRobotHW hw_vel_base("vel_"); + MyRobotHW hw_eff_base("eff_"); + DerivedMyRobotHW hw_vel_derived("vel_"); + DerivedMyRobotHW hw_eff_derived("eff_"); + hardware_interface::RobotHW hw_combined; + hw_combined.registerInterfaceManager(&hw_vel_base); + hw_combined.registerInterfaceManager(&hw_eff_base); + hw_combined.registerInterfaceManager(&hw_vel_derived); + hw_combined.registerInterfaceManager(&hw_eff_derived); + + ros::NodeHandle nh; + controller_manager::ControllerManager cm(&hw_combined, nh); + + ros::Duration period(1.0); + while (ros::ok()) + { + ROS_INFO("loop"); + hw_vel_base.read(); + hw_eff_base.read(); + hw_vel_derived.read(); + hw_eff_derived.read(); + cm.update(ros::Time::now(), period); + hw_vel_base.write(); + hw_eff_base.write(); + hw_vel_derived.write(); + hw_eff_derived.write(); + period.sleep(); + } +} + diff --git a/controller_manager_tests/src/multi_robot_hw.cpp b/controller_manager_tests/src/multi_robot_hw.cpp new file mode 100644 index 000000000..42a7fa185 --- /dev/null +++ b/controller_manager_tests/src/multi_robot_hw.cpp @@ -0,0 +1,91 @@ +// Author: Kelsey Hawkins +// Based on code by: +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2012, hiDOF INC. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of hiDOF Inc nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/* + * Author: Wim Meeussen, Kelsey Hawkins + */ + + +#include + +namespace controller_manager_tests +{ + +TypeARobotHW::TypeARobotHW(int id) +{ + using namespace hardware_interface; + + // Initialize raw data + joint_position_.resize(2); + joint_velocity_.resize(2); + joint_effort_.resize(2); + joint_effort_command_.resize(2); + joint_velocity_command_.resize(2); + joint_name_.resize(2); + + joint_name_[0] = "hiDOF_joint1"; + joint_position_[0] = 1.0; + joint_velocity_[0] = 0.0; + joint_effort_[0] = 0.1; + joint_effort_command_[0] = 0.0; + joint_velocity_command_[0] = 0.0; + + joint_name_[1] = "hiDOF_joint2"; + joint_position_[1] = 1.0; + joint_velocity_[1] = 0.0; + joint_effort_[1] = 0.1; + joint_effort_command_[1] = 0.0; + joint_velocity_command_[1] = 0.0; + + // Populate hardware interfaces + js_interface_.registerHandle(JointStateHandle(joint_name_[0], &joint_position_[0], &joint_velocity_[0], &joint_effort_[0])); + js_interface_.registerHandle(JointStateHandle(joint_name_[1], &joint_position_[1], &joint_velocity_[1], &joint_effort_[1])); + + ej_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[0]), &joint_effort_command_[0])); + ej_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[1]), &joint_effort_command_[1])); + + vj_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[0]), &joint_velocity_command_[0])); + vj_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[1]), &joint_velocity_command_[1])); + + registerInterface(&js_interface_); + registerInterface(&ej_interface_); + registerInterface(&vj_interface_); +} + + +void TypeARobotHW::read() +{ + +} + +void TypeARobotHW::write() +{ +} + +} diff --git a/controller_manager_tests/src/my_robot_hw.cpp b/controller_manager_tests/src/my_robot_hw.cpp index 0a00b2ebe..fefe8ca67 100644 --- a/controller_manager_tests/src/my_robot_hw.cpp +++ b/controller_manager_tests/src/my_robot_hw.cpp @@ -35,7 +35,7 @@ namespace controller_manager_tests { -MyRobotHW::MyRobotHW() +MyRobotHW::MyRobotHW(std::string joint_prefix) { using namespace hardware_interface; @@ -47,14 +47,14 @@ MyRobotHW::MyRobotHW() joint_velocity_command_.resize(2); joint_name_.resize(2); - joint_name_[0] = "hiDOF_joint1"; + joint_name_[0] = joint_prefix + "joint1"; joint_position_[0] = 1.0; joint_velocity_[0] = 0.0; joint_effort_[0] = 0.1; joint_effort_command_[0] = 0.0; joint_velocity_command_[0] = 0.0; - joint_name_[1] = "hiDOF_joint2"; + joint_name_[1] = joint_prefix + "joint2"; joint_position_[1] = 1.0; joint_velocity_[1] = 0.0; joint_effort_[1] = 0.1; diff --git a/controller_manager_tests/src/vel_effort_test_controller.cpp b/controller_manager_tests/src/vel_effort_test_controller.cpp new file mode 100644 index 000000000..6b94b8c38 --- /dev/null +++ b/controller_manager_tests/src/vel_effort_test_controller.cpp @@ -0,0 +1,85 @@ +// Author: Kelsey Hawkins +// Based on code by: +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2012, hiDOF INC. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of hiDOF Inc nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + + +#include + +using namespace controller_manager_tests; + +bool VelocityEffortTestController::init(hardware_interface::VelocityJointInterface* hw_vel, + hardware_interface::EffortJointInterface* hw_eff, + ros::NodeHandle &n) +{ + // get all joint states from the hardware interface + // const std::vector& all_vel_joint_names = hw_vel->getNames(); + // for (unsigned i=0; i& all_eff_joint_names = hw_eff->getNames(); + // for (unsigned i=0; i velocity_joint_names; + velocity_joint_names.push_back("vel_joint1"); + velocity_joint_names.push_back("vel_joint2"); + velocity_joint_names.push_back("vel_derived_joint1"); + velocity_joint_names.push_back("vel_derived_joint2"); + + std::vector effort_joint_names; + effort_joint_names.push_back("eff_joint1"); + effort_joint_names.push_back("eff_joint2"); + effort_joint_names.push_back("eff_derived_joint1"); + effort_joint_names.push_back("eff_derived_joint2"); + + for (unsigned i=0; i<4; i++) { + joint_velocity_commands_.push_back(hw_vel->getHandle(velocity_joint_names[i])); + joint_effort_commands_.push_back(hw_eff->getHandle(effort_joint_names[i])); + } + + return true; +} + +void VelocityEffortTestController::starting(const ros::Time& time) +{ + ROS_INFO("Starting VelocityEffortTestController Controller"); +} + +void VelocityEffortTestController::update(const ros::Time& time, const ros::Duration& period) +{ + for (unsigned int i=0; i < joint_effort_commands_.size(); i++) + { + + } +} + +void VelocityEffortTestController::stopping(const ros::Time& time) +{ + ROS_INFO("Stopping VelocityEffortTestController Controller"); +} + +PLUGINLIB_EXPORT_CLASS( controller_manager_tests::VelocityEffortTestController, controller_interface::ControllerBase) diff --git a/controller_manager_tests/test/multi_cm_test.cpp b/controller_manager_tests/test/multi_cm_test.cpp new file mode 100644 index 000000000..bc1e73960 --- /dev/null +++ b/controller_manager_tests/test/multi_cm_test.cpp @@ -0,0 +1,64 @@ +// Author: Kelsey Hawkins +// Based on code by: +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2012, hiDOF INC. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of hiDOF Inc nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +//! /author Vijay Pradeep, Kelsey Hawkins + +#include +#include + +#include + +using namespace controller_manager_msgs; + +TEST(CMTests, spawnTestGood) +{ + ros::NodeHandle nh; + ros::ServiceClient client = nh.serviceClient("/controller_manager/load_controller"); + LoadController srv; + srv.request.name = "multi_controller"; + bool call_success = client.call(srv); + EXPECT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "ControllerManagerTestNode"); + + ros::AsyncSpinner spinner(1); + + // wait for services + ROS_INFO("Waiting for service"); + ros::service::waitForService("/controller_manager/load_controller"); + ROS_INFO("Start tests"); + spinner.start(); + + return RUN_ALL_TESTS(); +} diff --git a/controller_manager_tests/test/multi_cm_test.test b/controller_manager_tests/test/multi_cm_test.test new file mode 100644 index 000000000..84eaf8055 --- /dev/null +++ b/controller_manager_tests/test/multi_cm_test.test @@ -0,0 +1,9 @@ + + + multi_controller: + type: controller_manager_tests/VelocityEffortTestController + + + + + diff --git a/controller_manager_tests/test_controllers_plugin.xml b/controller_manager_tests/test_controllers_plugin.xml index 28bd9a0aa..5ae01b69e 100644 --- a/controller_manager_tests/test_controllers_plugin.xml +++ b/controller_manager_tests/test_controllers_plugin.xml @@ -5,6 +5,12 @@ + + + The velocity/effort test controller expects both a VelocityJointInterface and a EffortJointInterface type of hardware interface. + + + This is a stub controller that uses an interface that likely no-one will support From 613f30db74ed7f467cf16f790c80e96e9b1ac527 Mon Sep 17 00:00:00 2001 From: Kelsey Date: Sun, 19 Jan 2014 20:20:15 -0500 Subject: [PATCH 13/16] Added Controller2/3/4 for different numbers of interfaces. --- .../controller_interface/multi_controller.h | 288 +++++++++++++++++- .../vel_effort_test_controller.h | 4 +- 2 files changed, 281 insertions(+), 11 deletions(-) diff --git a/controller_interface/include/controller_interface/multi_controller.h b/controller_interface/include/controller_interface/multi_controller.h index 31370fc83..1d8f85cdf 100644 --- a/controller_interface/include/controller_interface/multi_controller.h +++ b/controller_interface/include/controller_interface/multi_controller.h @@ -31,7 +31,7 @@ #ifndef CONTROLLER_INTERFACE_MULTI_CONTROLLER_H #define CONTROLLER_INTERFACE_MULTI_CONTROLLER_H -#include +#include #include #include #include @@ -43,22 +43,27 @@ namespace controller_interface /** \brief %Controller with a specific hardware interface * - * \tparam T The hardware interface type used by this controller. This enforces + * \tparam T1 The hardware interface type used by this controller. This enforces + * semantic compatibility between the controller and the hardware it's meant to + * control. + * \tparam T2 The hardware interface type used by this controller. This enforces * semantic compatibility between the controller and the hardware it's meant to * control. */ template -class Controller: public ControllerBase +class Controller2: public ControllerBase { public: - Controller() {state_ = CONSTRUCTED;} - virtual ~Controller(){} + Controller2() {state_ = CONSTRUCTED;} + virtual ~Controller2(){} /** \brief The init function is called to initialize the controller from a * non-realtime thread with a pointer to the hardware interface, itself, * instead of a pointer to a RobotHW. * - * \param hw The specific hardware interface used by this controller. + * \param hw1 The specific hardware interface used by this controller. + * + * \param hw2 The specific hardware interface used by this controller. * * \param controller_nh A NodeHandle in the namespace from which the controller * should read its configuration, and where it should set up its ROS @@ -73,7 +78,9 @@ class Controller: public ControllerBase * non-realtime thread with a pointer to the hardware interface, itself, * instead of a pointer to a RobotHW. * - * \param hw The specific hardware interface used by this controller. + * \param hw1 The specific hardware interface used by this controller. + * + * \param hw2 The specific hardware interface used by this controller. * * \param root_nh A NodeHandle in the root of the controller manager namespace. * This is where the ROS interfaces are setup (publishers, subscribers, services). @@ -141,8 +148,271 @@ class Controller: public ControllerBase } private: - Controller(const Controller &c); - Controller& operator =(const Controller &c); + Controller2(const Controller2 &c); + Controller2& operator =(const Controller2 &c); + +}; + +/** \brief %Controller with a specific hardware interface + * + * \tparam T1 The hardware interface type used by this controller. This enforces + * semantic compatibility between the controller and the hardware it's meant to + * control. + * \tparam T2 The hardware interface type used by this controller. This enforces + * semantic compatibility between the controller and the hardware it's meant to + * control. + * \tparam T3 The hardware interface type used by this controller. This enforces + * semantic compatibility between the controller and the hardware it's meant to + * control. + */ +template +class Controller3: public ControllerBase +{ +public: + Controller3() {state_ = CONSTRUCTED;} + virtual ~Controller3(){} + + /** \brief The init function is called to initialize the controller from a + * non-realtime thread with a pointer to the hardware interface, itself, + * instead of a pointer to a RobotHW. + * + * \param hw1 The specific hardware interface used by this controller. + * + * \param hw2 The specific hardware interface used by this controller. + * + * \param hw3 The specific hardware interface used by this controller. + * + * \param controller_nh A NodeHandle in the namespace from which the controller + * should read its configuration, and where it should set up its ROS + * interface. + * + * \returns True if initialization was successful and the controller + * is ready to be started. + */ + virtual bool init(T1* hw1, T2* hw2, T3* hw3, ros::NodeHandle &controller_nh) {return true;}; + + /** \brief The init function is called to initialize the controller from a + * non-realtime thread with a pointer to the hardware interface, itself, + * instead of a pointer to a RobotHW. + * + * \param hw1 The specific hardware interface used by this controller. + * + * \param hw2 The specific hardware interface used by this controller. + * + * \param hw3 The specific hardware interface used by this controller. + * + * \param root_nh A NodeHandle in the root of the controller manager namespace. + * This is where the ROS interfaces are setup (publishers, subscribers, services). + * + * \param controller_nh A NodeHandle in the namespace of the controller. + * This is where the controller-specific configuration resides. + * + * \returns True if initialization was successful and the controller + * is ready to be started. + */ + virtual bool init(T1* hw1, T2* hw2, T3* hw3, ros::NodeHandle& root_nh, ros::NodeHandle &controller_nh) {return true;}; + + +protected: + /** \brief Initialize the controller from a RobotHW pointer + * + * This calls \ref init with the hardware interface for this controller if it + * can extract the correct interface from \c robot_hw. + * + */ + virtual bool initRequest(hardware_interface::RobotHW* robot_hw, + ros::NodeHandle& root_nh, ros::NodeHandle &controller_nh, + std::set &claimed_resources) + { + // check if construction finished cleanly + if (state_ != CONSTRUCTED){ + ROS_ERROR("Cannot initialize this controller because it failed to be constructed"); + return false; + } + + // get a pointer to the hardware interface + T1* hw1 = robot_hw->get(); + T2* hw2 = robot_hw->get(); + T3* hw3 = robot_hw->get(); + if (!hw1 || !hw2 || !hw3) + { + ROS_ERROR("This controller requires a hardware interface of type '%s'." + " Make sure this is registered in the hardware_interface::RobotHW class.", + getHardwareInterfaceType().c_str()); + return false; + } + + // return which resources are claimed by this controller + hw1->clearClaims(); + hw2->clearClaims(); + hw3->clearClaims(); + if (!init(hw1, hw2, hw3, controller_nh) || !init(hw1, hw2, hw3, root_nh, controller_nh)) + { + ROS_ERROR("Failed to initialize the controller"); + return false; + } + claimed_resources = hw1->getClaims(); + std::set claimed_resources2 = hw2->getClaims(); + std::set claimed_resources3 = hw3->getClaims(); + claimed_resources.insert(claimed_resources2.begin(), claimed_resources2.end()); + claimed_resources.insert(claimed_resources3.begin(), claimed_resources3.end()); + hw1->clearClaims(); + hw2->clearClaims(); + hw3->clearClaims(); + + // success + state_ = INITIALIZED; + return true; + } + + virtual std::string getHardwareInterfaceType() const + { + return "<" + hardware_interface::internal::demangledTypeName() + "," + + hardware_interface::internal::demangledTypeName() + "," + + hardware_interface::internal::demangledTypeName() + ">"; + } + +private: + Controller3(const Controller3 &c); + Controller3& operator =(const Controller3 &c); + +}; + +/** \brief %Controller with a specific hardware interface + * + * \tparam T1 The hardware interface type used by this controller. This enforces + * semantic compatibility between the controller and the hardware it's meant to + * control. + * \tparam T2 The hardware interface type used by this controller. This enforces + * semantic compatibility between the controller and the hardware it's meant to + * control. + * \tparam T3 The hardware interface type used by this controller. This enforces + * semantic compatibility between the controller and the hardware it's meant to + * control. + * \tparam T4 The hardware interface type used by this controller. This enforces + * semantic compatibility between the controller and the hardware it's meant to + * control. + */ +template +class Controller4: public ControllerBase +{ +public: + Controller4() {state_ = CONSTRUCTED;} + virtual ~Controller4(){} + + /** \brief The init function is called to initialize the controller from a + * non-realtime thread with a pointer to the hardware interface, itself, + * instead of a pointer to a RobotHW. + * + * \param hw1 The specific hardware interface used by this controller. + * + * \param hw2 The specific hardware interface used by this controller. + * + * \param hw3 The specific hardware interface used by this controller. + * + * \param hw4 The specific hardware interface used by this controller. + * + * \param controller_nh A NodeHandle in the namespace from which the controller + * should read its configuration, and where it should set up its ROS + * interface. + * + * \returns True if initialization was successful and the controller + * is ready to be started. + */ + virtual bool init(T1* hw1, T2* hw2, T3* hw3, T4* hw4, ros::NodeHandle &controller_nh) {return true;}; + + /** \brief The init function is called to initialize the controller from a + * non-realtime thread with a pointer to the hardware interface, itself, + * instead of a pointer to a RobotHW. + * + * \param hw1 The specific hardware interface used by this controller. + * + * \param hw2 The specific hardware interface used by this controller. + * + * \param hw3 The specific hardware interface used by this controller. + * + * \param hw4 The specific hardware interface used by this controller. + * + * \param root_nh A NodeHandle in the root of the controller manager namespace. + * This is where the ROS interfaces are setup (publishers, subscribers, services). + * + * \param controller_nh A NodeHandle in the namespace of the controller. + * This is where the controller-specific configuration resides. + * + * \returns True if initialization was successful and the controller + * is ready to be started. + */ + virtual bool init(T1* hw1, T2* hw2, T3* hw3, T4* hw4, ros::NodeHandle& root_nh, ros::NodeHandle &controller_nh) {return true;}; + + +protected: + /** \brief Initialize the controller from a RobotHW pointer + * + * This calls \ref init with the hardware interface for this controller if it + * can extract the correct interface from \c robot_hw. + * + */ + virtual bool initRequest(hardware_interface::RobotHW* robot_hw, + ros::NodeHandle& root_nh, ros::NodeHandle &controller_nh, + std::set &claimed_resources) + { + // check if construction finished cleanly + if (state_ != CONSTRUCTED){ + ROS_ERROR("Cannot initialize this controller because it failed to be constructed"); + return false; + } + + // get a pointer to the hardware interface + T1* hw1 = robot_hw->get(); + T2* hw2 = robot_hw->get(); + T3* hw3 = robot_hw->get(); + T4* hw4 = robot_hw->get(); + if (!hw1 || !hw2 || !hw3 || !hw4) + { + ROS_ERROR("This controller requires a hardware interface of type '%s'." + " Make sure this is registered in the hardware_interface::RobotHW class.", + getHardwareInterfaceType().c_str()); + return false; + } + + // return which resources are claimed by this controller + hw1->clearClaims(); + hw2->clearClaims(); + hw3->clearClaims(); + hw4->clearClaims(); + if (!init(hw1, hw2, hw3, hw4, controller_nh) || !init(hw1, hw2, hw3, hw4, root_nh, controller_nh)) + { + ROS_ERROR("Failed to initialize the controller"); + return false; + } + claimed_resources = hw1->getClaims(); + std::set claimed_resources2 = hw2->getClaims(); + std::set claimed_resources3 = hw3->getClaims(); + std::set claimed_resources4 = hw4->getClaims(); + claimed_resources.insert(claimed_resources2.begin(), claimed_resources2.end()); + claimed_resources.insert(claimed_resources3.begin(), claimed_resources3.end()); + claimed_resources.insert(claimed_resources4.begin(), claimed_resources4.end()); + hw1->clearClaims(); + hw2->clearClaims(); + hw3->clearClaims(); + hw4->clearClaims(); + + // success + state_ = INITIALIZED; + return true; + } + + virtual std::string getHardwareInterfaceType() const + { + return "<" + hardware_interface::internal::demangledTypeName() + "," + + hardware_interface::internal::demangledTypeName() + "," + + hardware_interface::internal::demangledTypeName() + "," + + hardware_interface::internal::demangledTypeName() + ">"; + } + +private: + Controller4(const Controller4 &c); + Controller4& operator =(const Controller4 &c); }; diff --git a/controller_manager_tests/include/controller_manager_tests/vel_effort_test_controller.h b/controller_manager_tests/include/controller_manager_tests/vel_effort_test_controller.h index 6d673a326..553e7dde9 100644 --- a/controller_manager_tests/include/controller_manager_tests/vel_effort_test_controller.h +++ b/controller_manager_tests/include/controller_manager_tests/vel_effort_test_controller.h @@ -41,8 +41,8 @@ namespace controller_manager_tests class VelocityEffortTestController: - public controller_interface::Controller + public controller_interface::Controller2 { public: VelocityEffortTestController(){} From d3b838b9ca936f86adf9dd1af2e2f310c1c893bf Mon Sep 17 00:00:00 2001 From: Kelsey Date: Tue, 19 Aug 2014 16:19:43 -0400 Subject: [PATCH 14/16] Adding PosVelAccJointInterface, a JointHandle which also allows for commanding a velocity and acceleration. --- .../pos_vel_acc_joint_interface.h | 61 +++++++++++++++++++ 1 file changed, 61 insertions(+) create mode 100644 hardware_interface/include/hardware_interface/pos_vel_acc_joint_interface.h diff --git a/hardware_interface/include/hardware_interface/pos_vel_acc_joint_interface.h b/hardware_interface/include/hardware_interface/pos_vel_acc_joint_interface.h new file mode 100644 index 000000000..bf4c842b5 --- /dev/null +++ b/hardware_interface/include/hardware_interface/pos_vel_acc_joint_interface.h @@ -0,0 +1,61 @@ +/// \author Kelsey Hawkins + +#ifndef POS_VEL_ACC_JOINT_INTERFACE_H +#define POS_VEL_ACC_JOINT_INTERFACE_H + +#include +#include +#include +#include + +namespace hardware_interface +{ + +/** \brief A handle used to read and command a single joint. */ +class PosVelAccJointHandle : public JointHandle +{ +public: + PosVelAccJointHandle() : hardware_interface::JointHandle(), vel_(0), acc_(0) {} + + /** + * \param js This joint's handle + * \param pos A pointer to the storage for this joint's position command + * \param vel A pointer to the storage for this joint's velocity command + * \param acc A pointer to the storage for this joint's acceleration command + */ + PosVelAccJointHandle(const JointStateHandle& js, double* pos, double* vel, double* acc) + : JointHandle(js, pos), vel_(vel), acc_(acc) + { + if (!vel_ || !acc_) + { + throw HardwareInterfaceException( + "Cannot create handle '" + js.getName() + "'. Some command data pointer is null."); + } + } + + void setPosition(double pos) {setCommand(pos);} + double getPosition() const {return getCommand();} + + void setVelocity(double vel) {assert(vel_); *vel_ = vel;} + double getVelocity() const {assert(vel_); return *vel_;} + + void setAcceleration(double acc) {assert(acc_); *acc_ = acc;} + double getAcceleration() const {assert(acc_); return *acc_;} + +private: + double* vel_; + double* acc_; +}; + +/** \brief Hardware interface to support commanding an array of joints by + * position, velocity, and acceleration. + * + * \note Getting a joint handle through the getHandle() method \e will claim that resource. + * + */ +class PosVelAccJointInterface : + public HardwareResourceManager {}; + +} + +#endif From 841123a54f2250a752e7661f4552e1947b334507 Mon Sep 17 00:00:00 2001 From: Kelsey Date: Tue, 19 Aug 2014 18:41:59 -0400 Subject: [PATCH 15/16] Making ControllerInfo::hardware_interface a set This replaces all instances of ControllerInfo::hardware_interface with std::set hardware_interfaces instead. This allows one to properly represent multi-controllers which use different types of interfaces. getHardwareInterfaceType is also converted to getHardwareInterfaceTypes. A legacy version of getHardwareInterfaceType is added which calls the other and returns a random interface. A warning is displayed if there is more than one interface available. The ControllerState message is also affected. --- .../include/controller_interface/controller.h | 8 ++- .../controller_interface/controller_base.h | 20 ++++++- .../controller_interface/multi_controller.h | 58 ++++++++++++------- controller_manager/src/controller_manager.cpp | 7 ++- .../controller_manager_interface.py | 5 +- .../msg/ControllerState.msg | 2 +- .../hardware_interface/controller_info.h | 3 +- hardware_interface/test/robot_hw_test.cpp | 6 +- 8 files changed, 76 insertions(+), 33 deletions(-) diff --git a/controller_interface/include/controller_interface/controller.h b/controller_interface/include/controller_interface/controller.h index aa299ee4b..0a3aee17a 100644 --- a/controller_interface/include/controller_interface/controller.h +++ b/controller_interface/include/controller_interface/controller.h @@ -111,7 +111,7 @@ class Controller: public ControllerBase { ROS_ERROR("This controller requires a hardware interface of type '%s'." " Make sure this is registered in the hardware_interface::RobotHW class.", - getHardwareInterfaceType().c_str()); + getHardwareInterfaceTypes().begin()->c_str()); return false; } @@ -130,9 +130,11 @@ class Controller: public ControllerBase return true; } - virtual std::string getHardwareInterfaceType() const + virtual std::set getHardwareInterfaceTypes() const { - return hardware_interface::internal::demangledTypeName(); + std::set ret_types; + ret_types.insert(hardware_interface::internal::demangledTypeName()); + return ret_types; } private: diff --git a/controller_interface/include/controller_interface/controller_base.h b/controller_interface/include/controller_interface/controller_base.h index 013e1d7f3..58c2011b7 100644 --- a/controller_interface/include/controller_interface/controller_base.h +++ b/controller_interface/include/controller_interface/controller_base.h @@ -122,8 +122,24 @@ class ControllerBase /** \name Non Real-Time Safe Functions *\{*/ - /// Get the name of this controller's hardware interface type - virtual std::string getHardwareInterfaceType() const = 0; + /// Get the names of the hardware interface types this controller uses + virtual std::set getHardwareInterfaceTypes() const = 0; + + /// Get the single name of the hardware interface this controller uses + virtual std::string getHardwareInterfaceType() + { + std::set types = getHardwareInterfaceTypes(); + + if(types.size() == 0) { + ROS_WARN("This controller has no interface type!"); + return std::string(""); + } + + if(types.size() > 1) + ROS_WARN("This controller has more than one interface type!" + "Use 'getHardwareInterfaceTypes' instead."); + return *(types.begin()); + } /** \brief Request that the controller be initialized * diff --git a/controller_interface/include/controller_interface/multi_controller.h b/controller_interface/include/controller_interface/multi_controller.h index 1d8f85cdf..dd9f124ce 100644 --- a/controller_interface/include/controller_interface/multi_controller.h +++ b/controller_interface/include/controller_interface/multi_controller.h @@ -116,9 +116,12 @@ class Controller2: public ControllerBase T2* hw2 = robot_hw->get(); if (!hw1 || !hw2) { - ROS_ERROR("This controller requires a hardware interface of type '%s'." - " Make sure this is registered in the hardware_interface::RobotHW class.", - getHardwareInterfaceType().c_str()); + std::set::iterator types_iter = getHardwareInterfaceTypes().begin(); + std::string type1 = *types_iter; types_iter++; + std::string type2 = *types_iter; + ROS_ERROR("This controller requires hardware interfaces of types '%s' and '%s'." + " Make sure they are registered in the hardware_interface::RobotHW class.", + type1.c_str(), type2.c_str()); return false; } @@ -141,10 +144,12 @@ class Controller2: public ControllerBase return true; } - virtual std::string getHardwareInterfaceType() const + virtual std::set getHardwareInterfaceTypes() const { - return "<" + hardware_interface::internal::demangledTypeName() + "," + - hardware_interface::internal::demangledTypeName() + ">"; + std::set ret_types; + ret_types.insert(hardware_interface::internal::demangledTypeName()); + ret_types.insert(hardware_interface::internal::demangledTypeName()); + return ret_types; } private: @@ -236,9 +241,13 @@ class Controller3: public ControllerBase T3* hw3 = robot_hw->get(); if (!hw1 || !hw2 || !hw3) { - ROS_ERROR("This controller requires a hardware interface of type '%s'." - " Make sure this is registered in the hardware_interface::RobotHW class.", - getHardwareInterfaceType().c_str()); + std::set::iterator types_iter = getHardwareInterfaceTypes().begin(); + std::string type1 = *types_iter; types_iter++; + std::string type2 = *types_iter; types_iter++; + std::string type3 = *types_iter; + ROS_ERROR("This controller requires hardware interfaces of types '%s', '%s', and '%s'." + " Make sure they are registered in the hardware_interface::RobotHW class.", + type1.c_str(), type2.c_str(), type3.c_str()); return false; } @@ -265,11 +274,13 @@ class Controller3: public ControllerBase return true; } - virtual std::string getHardwareInterfaceType() const + virtual std::set getHardwareInterfaceTypes() const { - return "<" + hardware_interface::internal::demangledTypeName() + "," + - hardware_interface::internal::demangledTypeName() + "," + - hardware_interface::internal::demangledTypeName() + ">"; + std::set ret_types; + ret_types.insert(hardware_interface::internal::demangledTypeName()); + ret_types.insert(hardware_interface::internal::demangledTypeName()); + ret_types.insert(hardware_interface::internal::demangledTypeName()); + return ret_types; } private: @@ -369,9 +380,14 @@ class Controller4: public ControllerBase T4* hw4 = robot_hw->get(); if (!hw1 || !hw2 || !hw3 || !hw4) { - ROS_ERROR("This controller requires a hardware interface of type '%s'." - " Make sure this is registered in the hardware_interface::RobotHW class.", - getHardwareInterfaceType().c_str()); + std::set::iterator types_iter = getHardwareInterfaceTypes().begin(); + std::string type1 = *types_iter; types_iter++; + std::string type2 = *types_iter; types_iter++; + std::string type3 = *types_iter; types_iter++; + std::string type4 = *types_iter; + ROS_ERROR("This controller requires hardware interfaces of types '%s', '%s', '%s', and '%s'." + " Make sure they are registered in the hardware_interface::RobotHW class.", + type1.c_str(), type2.c_str(), type3.c_str(), type4.c_str()); return false; } @@ -404,10 +420,12 @@ class Controller4: public ControllerBase virtual std::string getHardwareInterfaceType() const { - return "<" + hardware_interface::internal::demangledTypeName() + "," + - hardware_interface::internal::demangledTypeName() + "," + - hardware_interface::internal::demangledTypeName() + "," + - hardware_interface::internal::demangledTypeName() + ">"; + std::set ret_types; + ret_types.insert(hardware_interface::internal::demangledTypeName()); + ret_types.insert(hardware_interface::internal::demangledTypeName()); + ret_types.insert(hardware_interface::internal::demangledTypeName()); + ret_types.insert(hardware_interface::internal::demangledTypeName()); + return ret_types; } private: diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c685901f4..4fa8465cd 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -248,7 +248,7 @@ bool ControllerManager::loadController(const std::string& name) // Adds the controller to the new list to.resize(to.size() + 1); to[to.size()-1].info.type = type; - to[to.size()-1].info.hardware_interface = c->getHardwareInterfaceType(); + to[to.size()-1].info.hardware_interfaces = c->getHardwareInterfaceTypes(); to[to.size()-1].info.name = name; to[to.size()-1].info.resources = claimed_resources; to[to.size()-1].c = c; @@ -564,7 +564,10 @@ bool ControllerManager::listControllersSrv( controller_manager_msgs::ControllerState& cs = resp.controller[i]; cs.name = controllers[i].info.name; cs.type = controllers[i].info.type; - cs.hardware_interface = controllers[i].info.hardware_interface; + cs.hardware_interfaces.clear(); + cs.hardware_interfaces.reserve(controllers[i].info.hardware_interfaces.size()); + for (std::set::iterator it = controllers[i].info.hardware_interfaces.begin(); it != controllers[i].info.hardware_interfaces.end(); ++it) + cs.hardware_interfaces.push_back(*it); cs.resources.clear(); cs.resources.reserve(controllers[i].info.resources.size()); for (std::set::iterator it = controllers[i].info.resources.begin(); it != controllers[i].info.resources.end(); ++it) diff --git a/controller_manager/src/controller_manager/controller_manager_interface.py b/controller_manager/src/controller_manager/controller_manager_interface.py index 062fba0da..a4aa92ec1 100755 --- a/controller_manager/src/controller_manager/controller_manager_interface.py +++ b/controller_manager/src/controller_manager/controller_manager_interface.py @@ -57,7 +57,10 @@ def list_controllers(): print "No controllers are loaded in mechanism control" else: for c in resp.controller: - print '%s - %s ( %s )'%(c.name, c.hardware_interface, c.state) + if len(c.hardware_interfaces) == 1: + print '%s - %s ( %s )'%(c.name, c.hardware_interfaces[0], c.state) + else: + print '%s - [%s] ( %s )'%(c.name, ", ".join(c.hardware_interfaces), c.state) def load_controller(name): diff --git a/controller_manager_msgs/msg/ControllerState.msg b/controller_manager_msgs/msg/ControllerState.msg index 8c39b8885..9b8264b1b 100644 --- a/controller_manager_msgs/msg/ControllerState.msg +++ b/controller_manager_msgs/msg/ControllerState.msg @@ -1,5 +1,5 @@ string name string state string type -string hardware_interface +string[] hardware_interfaces string[] resources diff --git a/hardware_interface/include/hardware_interface/controller_info.h b/hardware_interface/include/hardware_interface/controller_info.h index 8f665a8e0..ece2772f4 100644 --- a/hardware_interface/include/hardware_interface/controller_info.h +++ b/hardware_interface/include/hardware_interface/controller_info.h @@ -42,7 +42,8 @@ namespace hardware_interface */ struct ControllerInfo { - std::string name, type, hardware_interface; + std::string name, type; + std::set hardware_interfaces; std::set resources; }; diff --git a/hardware_interface/test/robot_hw_test.cpp b/hardware_interface/test/robot_hw_test.cpp index 03b35f2b3..bbdb307c6 100644 --- a/hardware_interface/test/robot_hw_test.cpp +++ b/hardware_interface/test/robot_hw_test.cpp @@ -117,19 +117,19 @@ TEST_F(RobotHWTest, ConflictChecking) ControllerInfo info1; info1.name = "controller_1"; info1.type = "type_1"; - info1.hardware_interface = "interface_1"; + info1.hardware_interfaces.insert("interface_1"); info1.resources.insert("resource_1"); ControllerInfo info2; info2.name = "controller_2"; info2.type = "type_2"; - info2.hardware_interface = "interface_2"; + info2.hardware_interfaces.insert("interface_2"); info2.resources.insert("resource_2"); ControllerInfo info12; info12.name = "controller_12"; info12.type = "type_12"; - info12.hardware_interface = "interface_12"; + info12.hardware_interfaces.insert("interface_12"); info12.resources.insert("resource_1"); info12.resources.insert("resource_2"); From 079af40eb6ce75a7be9e892c3ecbb7e348052ce1 Mon Sep 17 00:00:00 2001 From: Kelsey Date: Thu, 21 Aug 2014 19:33:51 -0400 Subject: [PATCH 16/16] Fixing hardware_interface reference in controller_manager_gui --- .../rqt_controller_manager/controller_manager_gui.py | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/rqt_controller_manager/src/rqt_controller_manager/controller_manager_gui.py b/rqt_controller_manager/src/rqt_controller_manager/controller_manager_gui.py index 1cb6869ce..3b2608afa 100644 --- a/rqt_controller_manager/src/rqt_controller_manager/controller_manager_gui.py +++ b/rqt_controller_manager/src/rqt_controller_manager/controller_manager_gui.py @@ -282,6 +282,11 @@ def refresh_ctrlers(self): controller_list = resp.controller new_ctrlers = {} for c in controller_list: + if len(c.hardware_interfaces) == 1: + hw_iface_name = c.hardware_interfaces[0] + else: + hw_iface_name = "[" + ",".join(sorted(c.hardware_interfaces)) + "]" + if c.name not in self._ctrlers: # new controller item = QTreeWidgetItem(self.ctrl_list_tree_widget) @@ -289,7 +294,7 @@ def refresh_ctrlers(self): ctrler = {'item' : item, 'state' : c.state, 'type' : c.type, - 'hw_iface' : c.hardware_interface, + 'hw_iface' : hw_iface_name, 'resources' : "[" + ", ".join(c.resources) + "]"} ctrler['item'].setText(self._column_index['name'], c.name) update_type = True @@ -299,11 +304,11 @@ def refresh_ctrlers(self): ctrler = self._ctrlers[c.name] update_type = False update_state = False - if ctrler['type'] != c.type or ctrler['hw_iface'] != c.hardware_interface: + if ctrler['type'] != c.type or ctrler['hw_iface'] != hw_iface_name: # total controller change ctrler['state'] = c.state ctrler['type'] = c.type - ctrler['hw_iface'] = c.hardware_interface + ctrler['hw_iface'] = hw_iface_name ctrler['resources'] = "[" + ", ".join(c.resources) + "]" update_type = True if ctrler['state'] != c.state: