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


combined_robot_hw_tests
Author(s): Toni Oliver
autogenerated on Mon Apr 20 2020 03:52:11