From 097c05ec9264ec69a379042c0685b261c72213a2 Mon Sep 17 00:00:00 2001 From: Toni Oliver Date: Thu, 26 Nov 2015 20:05:43 +0000 Subject: [PATCH 1/2] Allow the InterfaceManager class to register other InterfaceManagers. This will make it possible to combine several RobotHW objects into a single one. --- .../internal/interface_manager.h | 122 +++++++++++++++-- .../internal/resource_manager.h | 35 ++++- hardware_interface/test/robot_hw_test.cpp | 129 ++++++++++++++++++ 3 files changed, 275 insertions(+), 11 deletions(-) diff --git a/hardware_interface/include/hardware_interface/internal/interface_manager.h b/hardware_interface/include/hardware_interface/internal/interface_manager.h index 753387241..1c55c3452 100644 --- a/hardware_interface/include/hardware_interface/internal/interface_manager.h +++ b/hardware_interface/include/hardware_interface/internal/interface_manager.h @@ -26,20 +26,60 @@ // POSSIBILITY OF SUCH DAMAGE. ////////////////////////////////////////////////////////////////////////////// -/// \author Wim Meussen, Adolfo Rodriguez Tsouroukdissian +/// \author Wim Meussen, Adolfo Rodriguez Tsouroukdissian, Kelsey P. Hawkins #ifndef HARDWARE_INTERFACE_INTERFACE_MANAGER_H #define HARDWARE_INTERFACE_INTERFACE_MANAGER_H #include #include +#include +#include #include #include +#include namespace hardware_interface { +// SFINAE workaround, so that we have reflection inside the template functions +template +struct CheckIsResourceManager { + // variable definitions for compiler-time logic + typedef char yes[1]; + typedef char no[2]; + + // method called if C is a ResourceManager + template + static yes& testRM(typename C::resource_manager_type*); + + // method called if C is not a ResourceManager + template + static no& testRM(...); + + // CheckIsResourceManager::value == true when T is a ResourceManager + static const bool value = (sizeof(testRM(0)) == sizeof(yes)); + + // method called if C is a ResourceManager + template + static yes& callCM(typename std::vector& managers, C* result, typename C::resource_manager_type*) + { + std::vector managers_in; + // we have to typecase back to base class + for(typename std::vector::iterator it = managers.begin(); it != managers.end(); ++it) + managers_in.push_back(static_cast(*it)); + C::concatManagers(managers_in, result); + } + + // method called if C is not a ResourceManager + template + static no& callCM(typename std::vector& managers, C* result, ...) {} + + // calls ResourceManager::concatManagers if C is a ResourceManager + static const void callConcatManagers(typename std::vector& managers, T* result) + { callCM(managers, result, 0); } +}; class InterfaceManager { @@ -64,6 +104,11 @@ class InterfaceManager interfaces_[internal::demangledTypeName()] = iface; } + void registerInterfaceManager(InterfaceManager* iface_man) + { + interface_managers_.push_back(iface_man); + } + /** * \brief Get an interface. * @@ -77,18 +122,68 @@ class InterfaceManager template T* get() { - InterfaceMap::iterator it = interfaces_.find(internal::demangledTypeName()); - if (it == interfaces_.end()) - return NULL; + std::string type_name = internal::demangledTypeName(); + std::vector iface_list; - T* iface = static_cast(it->second); - if (!iface) - { - ROS_ERROR_STREAM("Failed reconstructing type T = '" << internal::demangledTypeName().c_str() << - "'. This should never happen"); + // look for interfaces registered here + InterfaceMap::iterator it = interfaces_.find(type_name); + if (it != interfaces_.end()) { + T* iface = static_cast(it->second); + if (!iface) { + ROS_ERROR_STREAM("Failed reconstructing type T = '" << type_name.c_str() << + "'. This should never happen"); + return NULL; + } + iface_list.push_back(iface); + } + + // look for interfaces registered in the registered hardware + for(InterfaceManagerVector::iterator it = interface_managers_.begin(); + it != interface_managers_.end(); ++it) { + T* iface = (*it)->get(); + if (iface) + iface_list.push_back(iface); + } + + if(iface_list.size() == 0) return NULL; + + if(iface_list.size() == 1) + return iface_list.front(); + + // if we're here, we have multiple interfaces, and thus we must construct a new + // combined interface, or return one already constructed + T* iface_combo; + InterfaceMap::iterator it_combo = interfaces_combo_.find(type_name); + if(it_combo != interfaces_combo_.end() && + num_ifaces_registered_[type_name] == iface_list.size()) { + // there exists a combined interface with the same number of interfaces combined + // (since you cannot unregister interfaces, this will be guaranteed to be the + // same interfaces from previous calls) + iface_combo = static_cast(it_combo->second); + } else { + // no existing combined interface + if(CheckIsResourceManager::value) { + // it is a ResourceManager + + // create a new combined interface + iface_combo = new T; + // save the new interface pointer to allow for its correct destruction + interface_destruction_list_.push_back(reinterpret_cast(iface_combo)); + // concat all of the resource managers together + CheckIsResourceManager::callConcatManagers(iface_list, iface_combo); + // save the combined interface for if this is called again + interfaces_combo_[type_name] = iface_combo; + num_ifaces_registered_[type_name] = iface_list.size(); + } else { + // it is not a ResourceManager + ROS_ERROR("You cannot register multiple interfaces of the same type which are " + "not of type ResourceManager. There is no established protocol " + "for combining them."); + iface_combo = NULL; + } } - return iface; + return iface_combo; } /** \return Vector of interface names registered to this instance. */ @@ -105,7 +200,14 @@ class InterfaceManager protected: typedef std::map InterfaceMap; + typedef std::vector InterfaceManagerVector; + typedef std::map SizeMap; + InterfaceMap interfaces_; + InterfaceMap interfaces_combo_; + InterfaceManagerVector interface_managers_; + SizeMap num_ifaces_registered_; + boost::ptr_vector interface_destruction_list_; }; } // namespace diff --git a/hardware_interface/include/hardware_interface/internal/resource_manager.h b/hardware_interface/include/hardware_interface/internal/resource_manager.h index 78585cbe8..2cb409153 100644 --- a/hardware_interface/include/hardware_interface/internal/resource_manager.h +++ b/hardware_interface/include/hardware_interface/internal/resource_manager.h @@ -44,6 +44,17 @@ namespace hardware_interface { +/** + * \brief Non-templated Base Class that contains a virtual destructor. + * + * This will allow to destroy the templated children without having to know the template type. + */ +class ResourceManagerBase +{ +public: + virtual ~ResourceManagerBase() {} +}; + /** * \brief Class for handling named resources. * @@ -55,9 +66,10 @@ namespace hardware_interface * \endcode */ template -class ResourceManager +class ResourceManager : public ResourceManagerBase { public: + typedef ResourceManager resource_manager_type; /** \name Non Real-Time Safe Functions *\{*/ @@ -113,6 +125,27 @@ class ResourceManager return it->second; } + /** + * \brief Combine a list of interfaces into one. + * + * Every registered handle in each of the managers is registered into the result interface + * \param managers The list of resource managers to be combined. + * \param result The interface where all the handles will be registered. + * \return Resource associated to \e name. If the resource name is not found, an exception is thrown. + */ + static void concatManagers(std::vector& managers, + resource_manager_type* result) + { + for(typename std::vector::iterator it_man = managers.begin(); + it_man != managers.end(); ++it_man) { + std::vector handle_names = (*it_man)->getNames(); + for(std::vector::iterator it_nms = handle_names.begin(); + it_nms != handle_names.end(); ++it_nms) { + result->registerHandle((*it_man)->getHandle(*it_nms)); + } + } + } + /*\}*/ protected: diff --git a/hardware_interface/test/robot_hw_test.cpp b/hardware_interface/test/robot_hw_test.cpp index a050cd18c..0c7f9baf0 100644 --- a/hardware_interface/test/robot_hw_test.cpp +++ b/hardware_interface/test/robot_hw_test.cpp @@ -225,6 +225,135 @@ TEST_F(RobotHWTest, ConflictChecking) } } +TEST_F(RobotHWTest, CombineDifferentInterfaces) +{ + // Populate hardware interfaces + JointStateInterface state_iface; + state_iface.registerHandle(hs1); + state_iface.registerHandle(hs2); + + EffortJointInterface eff_cmd_iface; + eff_cmd_iface.registerHandle(hc1); + eff_cmd_iface.registerHandle(hc2); + + PositionJointInterface pos_cmd_iface; + pos_cmd_iface.registerHandle(hc1); + pos_cmd_iface.registerHandle(hc2); + + // Register them to different RobotHW instances + RobotHW hw1, hw2; + RobotHW hw_grp; + hw1.registerInterface(&state_iface); + hw2.registerInterface(&eff_cmd_iface); + hw_grp.registerInterface(&pos_cmd_iface); + + hw_grp.registerInterfaceManager(&hw1); + hw_grp.registerInterfaceManager(&hw2); + + // Get interfaces + EXPECT_TRUE(&state_iface == hw_grp.get()); + EXPECT_TRUE(&eff_cmd_iface == hw_grp.get()); + EXPECT_TRUE(&pos_cmd_iface == hw_grp.get()); + EXPECT_FALSE(hw_grp.get()); +} + +TEST_F(RobotHWTest, CombineSameInterfaces) +{ + // Populate hardware interfaces + JointStateInterface state_iface1; + state_iface1.registerHandle(hs1); + EffortJointInterface eff_cmd_iface1; + eff_cmd_iface1.registerHandle(hc1); + + JointStateInterface state_iface2; + state_iface2.registerHandle(hs2); + EffortJointInterface eff_cmd_iface2; + eff_cmd_iface2.registerHandle(hc2); + + // Register them to different RobotHW instances + RobotHW hw1, hw2; + RobotHW hw_grp; + hw1.registerInterface(&state_iface1); + hw1.registerInterface(&eff_cmd_iface1); + hw2.registerInterface(&state_iface2); + hw2.registerInterface(&eff_cmd_iface2); + + hw_grp.registerInterfaceManager(&hw1); + hw_grp.registerInterfaceManager(&hw2); + + // Get interfaces + JointStateInterface* js_combo = hw_grp.get(); + EffortJointInterface* ej_combo = hw_grp.get(); + + // confirm that the combined interfaces are different from the originals + EXPECT_FALSE(&state_iface1 == js_combo); + EXPECT_FALSE(&state_iface2 == js_combo); + EXPECT_FALSE(&eff_cmd_iface1 == ej_combo); + EXPECT_FALSE(&eff_cmd_iface2 == ej_combo); + EXPECT_FALSE(hw_grp.get()); + + // confirm that each RobotHW is still working properly independently + EXPECT_TRUE(&state_iface1 == hw1.get()); + EXPECT_TRUE(&state_iface2 == hw2.get()); + EXPECT_TRUE(&eff_cmd_iface1 == hw1.get()); + EXPECT_TRUE(&eff_cmd_iface2 == hw2.get()); + + // Retrieve all the handles from the combined interfaces + JointStateHandle hs1_ret = js_combo->getHandle(name1); + JointStateHandle hs2_ret = js_combo->getHandle(name2); + JointHandle hc1_ret = ej_combo->getHandle(name1); + JointHandle hc2_ret = ej_combo->getHandle(name2); + + // confirm the handles are proper copies + EXPECT_TRUE(hs1.getPosition() == hs1_ret.getPosition()); + EXPECT_TRUE(hs2.getPosition() == hs2_ret.getPosition()); + hc1.setCommand(3.14); + EXPECT_EQ(3.14, hc1_ret.getCommand()); + hc2.setCommand(6.28); + EXPECT_EQ(6.28, hc2_ret.getCommand()); + + // check to make sure further calls return the same combined interface objects + JointStateInterface* js_combo2 = hw_grp.get(); + EffortJointInterface* ej_combo2 = hw_grp.get(); + EXPECT_TRUE(js_combo == js_combo2); + EXPECT_TRUE(ej_combo == ej_combo2); +} + +TEST_F(RobotHWTest, IncrementalSameInterfaces) +{ + // Populate hardware interfaces + JointStateInterface state_iface1; + state_iface1.registerHandle(hs1); + + JointStateInterface state_iface2; + state_iface2.registerHandle(hs2); + + // Register them to different RobotHW instances + RobotHW hw1, hw2; + hw1.registerInterface(&state_iface1); + hw2.registerInterface(&state_iface2); + + RobotHW hw_grp; + hw_grp.registerInterfaceManager(&hw1); + JointStateInterface* js_combo1 = hw_grp.get(); + // only one interface exists, so the combined should be exactly the registered interface object + EXPECT_TRUE(&state_iface1 == js_combo1); + // check that it contains hs1 handle + JointStateHandle hs1_ret1 = js_combo1->getHandle(name1); + EXPECT_TRUE(hs1.getPosition() == hs1_ret1.getPosition()); + + hw_grp.registerInterfaceManager(&hw2); + JointStateInterface* js_combo2 = hw_grp.get(); + EXPECT_FALSE(&state_iface1 == js_combo2); + EXPECT_FALSE(&state_iface2 == js_combo2); + + // check to see if both joint handles are here + JointStateHandle hs1_ret2 = js_combo2->getHandle(name1); + JointStateHandle hs2_ret = js_combo2->getHandle(name2); + EXPECT_TRUE(hs1.getPosition() == hs1_ret2.getPosition()); + EXPECT_TRUE(hs2.getPosition() == hs2_ret.getPosition()); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); From d216407a521ba51d4a54439e7d2deb4e2efe86bc Mon Sep 17 00:00:00 2001 From: Toni Oliver Date: Mon, 30 Nov 2015 19:17:37 +0000 Subject: [PATCH 2/2] Add packages combined_robot_hw and combined_robot_hw_tests. combined_robot_hw allows to load different RobotHW as plugins and combines them into a single RobotHW. A single controller manager can then access resources from any robot. --- combined_robot_hw/CMakeLists.txt | 34 ++ .../combined_robot_hw/combined_robot_hw.h | 121 ++++ combined_robot_hw/package.xml | 27 + combined_robot_hw/src/combined_robot_hw.cpp | 252 +++++++++ combined_robot_hw_tests/CMakeLists.txt | 63 +++ .../combined_robot_hw_tests/my_robot_hw_1.h | 70 +++ .../combined_robot_hw_tests/my_robot_hw_2.h | 70 +++ .../combined_robot_hw_tests/my_robot_hw_3.h | 66 +++ .../combined_robot_hw_tests/my_robot_hw_4.h | 67 +++ combined_robot_hw_tests/package.xml | 33 ++ combined_robot_hw_tests/src/dummy_app.cpp | 55 ++ combined_robot_hw_tests/src/my_robot_hw_1.cpp | 175 ++++++ combined_robot_hw_tests/src/my_robot_hw_2.cpp | 155 ++++++ combined_robot_hw_tests/src/my_robot_hw_3.cpp | 94 ++++ combined_robot_hw_tests/src/my_robot_hw_4.cpp | 113 ++++ combined_robot_hw_tests/test/cm_test.cpp | 521 ++++++++++++++++++ combined_robot_hw_tests/test/cm_test.test | 68 +++ .../test/combined_robot_hw_test.cpp | 207 +++++++ .../test/combined_robot_hw_test.test | 25 + .../test_robot_hw_plugin.xml | 25 + .../internal/interface_manager.h | 49 +- .../include/hardware_interface/robot_hw.h | 34 ++ 22 files changed, 2322 insertions(+), 2 deletions(-) create mode 100644 combined_robot_hw/CMakeLists.txt create mode 100644 combined_robot_hw/include/combined_robot_hw/combined_robot_hw.h create mode 100644 combined_robot_hw/package.xml create mode 100644 combined_robot_hw/src/combined_robot_hw.cpp create mode 100644 combined_robot_hw_tests/CMakeLists.txt create mode 100644 combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_1.h create mode 100644 combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_2.h create mode 100644 combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_3.h create mode 100644 combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_4.h create mode 100644 combined_robot_hw_tests/package.xml create mode 100644 combined_robot_hw_tests/src/dummy_app.cpp create mode 100644 combined_robot_hw_tests/src/my_robot_hw_1.cpp create mode 100644 combined_robot_hw_tests/src/my_robot_hw_2.cpp create mode 100644 combined_robot_hw_tests/src/my_robot_hw_3.cpp create mode 100644 combined_robot_hw_tests/src/my_robot_hw_4.cpp create mode 100644 combined_robot_hw_tests/test/cm_test.cpp create mode 100644 combined_robot_hw_tests/test/cm_test.test create mode 100644 combined_robot_hw_tests/test/combined_robot_hw_test.cpp create mode 100644 combined_robot_hw_tests/test/combined_robot_hw_test.test create mode 100644 combined_robot_hw_tests/test_robot_hw_plugin.xml diff --git a/combined_robot_hw/CMakeLists.txt b/combined_robot_hw/CMakeLists.txt new file mode 100644 index 000000000..9a76b2dc1 --- /dev/null +++ b/combined_robot_hw/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 2.8.3) +project(combined_robot_hw) + +find_package(catkin REQUIRED COMPONENTS + hardware_interface + pluginlib + roscpp +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS hardware_interface pluginlib roscpp +) + +add_library(${PROJECT_NAME} + src/combined_robot_hw.cpp +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + + +# Install +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +install(TARGETS ${PROJECT_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + diff --git a/combined_robot_hw/include/combined_robot_hw/combined_robot_hw.h b/combined_robot_hw/include/combined_robot_hw/combined_robot_hw.h new file mode 100644 index 000000000..5915f76d7 --- /dev/null +++ b/combined_robot_hw/include/combined_robot_hw/combined_robot_hw.h @@ -0,0 +1,121 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2015, Shadow Robot Company Ltd. +// +// 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 Shadow Robot Company Ltd. 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 COMBINED_ROBOT_HW_COMBINED_ROBOT_HW_H +#define COMBINED_ROBOT_HW_COMBINED_ROBOT_HW_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace combined_robot_hw +{ + +/** \brief CombinedRobotHW + * + * This class provides a way to combine RobotHW objects. + * + * + * + */ +class CombinedRobotHW : public hardware_interface::RobotHW +{ +public: + CombinedRobotHW(); + + virtual ~CombinedRobotHW(){} + + /** \brief The init function is called to initialize the RobotHW from a + * non-realtime thread. + * + * \param root_nh A NodeHandle in the root of the caller namespace. + * + * \param robot_hw_nh A NodeHandle in the namespace from which the RobotHW + * should read its configuration. + * + * \returns True if initialization was successful + */ + virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh); + + + /** + * Check (in non-realtime) if given controllers could be started and stopped from the current state of the RobotHW + * with regard to necessary hardware interface switches and prepare the switching. Start and stop list are disjoint. + * This handles the check and preparation, the actual switch is commited in doSwitch() + */ + virtual bool prepareSwitch(const std::list& start_list, + const std::list& stop_list); + + /** + * Perform (in realtime) all necessary hardware interface switches in order to start and stop the given controllers. + * Start and stop list are disjoint. The feasability was checked in prepareSwitch() beforehand. + */ + virtual void doSwitch(const std::list& start_list, + const std::list& stop_list); + + /** + * Reads data from the robot HW + * + * \param time The current time + * \param period The time passed since the last call to \ref read + */ + virtual void read(const ros::Time& time, const ros::Duration& period); + + /** + * Writes data to the robot HW + * + * \param time The current time + * \param period The time passed since the last call to \ref write + */ + virtual void write(const ros::Time& time, const ros::Duration& period); + +protected: + ros::NodeHandle root_nh_; + ros::NodeHandle robot_hw_nh_; + pluginlib::ClassLoader robot_hw_loader_; + std::vector > robot_hw_list_; + + virtual bool loadRobotHW(const std::string& name); + + /** \brief Filters the start and stop lists so that they only contain the controllers and + * resources that correspond to the robot_hw interface manager + */ + void filterControllerList(const std::list& list, + std::list& filtered_list, + boost::shared_ptr robot_hw); +}; + +} + +#endif diff --git a/combined_robot_hw/package.xml b/combined_robot_hw/package.xml new file mode 100644 index 000000000..8f7c503a9 --- /dev/null +++ b/combined_robot_hw/package.xml @@ -0,0 +1,27 @@ + + + combined_robot_hw + 0.0.0 + Combined Robot HW class. + Toni Oliver + + BSD + + https://github.com/ros-controls/ros_control/wiki + https://github.com/ros-controls/ros_control/issues + https://github.com/ros-controls/ros_control + + Toni Oliver + + catkin + hardware_interface + pluginlib + roscpp + hardware_interface + pluginlib + roscpp + + + + + \ No newline at end of file diff --git a/combined_robot_hw/src/combined_robot_hw.cpp b/combined_robot_hw/src/combined_robot_hw.cpp new file mode 100644 index 000000000..18a352be1 --- /dev/null +++ b/combined_robot_hw/src/combined_robot_hw.cpp @@ -0,0 +1,252 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2015, Shadow Robot Company Ltd. +// +// 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 Shadow Robot Company Ltd. 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 "combined_robot_hw/combined_robot_hw.h" + +namespace combined_robot_hw +{ + CombinedRobotHW::CombinedRobotHW() : + robot_hw_loader_("hardware_interface", "hardware_interface::RobotHW") + {} + + bool CombinedRobotHW::init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh) + { + root_nh_ = root_nh; + robot_hw_nh_ = robot_hw_nh; + + std::vector robots; + std::string param_name = "robot_hardware"; + if (!robot_hw_nh.getParam(param_name, robots)) + { + ROS_ERROR_STREAM("Could not find '" << param_name << "' parameter (namespace: " << robot_hw_nh.getNamespace() << ")."); + return false; + } + + std::vector::iterator it; + for (it = robots.begin(); it != robots.end(); it++) + { + if (!loadRobotHW(*it)) + { + return false; + } + } + return true; + } + + bool CombinedRobotHW::prepareSwitch(const std::list& start_list, + const std::list& stop_list) + { + // Call the prepareSwitch method of the single RobotHW objects. + std::vector >::iterator robot_hw; + for (robot_hw = robot_hw_list_.begin(); robot_hw != robot_hw_list_.end(); ++robot_hw) + { + std::list filtered_start_list; + std::list filtered_stop_list; + + // Generate a filtered version of start_list and stop_list for each RobotHW before calling prepareSwitch + filterControllerList(start_list, filtered_start_list, *robot_hw); + filterControllerList(stop_list, filtered_stop_list, *robot_hw); + + if (!(*robot_hw)->prepareSwitch(filtered_start_list, filtered_stop_list)) + return false; + } + return true; + } + + void CombinedRobotHW::doSwitch(const std::list& start_list, + const std::list& stop_list) + { + // Call the doSwitch method of the single RobotHW objects. + std::vector >::iterator robot_hw; + for (robot_hw = robot_hw_list_.begin(); robot_hw != robot_hw_list_.end(); ++robot_hw) + { + std::list filtered_start_list; + std::list filtered_stop_list; + + // Generate a filtered version of start_list and stop_list for each RobotHW before calling doSwitch + filterControllerList(start_list, filtered_start_list, *robot_hw); + filterControllerList(stop_list, filtered_stop_list, *robot_hw); + + (*robot_hw)->doSwitch(filtered_start_list, filtered_stop_list); + } + } + + bool CombinedRobotHW::loadRobotHW(const std::string& name) + { + ROS_DEBUG("Will load robot HW '%s'", name.c_str()); + + ros::NodeHandle c_nh; + // Constructs the robot HW + try + { + c_nh = ros::NodeHandle(robot_hw_nh_, name); + } + catch(std::exception &e) + { + ROS_ERROR("Exception thrown while constructing nodehandle for robot HW with name '%s':\n%s", name.c_str(), e.what()); + return false; + } + catch(...) + { + ROS_ERROR("Exception thrown while constructing nodehandle for robot HW with name '%s'", name.c_str()); + return false; + } + + boost::shared_ptr robot_hw; + std::string type; + if (c_nh.getParam("type", type)) + { + ROS_DEBUG("Constructing robot HW '%s' of type '%s'", name.c_str(), type.c_str()); + try + { + std::vector cur_types = robot_hw_loader_.getDeclaredClasses(); + for(size_t i=0; i < cur_types.size(); i++) + { + if (type == cur_types[i]) + { + robot_hw = robot_hw_loader_.createInstance(type); + } + } + } + catch (const std::runtime_error &ex) + { + ROS_ERROR("Could not load class %s: %s", type.c_str(), ex.what()); + } + } + else + { + ROS_ERROR("Could not load robot HW '%s' because the type was not specified. Did you load the robot HW configuration on the parameter server (namespace: '%s')?", name.c_str(), c_nh.getNamespace().c_str()); + return false; + } + + // checks if robot HW was constructed + if (!robot_hw) + { + ROS_ERROR("Could not load robot HW '%s' because robot HW type '%s' does not exist.", name.c_str(), type.c_str()); + return false; + } + + // Initializes the robot HW + ROS_DEBUG("Initializing robot HW '%s'", name.c_str()); + bool initialized; + try + { + initialized = robot_hw->init(root_nh_, c_nh); + } + catch(std::exception &e) + { + ROS_ERROR("Exception thrown while initializing robot HW %s.\n%s", name.c_str(), e.what()); + initialized = false; + } + catch(...) + { + ROS_ERROR("Exception thrown while initializing robot HW %s", name.c_str()); + initialized = false; + } + + if (!initialized) + { + ROS_ERROR("Initializing robot HW '%s' failed", name.c_str()); + return false; + } + ROS_DEBUG("Initialized robot HW '%s' successful", name.c_str()); + + robot_hw_list_.push_back(robot_hw); + + this->registerInterfaceManager(robot_hw.get()); + + ROS_DEBUG("Successfully load robot HW '%s'", name.c_str()); + return true; + } + + void CombinedRobotHW::read(const ros::Time& time, const ros::Duration& period) + { + // Call the read method of the single RobotHW objects. + std::vector >::iterator robot_hw; + for (robot_hw = robot_hw_list_.begin(); robot_hw != robot_hw_list_.end(); ++robot_hw) + { + (*robot_hw)->read(time, period); + } + } + + + void CombinedRobotHW::write(const ros::Time& time, const ros::Duration& period) + { + // Call the write method of the single RobotHW objects. + std::vector >::iterator robot_hw; + for (robot_hw = robot_hw_list_.begin(); robot_hw != robot_hw_list_.end(); ++robot_hw) + { + (*robot_hw)->write(time, period); + } + } + + void CombinedRobotHW::filterControllerList(const std::list& list, + std::list& filtered_list, + boost::shared_ptr robot_hw) + { + filtered_list.clear(); + for (std::list::const_iterator it = list.begin(); it != list.end(); ++it) + { + hardware_interface::ControllerInfo filtered_controller; + filtered_controller.name = it->name; + filtered_controller.type = it->type; + + if (it->claimed_resources.empty()) + { + filtered_list.push_back(filtered_controller); + continue; + } + for (std::vector::const_iterator res_it = it->claimed_resources.begin(); res_it != it->claimed_resources.end(); ++res_it) + { + hardware_interface::InterfaceResources filtered_iface_resources; + filtered_iface_resources.hardware_interface = res_it->hardware_interface; + std::vector r_hw_ifaces = robot_hw->getNames(); + + std::vector::iterator if_name = std::find(r_hw_ifaces.begin(), r_hw_ifaces.end(), filtered_iface_resources.hardware_interface); + if (if_name == r_hw_ifaces.end()) // this hardware_interface is not registered in r_hw, so we filter it out + { + continue; + } + + std::vector r_hw_iface_resources = robot_hw->getInterfaceResources(filtered_iface_resources.hardware_interface); + std::set filtered_resources; + for (std::set::const_iterator ctrl_res = res_it->resources.begin(); ctrl_res != res_it->resources.end(); ++ctrl_res) + { + std::vector::iterator res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), *ctrl_res); + if (res_name != r_hw_iface_resources.end()) + { + filtered_resources.insert(*ctrl_res); + } + } + filtered_iface_resources.resources = filtered_resources; + filtered_controller.claimed_resources.push_back(filtered_iface_resources); + } + filtered_list.push_back(filtered_controller); + } + } +} diff --git a/combined_robot_hw_tests/CMakeLists.txt b/combined_robot_hw_tests/CMakeLists.txt new file mode 100644 index 000000000..1d088859b --- /dev/null +++ b/combined_robot_hw_tests/CMakeLists.txt @@ -0,0 +1,63 @@ +cmake_minimum_required(VERSION 2.8.3) +project(combined_robot_hw_tests) + +find_package(catkin REQUIRED COMPONENTS + combined_robot_hw + controller_manager + controller_manager_tests + hardware_interface + roscpp +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS combined_robot_hw hardware_interface roscpp +) + +add_library(${PROJECT_NAME} + src/my_robot_hw_1.cpp + src/my_robot_hw_2.cpp + src/my_robot_hw_3.cpp + src/my_robot_hw_4.cpp +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_executable(combined_robot_hw_dummy_app src/dummy_app.cpp) +target_link_libraries(combined_robot_hw_dummy_app ${PROJECT_NAME} ${catkin_LIBRARIES}) + +if(CATKIN_ENABLE_TESTING) + + find_package(rostest REQUIRED) + add_rostest_gtest(combined_robot_hw_test + test/combined_robot_hw_test.test + test/combined_robot_hw_test.cpp + ) + target_link_libraries(combined_robot_hw_test ${PROJECT_NAME} ${catkin_LIBRARIES}) + + add_rostest_gtest(combined_robot_hw_cm_test + test/cm_test.test + test/cm_test.cpp + ) + target_link_libraries(combined_robot_hw_cm_test ${PROJECT_NAME} ${catkin_LIBRARIES}) + +endif() + +# Install +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + +install(FILES test_robot_hw_plugin.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + diff --git a/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_1.h b/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_1.h new file mode 100644 index 000000000..bdbe64263 --- /dev/null +++ b/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_1.h @@ -0,0 +1,70 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2015, Shadow Robot Company Ltd. +// +// 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_MY_ROBOT_HW_1_H +#define CONTROLLER_MANAGER_TESTS_MY_ROBOT_HW_1_H + +#include +#include +#include + +namespace combined_robot_hw_tests +{ + +class MyRobotHW1 : public hardware_interface::RobotHW +{ +public: + MyRobotHW1(); + virtual ~MyRobotHW1(){}; + virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh); + virtual void read(const ros::Time& time, const ros::Duration& period); + virtual void write(const ros::Time& time, const ros::Duration& period); + virtual bool prepareSwitch(const std::list& start_list, + const std::list& stop_list); + virtual void doSwitch(const std::list& start_list, + const std::list& stop_list); + +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/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_2.h b/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_2.h new file mode 100644 index 000000000..1edc9946e --- /dev/null +++ b/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_2.h @@ -0,0 +1,70 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2015, Shadow Robot Company Ltd. +// +// 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_MY_ROBOT_HW_2_H +#define CONTROLLER_MANAGER_TESTS_MY_ROBOT_HW_2_H + +#include +#include +#include + +namespace combined_robot_hw_tests +{ + +class MyRobotHW2 : public hardware_interface::RobotHW +{ +public: + MyRobotHW2(); + virtual ~MyRobotHW2(){}; + virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh); + void read(const ros::Time& time, const ros::Duration& period); + void write(const ros::Time& time, const ros::Duration& period); + virtual bool prepareSwitch(const std::list& start_list, + const std::list& stop_list); + virtual void doSwitch(const std::list& start_list, + const std::list& stop_list); + +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/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_3.h b/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_3.h new file mode 100644 index 000000000..bead60306 --- /dev/null +++ b/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_3.h @@ -0,0 +1,66 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2015, Shadow Robot Company Ltd. +// +// 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_MY_ROBOT_HW_3_H +#define CONTROLLER_MANAGER_TESTS_MY_ROBOT_HW_3_H + +#include +#include +#include + +namespace combined_robot_hw_tests +{ + +class MyRobotHW3 : public hardware_interface::RobotHW +{ +public: + MyRobotHW3(); + virtual ~MyRobotHW3(){}; + virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh); + void read(const ros::Time& time, const ros::Duration& period); + void write(const ros::Time& time, const ros::Duration& period); + +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/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_4.h b/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_4.h new file mode 100644 index 000000000..12076bd43 --- /dev/null +++ b/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_4.h @@ -0,0 +1,67 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2015, Shadow Robot Company Ltd. +// +// 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_MY_ROBOT_HW_4_H +#define CONTROLLER_MANAGER_TESTS_MY_ROBOT_HW_4_H + +#include +#include +#include + +namespace combined_robot_hw_tests +{ + +class MyRobotHW4 : public hardware_interface::RobotHW +{ +public: + MyRobotHW4(); + virtual ~MyRobotHW4(){}; + virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh); + void read(const ros::Time& time, const ros::Duration& period); + void write(const ros::Time& time, const ros::Duration& period); + virtual bool prepareSwitch(const std::list& start_list, + const std::list& stop_list); + virtual void doSwitch(const std::list& start_list, + const std::list& stop_list); + +protected: + +private: + hardware_interface::ForceTorqueSensorInterface ft_sensor_interface_; + hardware_interface::HardwareInterface a_plain_hw_interface_; + + double force_[3]; + double torque_[3]; + std::string sensor_name_; + std::string frame_id_; +}; +} + + +#endif diff --git a/combined_robot_hw_tests/package.xml b/combined_robot_hw_tests/package.xml new file mode 100644 index 000000000..d9693aa52 --- /dev/null +++ b/combined_robot_hw_tests/package.xml @@ -0,0 +1,33 @@ + + + combined_robot_hw_tests + 0.0.0 + The combined_robot_hw_tests package + Toni Oliver + + BSD + + https://github.com/ros-controls/ros_control/wiki + https://github.com/ros-controls/ros_control/issues + https://github.com/ros-controls/ros_control + + Toni Oliver + + catkin + combined_robot_hw + controller_manager + controller_manager_tests + hardware_interface + roscpp + rostest + combined_robot_hw + controller_manager + controller_manager_tests + hardware_interface + roscpp + rostest + + + + + \ No newline at end of file diff --git a/combined_robot_hw_tests/src/dummy_app.cpp b/combined_robot_hw_tests/src/dummy_app.cpp new file mode 100644 index 000000000..cb0e613dc --- /dev/null +++ b/combined_robot_hw_tests/src/dummy_app.cpp @@ -0,0 +1,55 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2012, Shadow Robot Company Ltd. +// +// 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 Shadow Robot Company Ltd. 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 + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "DummyApp"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + ros::NodeHandle nh; + combined_robot_hw::CombinedRobotHW hw; + bool init_success = hw.init(nh, nh); + + controller_manager::ControllerManager cm(&hw, nh); + + ros::Duration period(1.0); + while (ros::ok()) + { + ROS_INFO("loop"); + hw.read(ros::Time::now(), period); + cm.update(ros::Time::now(), period); + hw.write(ros::Time::now(), period); + period.sleep(); + } +} + diff --git a/combined_robot_hw_tests/src/my_robot_hw_1.cpp b/combined_robot_hw_tests/src/my_robot_hw_1.cpp new file mode 100644 index 000000000..5b4dd262e --- /dev/null +++ b/combined_robot_hw_tests/src/my_robot_hw_1.cpp @@ -0,0 +1,175 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2015, Shadow Robot Company Ltd. +// +// 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 Shadow Robot Company Ltd. 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 + +namespace combined_robot_hw_tests +{ + +MyRobotHW1::MyRobotHW1() +{ +} + +bool MyRobotHW1::init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh) +{ + using namespace hardware_interface; + + // Initialize raw data + joint_position_.resize(3); + joint_velocity_.resize(3); + joint_effort_.resize(3); + joint_effort_command_.resize(3); + joint_velocity_command_.resize(3); + joint_name_.resize(3); + + joint_name_[0] = "test_joint1"; + joint_position_[0] = 1.0; + joint_velocity_[0] = 0.0; + joint_effort_[0] = 0.1; + joint_effort_command_[0] = 3.0; + joint_velocity_command_[0] = 0.0; + + joint_name_[1] = "test_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; + + joint_name_[2] = "test_joint3"; + joint_position_[2] = 1.0; + joint_velocity_[2] = 0.0; + joint_effort_[2] = 0.1; + joint_effort_command_[2] = 0.0; + joint_velocity_command_[2] = 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])); + js_interface_.registerHandle(JointStateHandle(joint_name_[2], &joint_position_[2], &joint_velocity_[2], &joint_effort_[2])); + + 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])); + ej_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[2]), &joint_effort_command_[2])); + + 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])); + vj_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[2]), &joint_velocity_command_[2])); + + registerInterface(&js_interface_); + registerInterface(&ej_interface_); + registerInterface(&vj_interface_); + + return true; +} + + +void MyRobotHW1::read(const ros::Time& time, const ros::Duration& period) +{ + joint_position_[0] = 2.7; +} + +void MyRobotHW1::write(const ros::Time& time, const ros::Duration& period) +{ + // Just to test that write() is called + joint_effort_command_[1] = joint_effort_command_[0]; +} + +bool MyRobotHW1::prepareSwitch(const std::list& start_list, + const std::list& stop_list) +{ + for (std::list::const_iterator it = start_list.begin(); it != start_list.end(); ++it) + { + if (it->claimed_resources.empty()) + { + continue; + } + for (std::vector::const_iterator res_it = it->claimed_resources.begin(); res_it != it->claimed_resources.end(); ++res_it) + { + std::vector r_hw_ifaces = this->getNames(); + + std::vector::iterator if_name = std::find(r_hw_ifaces.begin(), r_hw_ifaces.end(), res_it->hardware_interface); + if (if_name == r_hw_ifaces.end()) // this hardware_interface is not registered on this RobotHW + { + ROS_ERROR_STREAM("Bad interface: " << res_it->hardware_interface); + std::cout << res_it->hardware_interface; + return false; + } + + std::vector r_hw_iface_resources = this->getInterfaceResources(res_it->hardware_interface); + for (std::set::const_iterator ctrl_res = res_it->resources.begin(); ctrl_res != res_it->resources.end(); ++ctrl_res) + { + std::vector::iterator res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), *ctrl_res); + if (res_name == r_hw_iface_resources.end()) // this resource is not registered on this RobotHW + { + ROS_ERROR_STREAM("Bad resource: " << (*ctrl_res)); + std::cout << (*ctrl_res); + return false; + } + } + } + } + return true; +} + +void MyRobotHW1::doSwitch(const std::list& start_list, + const std::list& stop_list) +{ + for (std::list::const_iterator it = start_list.begin(); it != start_list.end(); ++it) + { + if (it->claimed_resources.empty()) + { + continue; + } + for (std::vector::const_iterator res_it = it->claimed_resources.begin(); res_it != it->claimed_resources.end(); ++res_it) + { + std::vector r_hw_ifaces = this->getNames(); + + std::vector::iterator if_name = std::find(r_hw_ifaces.begin(), r_hw_ifaces.end(), res_it->hardware_interface); + if (if_name == r_hw_ifaces.end()) // this hardware_interface is not registered on this RobotHW + { + throw hardware_interface::HardwareInterfaceException("Hardware_interface " + res_it->hardware_interface + " is not registered"); + } + + std::vector r_hw_iface_resources = this->getInterfaceResources(res_it->hardware_interface); + for (std::set::const_iterator ctrl_res = res_it->resources.begin(); ctrl_res != res_it->resources.end(); ++ctrl_res) + { + std::vector::iterator res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), *ctrl_res); + if (res_name == r_hw_iface_resources.end()) // this resource is not registered on this RobotHW + { + throw hardware_interface::HardwareInterfaceException("Resource " + *ctrl_res + " is not registered"); + } + } + } + } +} + +} + +PLUGINLIB_EXPORT_CLASS( combined_robot_hw_tests::MyRobotHW1, hardware_interface::RobotHW) diff --git a/combined_robot_hw_tests/src/my_robot_hw_2.cpp b/combined_robot_hw_tests/src/my_robot_hw_2.cpp new file mode 100644 index 000000000..43407f43e --- /dev/null +++ b/combined_robot_hw_tests/src/my_robot_hw_2.cpp @@ -0,0 +1,155 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2015, Shadow Robot Company Ltd. +// +// 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 Shadow Robot Company Ltd. 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 + +namespace combined_robot_hw_tests +{ + +MyRobotHW2::MyRobotHW2() +{ +} + +bool MyRobotHW2::init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh) +{ + using namespace hardware_interface; + + std::vector joints; + if (!robot_hw_nh.getParam("joints", joints)) {return false;} + + // Initialize raw data + size_t nb_joints = joints.size(); + joint_position_.resize(nb_joints); + joint_velocity_.resize(nb_joints); + joint_effort_.resize(nb_joints); + joint_effort_command_.resize(nb_joints); + joint_velocity_command_.resize(nb_joints); + joint_name_.resize(nb_joints); + + for (size_t i = 0; i < nb_joints; i++) + { + joint_name_[i] = joints[i]; + joint_position_[i] = 1.0; + joint_velocity_[i] = 0.0; + joint_effort_[i] = 0.1; + joint_effort_command_[i] = 0.0; + joint_velocity_command_[i] = 0.0; + // Populate hardware interfaces + js_interface_.registerHandle(JointStateHandle(joint_name_[i], &joint_position_[i], &joint_velocity_[i], &joint_effort_[i])); + ej_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[i]), &joint_effort_command_[i])); + vj_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[i]), &joint_velocity_command_[i])); + } + + registerInterface(&js_interface_); + registerInterface(&ej_interface_); + registerInterface(&vj_interface_); + + return true; +} + + +void MyRobotHW2::read(const ros::Time& time, const ros::Duration& period) +{ + +} + +void MyRobotHW2::write(const ros::Time& time, const ros::Duration& period) +{ +} + +bool MyRobotHW2::prepareSwitch(const std::list& start_list, + const std::list& stop_list) +{ + for (std::list::const_iterator it = start_list.begin(); it != start_list.end(); ++it) + { + if (it->claimed_resources.empty()) + { + continue; + } + for (std::vector::const_iterator res_it = it->claimed_resources.begin(); res_it != it->claimed_resources.end(); ++res_it) + { + std::vector r_hw_ifaces = this->getNames(); + + std::vector::iterator if_name = std::find(r_hw_ifaces.begin(), r_hw_ifaces.end(), res_it->hardware_interface); + if (if_name == r_hw_ifaces.end()) // this hardware_interface is not registered on this RobotHW + { + ROS_ERROR_STREAM("Bad interface: " << res_it->hardware_interface); + return false; + } + + std::vector r_hw_iface_resources = this->getInterfaceResources(res_it->hardware_interface); + for (std::set::const_iterator ctrl_res = res_it->resources.begin(); ctrl_res != res_it->resources.end(); ++ctrl_res) + { + std::vector::iterator res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), *ctrl_res); + if (res_name == r_hw_iface_resources.end()) // this resource is not registered on this RobotHW + { + ROS_ERROR_STREAM("Bad resource: " << (*ctrl_res)); + return false; + } + } + } + } + return true; +} + +void MyRobotHW2::doSwitch(const std::list& start_list, + const std::list& stop_list) +{ + for (std::list::const_iterator it = start_list.begin(); it != start_list.end(); ++it) + { + if (it->claimed_resources.empty()) + { + continue; + } + for (std::vector::const_iterator res_it = it->claimed_resources.begin(); res_it != it->claimed_resources.end(); ++res_it) + { + std::vector r_hw_ifaces = this->getNames(); + + std::vector::iterator if_name = std::find(r_hw_ifaces.begin(), r_hw_ifaces.end(), res_it->hardware_interface); + if (if_name == r_hw_ifaces.end()) // this hardware_interface is not registered on this RobotHW + { + throw hardware_interface::HardwareInterfaceException("Hardware_interface " + res_it->hardware_interface + " is not registered"); + } + + std::vector r_hw_iface_resources = this->getInterfaceResources(res_it->hardware_interface); + for (std::set::const_iterator ctrl_res = res_it->resources.begin(); ctrl_res != res_it->resources.end(); ++ctrl_res) + { + std::vector::iterator res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), *ctrl_res); + if (res_name == r_hw_iface_resources.end()) // this resource is not registered on this RobotHW + { + throw hardware_interface::HardwareInterfaceException("Resource " + *ctrl_res + " is not registered"); + } + } + } + } +} + +} + +PLUGINLIB_EXPORT_CLASS( combined_robot_hw_tests::MyRobotHW2, hardware_interface::RobotHW) diff --git a/combined_robot_hw_tests/src/my_robot_hw_3.cpp b/combined_robot_hw_tests/src/my_robot_hw_3.cpp new file mode 100644 index 000000000..f18e626f1 --- /dev/null +++ b/combined_robot_hw_tests/src/my_robot_hw_3.cpp @@ -0,0 +1,94 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2015, Shadow Robot Company Ltd. +// +// 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 Shadow Robot Company Ltd. 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 + +namespace combined_robot_hw_tests +{ + +MyRobotHW3::MyRobotHW3() +{ +} + +bool MyRobotHW3::init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh) +{ + using namespace hardware_interface; + + std::string robot_description; + if (!robot_hw_nh.getParam("robot_description", robot_description)) {return false;} + + // E.g. Read URDF and parse joint names. + // Then read joint_name_filter param to know what joints belong to this RobotHW + + std::vector joints; + joints.push_back("right_arm_joint_1"); + + // Initialize raw data + size_t nb_joints = joints.size(); + joint_position_.resize(nb_joints); + joint_velocity_.resize(nb_joints); + joint_effort_.resize(nb_joints); + joint_effort_command_.resize(nb_joints); + joint_velocity_command_.resize(nb_joints); + joint_name_.resize(nb_joints); + + for (size_t i = 0; i < nb_joints; i++) + { + joint_name_[i] = joints[i]; + joint_position_[i] = 1.0; + joint_velocity_[i] = 0.0; + joint_effort_[i] = 0.1; + joint_effort_command_[i] = 1.5; + joint_velocity_command_[i] = 0.0; + // Populate hardware interfaces + js_interface_.registerHandle(JointStateHandle(joint_name_[i], &joint_position_[i], &joint_velocity_[i], &joint_effort_[i])); + ej_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[i]), &joint_effort_command_[i])); + vj_interface_.registerHandle(JointHandle(js_interface_.getHandle(joint_name_[i]), &joint_velocity_command_[i])); + } + + registerInterface(&js_interface_); + registerInterface(&ej_interface_); + registerInterface(&vj_interface_); + + return true; +} + + +void MyRobotHW3::read(const ros::Time& time, const ros::Duration& period) +{ + +} + +void MyRobotHW3::write(const ros::Time& time, const ros::Duration& period) +{ +} + +} + +PLUGINLIB_EXPORT_CLASS( combined_robot_hw_tests::MyRobotHW3, hardware_interface::RobotHW) + diff --git a/combined_robot_hw_tests/src/my_robot_hw_4.cpp b/combined_robot_hw_tests/src/my_robot_hw_4.cpp new file mode 100644 index 000000000..2dc74687b --- /dev/null +++ b/combined_robot_hw_tests/src/my_robot_hw_4.cpp @@ -0,0 +1,113 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2015, Shadow Robot Company Ltd. +// +// 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 Shadow Robot Company Ltd. 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 + +namespace combined_robot_hw_tests +{ + +MyRobotHW4::MyRobotHW4() +{ +} + +bool MyRobotHW4::init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh) +{ + using namespace hardware_interface; + + force_[0] = 0.0; + force_[1] = 0.1; + force_[2] = 0.2; + torque_[0] = 0.0; + torque_[1] = 0.1; + torque_[2] = 0.2; + sensor_name_ = "ft_sensor_1"; + frame_id_ = "link_1"; + + ft_sensor_interface_.registerHandle(ForceTorqueSensorHandle(sensor_name_, frame_id_, force_, torque_)); + + registerInterface(&ft_sensor_interface_); + registerInterface(&a_plain_hw_interface_); + + return true; +} + + +void MyRobotHW4::read(const ros::Time& time, const ros::Duration& period) +{ + force_[2] = 1.2; +} + +void MyRobotHW4::write(const ros::Time& time, const ros::Duration& period) +{ +} + +bool MyRobotHW4::prepareSwitch(const std::list& start_list, + const std::list& stop_list) +{ + // To easily test a failure case, any controller that claims resources on MyRobotHW4 will fail + if (!start_list.empty() || !stop_list.empty()) + { + for (std::list::const_iterator it = start_list.begin(); it != start_list.end(); ++it) + { + if (it->claimed_resources.empty()) + { + continue; + } + else + { + return false; + } + } + } + return true; +} + +void MyRobotHW4::doSwitch(const std::list& start_list, + const std::list& stop_list) +{ + // To easily test a failure case, any controller that claims resources on MyRobotHW4 will fail + if (!start_list.empty() || !stop_list.empty()) + { + for (std::list::const_iterator it = start_list.begin(); it != start_list.end(); ++it) + { + if (it->claimed_resources.empty()) + { + continue; + } + else + { + throw hardware_interface::HardwareInterfaceException("MyRobotHW4 can't switch controllers"); + } + } + } +} + +} + +PLUGINLIB_EXPORT_CLASS( combined_robot_hw_tests::MyRobotHW4, hardware_interface::RobotHW) + diff --git a/combined_robot_hw_tests/test/cm_test.cpp b/combined_robot_hw_tests/test/cm_test.cpp new file mode 100644 index 000000000..025854d4d --- /dev/null +++ b/combined_robot_hw_tests/test/cm_test.cpp @@ -0,0 +1,521 @@ +/////////////////////////////////////////////////////////////////////////////// +// 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 Adolfo Rodríguez Tsouroukdissian +/// \author Vijay Pradeep +/// \author Toni Oliver + +#include +#include + +#include +#include +#include +#include +#include + + +using namespace controller_manager_msgs; + +TEST(CMTests, loadUnloadOk) +{ + ros::NodeHandle nh; + ros::ServiceClient load_client = nh.serviceClient("/controller_manager/load_controller"); + ros::ServiceClient unload_client = nh.serviceClient("/controller_manager/unload_controller"); + + // Load single-interface controller + { + LoadController srv; + srv.request.name = "my_controller"; + bool call_success = load_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Load multi-interface controller: + // Two required interfaces + { + LoadController srv; + srv.request.name = "vel_eff_controller"; + bool call_success = load_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Load multi-interface controller: + // One required (and existing) interface and one optional (and non-existent) interface + { + LoadController srv; + srv.request.name = "optional_interfaces_controller"; + bool call_success = load_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Unload single-interface controller + { + UnloadController srv; + srv.request.name = "my_controller"; + bool call_success = unload_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Unload multi-interface controllers + { + UnloadController srv; + srv.request.name = "vel_eff_controller"; + bool call_success = unload_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + { + UnloadController srv; + srv.request.name = "optional_interfaces_controller"; + bool call_success = unload_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } +} + +TEST(CMTests, loadUnloadKo) +{ + ros::NodeHandle nh; + ros::ServiceClient load_client = nh.serviceClient("/controller_manager/load_controller"); + ros::ServiceClient unload_client = nh.serviceClient("/controller_manager/unload_controller"); + + // Load non-existent controller + { + LoadController srv; + srv.request.name = "nonexistent_controller"; + bool call_success = load_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } + + // Load controller requesting non-existient HW interface + { + LoadController srv; + srv.request.name = "non_existent_interface_controller"; + bool call_success = load_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } + + // Load controller requesting non-existent resource from valid HW interface + { + LoadController srv; + srv.request.name = "non_existent_resource_controller"; + bool call_success = load_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } + + // Load multi-interface controller: + // Two required HW interfaces, of which one is non-existent + { + LoadController srv; + srv.request.name = "non_existing_multi_interface_controller"; + bool call_success = load_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } + + // Unload not loaded controller + { + UnloadController srv; + srv.request.name = "my_controller"; + bool call_success = unload_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } +} + +TEST(CMTests, switchController) +{ + ros::NodeHandle nh; + ros::ServiceClient load_client = nh.serviceClient("/controller_manager/load_controller"); + ros::ServiceClient unload_client = nh.serviceClient("/controller_manager/unload_controller"); + ros::ServiceClient switch_client = nh.serviceClient("/controller_manager/switch_controller"); + + // Load controllers + { + LoadController srv; + srv.request.name = "my_controller"; + load_client.call(srv); + srv.request.name = "my_controller2"; + load_client.call(srv); + srv.request.name = "vel_eff_controller"; + load_client.call(srv); + srv.request.name = "self_conflict_controller"; + load_client.call(srv); + } + + // Successful STRICT start + { + SwitchController srv; + srv.request.start_controllers.push_back("my_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Successful STRICT stop + { + SwitchController srv; + srv.request.stop_controllers.push_back("my_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Successful STRICT start + { + SwitchController srv; + srv.request.start_controllers.push_back("vel_eff_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Successful STRICT stop+start + { + SwitchController srv; + srv.request.start_controllers.push_back("my_controller"); + srv.request.stop_controllers.push_back("vel_eff_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Back to no running controllers + { + SwitchController srv; + srv.request.stop_controllers.push_back("my_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Unsuccessful STRICT start + { + SwitchController srv; + srv.request.start_controllers.push_back("non_existent_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } + + // Unsuccessful STRICT stop + { + SwitchController srv; + srv.request.stop_controllers.push_back("non_existent_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } + + // Unsuccessful STRICT switch: invalid stop + { + SwitchController srv; + srv.request.start_controllers.push_back("my_controller"); + srv.request.stop_controllers.push_back("non_existent_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } + + // Unsuccessful STRICT switch: invalid start + { + SwitchController srv; + srv.request.start_controllers.push_back("non_existent_controller"); + srv.request.stop_controllers.push_back("my_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } + + // Unsuccessful STRICT start: Resource conflict within single controller + { + SwitchController srv; + srv.request.start_controllers.push_back("self_conflict_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } + + // Unsuccessful STRICT start: Resource conflict between two controllers + { + SwitchController srv; + srv.request.start_controllers.push_back("my_controller"); + srv.request.start_controllers.push_back("my_controller2"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } + + // Successful BEST_EFFORT switch: No-op + { + SwitchController srv; + srv.request.start_controllers.push_back("non_existent_controller"); + srv.request.stop_controllers.push_back("non_existent_controller"); + srv.request.strictness = srv.request.BEST_EFFORT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Successful BEST_EFFORT switch: Partial success, only one started controller + { + SwitchController srv; + srv.request.start_controllers.push_back("my_controller2"); + srv.request.start_controllers.push_back("non_existent_controller"); + srv.request.stop_controllers.push_back("non_existent_controller"); + srv.request.stop_controllers.push_back("my_controller"); + srv.request.strictness = srv.request.BEST_EFFORT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Successful BEST_EFFORT switch: Partial success, one started and one stopped controller + { + SwitchController srv; + srv.request.start_controllers.push_back("my_controller"); + srv.request.start_controllers.push_back("non_existent_controller"); + srv.request.stop_controllers.push_back("non_existent_controller"); + srv.request.stop_controllers.push_back("my_controller2"); + srv.request.strictness = srv.request.BEST_EFFORT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Back to no running controllers + { + SwitchController srv; + srv.request.stop_controllers.push_back("my_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Unload controllers + { + UnloadController srv; + srv.request.name = "my_controller"; + unload_client.call(srv); + srv.request.name = "my_controller2"; + unload_client.call(srv); + srv.request.name = "vel_eff_controller"; + unload_client.call(srv); + srv.request.name = "self_conflict_controller"; + unload_client.call(srv); + } +} + +TEST(CMTests, stopBeforeUnload) +{ + ros::NodeHandle nh; + ros::ServiceClient load_client = nh.serviceClient("/controller_manager/load_controller"); + ros::ServiceClient unload_client = nh.serviceClient("/controller_manager/unload_controller"); + ros::ServiceClient switch_client = nh.serviceClient("/controller_manager/switch_controller"); + + // Load controller + { + LoadController srv; + srv.request.name = "my_controller"; + bool call_success = load_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Start controller + { + SwitchController srv; + srv.request.start_controllers.push_back("my_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Try to unload running controller and fail + { + UnloadController srv; + srv.request.name = "my_controller"; + bool call_success = unload_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_FALSE(srv.response.ok); + } + + // Stop controller + { + SwitchController srv; + srv.request.stop_controllers.push_back("my_controller"); + srv.request.strictness = srv.request.STRICT; + bool call_success = switch_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } + + // Unload stopped controller + { + UnloadController srv; + srv.request.name = "my_controller"; + bool call_success = unload_client.call(srv); + ASSERT_TRUE(call_success); + EXPECT_TRUE(srv.response.ok); + } +} + +TEST(CMTests, listControllerTypes) +{ + ros::NodeHandle nh; + ros::ServiceClient types_client = nh.serviceClient("/controller_manager/list_controller_types"); + + ListControllerTypes srv; + bool call_success = types_client.call(srv); + ASSERT_TRUE(call_success); + // Weak test that the number of available types and base classes is not lower than those defined in this test package + EXPECT_GE(srv.response.types.size(), 3); + EXPECT_GE(srv.response.base_classes.size(), 3); +} + +TEST(CMTests, listControllers) +{ + ros::NodeHandle nh; + ros::ServiceClient load_client = nh.serviceClient("/controller_manager/load_controller"); + ros::ServiceClient unload_client = nh.serviceClient("/controller_manager/unload_controller"); + ros::ServiceClient switch_client = nh.serviceClient("/controller_manager/switch_controller"); + ros::ServiceClient list_client = nh.serviceClient("/controller_manager/list_controllers"); + + // Load controllers + { + LoadController srv; + srv.request.name = "my_controller"; + load_client.call(srv); + srv.request.name = "vel_eff_controller"; + load_client.call(srv); + } + + // Start one controller + { + SwitchController srv; + srv.request.start_controllers.push_back("my_controller"); + srv.request.strictness = srv.request.STRICT; + switch_client.call(srv); + } + + // List controllers + { + ListControllers srv; + bool call_success = list_client.call(srv); + ASSERT_TRUE(call_success); + ASSERT_EQ(srv.response.controller.size(), 2); + + ControllerState state1, state2; + if (srv.response.controller[0].name == "my_controller") + { + state1 = srv.response.controller[0]; + state2 = srv.response.controller[1]; + } + else + { + state1 = srv.response.controller[1]; + state2 = srv.response.controller[0]; + } + + EXPECT_EQ(state1.name, "my_controller"); + EXPECT_EQ(state1.state, "running"); + EXPECT_EQ(state1.type, "controller_manager_tests/EffortTestController"); + ASSERT_EQ(state1.claimed_resources.size(), 1); + EXPECT_EQ(state1.claimed_resources[0].hardware_interface, "hardware_interface::EffortJointInterface"); + ASSERT_EQ(state1.claimed_resources[0].resources.size(), 2); + EXPECT_EQ(state1.claimed_resources[0].resources[0], "hiDOF_joint1"); + EXPECT_EQ(state1.claimed_resources[0].resources[1], "hiDOF_joint2"); + + EXPECT_EQ(state2.name, "vel_eff_controller"); + EXPECT_EQ(state2.state, "stopped"); + EXPECT_EQ(state2.type, "controller_manager_tests/VelEffController"); + EXPECT_EQ(state2.claimed_resources.size(), 2); + EXPECT_EQ(state2.claimed_resources[0].hardware_interface, "hardware_interface::VelocityJointInterface"); + ASSERT_EQ(state2.claimed_resources[0].resources.size(), 2); + EXPECT_EQ(state2.claimed_resources[0].resources[0], "test_joint1"); + EXPECT_EQ(state2.claimed_resources[0].resources[1], "test_joint2"); + EXPECT_EQ(state2.claimed_resources[1].hardware_interface, "hardware_interface::EffortJointInterface"); + ASSERT_EQ(state2.claimed_resources[1].resources.size(), 1); + EXPECT_EQ(state2.claimed_resources[1].resources[0], "test_joint4"); + } + + // Stop running controller + { + SwitchController srv; + srv.request.stop_controllers.push_back("my_controller"); + srv.request.strictness = srv.request.STRICT; + switch_client.call(srv); + } + + // Unload controllers + { + UnloadController srv; + srv.request.name = "my_controller"; + unload_client.call(srv); + srv.request.name = "vel_eff_controller"; + unload_client.call(srv); + } +} + +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(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/combined_robot_hw_tests/test/cm_test.test b/combined_robot_hw_tests/test/cm_test.test new file mode 100644 index 000000000..f36390ae5 --- /dev/null +++ b/combined_robot_hw_tests/test/cm_test.test @@ -0,0 +1,68 @@ + + + robot_hardware: + - my_robot_hw_1 + - my_robot_hw_2 + - my_robot_hw_3 + - my_robot_hw_4 + my_robot_hw_1: + type: combined_robot_hw_tests/MyRobotHW1 + my_robot_hw_2: + type: combined_robot_hw_tests/MyRobotHW2 + joints: + - test_joint4 + - test_joint5 + - hiDOF_joint1 + - hiDOF_joint2 + my_robot_hw_3: + type: combined_robot_hw_tests/MyRobotHW3 + robot_description: robot_description + joint_name_filter: right_arm + my_robot_hw_4: + type: combined_robot_hw_tests/MyRobotHW4 + my_controller: + type: controller_manager_tests/EffortTestController + my_controller2: + type: controller_manager_tests/EffortTestController + non_existent_interface_controller: + type: controller_manager_tests/MyDummyController + vel_eff_controller: + type: controller_manager_tests/VelEffController + velocity_joints: + - test_joint1 + - test_joint2 + effort_joints: + - test_joint4 + non_existent_resource_controller: + type: controller_manager_tests/VelEffController + velocity_joints: + - test_joint1 + - test_joint2 + effort_joints: + - nonexistent_resource + self_conflict_controller: + type: controller_manager_tests/VelEffController + velocity_joints: + - test_joint1 + - test_joint2 + effort_joints: + - test_joint1 + optional_interfaces_controller: + type: controller_manager_tests/PosEffOptController + position_joints: + - test_joint1 + - test_joint2 + effort_joints: + - test_joint1 + non_existing_multi_interface_controller: + type: controller_manager_tests/PosEffController + position_joints: + - test_joint1 + - test_joint2 + effort_joints: + - test_joint1 + + + + + diff --git a/combined_robot_hw_tests/test/combined_robot_hw_test.cpp b/combined_robot_hw_tests/test/combined_robot_hw_test.cpp new file mode 100644 index 000000000..21e02e963 --- /dev/null +++ b/combined_robot_hw_tests/test/combined_robot_hw_test.cpp @@ -0,0 +1,207 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2015, Shadow Robot Company Ltd. +// +// 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 Shadow Robot Company Ltd. 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 +#include +#include +#include + +using combined_robot_hw::CombinedRobotHW; + +TEST(CombinedRobotHWTests, combinationOk) +{ + ros::NodeHandle nh; + + CombinedRobotHW robot_hw; + bool init_success = robot_hw.init(nh, nh); + ASSERT_TRUE(init_success); + + hardware_interface::JointStateInterface* js_interface = robot_hw.get(); + hardware_interface::EffortJointInterface* ej_interface = robot_hw.get(); + hardware_interface::VelocityJointInterface* vj_interface = robot_hw.get(); + hardware_interface::ForceTorqueSensorInterface* ft_interface = robot_hw.get(); + hardware_interface::HardwareInterface* plain_hw_interface = robot_hw.get(); + hardware_interface::PositionJointInterface* pj_interface = robot_hw.get(); + + ASSERT_TRUE(js_interface != NULL); + ASSERT_TRUE(ej_interface != NULL); + ASSERT_TRUE(vj_interface != NULL); + ASSERT_TRUE(ft_interface != NULL); + ASSERT_TRUE(plain_hw_interface != NULL); + + // Test that no PositionJointInterface was found + ASSERT_EQ(NULL, pj_interface); + + // Test some handles from my_robot_hw_1 + hardware_interface::JointStateHandle js_handle = js_interface->getHandle("test_joint1"); + hardware_interface::JointHandle ej_handle = ej_interface->getHandle("test_joint1"); + hardware_interface::JointHandle vj_handle = vj_interface->getHandle("test_joint1"); + ASSERT_FLOAT_EQ(1.0, js_handle.getPosition()); + ASSERT_FLOAT_EQ(0.0, ej_handle.getVelocity()); + ASSERT_FLOAT_EQ(3.0, ej_handle.getCommand()); + + // Test some handles from my_robot_hw_3 + js_handle = js_interface->getHandle("right_arm_joint_1"); + ej_handle = ej_interface->getHandle("right_arm_joint_1"); + vj_handle = vj_interface->getHandle("right_arm_joint_1"); + ASSERT_FLOAT_EQ(1.0, js_handle.getPosition()); + ASSERT_FLOAT_EQ(0.0, ej_handle.getVelocity()); + ASSERT_FLOAT_EQ(1.5, ej_handle.getCommand()); + + // Test some handles from my_robot_hw_4 + hardware_interface::ForceTorqueSensorHandle ft_handle = ft_interface->getHandle("ft_sensor_1"); + ASSERT_FLOAT_EQ(0.2, ft_handle.getForce()[2]); + + // Test non-existent handle throws exception + ASSERT_ANY_THROW(ej_interface->getHandle("non_existent_joint")); + + // Test read and write functions + ros::Duration period(1.0); + robot_hw.read(ros::Time::now(), period); + js_handle = js_interface->getHandle("test_joint1"); + ASSERT_FLOAT_EQ(2.7, js_handle.getPosition()); + ASSERT_FLOAT_EQ(1.2, ft_handle.getForce()[2]); + + ej_handle = ej_interface->getHandle("test_joint1"); + ej_handle.setCommand(3.5); + robot_hw.write(ros::Time::now(), period); + ej_handle = ej_interface->getHandle("test_joint2"); + ASSERT_FLOAT_EQ(3.5, ej_handle.getCommand()); +} + +TEST(CombinedRobotHWTests, switchOk) +{ + ros::NodeHandle nh; + + CombinedRobotHW robot_hw; + bool init_success = robot_hw.init(nh, nh); + ASSERT_TRUE(init_success); + + // Test empty list (it is expected to work) + { + std::list start_list; + std::list stop_list; + ASSERT_TRUE(robot_hw.prepareSwitch(start_list, stop_list)); + ASSERT_NO_THROW(robot_hw.doSwitch(start_list, stop_list)); + } + + // Test failure + { + std::list start_list; + std::list stop_list; + hardware_interface::ControllerInfo controller_1; + controller_1.name = "ctrl_1"; + controller_1.type = "some_type"; + hardware_interface::InterfaceResources iface_res_1; + iface_res_1.hardware_interface = "hardware_interface::ForceTorqueSensorInterface"; + iface_res_1.resources.insert("ft_sensor_1"); + controller_1.claimed_resources.push_back(iface_res_1); + start_list.push_back(controller_1); + ASSERT_FALSE(robot_hw.prepareSwitch(start_list, stop_list)); + ASSERT_ANY_THROW(robot_hw.doSwitch(start_list, stop_list)); + } + + // Test existing interfaces and resources + { + std::list start_list; + std::list stop_list; + hardware_interface::ControllerInfo controller_1; + controller_1.name = "ctrl_1"; + controller_1.type = "some_type"; + hardware_interface::InterfaceResources iface_res_1; + iface_res_1.hardware_interface = "hardware_interface::EffortJointInterface"; + iface_res_1.resources.insert("test_joint1"); + iface_res_1.resources.insert("test_joint2"); + iface_res_1.resources.insert("test_joint3"); + iface_res_1.resources.insert("test_joint4"); + controller_1.claimed_resources.push_back(iface_res_1); + hardware_interface::InterfaceResources iface_res_2; + iface_res_2.hardware_interface = "hardware_interface::VelocityJointInterface"; + iface_res_2.resources.insert("test_joint1"); + iface_res_2.resources.insert("test_joint4"); + controller_1.claimed_resources.push_back(iface_res_1); + start_list.push_back(controller_1); + + hardware_interface::ControllerInfo controller_2; + hardware_interface::InterfaceResources iface_res_3; + iface_res_3.hardware_interface = "hardware_interface::VelocityJointInterface"; + iface_res_3.resources.insert("test_joint3"); + iface_res_3.resources.insert("test_joint5"); + controller_2.claimed_resources.push_back(iface_res_3); + start_list.push_back(controller_2); + ASSERT_TRUE(robot_hw.prepareSwitch(start_list, stop_list)); + ASSERT_NO_THROW(robot_hw.doSwitch(start_list, stop_list)); + } + + // Test non-registered interfaces and resources + // (this should also work, as CombinedRobotHW will filter out the non-registered ones) + { + std::list start_list; + std::list stop_list; + hardware_interface::ControllerInfo controller_1; + controller_1.name = "ctrl_1"; + controller_1.type = "some_type"; + hardware_interface::InterfaceResources iface_res_1; + iface_res_1.hardware_interface = "hardware_interface::NonRegisteredInterface"; + iface_res_1.resources.insert("test_joint1"); + iface_res_1.resources.insert("test_joint2"); + iface_res_1.resources.insert("test_joint3"); + iface_res_1.resources.insert("test_joint4"); + controller_1.claimed_resources.push_back(iface_res_1); + hardware_interface::InterfaceResources iface_res_2; + iface_res_2.hardware_interface = "hardware_interface::VelocityJointInterface"; + iface_res_2.resources.insert("test_joint1"); + iface_res_2.resources.insert("non_registered_joint1"); + controller_1.claimed_resources.push_back(iface_res_1); + start_list.push_back(controller_1); + + hardware_interface::ControllerInfo controller_2; + hardware_interface::InterfaceResources iface_res_3; + iface_res_3.hardware_interface = "hardware_interface::VelocityJointInterface"; + iface_res_3.resources.insert("test_joint3"); + iface_res_3.resources.insert("non_registered_joint2"); + controller_2.claimed_resources.push_back(iface_res_3); + start_list.push_back(controller_2); + ASSERT_TRUE(robot_hw.prepareSwitch(start_list, stop_list)); + ASSERT_NO_THROW(robot_hw.doSwitch(start_list, stop_list)); + } + +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "CombinedRobotHWTestNode"); + + int ret = RUN_ALL_TESTS(); + + ros::shutdown(); + return ret; +} diff --git a/combined_robot_hw_tests/test/combined_robot_hw_test.test b/combined_robot_hw_tests/test/combined_robot_hw_test.test new file mode 100644 index 000000000..b963f2548 --- /dev/null +++ b/combined_robot_hw_tests/test/combined_robot_hw_test.test @@ -0,0 +1,25 @@ + + + robot_hardware: + - my_robot_hw_1 + - my_robot_hw_2 + - my_robot_hw_3 + - my_robot_hw_4 + my_robot_hw_1: + type: combined_robot_hw_tests/MyRobotHW1 + my_robot_hw_2: + type: combined_robot_hw_tests/MyRobotHW2 + joints: + - test_joint4 + - test_joint5 + my_robot_hw_3: + type: combined_robot_hw_tests/MyRobotHW3 + robot_description: robot_description + joint_name_filter: right_arm + my_robot_hw_4: + type: combined_robot_hw_tests/MyRobotHW4 + + + + + \ No newline at end of file diff --git a/combined_robot_hw_tests/test_robot_hw_plugin.xml b/combined_robot_hw_tests/test_robot_hw_plugin.xml new file mode 100644 index 000000000..43384c87b --- /dev/null +++ b/combined_robot_hw_tests/test_robot_hw_plugin.xml @@ -0,0 +1,25 @@ + + + + A type of RobotHW + + + + + + A type of RobotHW + + + + + + A type of RobotHW + + + + + + A type of RobotHW + + + diff --git a/hardware_interface/include/hardware_interface/internal/interface_manager.h b/hardware_interface/include/hardware_interface/internal/interface_manager.h index 1c55c3452..392fb58c0 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, Kelsey P. Hawkins +/// \author Wim Meussen, Adolfo Rodriguez Tsouroukdissian, Kelsey P. Hawkins, Toni Oliver #ifndef HARDWARE_INTERFACE_INTERFACE_MANAGER_H #define HARDWARE_INTERFACE_INTERFACE_MANAGER_H @@ -79,6 +79,22 @@ struct CheckIsResourceManager { // calls ResourceManager::concatManagers if C is a ResourceManager static const void callConcatManagers(typename std::vector& managers, T* result) { callCM(managers, result, 0); } + + + // method called if C is a ResourceManager + template + static std::vector callGR(C* iface, typename C::resource_manager_type*) + { + return iface->getNames(); + } + + // method called if C is not a ResourceManager + template + static std::vector callGR(T* iface, ...) {} + + // calls ResourceManager::concatManagers if C is a ResourceManager + static std::vector callGetResources(T* iface) + { return callGR(iface, 0); } }; class InterfaceManager @@ -101,7 +117,15 @@ class InterfaceManager { ROS_WARN_STREAM("Replacing previously registered interface '" << iface_name << "'."); } - interfaces_[internal::demangledTypeName()] = iface; + interfaces_[iface_name] = iface; + + std::vector resources; + if(CheckIsResourceManager::value) + { + // it is a ResourceManager. Get the names of the resources + resources = CheckIsResourceManager::callGetResources(iface); + } + resources_[iface_name] = resources; } void registerInterfaceManager(InterfaceManager* iface_man) @@ -198,16 +222,37 @@ class InterfaceManager return out; } + /** + * \brief Get the resource names registered to an interface, specified by type + * (as this class only stores one interface per type) + * + * \param iface_type A string with the demangled type name of the interface + * \return A vector of resource names registered to this interface + */ + std::vector getInterfaceResources(std::string iface_type) const + { + std::vector out; + ResourceMap::const_iterator it = resources_.find(iface_type); + if(it != resources_.end()) + { + out = it->second; + } + return out; + } + protected: typedef std::map InterfaceMap; typedef std::vector InterfaceManagerVector; typedef std::map SizeMap; + typedef std::map > ResourceMap; InterfaceMap interfaces_; InterfaceMap interfaces_combo_; InterfaceManagerVector interface_managers_; SizeMap num_ifaces_registered_; boost::ptr_vector interface_destruction_list_; + /// This will allow us to check the resources based on the demangled type name of the interface + ResourceMap resources_; }; } // namespace diff --git a/hardware_interface/include/hardware_interface/robot_hw.h b/hardware_interface/include/hardware_interface/robot_hw.h index da6e30b01..277c812c1 100644 --- a/hardware_interface/include/hardware_interface/robot_hw.h +++ b/hardware_interface/include/hardware_interface/robot_hw.h @@ -36,6 +36,7 @@ #include #include #include +#include namespace hardware_interface { @@ -61,6 +62,23 @@ class RobotHW : public InterfaceManager } + virtual ~RobotHW() + { + + } + + /** \brief The init function is called to initialize the RobotHW from a + * non-realtime thread. + * + * \param root_nh A NodeHandle in the root of the caller namespace. + * + * \param robot_hw_nh A NodeHandle in the namespace from which the RobotHW + * should read its configuration. + * + * \returns True if initialization was successful + */ + virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh) {return true;} + /** \name Resource Management *\{*/ @@ -128,6 +146,22 @@ class RobotHW : public InterfaceManager */ virtual void doSwitch(const std::list& /*start_list*/, const std::list& /*stop_list*/) {} + + /** + * Reads data from the robot HW + * + * \param time The current time + * \param period The time passed since the last call to \ref read + */ + virtual void read(const ros::Time& time, const ros::Duration& period) {} + + /** + * Writes data to the robot HW + * + * \param time The current time + * \param period The time passed since the last call to \ref write + */ + virtual void write(const ros::Time& time, const ros::Duration& period) {} }; }