diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index a80c97b2f..d44f110f6 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -26,6 +26,17 @@ if(catkin_EXPORTED_TARGETS) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) endif() +if(CATKIN_ENABLE_TESTING) + + find_package(rostest REQUIRED) + add_rostest_gtest(controller_manager_hwi_switch_test + test/hwi_switch_test.test + test/hwi_switch_test.cpp + ) + target_link_libraries(controller_manager_hwi_switch_test ${PROJECT_NAME} ${catkin_LIBRARIES}) + +endif() + # Install install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 0fe09f374..c7dee40b5 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -11,6 +11,7 @@ https://github.com/ros-controls/ros_control Wim Meeussen + Mathias Lüdtke catkin diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index dcdb8c0c0..20439c064 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -408,7 +408,8 @@ bool ControllerManager::switchController(const std::vector& start_c ROS_DEBUG("Start request vector has size %i", (int)start_request_.size()); // Do the resource management checking - std::list info_list; + std::list info_list, start_list, stop_list; + std::vector &controllers = controllers_lists_[current_controllers_list_]; for (size_t i = 0; i < controllers.size(); ++i) { @@ -422,13 +423,44 @@ bool ControllerManager::switchController(const std::vector& start_c in_start_list = in_start_list || (start_request_[j] == controllers[i].c.get()); bool add_to_list = controllers[i].c->isRunning(); + hardware_interface::ControllerInfo &info = controllers[i].info; + + if(!add_to_list && in_stop_list){ // check for double stop + if(strictness == controller_manager_msgs::SwitchController::Request::STRICT){ + ROS_ERROR_STREAM("Could not stop controller '" << info.name << "' since it is not running"); + stop_request_.clear(); + start_request_.clear(); + return false; + } else { + in_stop_list = false; + } + } + + if(add_to_list && !in_stop_list && in_start_list){ // check for doubled start + if(strictness == controller_manager_msgs::SwitchController::Request::STRICT){ + ROS_ERROR_STREAM("Controller '" << info.name << "' is already running"); + stop_request_.clear(); + start_request_.clear(); + return false; + } else { + in_start_list = false; + } + } + + if(add_to_list && in_stop_list && !in_start_list){ // running and real stop + stop_list.push_back(info); + } + else if(!in_stop_list && in_start_list){ // start, but no restart + start_list.push_back(info); + } + if (in_stop_list) add_to_list = false; if (in_start_list) add_to_list = true; if (add_to_list) - info_list.push_back(controllers[i].info); + info_list.push_back(info); } bool in_conflict = robot_hw_->checkForConflict(info_list); @@ -440,6 +472,16 @@ bool ControllerManager::switchController(const std::vector& start_c return false; } + if (!robot_hw_->canSwitch(start_list, stop_list)) + { + ROS_ERROR("Could not switch controllers. The hardware interface combination for the requested controllers is unfeasible."); + stop_request_.clear(); + start_request_.clear(); + return false; + } + + robot_hw_->doSwitch(start_list, stop_list); + // start the atomic controller switching switch_strictness_ = strictness; please_switch_ = true; diff --git a/controller_manager/test/hwi_switch_test.cpp b/controller_manager/test/hwi_switch_test.cpp new file mode 100644 index 000000000..e1b71e9a1 --- /dev/null +++ b/controller_manager/test/hwi_switch_test.cpp @@ -0,0 +1,452 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2015, Fraunhofer IPA +// +// 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 Fraunhofer IPA 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 Mathias Lüdtke + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace hardware_interface { +bool operator<(hardware_interface::ControllerInfo const& i1, hardware_interface::ControllerInfo const& i2) +{ + return i1.name < i2.name; +} +} +template T intersect(const T& v1, const T &v2) +{ + + std::vector sorted1(v1.begin(), v1.end()), sorted2(v2.begin(), v2.end()); + T res; + + std::sort(sorted1.begin(), sorted1.end()); + std::sort(sorted2.begin(), sorted2.end()); + + std::set_intersection(sorted1.begin(), sorted1.end(), sorted2.begin(), sorted2.end(), std::back_inserter(res)); + return res; +} + +class SwitchBot : public hardware_interface::RobotHW +{ + hardware_interface::JointStateInterface jsi_; + hardware_interface::PositionJointInterface pji_; + hardware_interface::VelocityJointInterface vji_; + hardware_interface::EffortJointInterface eji_; + + class Joint + { + double dummy_; + std::set interfaces_; + std::string current_; + hardware_interface::JointStateHandle jsh_; + public: + Joint(const std::string &n, hardware_interface::JointStateInterface &iface): jsh_(n, &dummy_, &dummy_, &dummy_) + { + iface.registerHandle(jsh_); + } + + template void registerHandle(Interface &iface, bool can_switch) + { + if(can_switch) interfaces_.insert(hardware_interface::internal::demangledTypeName() ); + iface.registerHandle(typename Interface::ResourceHandleType(jsh_, &dummy_)); + } + bool canSwitch(const std::string &n) const + { + if(interfaces_.find(n) == interfaces_.end()) return false; + return n >= current_; + } + void doSwitch(const std::string &n) + { + current_ = n; + } + }; + + std::map > joints_; + + boost::shared_ptr makeJoint(const std::string & name) + { + boost::shared_ptr j(new Joint(name, jsi_)); + joints_.insert(std::make_pair(name, j)); + return j; + } + + std::vector started_; + std::vector stopped_; +public: + SwitchBot() + { + boost::shared_ptr j; + + j = makeJoint("j_pv"); + + j->registerHandle(pji_, true); + j->registerHandle(vji_, true); + j->registerHandle(eji_, false); + + j = makeJoint("j_pe"); + j->registerHandle(pji_, true); + j->registerHandle(vji_, false); + j->registerHandle(eji_, true); + + j = makeJoint("j_ve"); + j->registerHandle(pji_, false); + j->registerHandle(vji_, true); + j->registerHandle(eji_, true); + + registerInterface(&pji_); + registerInterface(&vji_); + registerInterface(&eji_); + + } + + virtual bool canSwitch(const std::list &start_list, const std::list &stop_list) const + { + + if(!RobotHW::canSwitch(start_list, stop_list)) + { + ROS_ERROR("Something is wrong with RobotHW"); + return false; + } + + if(!intersect(start_list, stop_list).empty()) + { + ROS_ERROR_STREAM("start_list and stop_list intersect"); + return false; + } + + bool j_pe_e = false; + bool j_ve_v = false; + + for(std::list::const_iterator it = start_list.begin(); it != start_list.end(); ++it) + { + for (std::set::const_iterator res_it = it->resources.begin(); res_it != it->resources.end(); ++res_it) + { + + // special check + if(it->hardware_interface == "hardware_interface::EffortJointInterface" && *res_it == "j_pe") j_pe_e = true; + else if(it->hardware_interface == "hardware_interface::VelocityJointInterface" && *res_it == "j_ve") j_ve_v = true; + + // per joint check + try + { + if(!joints_.at(*res_it)->canSwitch(it->hardware_interface)) + { + ROS_ERROR_STREAM("Cannot switch " << *res_it << " to " << it->hardware_interface); + return false; + } + } + catch(...) + { + ROS_FATAL("This should never happen!"); + return false; + } + } + } + return !(j_pe_e && j_ve_v); // check inter-joint hardware interface conflict + } + virtual void doSwitch(const std::list &start_list, const std::list &stop_list) + { + RobotHW::doSwitch(start_list, stop_list); // check if member is defined + + std::map switches; + for(std::list::const_iterator it = stop_list.begin(); it != stop_list.end(); ++it) + { + started_.erase(std::remove(started_.begin(), started_.end(), it->name), started_.end()); + stopped_.push_back(it->name); + for (std::set::const_iterator res_it = it->resources.begin(); res_it != it->resources.end(); ++res_it) + { + switches[*res_it] = ""; + } + } + for(std::list::const_iterator it = start_list.begin(); it != start_list.end(); ++it) + { + + stopped_.erase(std::remove(stopped_.begin(), stopped_.end(), it->name), stopped_.end()); + started_.push_back(it->name); + for (std::set::const_iterator res_it = it->resources.begin(); res_it != it->resources.end(); ++res_it) + { + switches[*res_it] = it->hardware_interface; + } + } + for(std::map::iterator it = switches.begin(); it != switches.end(); ++it) + { + joints_[it->first]->doSwitch(it->second); + } + } + bool checkUnqiue() const + { + return intersect(started_, stopped_).empty(); + } + bool checkNotRunning() const + { + return started_.empty(); + } +}; + + +class DummyControllerLoader: public controller_manager::ControllerLoaderInterface +{ + class DummyController : public controller_interface::ControllerBase + { + const std::string type_name; + public: + DummyController(const std::string &name) : type_name(name) {} + virtual void update(const ros::Time& time, const ros::Duration& period) {} + virtual bool initRequest(hardware_interface::RobotHW* hw, ros::NodeHandle& root_nh, ros::NodeHandle &controller_nh, + std::set& claimed_resources) + { + std::vector joints; + if(!controller_nh.getParam("joints", joints)) + { + ROS_ERROR("Could not parse joint names"); + return false; + } + claimed_resources.insert(joints.begin(), joints.end()); + state_ = INITIALIZED; + return true; + } + virtual std::string getHardwareInterfaceType() const + { + return type_name; + } + }; + + std::map classes; + void add(const std::string type) + { + classes.insert(std::make_pair("Dummy" + type + "Controller", "hardware_interface::" + type)); + } +public: + DummyControllerLoader() : ControllerLoaderInterface("controller_interface::ControllerBase") + { + add("EffortJointInterface"); + add("PositionJointInterface"); + add("VelocityJointInterface"); + } + virtual boost::shared_ptr createInstance(const std::string& lookup_name) + { + return boost::shared_ptr(new DummyController(classes.at(lookup_name))); + } + virtual std::vector getDeclaredClasses() + { + std::vector v; + for(std::map::iterator it = classes.begin(); it != classes.end(); ++it) + { + v.push_back(it->first); + } + return v; + } + virtual void reload() {} +}; + +void update(controller_manager::ControllerManager &cm, const ros::TimerEvent& e) +{ + cm.update(e.current_real, e.current_real - e.last_real); +} + +class GuardROS +{ +public: + ~GuardROS() + { + ros::shutdown(); + ros::waitForShutdown(); + } +}; + +TEST(SwitchInterfacesTest, SwitchInterfaces) +{ + GuardROS guard; + + SwitchBot bot; + ros::NodeHandle nh; + + controller_manager::ControllerManager cm(&bot); + + cm.registerControllerLoader(boost::make_shared()); + + ros::Timer timer = nh.createTimer(ros::Duration(0.01), boost::bind(update, boost::ref(cm), _1)); + + ASSERT_TRUE(cm.loadController("group_pos")); + ASSERT_TRUE(cm.loadController("another_group_pos")); + ASSERT_TRUE(cm.loadController("group_vel")); + ASSERT_TRUE(cm.loadController("group_eff")); + ASSERT_TRUE(cm.loadController("single_pos")); + ASSERT_TRUE(cm.loadController("single_eff")); + ASSERT_TRUE(cm.loadController("invalid_group_pos")); + ASSERT_FALSE(cm.loadController("totally_random_name")); + + { // test hardware interface conflict + std::vector start, stop; + start.push_back("invalid_group_pos"); + ASSERT_FALSE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT)); + } + { // test resource conflict + std::vector start, stop; + start.push_back("group_pos"); + start.push_back("group_vel"); + ASSERT_FALSE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT)); + } + { // test pos group + std::vector start, stop; + start.push_back("group_pos"); + ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT)); + ASSERT_TRUE(cm.switchController(stop, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // clean-up + } + { // test same hardware interface switch + std::vector start, stop, next_start; + start.push_back("group_pos"); + ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT)); + + next_start.push_back("group_pos"); + ASSERT_TRUE(cm.switchController(next_start, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); + + ASSERT_TRUE(cm.switchController(stop, next_start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // clean-up + } + { // test same hardware interface switch + std::vector start, stop, next_start; + start.push_back("group_pos"); + ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT)); + + next_start.push_back("another_group_pos"); + ASSERT_TRUE(cm.switchController(next_start, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); + + ASSERT_TRUE(cm.switchController(stop, next_start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // clean-up + } + { // test vel group + std::vector start, stop; + start.push_back("group_vel"); + ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT)); + ASSERT_TRUE(cm.switchController(stop, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // clean-up + } + { // test eff group + std::vector start, stop; + start.push_back("group_eff"); + ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT)); + ASSERT_TRUE(cm.switchController(stop, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // clean-up + } + + { // test direct hardware interface upgrades (okay) and downgrades (conflict) + std::vector start, stop, next_start; + start.push_back("group_eff"); + ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT)); + + next_start.push_back("group_vel"); + ASSERT_TRUE(cm.switchController(next_start, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // upgrade + + ASSERT_FALSE(cm.switchController(start, next_start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // downgrade + + ASSERT_TRUE(cm.switchController(stop, next_start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // clean-up + } + + { // test single pos + std::vector start, stop; + start.push_back("single_pos"); + ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT)); + ASSERT_TRUE(cm.switchController(stop, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // clean-up + } + { // test single eff + std::vector start, stop; + start.push_back("single_pos"); + ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT)); + ASSERT_TRUE(cm.switchController(stop, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // clean-up + } + { // test single pos + group_eff (resource conflict) + std::vector start, stop; + start.push_back("single_pos"); + start.push_back("group_eff"); + ASSERT_FALSE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT)); + } + { // test single pos + group_eff (hardware interface conflict) + std::vector start, stop; + start.push_back("single_eff"); + start.push_back("group_vel"); + ASSERT_FALSE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT)); + } + { // test single pos + group_vel + std::vector start, stop; + start.push_back("single_pos"); + start.push_back("group_vel"); + ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT)); + ASSERT_TRUE(cm.switchController(stop, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // clean-up + } + { // test single pos + group_vel + totally_random_name + std::vector start, stop; + start.push_back("single_pos"); + start.push_back("group_vel"); + start.push_back("totally_random_name"); + ASSERT_FALSE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT)); + ASSERT_FALSE(cm.switchController(stop, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); + + ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::BEST_EFFORT)); + ASSERT_TRUE(cm.switchController(stop, start, controller_manager_msgs::SwitchControllerRequest::BEST_EFFORT)); // clean-up + } + { // test restart + std::vector start, stop; + start.push_back("single_pos"); + ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT)); + + ASSERT_TRUE(cm.switchController(start, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // restart + ASSERT_TRUE(bot.checkUnqiue()); + + ASSERT_TRUE(cm.switchController(stop, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // clean-up + } + { // test stop for controller that is not running + std::vector start, stop; + stop.push_back("single_pos"); + ASSERT_FALSE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT)); + ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::BEST_EFFORT)); + } + { // test stop for controller that is not running + std::vector start, stop; + start.push_back("single_pos"); + ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT)); + ASSERT_FALSE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT)); + ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::BEST_EFFORT)); + ASSERT_TRUE(cm.switchController(stop, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // clean-up + } + ASSERT_TRUE(bot.checkNotRunning()); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "controller_manager_switch_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/controller_manager/test/hwi_switch_test.test b/controller_manager/test/hwi_switch_test.test new file mode 100644 index 000000000..8ad063a6a --- /dev/null +++ b/controller_manager/test/hwi_switch_test.test @@ -0,0 +1,4 @@ + + + + diff --git a/controller_manager/test/hwi_switch_test.yaml b/controller_manager/test/hwi_switch_test.yaml new file mode 100644 index 000000000..b585a6e55 --- /dev/null +++ b/controller_manager/test/hwi_switch_test.yaml @@ -0,0 +1,29 @@ +group_pos: + type: DummyPositionJointInterfaceController + joints: [j_pv, j_pe] + +another_group_pos: + type: DummyPositionJointInterfaceController + joints: [j_pv, j_pe] + +group_vel: + type: DummyVelocityJointInterfaceController + joints: [j_pv, j_ve] + +group_eff: + type: DummyEffortJointInterfaceController + joints: [j_ve, j_pe] + +single_eff: + type: DummyEffortJointInterfaceController + joints: [j_pe] + +single_pos: + type: DummyPositionJointInterfaceController + joints: [j_pe] + + +invalid_group_pos: + type: DummyPositionJointInterfaceController + joints: [j_ve, j_pe] + diff --git a/hardware_interface/include/hardware_interface/robot_hw.h b/hardware_interface/include/hardware_interface/robot_hw.h index 812d466b2..5576071d0 100644 --- a/hardware_interface/include/hardware_interface/robot_hw.h +++ b/hardware_interface/include/hardware_interface/robot_hw.h @@ -94,6 +94,21 @@ class RobotHW : public InterfaceManager return in_conflict; } +/** \name Hardware Interface Switching + *\{*/ + + /** + * 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. Start and stop list are disjoint. + * This is just a check, the actual switch is done in doSwitch() + */ + virtual bool canSwitch(const std::list &start_list, const std::list &stop_list) const { return true; } + + /** + * Perform (in non-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 canSwitch() beforehand. + */ + virtual void doSwitch(const std::list &start_list, const std::list &stop_list) {} }; }