hwi_switch_test.cpp
Go to the documentation of this file.
1 // Copyright (C) 2015, Fraunhofer IPA
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright notice,
7 // this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of Fraunhofer IPA nor the names of its
12 // contributors may be used to endorse or promote products derived from
13 // this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 // POSSIBILITY OF SUCH DAMAGE.
27 
29 
30 #include <gtest/gtest.h>
31 
32 #include <map>
33 #include <set>
34 #include <string>
35 #include <algorithm>
36 #include <iterator>
37 
41 
42 namespace hardware_interface {
44 {
45  return i1.name < i2.name;
46 }
47 }
48 template<typename T> T intersect(const T& v1, const T &v2)
49 {
50 
51  std::vector<typename T::value_type> sorted1(v1.begin(), v1.end()), sorted2(v2.begin(), v2.end());
52  T res;
53 
54  std::sort(sorted1.begin(), sorted1.end());
55  std::sort(sorted2.begin(), sorted2.end());
56 
57  std::set_intersection(sorted1.begin(), sorted1.end(), sorted2.begin(), sorted2.end(), std::back_inserter(res));
58  return res;
59 }
60 
61 // NOTE: For test simplicity, this robot abstraction assumes that it will work
62 // with controllers claiming resources from only one hardware interface.
63 // Note that in the general case, controllers can claim resources from multiple
64 // hardware interfaces.
66 {
71 
72  class Joint
73  {
74  double dummy_;
75  std::set<std::string> interfaces_;
76  std::string current_;
78  public:
79  Joint(const std::string &n, hardware_interface::JointStateInterface &iface): jsh_(n, &dummy_, &dummy_, &dummy_)
80  {
81  iface.registerHandle(jsh_);
82  }
83 
84  template<typename Interface> void registerHandle(Interface &iface, bool can_switch)
85  {
86  if(can_switch) interfaces_.insert(hardware_interface::internal::demangledTypeName<Interface>() );
87  iface.registerHandle(typename Interface::ResourceHandleType(jsh_, &dummy_));
88  }
89  bool prepareSwitch(const std::string &n)
90  {
91  if(interfaces_.find(n) == interfaces_.end()) return false;
92  return n >= current_;
93  }
94  void doSwitch(const std::string &n)
95  {
96  current_ = n;
97  }
98  };
99 
101 
102  std::map<std::string, JointSharedPtr> joints_;
103 
104  JointSharedPtr makeJoint(const std::string & name)
105  {
106  JointSharedPtr j(new Joint(name, jsi_));
107  joints_.insert(std::make_pair(name, j));
108  return j;
109  }
110 
111  std::vector<std::string> started_;
112  std::vector<std::string> stopped_;
113 public:
115  {
116  JointSharedPtr j;
117 
118  j = makeJoint("j_pv");
119 
120  j->registerHandle(pji_, true);
121  j->registerHandle(vji_, true);
122  j->registerHandle(eji_, false);
123 
124  j = makeJoint("j_pe");
125  j->registerHandle(pji_, true);
126  j->registerHandle(vji_, false);
127  j->registerHandle(eji_, true);
128 
129  j = makeJoint("j_ve");
130  j->registerHandle(pji_, false);
131  j->registerHandle(vji_, true);
132  j->registerHandle(eji_, true);
133 
134  registerInterface(&pji_);
135  registerInterface(&vji_);
136  registerInterface(&eji_);
137 
138  }
139 
140  virtual bool prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
141  const std::list<hardware_interface::ControllerInfo>& stop_list)
142  {
143 
144  if(!RobotHW::prepareSwitch(start_list, stop_list))
145  {
146  ROS_ERROR("Something is wrong with RobotHW");
147  return false;
148  }
149 
150  if(!intersect(start_list, stop_list).empty())
151  {
152  ROS_ERROR_STREAM("start_list and stop_list intersect");
153  return false;
154  }
155 
156  bool j_pe_e = false;
157  bool j_ve_v = false;
158 
159  for(std::list<hardware_interface::ControllerInfo>::const_iterator it = start_list.begin(); it != start_list.end(); ++it)
160  {
161  if (it->claimed_resources.size() != 1)
162  {
163  ROS_FATAL("We expect controllers to claim resoures from only one interface. This should never happen!");
164  return false;
165  }
166  const hardware_interface::InterfaceResources& iface_res = it->claimed_resources.front();
167  for (std::set<std::string>::const_iterator res_it = iface_res.resources.begin(); res_it != iface_res.resources.end(); ++res_it)
168  {
169  // special check
170  if(iface_res.hardware_interface == "hardware_interface::EffortJointInterface" && *res_it == "j_pe") j_pe_e = true;
171  else if(iface_res.hardware_interface == "hardware_interface::VelocityJointInterface" && *res_it == "j_ve") j_ve_v = true;
172 
173  // per joint check
174  try
175  {
176  if(!joints_.at(*res_it)->prepareSwitch(iface_res.hardware_interface))
177  {
178  ROS_ERROR_STREAM("Cannot switch " << *res_it << " to " << iface_res.hardware_interface);
179  return false;
180  }
181  }
182  catch(...)
183  {
184  ROS_FATAL("This should never happen!");
185  return false;
186  }
187  }
188  }
189  return !(j_pe_e && j_ve_v); // check inter-joint hardware interface conflict
190  }
191  virtual void doSwitch(const std::list<hardware_interface::ControllerInfo> &start_list, const std::list<hardware_interface::ControllerInfo> &stop_list)
192  {
193  RobotHW::doSwitch(start_list, stop_list); // check if member is defined
194 
195  std::map<std::string, std::string> switches;
196  for(std::list<hardware_interface::ControllerInfo>::const_iterator it = stop_list.begin(); it != stop_list.end(); ++it)
197  {
198  started_.erase(std::remove(started_.begin(), started_.end(), it->name), started_.end());
199  stopped_.push_back(it->name);
200  const hardware_interface::InterfaceResources& iface_res = it->claimed_resources.front();
201  for (std::set<std::string>::const_iterator res_it = iface_res.resources.begin(); res_it != iface_res.resources.end(); ++res_it)
202  {
203  switches[*res_it] = "";
204  }
205  }
206  for(std::list<hardware_interface::ControllerInfo>::const_iterator it = start_list.begin(); it != start_list.end(); ++it)
207  {
208  stopped_.erase(std::remove(stopped_.begin(), stopped_.end(), it->name), stopped_.end());
209  started_.push_back(it->name);
210  const hardware_interface::InterfaceResources& iface_res = it->claimed_resources.front();
211  for (std::set<std::string>::const_iterator res_it = iface_res.resources.begin(); res_it != iface_res.resources.end(); ++res_it)
212  {
213  switches[*res_it] = iface_res.hardware_interface;
214  }
215  }
216  for(std::map<std::string, std::string>::iterator it = switches.begin(); it != switches.end(); ++it)
217  {
218  joints_[it->first]->doSwitch(it->second);
219  }
220  }
221  bool checkUnqiue() const
222  {
223  return intersect(started_, stopped_).empty();
224  }
225  bool checkNotRunning() const
226  {
227  return started_.empty();
228  }
229 };
230 
231 
233 {
235  {
236  const std::string type_name;
237  public:
238  DummyController(const std::string &name) : type_name(name) {}
239  virtual void update(const ros::Time& /*time*/, const ros::Duration& /*period*/) {}
241  ros::NodeHandle& /*root_nh*/,
242  ros::NodeHandle& controller_nh,
243  ClaimedResources& claimed_resources)
244  {
245  std::vector<std::string> joints;
246  if(!controller_nh.getParam("joints", joints))
247  {
248  ROS_ERROR("Could not parse joint names");
249  return false;
250  }
251  std::set<std::string> resources(joints.begin(), joints.end());
252  hardware_interface::InterfaceResources iface_res(getHardwareInterfaceType(), resources);
253  claimed_resources.assign(1, iface_res);
254  state_ = INITIALIZED;
255  return true;
256  }
257  virtual std::string getHardwareInterfaceType() const
258  {
259  return type_name;
260  }
261  };
262 
263  std::map<std::string, std::string> classes;
264  void add(const std::string type)
265  {
266  classes.insert(std::make_pair("Dummy" + type + "Controller", "hardware_interface::" + type));
267  }
268 public:
269  DummyControllerLoader() : ControllerLoaderInterface("controller_interface::ControllerBase")
270  {
271  add("EffortJointInterface");
272  add("PositionJointInterface");
273  add("VelocityJointInterface");
274  }
275  virtual controller_interface::ControllerBaseSharedPtr createInstance(const std::string& lookup_name)
276  {
277  return controller_interface::ControllerBaseSharedPtr(new DummyController(classes.at(lookup_name)));
278  }
279  virtual std::vector<std::string> getDeclaredClasses()
280  {
281  std::vector<std::string> v;
282  for(std::map<std::string, std::string>::iterator it = classes.begin(); it != classes.end(); ++it)
283  {
284  v.push_back(it->first);
285  }
286  return v;
287  }
288  virtual void reload() {}
289 };
290 
292 {
294 }
295 
296 class GuardROS
297 {
298 public:
300  {
301  ros::shutdown();
303  }
304 };
305 
306 TEST(SwitchInterfacesTest, SwitchInterfaces)
307 {
308  GuardROS guard;
309 
310  SwitchBot bot;
311  ros::NodeHandle nh;
312 
314 
315  cm.registerControllerLoader(boost::make_shared<DummyControllerLoader>());
316 
317  ros::Timer timer = nh.createTimer(ros::Duration(0.01), boost::bind(update, boost::ref(cm), _1));
318 
319  ASSERT_TRUE(cm.loadController("group_pos"));
320  ASSERT_TRUE(cm.loadController("another_group_pos"));
321  ASSERT_TRUE(cm.loadController("group_vel"));
322  ASSERT_TRUE(cm.loadController("group_eff"));
323  ASSERT_TRUE(cm.loadController("single_pos"));
324  ASSERT_TRUE(cm.loadController("single_eff"));
325  ASSERT_TRUE(cm.loadController("invalid_group_pos"));
326  ASSERT_FALSE(cm.loadController("totally_random_name"));
327 
328  { // test hardware interface conflict
329  std::vector<std::string> start, stop;
330  start.push_back("invalid_group_pos");
331  ASSERT_FALSE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT));
332  }
333  { // test resource conflict
334  std::vector<std::string> start, stop;
335  start.push_back("group_pos");
336  start.push_back("group_vel");
337  ASSERT_FALSE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT));
338  }
339  { // test pos group
340  std::vector<std::string> start, stop;
341  start.push_back("group_pos");
342  ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT));
343  ASSERT_TRUE(cm.switchController(stop, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // clean-up
344  }
345  { // test same hardware interface switch
346  std::vector<std::string> start, stop, next_start;
347  start.push_back("group_pos");
348  ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT));
349 
350  next_start.push_back("group_pos");
351  ASSERT_TRUE(cm.switchController(next_start, start, controller_manager_msgs::SwitchControllerRequest::STRICT));
352 
353  ASSERT_TRUE(cm.switchController(stop, next_start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // clean-up
354  }
355  { // test same hardware interface switch
356  std::vector<std::string> start, stop, next_start;
357  start.push_back("group_pos");
358  ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT));
359 
360  next_start.push_back("another_group_pos");
361  ASSERT_TRUE(cm.switchController(next_start, start, controller_manager_msgs::SwitchControllerRequest::STRICT));
362 
363  ASSERT_TRUE(cm.switchController(stop, next_start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // clean-up
364  }
365  { // test vel group
366  std::vector<std::string> start, stop;
367  start.push_back("group_vel");
368  ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT));
369  ASSERT_TRUE(cm.switchController(stop, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // clean-up
370  }
371  { // test eff group
372  std::vector<std::string> start, stop;
373  start.push_back("group_eff");
374  ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT));
375  ASSERT_TRUE(cm.switchController(stop, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // clean-up
376  }
377 
378  { // test direct hardware interface upgrades (okay) and downgrades (conflict)
379  std::vector<std::string> start, stop, next_start;
380  start.push_back("group_eff");
381  ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT));
382 
383  next_start.push_back("group_vel");
384  ASSERT_TRUE(cm.switchController(next_start, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // upgrade
385 
386  ASSERT_FALSE(cm.switchController(start, next_start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // downgrade
387 
388  ASSERT_TRUE(cm.switchController(stop, next_start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // clean-up
389  }
390 
391  { // test single pos
392  std::vector<std::string> start, stop;
393  start.push_back("single_pos");
394  ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT));
395  ASSERT_TRUE(cm.switchController(stop, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // clean-up
396  }
397  { // test single eff
398  std::vector<std::string> start, stop;
399  start.push_back("single_pos");
400  ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT));
401  ASSERT_TRUE(cm.switchController(stop, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // clean-up
402  }
403  { // test single pos + group_eff (resource conflict)
404  std::vector<std::string> start, stop;
405  start.push_back("single_pos");
406  start.push_back("group_eff");
407  ASSERT_FALSE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT));
408  }
409  { // test single pos + group_eff (hardware interface conflict)
410  std::vector<std::string> start, stop;
411  start.push_back("single_eff");
412  start.push_back("group_vel");
413  ASSERT_FALSE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT));
414  }
415  { // test single pos + group_vel
416  std::vector<std::string> start, stop;
417  start.push_back("single_pos");
418  start.push_back("group_vel");
419  ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT));
420  ASSERT_TRUE(cm.switchController(stop, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // clean-up
421  }
422  { // test single pos + group_vel + totally_random_name
423  std::vector<std::string> start, stop;
424  start.push_back("single_pos");
425  start.push_back("group_vel");
426  start.push_back("totally_random_name");
427  ASSERT_FALSE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT));
428  ASSERT_FALSE(cm.switchController(stop, start, controller_manager_msgs::SwitchControllerRequest::STRICT));
429 
430  ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::BEST_EFFORT));
431  ASSERT_TRUE(cm.switchController(stop, start, controller_manager_msgs::SwitchControllerRequest::BEST_EFFORT)); // clean-up
432  }
433  { // test restart
434  std::vector<std::string> start, stop;
435  start.push_back("single_pos");
436  ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT));
437 
438  ASSERT_TRUE(cm.switchController(start, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // restart
439  ASSERT_TRUE(bot.checkUnqiue());
440 
441  ASSERT_TRUE(cm.switchController(stop, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // clean-up
442  }
443  { // test stop for controller that is not running
444  std::vector<std::string> start, stop;
445  stop.push_back("single_pos");
446  ASSERT_FALSE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT));
447  ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::BEST_EFFORT));
448  }
449  { // test stop for controller that is not running
450  std::vector<std::string> start, stop;
451  start.push_back("single_pos");
452  ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT));
453  ASSERT_FALSE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::STRICT));
454  ASSERT_TRUE(cm.switchController(start, stop, controller_manager_msgs::SwitchControllerRequest::BEST_EFFORT));
455  ASSERT_TRUE(cm.switchController(stop, start, controller_manager_msgs::SwitchControllerRequest::STRICT)); // clean-up
456  }
457  ASSERT_TRUE(bot.checkNotRunning());
458 }
459 
460 int main(int argc, char **argv)
461 {
462  testing::InitGoogleTest(&argc, argv);
463  ros::init(argc, argv, "controller_manager_switch_test");
464 
465  ros::AsyncSpinner spinner(1);
466  spinner.start();
467  int ret = RUN_ALL_TESTS();
468  return ret;
469 }
int main(int argc, char **argv)
TEST(SwitchInterfacesTest, SwitchInterfaces)
virtual bool initRequest(hardware_interface::RobotHW *, ros::NodeHandle &, ros::NodeHandle &controller_nh, ClaimedResources &claimed_resources)
ROS Controller Manager and Runner.
#define ROS_FATAL(...)
virtual void doSwitch(const std::list< ControllerInfo > &, const std::list< ControllerInfo > &)
ROSCPP_DECL void start()
bool loadController(const std::string &name)
Load a new controller by name.
bool switchController(const std::vector< std::string > &start_controllers, const std::vector< std::string > &stop_controllers, const int strictness)
Switch multiple controllers simultaneously.
hardware_interface::JointStateHandle jsh_
std::set< std::string > resources
std::set< std::string > interfaces_
virtual controller_interface::ControllerBaseSharedPtr createInstance(const std::string &lookup_name)
std::vector< std::string > stopped_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
std::vector< hardware_interface::InterfaceResources > ClaimedResources
hardware_interface::PositionJointInterface pji_
T intersect(const T &v1, const T &v2)
bool checkUnqiue() const
virtual std::vector< std::string > getDeclaredClasses()
virtual bool prepareSwitch(const std::list< ControllerInfo > &start_list, const std::list< ControllerInfo > &stop_list)
std::vector< std::string > started_
std::map< std::string, JointSharedPtr > joints_
void add(const std::string type)
virtual void doSwitch(const std::list< hardware_interface::ControllerInfo > &start_list, const std::list< hardware_interface::ControllerInfo > &stop_list)
boost::shared_ptr< Joint > JointSharedPtr
void doSwitch(const std::string &n)
bool checkNotRunning() const
hardware_interface::EffortJointInterface eji_
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
boost::shared_ptr< ControllerBase > ControllerBaseSharedPtr
DummyController(const std::string &name)
JointSharedPtr makeJoint(const std::string &name)
std::string current_
virtual bool prepareSwitch(const std::list< hardware_interface::ControllerInfo > &start_list, const std::list< hardware_interface::ControllerInfo > &stop_list)
void registerHandle(const JointStateHandle &handle)
void update(controller_manager::ControllerManager &cm, const ros::TimerEvent &e)
Joint(const std::string &n, hardware_interface::JointStateInterface &iface)
std::map< std::string, std::string > classes
void registerHandle(Interface &iface, bool can_switch)
bool getParam(const std::string &key, std::string &s) const
virtual void update(const ros::Time &, const ros::Duration &)
bool prepareSwitch(const std::string &n)
ROSCPP_DECL void shutdown()
void registerControllerLoader(ControllerLoaderInterfaceSharedPtr controller_loader)
Register a controller loader.
#define ROS_ERROR_STREAM(args)
void update(const ros::Time &time, const ros::Duration &period, bool reset_controllers=false)
Update all active controllers.
#define ROS_ERROR(...)
virtual std::string getHardwareInterfaceType() const
hardware_interface::VelocityJointInterface vji_
ROSCPP_DECL void waitForShutdown()
bool operator<(hardware_interface::ControllerInfo const &i1, hardware_interface::ControllerInfo const &i2)
hardware_interface::JointStateInterface jsi_


controller_manager
Author(s): Wim Meeussen, Mathias L├╝dtke
autogenerated on Mon Apr 20 2020 03:52:09