cm_test.cpp
Go to the documentation of this file.
1 // Copyright (C) 2012, hiDOF INC.
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 hiDOF Inc 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 
30 
31 #include <ros/ros.h>
32 #include <gtest/gtest.h>
33 
34 #include <controller_manager_msgs/ListControllers.h>
35 #include <controller_manager_msgs/ListControllerTypes.h>
36 #include <controller_manager_msgs/LoadController.h>
37 #include <controller_manager_msgs/SwitchController.h>
38 #include <controller_manager_msgs/UnloadController.h>
39 
40 
41 using namespace controller_manager_msgs;
42 
43 TEST(CMTests, loadUnloadOk)
44 {
45  ros::NodeHandle nh;
46  ros::ServiceClient load_client = nh.serviceClient<LoadController>("/controller_manager/load_controller");
47  ros::ServiceClient unload_client = nh.serviceClient<UnloadController>("/controller_manager/unload_controller");
48 
49  // Load single-interface controller
50  {
51  LoadController srv;
52  srv.request.name = "my_controller";
53  bool call_success = load_client.call(srv);
54  ASSERT_TRUE(call_success);
55  EXPECT_TRUE(srv.response.ok);
56  }
57 
58  // Load multi-interface controller:
59  // Two required interfaces
60  {
61  LoadController srv;
62  srv.request.name = "vel_eff_controller";
63  bool call_success = load_client.call(srv);
64  ASSERT_TRUE(call_success);
65  EXPECT_TRUE(srv.response.ok);
66  }
67 
68  // Load multi-interface controller:
69  // One required (and existing) interface and one optional (and non-existent) interface
70  {
71  LoadController srv;
72  srv.request.name = "optional_interfaces_controller";
73  bool call_success = load_client.call(srv);
74  ASSERT_TRUE(call_success);
75  EXPECT_TRUE(srv.response.ok);
76  }
77 
78  // Unload single-interface controller
79  {
80  UnloadController srv;
81  srv.request.name = "my_controller";
82  bool call_success = unload_client.call(srv);
83  ASSERT_TRUE(call_success);
84  EXPECT_TRUE(srv.response.ok);
85  }
86 
87  // Unload multi-interface controllers
88  {
89  UnloadController srv;
90  srv.request.name = "vel_eff_controller";
91  bool call_success = unload_client.call(srv);
92  ASSERT_TRUE(call_success);
93  EXPECT_TRUE(srv.response.ok);
94  }
95  {
96  UnloadController srv;
97  srv.request.name = "optional_interfaces_controller";
98  bool call_success = unload_client.call(srv);
99  ASSERT_TRUE(call_success);
100  EXPECT_TRUE(srv.response.ok);
101  }
102 }
103 
104 TEST(CMTests, loadUnloadKo)
105 {
106  ros::NodeHandle nh;
107  ros::ServiceClient load_client = nh.serviceClient<LoadController>("/controller_manager/load_controller");
108  ros::ServiceClient unload_client = nh.serviceClient<UnloadController>("/controller_manager/unload_controller");
109 
110  // Load non-existent controller
111  {
112  LoadController srv;
113  srv.request.name = "nonexistent_controller";
114  bool call_success = load_client.call(srv);
115  ASSERT_TRUE(call_success);
116  EXPECT_FALSE(srv.response.ok);
117  }
118 
119  // Load controller requesting non-existient HW interface
120  {
121  LoadController srv;
122  srv.request.name = "non_existent_interface_controller";
123  bool call_success = load_client.call(srv);
124  ASSERT_TRUE(call_success);
125  EXPECT_FALSE(srv.response.ok);
126  }
127 
128  // Load controller requesting non-existent resource from valid HW interface
129  {
130  LoadController srv;
131  srv.request.name = "non_existent_resource_controller";
132  bool call_success = load_client.call(srv);
133  ASSERT_TRUE(call_success);
134  EXPECT_FALSE(srv.response.ok);
135  }
136 
137  // Load multi-interface controller:
138  // Two required HW interfaces, of which one is non-existent
139  {
140  LoadController srv;
141  srv.request.name = "non_existing_multi_interface_controller";
142  bool call_success = load_client.call(srv);
143  ASSERT_TRUE(call_success);
144  EXPECT_FALSE(srv.response.ok);
145  }
146 
147  // Unload not loaded controller
148  {
149  UnloadController srv;
150  srv.request.name = "my_controller";
151  bool call_success = unload_client.call(srv);
152  ASSERT_TRUE(call_success);
153  EXPECT_FALSE(srv.response.ok);
154  }
155 }
156 
157 TEST(CMTests, switchController)
158 {
159  ros::NodeHandle nh;
160  ros::ServiceClient load_client = nh.serviceClient<LoadController>("/controller_manager/load_controller");
161  ros::ServiceClient unload_client = nh.serviceClient<UnloadController>("/controller_manager/unload_controller");
162  ros::ServiceClient switch_client = nh.serviceClient<SwitchController>("/controller_manager/switch_controller");
163 
164  // Load controllers
165  {
166  LoadController srv;
167  srv.request.name = "my_controller";
168  load_client.call(srv);
169  srv.request.name = "my_controller2";
170  load_client.call(srv);
171  srv.request.name = "vel_eff_controller";
172  load_client.call(srv);
173  }
174 
175  // Successful STRICT start
176  {
177  SwitchController srv;
178  srv.request.start_controllers.push_back("my_controller");
179  srv.request.strictness = srv.request.STRICT;
180  bool call_success = switch_client.call(srv);
181  ASSERT_TRUE(call_success);
182  EXPECT_TRUE(srv.response.ok);
183  }
184 
185  // Successful STRICT stop
186  {
187  SwitchController srv;
188  srv.request.stop_controllers.push_back("my_controller");
189  srv.request.strictness = srv.request.STRICT;
190  bool call_success = switch_client.call(srv);
191  ASSERT_TRUE(call_success);
192  EXPECT_TRUE(srv.response.ok);
193  }
194 
195  // Successful STRICT start
196  {
197  SwitchController srv;
198  srv.request.start_controllers.push_back("vel_eff_controller");
199  srv.request.strictness = srv.request.STRICT;
200  bool call_success = switch_client.call(srv);
201  ASSERT_TRUE(call_success);
202  EXPECT_TRUE(srv.response.ok);
203  }
204 
205  // Successful STRICT stop+start
206  {
207  SwitchController srv;
208  srv.request.start_controllers.push_back("my_controller");
209  srv.request.stop_controllers.push_back("vel_eff_controller");
210  srv.request.strictness = srv.request.STRICT;
211  bool call_success = switch_client.call(srv);
212  ASSERT_TRUE(call_success);
213  EXPECT_TRUE(srv.response.ok);
214  }
215 
216  // Back to no running controllers
217  {
218  SwitchController srv;
219  srv.request.stop_controllers.push_back("my_controller");
220  srv.request.strictness = srv.request.STRICT;
221  bool call_success = switch_client.call(srv);
222  ASSERT_TRUE(call_success);
223  EXPECT_TRUE(srv.response.ok);
224  }
225 
226  // Unsuccessful STRICT start
227  {
228  SwitchController srv;
229  srv.request.start_controllers.push_back("non_existent_controller");
230  srv.request.strictness = srv.request.STRICT;
231  bool call_success = switch_client.call(srv);
232  ASSERT_TRUE(call_success);
233  EXPECT_FALSE(srv.response.ok);
234  }
235 
236  // Unsuccessful STRICT stop
237  {
238  SwitchController srv;
239  srv.request.stop_controllers.push_back("non_existent_controller");
240  srv.request.strictness = srv.request.STRICT;
241  bool call_success = switch_client.call(srv);
242  ASSERT_TRUE(call_success);
243  EXPECT_FALSE(srv.response.ok);
244  }
245 
246  // Unsuccessful STRICT switch: invalid stop
247  {
248  SwitchController srv;
249  srv.request.start_controllers.push_back("my_controller");
250  srv.request.stop_controllers.push_back("non_existent_controller");
251  srv.request.strictness = srv.request.STRICT;
252  bool call_success = switch_client.call(srv);
253  ASSERT_TRUE(call_success);
254  EXPECT_FALSE(srv.response.ok);
255  }
256 
257  // Unsuccessful STRICT switch: invalid start
258  {
259  SwitchController srv;
260  srv.request.start_controllers.push_back("non_existent_controller");
261  srv.request.stop_controllers.push_back("my_controller");
262  srv.request.strictness = srv.request.STRICT;
263  bool call_success = switch_client.call(srv);
264  ASSERT_TRUE(call_success);
265  EXPECT_FALSE(srv.response.ok);
266  }
267 
268  // Unsuccessful STRICT start: Resource conflict within single controller
269  {
270  SwitchController srv;
271  srv.request.start_controllers.push_back("self_conflict_controller");
272  srv.request.strictness = srv.request.STRICT;
273  bool call_success = switch_client.call(srv);
274  ASSERT_TRUE(call_success);
275  EXPECT_FALSE(srv.response.ok);
276  }
277 
278  // Unsuccessful STRICT start: Resource conflict between two controllers
279  {
280  SwitchController srv;
281  srv.request.start_controllers.push_back("my_controller");
282  srv.request.start_controllers.push_back("my_controller2");
283  srv.request.strictness = srv.request.STRICT;
284  bool call_success = switch_client.call(srv);
285  ASSERT_TRUE(call_success);
286  EXPECT_FALSE(srv.response.ok);
287  }
288 
289  // Successful BEST_EFFORT switch: No-op
290  {
291  SwitchController srv;
292  srv.request.start_controllers.push_back("non_existent_controller");
293  srv.request.stop_controllers.push_back("non_existent_controller");
294  srv.request.strictness = srv.request.BEST_EFFORT;
295  bool call_success = switch_client.call(srv);
296  ASSERT_TRUE(call_success);
297  EXPECT_TRUE(srv.response.ok);
298  }
299 
300  // Successful BEST_EFFORT switch: Partial success, only one started controller
301  {
302  SwitchController srv;
303  srv.request.start_controllers.push_back("my_controller2");
304  srv.request.start_controllers.push_back("non_existent_controller");
305  srv.request.stop_controllers.push_back("non_existent_controller");
306  srv.request.stop_controllers.push_back("my_controller");
307  srv.request.strictness = srv.request.BEST_EFFORT;
308  bool call_success = switch_client.call(srv);
309  ASSERT_TRUE(call_success);
310  EXPECT_TRUE(srv.response.ok);
311  }
312 
313  // Successful BEST_EFFORT switch: Partial success, one started and one stopped controller
314  {
315  SwitchController srv;
316  srv.request.start_controllers.push_back("my_controller");
317  srv.request.start_controllers.push_back("non_existent_controller");
318  srv.request.stop_controllers.push_back("non_existent_controller");
319  srv.request.stop_controllers.push_back("my_controller2");
320  srv.request.strictness = srv.request.BEST_EFFORT;
321  bool call_success = switch_client.call(srv);
322  ASSERT_TRUE(call_success);
323  EXPECT_TRUE(srv.response.ok);
324  }
325 
326  // Back to no running controllers
327  {
328  SwitchController srv;
329  srv.request.stop_controllers.push_back("my_controller");
330  srv.request.strictness = srv.request.STRICT;
331  bool call_success = switch_client.call(srv);
332  ASSERT_TRUE(call_success);
333  EXPECT_TRUE(srv.response.ok);
334  }
335 
336  // Unload controllers
337  {
338  UnloadController srv;
339  srv.request.name = "my_controller";
340  unload_client.call(srv);
341  srv.request.name = "my_controller2";
342  unload_client.call(srv);
343  srv.request.name = "vel_eff_controller";
344  unload_client.call(srv);
345  }
346 }
347 
348 TEST(CMTests, stopBeforeUnload)
349 {
350  ros::NodeHandle nh;
351  ros::ServiceClient load_client = nh.serviceClient<LoadController>("/controller_manager/load_controller");
352  ros::ServiceClient unload_client = nh.serviceClient<UnloadController>("/controller_manager/unload_controller");
353  ros::ServiceClient switch_client = nh.serviceClient<SwitchController>("/controller_manager/switch_controller");
354 
355  // Load controller
356  {
357  LoadController srv;
358  srv.request.name = "my_controller";
359  bool call_success = load_client.call(srv);
360  ASSERT_TRUE(call_success);
361  EXPECT_TRUE(srv.response.ok);
362  }
363 
364  // Start controller
365  {
366  SwitchController srv;
367  srv.request.start_controllers.push_back("my_controller");
368  srv.request.strictness = srv.request.STRICT;
369  bool call_success = switch_client.call(srv);
370  ASSERT_TRUE(call_success);
371  EXPECT_TRUE(srv.response.ok);
372  }
373 
374  // Try to unload running controller and fail
375  {
376  UnloadController srv;
377  srv.request.name = "my_controller";
378  bool call_success = unload_client.call(srv);
379  ASSERT_TRUE(call_success);
380  EXPECT_FALSE(srv.response.ok);
381  }
382 
383  // Stop controller
384  {
385  SwitchController srv;
386  srv.request.stop_controllers.push_back("my_controller");
387  srv.request.strictness = srv.request.STRICT;
388  bool call_success = switch_client.call(srv);
389  ASSERT_TRUE(call_success);
390  EXPECT_TRUE(srv.response.ok);
391  }
392 
393  // Unload stopped controller
394  {
395  UnloadController srv;
396  srv.request.name = "my_controller";
397  bool call_success = unload_client.call(srv);
398  ASSERT_TRUE(call_success);
399  EXPECT_TRUE(srv.response.ok);
400  }
401 }
402 
403 TEST(CMTests, listControllerTypes)
404 {
405  ros::NodeHandle nh;
406  ros::ServiceClient types_client = nh.serviceClient<ListControllerTypes>("/controller_manager/list_controller_types");
407 
408  ListControllerTypes srv;
409  bool call_success = types_client.call(srv);
410  ASSERT_TRUE(call_success);
411  // Weak test that the number of available types and base classes is not lower than those defined in this test package
412  EXPECT_GE(srv.response.types.size(), 3);
413  EXPECT_GE(srv.response.base_classes.size(), 3);
414 }
415 
416 TEST(CMTests, listControllers)
417 {
418  ros::NodeHandle nh;
419  ros::ServiceClient load_client = nh.serviceClient<LoadController>("/controller_manager/load_controller");
420  ros::ServiceClient unload_client = nh.serviceClient<UnloadController>("/controller_manager/unload_controller");
421  ros::ServiceClient switch_client = nh.serviceClient<SwitchController>("/controller_manager/switch_controller");
422  ros::ServiceClient list_client = nh.serviceClient<ListControllers>("/controller_manager/list_controllers");
423 
424  // Load controllers
425  {
426  LoadController srv;
427  srv.request.name = "my_controller";
428  load_client.call(srv);
429  srv.request.name = "vel_eff_controller";
430  load_client.call(srv);
431  }
432 
433  // Start one controller
434  {
435  SwitchController srv;
436  srv.request.start_controllers.push_back("my_controller");
437  srv.request.strictness = srv.request.STRICT;
438  switch_client.call(srv);
439  }
440 
441  // List controllers
442  {
443  ListControllers srv;
444  bool call_success = list_client.call(srv);
445  ASSERT_TRUE(call_success);
446  ASSERT_EQ(srv.response.controller.size(), 2);
447 
448  ControllerState state1, state2;
449  if (srv.response.controller[0].name == "my_controller")
450  {
451  state1 = srv.response.controller[0];
452  state2 = srv.response.controller[1];
453  }
454  else
455  {
456  state1 = srv.response.controller[1];
457  state2 = srv.response.controller[0];
458  }
459 
460  EXPECT_EQ(state1.name, "my_controller");
461  EXPECT_EQ(state1.state, "running");
462  EXPECT_EQ(state1.type, "controller_manager_tests/EffortTestController");
463  ASSERT_EQ(state1.claimed_resources.size(), 1);
464  EXPECT_EQ(state1.claimed_resources[0].hardware_interface, "hardware_interface::EffortJointInterface");
465  ASSERT_EQ(state1.claimed_resources[0].resources.size(), 2);
466  EXPECT_EQ(state1.claimed_resources[0].resources[0], "hiDOF_joint1");
467  EXPECT_EQ(state1.claimed_resources[0].resources[1], "hiDOF_joint2");
468 
469  EXPECT_EQ(state2.name, "vel_eff_controller");
470  EXPECT_EQ(state2.state, "stopped");
471  EXPECT_EQ(state2.type, "controller_manager_tests/VelEffController");
472  EXPECT_EQ(state2.claimed_resources.size(), 2);
473  EXPECT_EQ(state2.claimed_resources[0].hardware_interface, "hardware_interface::VelocityJointInterface");
474  ASSERT_EQ(state2.claimed_resources[0].resources.size(), 2);
475  EXPECT_EQ(state2.claimed_resources[0].resources[0], "hiDOF_joint1");
476  EXPECT_EQ(state2.claimed_resources[0].resources[1], "hiDOF_joint2");
477  EXPECT_EQ(state2.claimed_resources[1].hardware_interface, "hardware_interface::EffortJointInterface");
478  ASSERT_EQ(state2.claimed_resources[1].resources.size(), 1);
479  EXPECT_EQ(state2.claimed_resources[1].resources[0], "hiDOF_joint3");
480  }
481 
482  // Stop running controller
483  {
484  SwitchController srv;
485  srv.request.stop_controllers.push_back("my_controller");
486  srv.request.strictness = srv.request.STRICT;
487  switch_client.call(srv);
488  }
489 
490  // Unload controllers
491  {
492  UnloadController srv;
493  srv.request.name = "my_controller";
494  unload_client.call(srv);
495  srv.request.name = "vel_eff_controller";
496  unload_client.call(srv);
497  }
498 }
499 
500 int main(int argc, char** argv)
501 {
502  testing::InitGoogleTest(&argc, argv);
503  ros::init(argc, argv, "ControllerManagerTestNode");
504 
505  ros::AsyncSpinner spinner(1);
506 
507  // wait for services
508  ROS_INFO("Waiting for service");
509  ros::service::waitForService("/controller_manager/load_controller");
510  ROS_INFO("Start tests");
511  spinner.start();
512  int ret = RUN_ALL_TESTS();
513  spinner.stop();
514  ros::shutdown();
515  return ret;
516 }
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
#define ROS_INFO(...)
ROSCPP_DECL void shutdown()
int main(int argc, char **argv)
Definition: cm_test.cpp:500
TEST(CMTests, loadUnloadOk)
Definition: cm_test.cpp:43
ROSCPP_DECL bool waitForService(const std::string &service_name, int32_t timeout)


controller_manager_tests
Author(s): Vijay Pradeep, Adolfo Rodriguez Tsouroukdissian
autogenerated on Fri Jun 7 2019 22:00:10