From 0e356cda84b09eb958eed373ed2a82d68ce28aae Mon Sep 17 00:00:00 2001 From: Mike Purvis Date: Tue, 6 Feb 2018 14:20:49 -0500 Subject: [PATCH 1/5] Add CompositeController template. --- .../composite_controller.h | 228 ++++++++++++++++++ 1 file changed, 228 insertions(+) create mode 100644 controller_interface/include/controller_interface/composite_controller.h diff --git a/controller_interface/include/controller_interface/composite_controller.h b/controller_interface/include/controller_interface/composite_controller.h new file mode 100644 index 000000000..00bfb32d4 --- /dev/null +++ b/controller_interface/include/controller_interface/composite_controller.h @@ -0,0 +1,228 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2018, Clearpath Robotics +// +// 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 names of Clearpath Robotics 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 Mike Purvis */ + +#ifndef CONTROLLER_INTERFACE_COMPOSITE_CONTROLLER_H +#define CONTROLLER_INTERFACE_COMPOSITE_CONTROLLER_H + +#include +#include +#include +#include + +namespace controller_interface +{ + +template +class CompositeController: public virtual ControllerBase, public Controllers... +{ +public: + CompositeController() + { + state_ = CONSTRUCTED; + } + + virtual ~CompositeController() + { + } + +protected: + /** \brief This init function is called to initialize the composite controller + * from a non-realtime thread. This is called after all sub-controllers have + * already initialized successfully, in order. + * + * \param root_nh A NodeHandle in the root of the controller manager namespace. + * This is where the ROS interfaces are setup (publishers, subscribers, services). + * + * \param controller_nh A NodeHandle in the namespace of the controller. + * This is where the controller-specific configuration resides. + * + * \returns True if initialization was successful and the controller + * is ready to be started. + */ + virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) + { + return true; + } + + /** + * \brief Calls the starting() methods on all sub-controllers, in order. + * + * This default implementation of starting may be overridden if a behaviour + * more complicated than simply initializing the sub-controllers in order + * is desired. + */ + void starting(const ros::Time& time) override + { + callStarting(time); + } + + /** + * \brief Calls the starting() methods on all sub-controllers, in order. + * + * This default implementation of update may be overridden if a behaviour + * more complicated than simply initializing the sub-controllers in order + * is desired. If update is overridden, you can call through to this method + * or call the individual update methods of your sub-controllers. + */ + void update(const ros::Time& time, const ros::Duration& period) override + { + callUpdate(time, period); + } + + /** + * \brief Calls the starting() methods on all sub-controllers, in order. + * + * This default implementation of stopping may be overridden if a behaviour + * more complicated than simply initializing the sub-controllers in order + * is desired. + */ + void stopping(const ros::Time& time) override + { + callStopping(time); + } + + /** + * \brief Initialize the sub-controllers from a RobotHW pointer, gathering up + * the total claimed resources across all of them. + * + * \param robot_hw The robot hardware abstraction. + * + * \param root_nh A NodeHandle in the root of the controller manager namespace. + * This is where the ROS interfaces are setup (publishers, subscribers, services). + * + * \param controller_nh A NodeHandle in the namespace of the controller. + * This is where the controller-specific configuration resides. + * + * \param[out] claimed_resources The resources claimed by all sub-controllers. + * They can belong to multiple hardware interfaces. + * + * \returns True if initialization was successful and the composite controller + * is ready to be started. + */ + virtual bool initRequest(hardware_interface::RobotHW* robot_hw, + ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh, + ClaimedResources& claimed_resources) + { + // check if construction finished cleanly + if (state_ != CONSTRUCTED){ + ROS_ERROR("Cannot initialize this controller because it failed to be constructed."); + return false; + } + + if (callInitRequest(robot_hw, root_nh, controller_nh, claimed_resources) && + init(root_nh, controller_nh)) + { + state_ = INITIALIZED; + return true; + } + + ROS_ERROR("One or more components of the composite controller failed to initialize."); + return false; + } + +private: + template + void callStarting(const ros::Time& time) + { + Controller::starting(time); + } + + template + void callStarting(const ros::Time& time) + { + callStarting(time); + callStarting(time); + } + + template + void callUpdate(const ros::Time& time, const ros::Duration& period) + { + Controller::update(time, period); + } + + template + void callUpdate(const ros::Time& time, const ros::Duration& period) + { + callUpdate(time, period); + callUpdate(time, period); + } + + template + void callStopping(const ros::Time& time) + { + Controller::stopping(time); + } + + template + void callStopping(const ros::Time& time) + { + callStopping(time); + callStopping(time); + } + + template + bool callInitRequest(hardware_interface::RobotHW* robot_hw, + ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh, + ClaimedResources& all_claimed_resources) + { + ClaimedResources cr; + bool ret = Controller::initRequest(robot_hw, root_nh, controller_nh, cr); + if (state_ != INITIALIZED) + { + return false; + } + + // Revert the global state variable so that follow-on controllers see an uninitialized state + // and can also initialize themselves. + state_ = CONSTRUCTED; + + // Add the resources claimed by this specific controller to the overall list. + all_claimed_resources.insert(std::end(all_claimed_resources), std::begin(cr), std::end(cr)); + return ret; + } + + template + bool callInitRequest(hardware_interface::RobotHW* robot_hw, + ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh, + ClaimedResources& all_claimed_resources) + { + return callInitRequest(robot_hw, root_nh, controller_nh, all_claimed_resources) && + callInitRequest(robot_hw, root_nh, controller_nh, all_claimed_resources); + } + + CompositeController(const CompositeController& c); + CompositeController& operator =(const CompositeController& c); +}; + +} // namespace + +#endif From 1f87b276090a421ab8302faf0da7c124c5cca4ed Mon Sep 17 00:00:00 2001 From: Mike Purvis Date: Thu, 8 Feb 2018 07:07:41 -0500 Subject: [PATCH 2/5] Non-naive ClaimedResource merging. --- controller_interface/CMakeLists.txt | 5 ++ .../composite_controller.h | 65 +++++++++++++++-- .../test/composite_controller_test.cpp | 71 +++++++++++++++++++ 3 files changed, 137 insertions(+), 4 deletions(-) create mode 100644 controller_interface/test/composite_controller_test.cpp diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index 22d1f6998..b7ad395e9 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -14,3 +14,8 @@ catkin_package( install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +if(CATKIN_ENABLE_TESTING) + include_directories(include ${catkin_INCLUDE_DIRS}) + set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++11") + catkin_add_gtest(composite_controller_test test/composite_controller_test.cpp) +endif() diff --git a/controller_interface/include/controller_interface/composite_controller.h b/controller_interface/include/controller_interface/composite_controller.h index 00bfb32d4..a24ace3b1 100644 --- a/controller_interface/include/controller_interface/composite_controller.h +++ b/controller_interface/include/controller_interface/composite_controller.h @@ -38,6 +38,56 @@ namespace controller_interface { +/** + * \brief Merge together two ClaimedResources vectors, detecting whether the + * any of the merged-in resources were already present. The merge will be + * fully executed even if the function returns false (eg, indicating a conflict). + * + * \param[out] target ClaimedResources to merge resources into. + * + * \param source ClaimedResources to draw resources from. + * + * \return true if no resources were duplicated between the target and source. + */ +static bool mergeClaimedResources(ControllerBase::ClaimedResources& target, + const ControllerBase::ClaimedResources& source) +{ + bool conflict = false; + for (const auto& claimed : source) + { + // Check if there's a ClaimedResource of this interface type already in the target. + bool resource_in_target = false; + for (auto& target_claimed : target) + { + if (target_claimed.hardware_interface == claimed.hardware_interface) + { + resource_in_target = true; + + size_t size_before = target_claimed.resources.size(); + target_claimed.resources.insert(claimed.resources.begin(), + claimed.resources.end()); + + // Since it's a set, if the resulting size isn't big enough, we know that + // one or more of the inserted items was already present. + if (target_claimed.resources.size() < size_before + claimed.resources.size()) + { + conflict = true; + } + + break; + } + } + + if (!resource_in_target) + { + // There isn't, so append the whole ClaimedResource to the vector. + target.push_back(claimed); + } + } + return !conflict; +} + + template class CompositeController: public virtual ControllerBase, public Controllers... { @@ -193,8 +243,8 @@ class CompositeController: public virtual ControllerBase, public Controllers... ros::NodeHandle& controller_nh, ClaimedResources& all_claimed_resources) { - ClaimedResources cr; - bool ret = Controller::initRequest(robot_hw, root_nh, controller_nh, cr); + ClaimedResources claimed_resources; + bool ret = Controller::initRequest(robot_hw, root_nh, controller_nh, claimed_resources); if (state_ != INITIALIZED) { return false; @@ -204,8 +254,15 @@ class CompositeController: public virtual ControllerBase, public Controllers... // and can also initialize themselves. state_ = CONSTRUCTED; - // Add the resources claimed by this specific controller to the overall list. - all_claimed_resources.insert(std::end(all_claimed_resources), std::begin(cr), std::end(cr)); + // Add the resources claimed by this specific controller to the overall list, checking + // first that they aren't already in there (as that would be a conflict). + if (!mergeClaimedResources(all_claimed_resources, claimed_resources)) + { + ROS_ERROR_STREAM("Unable to initialize composed controller; one or more resources " + "claimed by multiple sub-controllers."); + return false; + } + return ret; } diff --git a/controller_interface/test/composite_controller_test.cpp b/controller_interface/test/composite_controller_test.cpp new file mode 100644 index 000000000..b7638b4b7 --- /dev/null +++ b/controller_interface/test/composite_controller_test.cpp @@ -0,0 +1,71 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2017, Clearpath Robotics +// +// 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 Clearpath Robotics 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 Mike Purvis + +#include +#include +#include + +TEST(CompositeControllerTests, checkMerge) +{ + typedef controller_interface::ControllerBase::ClaimedResources ClaimedResources; + using hardware_interface::InterfaceResources; + + ClaimedResources base({ + InterfaceResources("FooInterface", {"a", "b"}), + InterfaceResources("BarInterface", {"c", "d", "e"}), + InterfaceResources("BazInterface", {"e", "f", "g", "h"}) + }); + + // Should succeed, none of these overlap with what's in the base claims. + ClaimedResources merge_success({ + InterfaceResources("FzzInterface", {"a"}), + InterfaceResources("BarInterface", {"a", "b"}), + InterfaceResources("FooInterface", {"c", "f", "g"}) + }); + ASSERT_TRUE(controller_interface::mergeClaimedResources(base, merge_success)); + ASSERT_EQ(std::set({"a", "b", "c", "f", "g"}), base[0].resources); + ASSERT_EQ(std::set({"a", "b", "c", "d", "e"}), base[1].resources); + ASSERT_EQ(std::set({"e", "f", "g", "h"}), base[2].resources); + ASSERT_EQ(std::set({"a"}), base[3].resources); + ASSERT_EQ(4, base.size()); + + // Should fail because "e" is already in BarInterface, above. + ClaimedResources merge_fail({ + InterfaceResources("BarInterface", {"e", "f", "g"}) + }); + ASSERT_FALSE(controller_interface::mergeClaimedResources(base, merge_fail)); + ASSERT_EQ(std::set({"a", "b", "c", "d", "e", "f", "g"}), base[1].resources); + ASSERT_EQ(4, base.size()); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 5ba09cb30d7e63ee36699e4b82a214295459689d Mon Sep 17 00:00:00 2001 From: Mike Purvis Date: Thu, 16 Nov 2017 11:22:16 -0500 Subject: [PATCH 3/5] Single unified Controller template for ROS Melodic. --- .../include/controller_interface/controller.h | 263 ++++++++++++--- .../internal/robothw_interfaces.h | 35 +- .../multi_interface_controller.h | 308 ++---------------- 3 files changed, 268 insertions(+), 338 deletions(-) diff --git a/controller_interface/include/controller_interface/controller.h b/controller_interface/include/controller_interface/controller.h index 2a0195b35..604ba31ee 100644 --- a/controller_interface/include/controller_interface/controller.h +++ b/controller_interface/include/controller_interface/controller.h @@ -1,5 +1,7 @@ /////////////////////////////////////////////////////////////////////////////// // Copyright (C) 2012, hiDOF INC. +// Copyright (C) 2015, PAL Robotics S.L. +// Copyright (C) 2018, Clearpath Robotics. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: @@ -8,7 +10,7 @@ // * 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 +// * Neither the names of the copyright holders or the names of their // contributors may be used to endorse or promote products derived from // this software without specific prior written permission. // @@ -25,41 +27,150 @@ // POSSIBILITY OF SUCH DAMAGE. ////////////////////////////////////////////////////////////////////////////// -/* - * Author: Wim Meeussen - */ +/** + * \author Wim Meeussen + * \author Adolfo Rodríguez Tsouroukdissian + * \author Mike Purvis + * */ #ifndef CONTROLLER_INTERFACE_CONTROLLER_H #define CONTROLLER_INTERFACE_CONTROLLER_H #include -#include +#include #include #include -#include - +#include namespace controller_interface { -/** \brief %Controller with a specific hardware interface +/** + * \brief %Controller is able to claim resources from multiple hardware interfaces. + * + * This controller implementation allows to claim resources from one or + * more different hardware interfaces. The types of these hardware interfaces + * are specified as template parameters. + * + * An example multi-interface controller could claim, for instance, resources + * from a position-controlled arm and velocity-controlled wheels. Another + * example would be a controller claiming both position and effort interfaces + * for the same robot resources, but this would require a robot with a custom + * (non-exclusive) resource handling policy. + * + * By default, all specified hardware interfaces are required, and their + * existence will be enforced by \ref initRequest. It is possible to make hardware + * interfaces optional by means of the \c allow_optional_interfaces + * \ref Controller::Controller "constructor" parameter. + * This allows to write controllers where some interfaces are mandatory, and + * others, if present, improve controller performance, but whose absence does not + * prevent the controller from running. + * + * The following is an example of a controller claiming resources from velocity- + * and effort-controlled joints. + * + * \code + * #include + * #include + * + * using namespace hardware_interface; + * class VelEffController : public + * controller_interface::Controller + * { + * public: + * VelEffController() {} + * + * bool init(VelocityJointInterface* v, EffortJointInterface* e, ros::NodeHandle &n) + * // v and e are guaranteed to be valid. Fetch resources from them, + * // perform rest of initialization + * + * return true; + * } + * void starting(const ros::Time& time); + * void update(const ros::Time& time, const ros::Duration& period); + * void stopping(const ros::Time& time); + * }; + * \endcode + * + * The following fragment is a modified version of the above example, where + * controller interfaces are not required. It is left to the controller + * implementer to verify interface validity. Only the initialization code is + * shown. * - * \tparam T The hardware interface type used by this controller. This enforces - * semantic compatibility between the controller and the hardware it's meant to - * control. + * \code + * class VelEffController : public + * controller_interface::Controller + * { + * public: + * // Note true flag passed to parent class, allowing requested hardware + * // interfaces to be optional + * VelEffController() + * : controller_interface::Controller (true) + * {} + * + * bool init(VelocityJointInterface* v, EffortJointInterface* e, ros::NodeHandle &n) + * { + * // v is a required interface + * if (!v) + * { + * return false; + * } + * + * // e is an optional interface. If present, additional features are enabled. + * // Controller can still function if interface or some of its resources are + * // absent + * if (e) + * { + * // ... + * } + * + * // Fetch resources from interfaces, perform rest of initialization + * // ... + * + * return true; + * } + * ... + * }; + * \endcode + * + * \tparam T... Hardware interface types. + * This parameter is \e required. */ -template +template class Controller: public virtual ControllerBase { public: - Controller() {state_ = CONSTRUCTED;} - virtual ~Controller(){} + /** + * \param allow_optional_interfaces If set to true, \ref initRequest will + * not fail if one or more of the requested interfaces is not present. + * If set to false (the default), all requested interfaces are required. + */ + Controller(bool allow_optional_interfaces = false) + : allow_optional_interfaces_(allow_optional_interfaces) + {state_ = CONSTRUCTED;} - /** \brief The init function is called to initialize the controller from a - * non-realtime thread with a pointer to the hardware interface, itself, - * instead of a pointer to a RobotHW. + virtual ~Controller() {} + + /** \name Non Real-Time Safe Functions + *\{*/ + + /** + * \brief Custom controller initialization logic. + * + * In this method resources from different interfaces are claimed, and other + * non real-time initialization is performed, such as setup of ROS interfaces + * and resource pre-allocation. * - * \param hw The specific hardware interface used by this controller. + * \param interfaces Pointers to the hardware interfaces used by this + * controller, as specified in the template parameter pack. + * If \ref Controller::Controller was called with \c allow_optional_interfaces + * set to \c false (the default), all pointers will be valid and non-NULL. + * If \c allow_optional_interfaces was set to \c true, some or all of the + * interface pointers may be NULL, and must be individually checked. + * Please refer to the code examples in the \ref Controller class description. * * \param controller_nh A NodeHandle in the namespace from which the controller * should read its configuration, and where it should set up its ROS @@ -68,13 +179,26 @@ class Controller: public virtual ControllerBase * \returns True if initialization was successful and the controller * is ready to be started. */ - virtual bool init(T* /*hw*/, ros::NodeHandle& /*controller_nh*/) {return true;}; + virtual bool init(Interfaces*... /*interfaces*/, + ros::NodeHandle& /*controller_nh*/) + { + return true; + } - /** \brief The init function is called to initialize the controller from a - * non-realtime thread with a pointer to the hardware interface, itself, - * instead of a pointer to a RobotHW. + /** + * \brief Custom controller initialization logic. + * + * In this method resources from different interfaces are claimed, and other + * non real-time initialization is performed, such as setup of ROS interfaces + * and resource pre-allocation. * - * \param hw The specific hardware interface used by this controller. + * \param interfaces Pointers to the hardware interfaces used by this + * controller, as specified in the template parameter pack. + * If \ref Controller::Controller was called with \c allow_optional_interfaces + * set to \c false (the default), all pointers will be valid and non-NULL. + * If \c allow_optional_interfaces was set to \c true, some or all of the + * interface pointers may be NULL, and must be individually checked. + * Please refer to the code examples in the \ref Controller class description. * * \param root_nh A NodeHandle in the root of the controller manager namespace. * This is where the ROS interfaces are setup (publishers, subscribers, services). @@ -85,65 +209,106 @@ class Controller: public virtual ControllerBase * \returns True if initialization was successful and the controller * is ready to be started. */ - virtual bool init(T* /*hw*/, ros::NodeHandle& /*root_nh*/, ros::NodeHandle& /*controller_nh*/) {return true;}; + virtual bool init(Interfaces*... /*interfaces*/, + ros::NodeHandle& /*root_nh*/, + ros::NodeHandle& /*controller_nh*/) + { + return true; + } protected: - /** \brief Initialize the controller from a RobotHW pointer + /** + * \brief Initialize the controller from a RobotHW pointer. + * + * This calls the user-supplied \ref init method with the hardware interfaces + * from the \c robot_hw pointer, some or all of which may be NULL, depending + * on the value of \c allow_optional_interfaces passed to the constructor. + * + * \param robot_hw The robot hardware abstraction. + * + * \param root_nh A NodeHandle in the root of the controller manager namespace. + * This is where the ROS interfaces are setup (publishers, subscribers, services). + * + * \param controller_nh A NodeHandle in the namespace of the controller. + * This is where the controller-specific configuration resides. * - * This calls \ref init with the hardware interface for this controller if it - * can extract the correct interface from \c robot_hw. + * \param[out] claimed_resources The resources claimed by this controller. + * They can belong to multiple hardware interfaces. * + * \returns True if initialization was successful and the controller + * is ready to be started. */ virtual bool initRequest(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh, ClaimedResources& claimed_resources) { - // check if construction finished cleanly + // Check if construction finished cleanly. if (state_ != CONSTRUCTED){ - ROS_ERROR("Cannot initialize this controller because it failed to be constructed"); + ROS_ERROR("Cannot initialize this controller because it failed to be constructed."); return false; } - // get a pointer to the hardware interface - T* hw = robot_hw->get(); - if (!hw) + // Check for required hardware interfaces. + if (!allow_optional_interfaces_ && !internal::hasInterfaces(robot_hw)) { - 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()); + // Error message has already been sent by the checking function. return false; } - // return which resources are claimed by this controller - hw->clearClaims(); - if (!init(hw, controller_nh) || !init(hw, root_nh, controller_nh)) + // Custom controller initialization. + if (!init(robot_hw->get()..., controller_nh) || + !init(robot_hw->get()..., root_nh, controller_nh)) { - ROS_ERROR("Failed to initialize the controller"); + ROS_ERROR("Failed to initialize the controller."); return false; } - hardware_interface::InterfaceResources iface_res(getHardwareInterfaceType(), hw->getClaims()); - claimed_resources.assign(1, iface_res); - hw->clearClaims(); - // success + // Populate claimed resources. + claimed_resources.clear(); + internal::populateClaimedResources(robot_hw, claimed_resources); + internal::clearClaims(robot_hw); + // NOTE: Above, claims are cleared since we only want to know what they are and report them back + // as an output parameter. Actual resource claiming by the controller is done when the controller + // is start()ed + + // Initialization has succeeded. state_ = INITIALIZED; return true; } - /// Get the name of this controller's hardware interface type - std::string getHardwareInterfaceType() const + /** + * This is provided for compatibility with Controller from when it only took a single hardware + * interface. It can't be used when Controller has more than on interface type. + */ + ROS_DEPRECATED std::string getHardwareInterfaceType() const { - return hardware_interface::internal::demangledTypeName(); + return hardware_interface::internal::demangledTypeName(); } + /*\}*/ + private: - Controller(const Controller &c); - Controller& operator =(const Controller &c); + /** Flag to indicate if hardware interfaces are considered optional (i.e. non-required). */ + bool allow_optional_interfaces_; + + /** + * Construction by copying prohibited. Controllers are not copyable. + */ + Controller(const Controller& c); + + /** + * Construction by assignment prohibited. Controllers are not copyable. + */ + Controller& operator =(const Controller& c); + // Everything other than this line actually compiles just fine under gcc without -std=c++11, + // barring warnings about varidic templates being unsupported. This guard can be removed in Melodic + // when the compiler is c++14 by default. + static_assert(sizeof...(Interfaces) >= 1, "Controller must have at least one hardware interface."); }; -} +} // namespace #endif diff --git a/controller_interface/include/controller_interface/internal/robothw_interfaces.h b/controller_interface/include/controller_interface/internal/robothw_interfaces.h index 46dfda02e..c47d36914 100644 --- a/controller_interface/include/controller_interface/internal/robothw_interfaces.h +++ b/controller_interface/include/controller_interface/internal/robothw_interfaces.h @@ -56,14 +56,14 @@ inline std::string enumerateElements(const T& val, const std::string sdp = suffix+delimiter+prefix; std::stringstream ss; ss << prefix; - std::copy(val.begin(), val.end(), std::ostream_iterator(ss, sdp.c_str())); + std::copy(val.begin(), val.end(), std::ostream_iterator(ss, sdp.c_str())); ret = ss.str(); if (!ret.empty()) {ret.erase(ret.size() - delimiter.size() - prefix.size());} return ret; } -template +template inline bool hasInterfaces(hardware_interface::RobotHW* robot_hw) { T* hw = robot_hw->get(); @@ -78,14 +78,14 @@ inline bool hasInterfaces(hardware_interface::RobotHW* robot_hw) return true; } -template +template inline bool hasInterfaces(hardware_interface::RobotHW* robot_hw) { return hasInterfaces(robot_hw) && hasInterfaces(robot_hw); } -template +template void clearClaims(hardware_interface::RobotHW* robot_hw) { T* hw = robot_hw->get(); @@ -95,7 +95,7 @@ void clearClaims(hardware_interface::RobotHW* robot_hw) } } -template +template void clearClaims(hardware_interface::RobotHW* robot_hw) { clearClaims(robot_hw); @@ -103,7 +103,7 @@ void clearClaims(hardware_interface::RobotHW* robot_hw) } -template +template inline void extractInterfaceResources(hardware_interface::RobotHW* robot_hw_in, hardware_interface::RobotHW* robot_hw_out) { @@ -111,7 +111,7 @@ inline void extractInterfaceResources(hardware_interface::RobotHW* robot_hw_in, if (hw) {robot_hw_out->registerInterface(hw);} } -template +template inline void extractInterfaceResources(hardware_interface::RobotHW* robot_hw_in, hardware_interface::RobotHW* robot_hw_out) { @@ -120,7 +120,24 @@ inline void extractInterfaceResources(hardware_interface::RobotHW* robot_hw_in, } -template +template +inline void populateInterfaces(hardware_interface::RobotHW* robot_hw, T hw) +{ + if (hw) + { + robot_hw->registerInterface(hw); + } +} + +template +inline void populateInterfaces(hardware_interface::RobotHW* robot_hw, T hw, More... more) +{ + populateInterfaces(robot_hw, hw); + populateInterfaces(robot_hw, more...); +} + + +template inline void populateClaimedResources(hardware_interface::RobotHW* robot_hw, ControllerBase::ClaimedResources& claimed_resources) { @@ -134,7 +151,7 @@ inline void populateClaimedResources(hardware_interface::RobotHW* robot_hw, } } -template +template inline void populateClaimedResources(hardware_interface::RobotHW* robot_hw, ControllerBase::ClaimedResources& claimed_resources) { diff --git a/controller_interface/include/controller_interface/multi_interface_controller.h b/controller_interface/include/controller_interface/multi_interface_controller.h index dbc8e5d79..e5768eda4 100644 --- a/controller_interface/include/controller_interface/multi_interface_controller.h +++ b/controller_interface/include/controller_interface/multi_interface_controller.h @@ -25,325 +25,73 @@ // POSSIBILITY OF SUCH DAMAGE. ////////////////////////////////////////////////////////////////////////////// -/** \author Adolfo Rodríguez Tsouroukdissian */ +/** \author Mike Purvis */ #ifndef CONTROLLER_INTERFACE_MULTI_INTERFACE_CONTROLLER_H #define CONTROLLER_INTERFACE_MULTI_INTERFACE_CONTROLLER_H -#include +#warning MultiInterfaceController is deprecated. Please use Controller instead. + +#include #include -#include -#include -#include #include namespace controller_interface { /** - * \brief %Controller able to claim resources from multiple hardware interfaces. - * - * This particular controller implementation allows to claim resources from one - * up to four different hardware interfaces. The types of these hardware - * interfaces are specified as template parameters. - * - * An example multi-interface controller could claim, for instance, resources - * from a position-controlled arm and velocity-controlled wheels. Another - * example would be a controller claiming both position and effort interfaces - * for the same robot resources, but this would require a robot with a custom - * (non-exclusive) resource handling policy. - * - * By default, all specified hardware interfaces are required, and their - * existence will be enforced by \ref initRequest. It is possible to make hardware - * interfaces optional by means of the \c allow_optional_interfaces - * \ref MultiInterfaceController::MultiInterfaceController "constructor" parameter. - * This allows to write controllers where some interfaces are mandatory, and - * others, if present, improve controller performance, but whose absence does not - * prevent the controller from running. - * - * The following is an example of a controller claiming resources from velocity- - * and effort-controlled joints. - * - * \code - * #include - * #include - * - * using namespace hardware_interface; - * class VelEffController : public - * controller_interface::MultiInterfaceController - * { - * public: - * VelEffController() {} - * - * bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle &n) - * { - * // robot_hw pointer only contains the two interfaces requested by the - * // controller. It is a subset of the entire robot, which may have more - * // hardware interfaces - * - * // v and e below are guarranteed to be valid - * VelocityJointInterface* v = robot_hw->get(); - * EffortJointInterface* e = robot_hw->get(); - * - * // Fetch resources from interfaces, perform rest of initialization - * //... - * - * return true; - * } - * void starting(const ros::Time& time); - * void update(const ros::Time& time, const ros::Duration& period); - * void stopping(const ros::Time& time); - * }; - * \endcode - * - * The following fragment is a modified version of the above example, where - * controller interfaces are not required. It is left to the controller - * implementer to verify interface validity. Only the initialization code is - * shown. - * - * \code - * class VelEffController : public - * controller_interface::MultiInterfaceController - * { - * public: - * // Note true flag passed to parent class, allowing requested hardware - * // interfaces to be optional - * VelEffController() - * : controller_interface::MultiInterfaceController (true) - * {} - * - * bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle &n) - * { - * // robot_hw pointer contains at most the two interfaces requested by the - * // controller. It may have none, only one or both, depending on whether the - * // robot exposes them - * - * // v is a required interface - * VelocityJointInterface* v = robot_hw->get(); - * if (!v) - * { - * return false; - * } - * - * // e is an optional interface. If present, additional features are enabled. - * // Controller can still function if interface or some of its resources are - * // absent - * EffortJointInterface* e = robot_hw->get(); - * - * // Fetch resources from interfaces, perform rest of initialization - * //... - * - * return true; - * } - * ... - * }; - * \endcode - * - * \tparam T... Hardware interface types. - * This parameter is \e required. + * \brief Adapter class providing the old MultiInterfaceController API. */ -template -class MultiInterfaceController: public virtual ControllerBase +template +class MultiInterfaceController: public Controller { public: - /** - * \param allow_optional_interfaces If set to true, \ref initRequest will - * not fail if one or more of the requested interfaces is not present. - * If set to false (the default), all requested interfaces are required. - */ - MultiInterfaceController(bool allow_optional_interfaces = false) - : allow_optional_interfaces_(allow_optional_interfaces) - {state_ = CONSTRUCTED;} - - virtual ~MultiInterfaceController() {} + using Controller::Controller; /** \name Non Real-Time Safe Functions *\{*/ - /** - * \brief Custom controller initialization logic. - * - * In this method resources from different interfaces are claimed, and other - * non real-time initialization is performed, such as setup of ROS interfaces - * and resource pre-allocation. - * - * \param robot_hw Robot hardware abstraction containing a subset of the entire - * robot. If \ref MultiInterfaceController::MultiInterfaceController - * "MultiInterfaceController" was called with \c allow_optional_interfaces set - * to \c false (the default), this parameter contains all the interfaces - * requested by the controller. - * If \c allow_optional_interfaces was set to \c false, this parameter may - * contain none, some or all interfaces requested by the controller, depending - * on whether the robot exposes them. Please refer to the code examples in the - * \ref MultiInterfaceController "class description". - * - * \param controller_nh A NodeHandle in the namespace from which the controller - * should read its configuration, and where it should set up its ROS - * interface. - * - * \returns True if initialization was successful and the controller - * is ready to be started. - */ virtual bool init(hardware_interface::RobotHW* /*robot_hw*/, - ros::NodeHandle& /*controller_nh*/) - {return true;} - - /** - * \brief Custom controller initialization logic. - * - * In this method resources from different interfaces are claimed, and other - * non real-time initialization is performed, such as setup of ROS interfaces - * and resource pre-allocation. - * - * \param robot_hw Robot hardware abstraction containing a subset of the entire - * robot. If \ref MultiInterfaceController::MultiInterfaceController - * "MultiInterfaceController" was called with \c allow_optional_interfaces set - * to \c false (the default), this parameter contains all the interfaces - * requested by the controller. - * If \c allow_optional_interfaces was set to \c false, this parameter may - * contain none, some or all interfaces requested by the controller, depending - * on whether the robot exposes them. Please refer to the code examples in the - * \ref MultiInterfaceController "class description". - * - * \param root_nh A NodeHandle in the root of the controller manager namespace. - * This is where the ROS interfaces are setup (publishers, subscribers, services). - * - * \param controller_nh A NodeHandle in the namespace of the controller. - * This is where the controller-specific configuration resides. - * - * \returns True if initialization was successful and the controller - * is ready to be started. - */ - virtual bool init(hardware_interface::RobotHW* /*robot_hw*/, - ros::NodeHandle& /*root_nh*/, - ros::NodeHandle& /*controller_nh*/) - {return true;} - -protected: - /** - * \brief Initialize the controller from a RobotHW pointer. - * - * This calls \ref init with a RobotHW that is a subset of the input - * \c robot_hw parameter, containing only the requested hardware interfaces - * (all or some, depending on the value of \c allow_optional_interfaces passed - * to the constructor). - * - * \param robot_hw The robot hardware abstraction. - * - * \param root_nh A NodeHandle in the root of the controller manager namespace. - * This is where the ROS interfaces are setup (publishers, subscribers, services). - * - * \param controller_nh A NodeHandle in the namespace of the controller. - * This is where the controller-specific configuration resides. - * - * \param[out] claimed_resources The resources claimed by this controller. - * They can belong to multiple hardware interfaces. - * - * \returns True if initialization was successful and the controller - * is ready to be started. - */ - virtual bool initRequest(hardware_interface::RobotHW* robot_hw, - ros::NodeHandle& root_nh, - ros::NodeHandle& controller_nh, - ClaimedResources& claimed_resources) + ros::NodeHandle& /*controller_nh*/) { - // check if construction finished cleanly - if (state_ != CONSTRUCTED){ - ROS_ERROR("Cannot initialize this controller because it failed to be constructed"); - return false; - } - - // check for required hardware interfaces - if (!allow_optional_interfaces_ && !hasRequiredInterfaces(robot_hw)) {return false;} - - // populate robot hardware abstraction containing only controller hardware interfaces (subset of robot) - hardware_interface::RobotHW* robot_hw_ctrl_p = &robot_hw_ctrl_; - extractInterfaceResources(robot_hw, robot_hw_ctrl_p); - - // custom controller initialization - clearClaims(robot_hw_ctrl_p); // claims will be populated on controller init - if (!init(robot_hw_ctrl_p, controller_nh) || !init(robot_hw_ctrl_p, root_nh, controller_nh)) - { - ROS_ERROR("Failed to initialize the controller"); - return false; - } - - // populate claimed resources - claimed_resources.clear(); - populateClaimedResources(robot_hw_ctrl_p, claimed_resources); - clearClaims(robot_hw_ctrl_p); - // NOTE: Above, claims are cleared since we only want to know what they are and report them back - // as an output parameter. Actual resource claiming by the controller is done when the controller - // is start()ed - - // initialization successful - state_ = INITIALIZED; return true; } - /*\}*/ - - /** - * \brief Check if robot hardware abstraction contains all required interfaces. - * \param robot_hw Robot hardware abstraction. - * \return true if all required hardware interfaces are exposed by \c robot_hw, - * false otherwise. - */ - static bool hasRequiredInterfaces(hardware_interface::RobotHW* robot_hw) + virtual bool init(hardware_interface::RobotHW* /*robot_hw*/, + ros::NodeHandle& /*root_nh*/, + ros::NodeHandle& /*controller_nh*/) { - return internal::hasInterfaces(robot_hw); + return true; } +private: /** - * \brief Clear claims from all hardware interfaces requested by this controller. - * \param robot_hw Robot hardware abstraction containing the interfaces whose - * claims will be cleared. + * \brief Overrides the new-API init function in the base class, + * and calls through to the virtual method with the old API. */ - static void clearClaims(hardware_interface::RobotHW* robot_hw) + bool init(Interfaces*... interfaces, + ros::NodeHandle& controller_nh) override { - internal::clearClaims(robot_hw); + internal::populateInterfaces(&robot_hw_ctrl_, interfaces...); + return init(&robot_hw_ctrl_, controller_nh); } /** - * \brief Extract all hardware interfaces requested by this controller from - * \c robot_hw_in, and add them also to \c robot_hw_out. - * \param[in] robot_hw_in Robot hardware abstraction containing the interfaces - * requested by this controller, and potentially others. - * \param[out] robot_hw_out Robot hardware abstraction containing \e only the - * interfaces requested by this controller. + * \brief Overrides the new-API init function in the base class, + * and calls through to the virtual method with the old API. */ - static void extractInterfaceResources(hardware_interface::RobotHW* robot_hw_in, - hardware_interface::RobotHW* robot_hw_out) + bool init(Interfaces*... interfaces, + ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh) override { - internal::extractInterfaceResources(robot_hw_in, robot_hw_out); + internal::populateInterfaces(&robot_hw_ctrl_, interfaces...); + return init(&robot_hw_ctrl_, root_nh, controller_nh); } /** - * \brief Extract all hardware interfaces requested by this controller from - * \c robot_hw_in, and add them also to \c robot_hw_out. - * \param[in] robot_hw_in Robot hardware abstraction containing the interfaces - * requested by this controller, and potentially others. - * \param[out] claimed_resources The resources claimed by this controller. - * They can belong to multiple hardware interfaces. + * Robot hardware abstraction containing only the subset of interfaces requested by the controller. */ - static void populateClaimedResources(hardware_interface::RobotHW* robot_hw, - ClaimedResources& claimed_resources) - { - internal::populateClaimedResources(robot_hw, claimed_resources); - } - - /** Robot hardware abstraction containing only the subset of interfaces requested by the controller. */ hardware_interface::RobotHW robot_hw_ctrl_; - - /** Flag to indicate if hardware interfaces are considered optional (i.e. non-required). */ - bool allow_optional_interfaces_; - -private: - MultiInterfaceController(const MultiInterfaceController& c); - MultiInterfaceController& operator =(const MultiInterfaceController& c); }; } // namespace From ab14ea48d73a1037f79686cf64f3379f9eac0a92 Mon Sep 17 00:00:00 2001 From: Jason Mercer Date: Sat, 8 Dec 2018 20:51:23 -0500 Subject: [PATCH 4/5] Switched from something that works only for gcc to something that works for gcc and clang. Original error is: (#1) robothw_interfaces.h:60:68: error: typedef 'value_type' cannot be referenced with a class specifier. --- .../include/controller_interface/internal/robothw_interfaces.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/internal/robothw_interfaces.h b/controller_interface/include/controller_interface/internal/robothw_interfaces.h index c47d36914..afc6cc93c 100644 --- a/controller_interface/include/controller_interface/internal/robothw_interfaces.h +++ b/controller_interface/include/controller_interface/internal/robothw_interfaces.h @@ -56,7 +56,7 @@ inline std::string enumerateElements(const T& val, const std::string sdp = suffix+delimiter+prefix; std::stringstream ss; ss << prefix; - std::copy(val.begin(), val.end(), std::ostream_iterator(ss, sdp.c_str())); + std::copy(val.begin(), val.end(), std::ostream_iterator(ss, sdp.c_str())); ret = ss.str(); if (!ret.empty()) {ret.erase(ret.size() - delimiter.size() - prefix.size());} return ret; From dceb56fb9afad6cf14dd12e8c97e78b0d57fd30e Mon Sep 17 00:00:00 2001 From: Mike Purvis Date: Thu, 1 Aug 2019 12:44:41 -0400 Subject: [PATCH 5/5] Drop deprecation of MultiInterfaceController for now. --- .../include/controller_interface/multi_interface_controller.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/multi_interface_controller.h b/controller_interface/include/controller_interface/multi_interface_controller.h index e5768eda4..8c936faf1 100644 --- a/controller_interface/include/controller_interface/multi_interface_controller.h +++ b/controller_interface/include/controller_interface/multi_interface_controller.h @@ -30,7 +30,8 @@ #ifndef CONTROLLER_INTERFACE_MULTI_INTERFACE_CONTROLLER_H #define CONTROLLER_INTERFACE_MULTI_INTERFACE_CONTROLLER_H -#warning MultiInterfaceController is deprecated. Please use Controller instead. +// TODO: Decide in the future if/when to deprecate MultiInterfaceController. +// #warning MultiInterfaceController is deprecated. Please use Controller instead. #include #include