From 841123a54f2250a752e7661f4552e1947b334507 Mon Sep 17 00:00:00 2001 From: Kelsey Date: Tue, 19 Aug 2014 18:41:59 -0400 Subject: [PATCH] Making ControllerInfo::hardware_interface a set This replaces all instances of ControllerInfo::hardware_interface with std::set hardware_interfaces instead. This allows one to properly represent multi-controllers which use different types of interfaces. getHardwareInterfaceType is also converted to getHardwareInterfaceTypes. A legacy version of getHardwareInterfaceType is added which calls the other and returns a random interface. A warning is displayed if there is more than one interface available. The ControllerState message is also affected. --- .../include/controller_interface/controller.h | 8 ++- .../controller_interface/controller_base.h | 20 ++++++- .../controller_interface/multi_controller.h | 58 ++++++++++++------- controller_manager/src/controller_manager.cpp | 7 ++- .../controller_manager_interface.py | 5 +- .../msg/ControllerState.msg | 2 +- .../hardware_interface/controller_info.h | 3 +- hardware_interface/test/robot_hw_test.cpp | 6 +- 8 files changed, 76 insertions(+), 33 deletions(-) diff --git a/controller_interface/include/controller_interface/controller.h b/controller_interface/include/controller_interface/controller.h index aa299ee4b..0a3aee17a 100644 --- a/controller_interface/include/controller_interface/controller.h +++ b/controller_interface/include/controller_interface/controller.h @@ -111,7 +111,7 @@ class Controller: public ControllerBase { ROS_ERROR("This controller requires a hardware interface of type '%s'." " Make sure this is registered in the hardware_interface::RobotHW class.", - getHardwareInterfaceType().c_str()); + getHardwareInterfaceTypes().begin()->c_str()); return false; } @@ -130,9 +130,11 @@ class Controller: public ControllerBase return true; } - virtual std::string getHardwareInterfaceType() const + virtual std::set getHardwareInterfaceTypes() const { - return hardware_interface::internal::demangledTypeName(); + std::set ret_types; + ret_types.insert(hardware_interface::internal::demangledTypeName()); + return ret_types; } private: diff --git a/controller_interface/include/controller_interface/controller_base.h b/controller_interface/include/controller_interface/controller_base.h index 013e1d7f3..58c2011b7 100644 --- a/controller_interface/include/controller_interface/controller_base.h +++ b/controller_interface/include/controller_interface/controller_base.h @@ -122,8 +122,24 @@ class ControllerBase /** \name Non Real-Time Safe Functions *\{*/ - /// Get the name of this controller's hardware interface type - virtual std::string getHardwareInterfaceType() const = 0; + /// Get the names of the hardware interface types this controller uses + virtual std::set getHardwareInterfaceTypes() const = 0; + + /// Get the single name of the hardware interface this controller uses + virtual std::string getHardwareInterfaceType() + { + std::set types = getHardwareInterfaceTypes(); + + if(types.size() == 0) { + ROS_WARN("This controller has no interface type!"); + return std::string(""); + } + + if(types.size() > 1) + ROS_WARN("This controller has more than one interface type!" + "Use 'getHardwareInterfaceTypes' instead."); + return *(types.begin()); + } /** \brief Request that the controller be initialized * diff --git a/controller_interface/include/controller_interface/multi_controller.h b/controller_interface/include/controller_interface/multi_controller.h index 1d8f85cdf..dd9f124ce 100644 --- a/controller_interface/include/controller_interface/multi_controller.h +++ b/controller_interface/include/controller_interface/multi_controller.h @@ -116,9 +116,12 @@ class Controller2: public ControllerBase T2* hw2 = robot_hw->get(); if (!hw1 || !hw2) { - ROS_ERROR("This controller requires a hardware interface of type '%s'." - " Make sure this is registered in the hardware_interface::RobotHW class.", - getHardwareInterfaceType().c_str()); + std::set::iterator types_iter = getHardwareInterfaceTypes().begin(); + std::string type1 = *types_iter; types_iter++; + std::string type2 = *types_iter; + ROS_ERROR("This controller requires hardware interfaces of types '%s' and '%s'." + " Make sure they are registered in the hardware_interface::RobotHW class.", + type1.c_str(), type2.c_str()); return false; } @@ -141,10 +144,12 @@ class Controller2: public ControllerBase return true; } - virtual std::string getHardwareInterfaceType() const + virtual std::set getHardwareInterfaceTypes() const { - return "<" + hardware_interface::internal::demangledTypeName() + "," + - hardware_interface::internal::demangledTypeName() + ">"; + std::set ret_types; + ret_types.insert(hardware_interface::internal::demangledTypeName()); + ret_types.insert(hardware_interface::internal::demangledTypeName()); + return ret_types; } private: @@ -236,9 +241,13 @@ class Controller3: public ControllerBase T3* hw3 = robot_hw->get(); if (!hw1 || !hw2 || !hw3) { - ROS_ERROR("This controller requires a hardware interface of type '%s'." - " Make sure this is registered in the hardware_interface::RobotHW class.", - getHardwareInterfaceType().c_str()); + std::set::iterator types_iter = getHardwareInterfaceTypes().begin(); + std::string type1 = *types_iter; types_iter++; + std::string type2 = *types_iter; types_iter++; + std::string type3 = *types_iter; + ROS_ERROR("This controller requires hardware interfaces of types '%s', '%s', and '%s'." + " Make sure they are registered in the hardware_interface::RobotHW class.", + type1.c_str(), type2.c_str(), type3.c_str()); return false; } @@ -265,11 +274,13 @@ class Controller3: public ControllerBase return true; } - virtual std::string getHardwareInterfaceType() const + virtual std::set getHardwareInterfaceTypes() const { - return "<" + hardware_interface::internal::demangledTypeName() + "," + - hardware_interface::internal::demangledTypeName() + "," + - hardware_interface::internal::demangledTypeName() + ">"; + std::set ret_types; + ret_types.insert(hardware_interface::internal::demangledTypeName()); + ret_types.insert(hardware_interface::internal::demangledTypeName()); + ret_types.insert(hardware_interface::internal::demangledTypeName()); + return ret_types; } private: @@ -369,9 +380,14 @@ class Controller4: public ControllerBase T4* hw4 = robot_hw->get(); if (!hw1 || !hw2 || !hw3 || !hw4) { - ROS_ERROR("This controller requires a hardware interface of type '%s'." - " Make sure this is registered in the hardware_interface::RobotHW class.", - getHardwareInterfaceType().c_str()); + std::set::iterator types_iter = getHardwareInterfaceTypes().begin(); + std::string type1 = *types_iter; types_iter++; + std::string type2 = *types_iter; types_iter++; + std::string type3 = *types_iter; types_iter++; + std::string type4 = *types_iter; + ROS_ERROR("This controller requires hardware interfaces of types '%s', '%s', '%s', and '%s'." + " Make sure they are registered in the hardware_interface::RobotHW class.", + type1.c_str(), type2.c_str(), type3.c_str(), type4.c_str()); return false; } @@ -404,10 +420,12 @@ class Controller4: public ControllerBase virtual std::string getHardwareInterfaceType() const { - return "<" + hardware_interface::internal::demangledTypeName() + "," + - hardware_interface::internal::demangledTypeName() + "," + - hardware_interface::internal::demangledTypeName() + "," + - hardware_interface::internal::demangledTypeName() + ">"; + std::set ret_types; + ret_types.insert(hardware_interface::internal::demangledTypeName()); + ret_types.insert(hardware_interface::internal::demangledTypeName()); + ret_types.insert(hardware_interface::internal::demangledTypeName()); + ret_types.insert(hardware_interface::internal::demangledTypeName()); + return ret_types; } private: diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c685901f4..4fa8465cd 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -248,7 +248,7 @@ bool ControllerManager::loadController(const std::string& name) // Adds the controller to the new list to.resize(to.size() + 1); to[to.size()-1].info.type = type; - to[to.size()-1].info.hardware_interface = c->getHardwareInterfaceType(); + to[to.size()-1].info.hardware_interfaces = c->getHardwareInterfaceTypes(); to[to.size()-1].info.name = name; to[to.size()-1].info.resources = claimed_resources; to[to.size()-1].c = c; @@ -564,7 +564,10 @@ bool ControllerManager::listControllersSrv( controller_manager_msgs::ControllerState& cs = resp.controller[i]; cs.name = controllers[i].info.name; cs.type = controllers[i].info.type; - cs.hardware_interface = controllers[i].info.hardware_interface; + cs.hardware_interfaces.clear(); + cs.hardware_interfaces.reserve(controllers[i].info.hardware_interfaces.size()); + for (std::set::iterator it = controllers[i].info.hardware_interfaces.begin(); it != controllers[i].info.hardware_interfaces.end(); ++it) + cs.hardware_interfaces.push_back(*it); cs.resources.clear(); cs.resources.reserve(controllers[i].info.resources.size()); for (std::set::iterator it = controllers[i].info.resources.begin(); it != controllers[i].info.resources.end(); ++it) diff --git a/controller_manager/src/controller_manager/controller_manager_interface.py b/controller_manager/src/controller_manager/controller_manager_interface.py index 062fba0da..a4aa92ec1 100755 --- a/controller_manager/src/controller_manager/controller_manager_interface.py +++ b/controller_manager/src/controller_manager/controller_manager_interface.py @@ -57,7 +57,10 @@ def list_controllers(): print "No controllers are loaded in mechanism control" else: for c in resp.controller: - print '%s - %s ( %s )'%(c.name, c.hardware_interface, c.state) + if len(c.hardware_interfaces) == 1: + print '%s - %s ( %s )'%(c.name, c.hardware_interfaces[0], c.state) + else: + print '%s - [%s] ( %s )'%(c.name, ", ".join(c.hardware_interfaces), c.state) def load_controller(name): diff --git a/controller_manager_msgs/msg/ControllerState.msg b/controller_manager_msgs/msg/ControllerState.msg index 8c39b8885..9b8264b1b 100644 --- a/controller_manager_msgs/msg/ControllerState.msg +++ b/controller_manager_msgs/msg/ControllerState.msg @@ -1,5 +1,5 @@ string name string state string type -string hardware_interface +string[] hardware_interfaces string[] resources diff --git a/hardware_interface/include/hardware_interface/controller_info.h b/hardware_interface/include/hardware_interface/controller_info.h index 8f665a8e0..ece2772f4 100644 --- a/hardware_interface/include/hardware_interface/controller_info.h +++ b/hardware_interface/include/hardware_interface/controller_info.h @@ -42,7 +42,8 @@ namespace hardware_interface */ struct ControllerInfo { - std::string name, type, hardware_interface; + std::string name, type; + std::set hardware_interfaces; std::set resources; }; diff --git a/hardware_interface/test/robot_hw_test.cpp b/hardware_interface/test/robot_hw_test.cpp index 03b35f2b3..bbdb307c6 100644 --- a/hardware_interface/test/robot_hw_test.cpp +++ b/hardware_interface/test/robot_hw_test.cpp @@ -117,19 +117,19 @@ TEST_F(RobotHWTest, ConflictChecking) ControllerInfo info1; info1.name = "controller_1"; info1.type = "type_1"; - info1.hardware_interface = "interface_1"; + info1.hardware_interfaces.insert("interface_1"); info1.resources.insert("resource_1"); ControllerInfo info2; info2.name = "controller_2"; info2.type = "type_2"; - info2.hardware_interface = "interface_2"; + info2.hardware_interfaces.insert("interface_2"); info2.resources.insert("resource_2"); ControllerInfo info12; info12.name = "controller_12"; info12.type = "type_12"; - info12.hardware_interface = "interface_12"; + info12.hardware_interfaces.insert("interface_12"); info12.resources.insert("resource_1"); info12.resources.insert("resource_2");