38 #include <gtest/gtest.h>
40 #include <boost/thread.hpp>
43 #include <pr2_mechanism_msgs/LoadController.h>
44 #include <pr2_mechanism_msgs/UnloadController.h>
45 #include <pr2_mechanism_msgs/ListControllers.h>
46 #include <pr2_mechanism_msgs/SwitchController.h>
47 #include <sensor_msgs/JointState.h>
48 #include <pr2_mechanism_msgs/MechanismStatistics.h>
49 #include <diagnostic_msgs/DiagnosticArray.h>
76 if (!
msg->status.empty()){
77 bool found_joint =
false;
78 bool found_controller =
false;
79 for (
unsigned int i=0;
i<
msg->status.size();
i++){
80 if (!found_joint &&
msg->status[
i].name.substr(0,5) ==
"Joint")
82 if (!found_controller &&
msg->status[
i].name.substr(0,10) ==
"Controller")
88 void callbackJs(
const sensor_msgs::JointStateConstPtr& msg)
90 if (!
msg->name.empty())
94 void callbackMs(
const pr2_mechanism_msgs::MechanismStatisticsConstPtr& msg)
99 void callback1(
const sensor_msgs::JointStateConstPtr& msg)
109 void callback4(
const sensor_msgs::JointStateConstPtr& msg)
117 pr2_mechanism_msgs::LoadController srv_msg;
118 srv_msg.request.name = name;
120 return srv_msg.response.ok;
126 pr2_mechanism_msgs::UnloadController srv_msg;
127 srv_msg.request.name = name;
129 return srv_msg.response.ok;
132 bool switchController(
const std::vector<std::string>& start,
const std::vector<std::string>& stop,
int strictness)
135 pr2_mechanism_msgs::SwitchController srv_msg;
136 srv_msg.request.start_controllers =
start;
137 srv_msg.request.stop_controllers = stop;
138 srv_msg.request.strictness = strictness;
140 return srv_msg.response.ok;
146 pr2_mechanism_msgs::ListControllers srv_msg;
148 return srv_msg.response.controllers.size();
154 pr2_mechanism_msgs::ListControllers srv_msg;
156 for (
unsigned int i=0;
i<srv_msg.response.controllers.size();
i++){
157 if (name == srv_msg.response.controllers[
i]){
158 if (srv_msg.response.state[
i] ==
"running")
return _running;
159 else if (srv_msg.response.state[
i] ==
"stopped")
return _stopped;
168 unsigned int random = rand();
169 random = random % 10;
172 return "controller" +
s.str();
180 for (
unsigned int i=0;
i<times;
i++){
181 unsigned int random = rand();
193 std::vector<std::string>
start, stop;
194 unsigned int start_size = rand() %10;
195 unsigned int stop_size = rand() %10;
196 for (
unsigned int i=0;
i<start_size;
i++)
198 for (
unsigned int i=0;
i<stop_size;
i++)
261 EXPECT_EQ(controllerState(
"controller1"),
_running);
262 EXPECT_EQ(controllerState(
"controller2"),
_stopped);
263 EXPECT_EQ(controllerState(
"controller3"),
_running);
264 EXPECT_EQ(controllerState(
"controller4"),
_running);
265 EXPECT_EQ(controllerState(
"controller5"),
_stopped);
266 EXPECT_EQ(controllerState(
"controller6"),
_stopped);
267 EXPECT_EQ(controllerState(
"controller7"),
_unloaded);
268 EXPECT_EQ(controllerState(
"controller8"),
_unloaded);
269 EXPECT_EQ(controllerState(
"controller9"),
_unloaded);
277 EXPECT_EQ(controllerState(
"controller1"),
_running);
278 EXPECT_EQ(controllerState(
"controller2"),
_stopped);
279 EXPECT_EQ(controllerState(
"controller3"),
_running);
280 EXPECT_EQ(controllerState(
"controller4"),
_running);
281 EXPECT_EQ(controllerState(
"controller5"),
_stopped);
282 EXPECT_EQ(controllerState(
"controller6"),
_stopped);
283 EXPECT_EQ(controllerState(
"controller7"),
_unloaded);
284 EXPECT_EQ(controllerState(
"controller8"),
_unloaded);
285 EXPECT_EQ(controllerState(
"controller9"),
_unloaded);
288 EXPECT_FALSE(loadController(
"controller1"));
289 EXPECT_FALSE(loadController(
"controller2"));
290 EXPECT_FALSE(loadController(
"controller3"));
291 EXPECT_FALSE(loadController(
"controller4"));
292 EXPECT_FALSE(loadController(
"controller5"));
293 EXPECT_FALSE(loadController(
"controller6"));
296 EXPECT_TRUE(loadController(
"controller7"));
299 EXPECT_FALSE(loadController(
"controller9"));
300 EXPECT_FALSE(loadController(
"controller11"));
301 EXPECT_FALSE(loadController(
"controller12"));
302 EXPECT_FALSE(loadController(
"controller13"));
303 EXPECT_FALSE(loadController(
"controller14"));
304 EXPECT_FALSE(loadController(
"controller15"));
305 EXPECT_FALSE(loadController(
"controller16"));
306 EXPECT_FALSE(loadController(
"controller17"));
307 EXPECT_FALSE(loadController(
"controller18"));
308 EXPECT_FALSE(loadController(
"controller19"));
311 EXPECT_FALSE(loadController(
"controller10"));
314 EXPECT_EQ(controllerState(
"controller1"),
_running);
315 EXPECT_EQ(controllerState(
"controller2"),
_stopped);
316 EXPECT_EQ(controllerState(
"controller3"),
_running);
317 EXPECT_EQ(controllerState(
"controller4"),
_running);
318 EXPECT_EQ(controllerState(
"controller5"),
_stopped);
319 EXPECT_EQ(controllerState(
"controller6"),
_stopped);
320 EXPECT_EQ(controllerState(
"controller7"),
_stopped);
321 EXPECT_EQ(controllerState(
"controller8"),
_unloaded);
322 EXPECT_EQ(controllerState(
"controller9"),
_unloaded);
330 EXPECT_EQ(controllerState(
"controller1"),
_running);
331 EXPECT_EQ(controllerState(
"controller2"),
_stopped);
332 EXPECT_EQ(controllerState(
"controller3"),
_running);
333 EXPECT_EQ(controllerState(
"controller4"),
_running);
334 EXPECT_EQ(controllerState(
"controller5"),
_stopped);
335 EXPECT_EQ(controllerState(
"controller6"),
_stopped);
336 EXPECT_EQ(controllerState(
"controller7"),
_stopped);
337 EXPECT_EQ(controllerState(
"controller8"),
_unloaded);
338 EXPECT_EQ(controllerState(
"controller9"),
_unloaded);
341 EXPECT_FALSE(unloadController(
"controller1"));
342 EXPECT_FALSE(unloadController(
"controller3"));
343 EXPECT_FALSE(unloadController(
"controller4"));
346 EXPECT_TRUE(unloadController(
"controller2"));
347 EXPECT_TRUE(unloadController(
"controller5"));
348 EXPECT_TRUE(unloadController(
"controller6"));
349 EXPECT_TRUE(unloadController(
"controller7"));
352 EXPECT_FALSE(unloadController(
"controller8"));
355 EXPECT_EQ(controllerState(
"controller1"),
_running);
356 EXPECT_EQ(controllerState(
"controller2"),
_unloaded);
357 EXPECT_EQ(controllerState(
"controller3"),
_running);
358 EXPECT_EQ(controllerState(
"controller4"),
_running);
359 EXPECT_EQ(controllerState(
"controller5"),
_unloaded);
360 EXPECT_EQ(controllerState(
"controller6"),
_unloaded);
361 EXPECT_EQ(controllerState(
"controller7"),
_unloaded);
362 EXPECT_EQ(controllerState(
"controller8"),
_unloaded);
363 EXPECT_EQ(controllerState(
"controller9"),
_unloaded);
371 EXPECT_EQ(controllerState(
"controller1"),
_running);
372 EXPECT_EQ(controllerState(
"controller2"),
_unloaded);
373 EXPECT_EQ(controllerState(
"controller3"),
_running);
374 EXPECT_EQ(controllerState(
"controller4"),
_running);
375 EXPECT_EQ(controllerState(
"controller5"),
_unloaded);
376 EXPECT_EQ(controllerState(
"controller6"),
_unloaded);
377 EXPECT_EQ(controllerState(
"controller7"),
_unloaded);
378 EXPECT_EQ(controllerState(
"controller8"),
_unloaded);
379 EXPECT_EQ(controllerState(
"controller9"),
_unloaded);
382 std::vector<std::string>
start, stop;
383 start.push_back(
"controller1");
384 EXPECT_TRUE(switchController(
start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
385 EXPECT_EQ(controllerState(
"controller1"),
_running);
388 start.push_back(
"controller2");
389 EXPECT_FALSE(switchController(
start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
390 EXPECT_EQ(controllerState(
"controller1"),
_running);
391 EXPECT_EQ(controllerState(
"controller2"),
_unloaded);
394 EXPECT_TRUE(loadController(
"controller2"));
395 EXPECT_TRUE(switchController(
start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
396 EXPECT_EQ(controllerState(
"controller1"),
_running);
397 EXPECT_EQ(controllerState(
"controller2"),
_running);
400 stop.push_back(
"controller2");
401 EXPECT_TRUE(switchController(
start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
402 EXPECT_EQ(controllerState(
"controller1"),
_running);
403 EXPECT_EQ(controllerState(
"controller2"),
_running);
406 stop.push_back(
"controller5");
407 EXPECT_FALSE(switchController(
start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
408 EXPECT_EQ(controllerState(
"controller1"),
_running);
409 EXPECT_EQ(controllerState(
"controller2"),
_running);
410 EXPECT_EQ(controllerState(
"controller5"),
_unloaded);
413 stop.push_back(
"controller4");
414 EXPECT_FALSE(switchController(
start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
415 EXPECT_EQ(controllerState(
"controller1"),
_running);
416 EXPECT_EQ(controllerState(
"controller2"),
_running);
417 EXPECT_EQ(controllerState(
"controller4"),
_running);
418 EXPECT_EQ(controllerState(
"controller5"),
_unloaded);
421 EXPECT_TRUE(loadController(
"controller5"));
422 EXPECT_TRUE(switchController(
start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
423 EXPECT_EQ(controllerState(
"controller1"),
_running);
424 EXPECT_EQ(controllerState(
"controller2"),
_running);
425 EXPECT_EQ(controllerState(
"controller4"),
_stopped);
426 EXPECT_EQ(controllerState(
"controller5"),
_stopped);
429 stop.push_back(
"controller3");
430 EXPECT_TRUE(switchController(
start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
431 EXPECT_EQ(controllerState(
"controller1"),
_running);
432 EXPECT_EQ(controllerState(
"controller2"),
_running);
433 EXPECT_EQ(controllerState(
"controller3"),
_stopped);
434 EXPECT_EQ(controllerState(
"controller4"),
_stopped);
435 EXPECT_EQ(controllerState(
"controller5"),
_stopped);
444 EXPECT_EQ(controllerState(
"controller1"),
_running);
445 EXPECT_EQ(controllerState(
"controller2"),
_running);
446 EXPECT_EQ(controllerState(
"controller3"),
_stopped);
447 EXPECT_EQ(controllerState(
"controller4"),
_stopped);
448 EXPECT_EQ(controllerState(
"controller5"),
_stopped);
449 EXPECT_EQ(controllerState(
"controller6"),
_unloaded);
450 EXPECT_EQ(controllerState(
"controller7"),
_unloaded);
451 EXPECT_EQ(controllerState(
"controller8"),
_unloaded);
452 EXPECT_EQ(controllerState(
"controller9"),
_unloaded);
455 std::vector<std::string>
start, stop;
456 start.push_back(
"controller1");
457 EXPECT_TRUE(switchController(
start, stop, pr2_mechanism_msgs::SwitchController::Request::BEST_EFFORT));
458 EXPECT_EQ(controllerState(
"controller1"),
_running);
461 start.push_back(
"controller3");
462 start.push_back(
"controller6");
463 EXPECT_TRUE(switchController(
start, stop, pr2_mechanism_msgs::SwitchController::Request::BEST_EFFORT));
464 EXPECT_EQ(controllerState(
"controller1"),
_running);
465 EXPECT_EQ(controllerState(
"controller3"),
_running);
466 EXPECT_EQ(controllerState(
"controller6"),
_unloaded);
475 EXPECT_EQ(controllerState(
"controller1"),
_running);
476 EXPECT_EQ(controllerState(
"controller4"),
_stopped);
478 callback1_timing_.resize(1000);
479 callback1_counter_ = 0;
482 ros::Subscriber sub1 = node_.subscribe<sensor_msgs::JointState>(
"controller1/my_topic", 100,
483 [
this](
auto&
msg){ callback1(
msg); });
484 ros::Subscriber sub4 = node_.subscribe<sensor_msgs::JointState>(
"controller4/my_topic", 100,
485 [
this](
auto&
msg){ callback4(
msg); });
487 std::string not_started =
"not_started";
488 callback1_name_ = not_started;
489 callback4_name_ = not_started;
496 EXPECT_EQ(callback1_name_,
"");
497 EXPECT_EQ(callback4_name_, not_started);
500 for (
unsigned int i=0;
i<1000;
i++){
501 EXPECT_LE(callback1_effort_, 0.0);
505 std::string test_message =
"Hoe gaat het met Wim?";
506 ros::ServiceClient srv_client1 = node_.serviceClient<pr2_mechanism_msgs::LoadController>(
"controller1/my_service");
507 ros::ServiceClient srv_client4 = node_.serviceClient<pr2_mechanism_msgs::LoadController>(
"controller1/my_service");
508 pr2_mechanism_msgs::LoadController srv_msg;
509 srv_msg.request.name = test_message;
510 EXPECT_TRUE(srv_client1.
call(srv_msg));
511 EXPECT_TRUE(srv_client4.call(srv_msg));
514 EXPECT_EQ(callback1_name_, test_message);
515 EXPECT_EQ(callback4_name_, not_started);
526 EXPECT_EQ(controllerState(
"controller1"),
_running);
529 ros::Subscriber sub1 = node_.subscribe<sensor_msgs::JointState>(
"controller1/my_topic", 100,
530 [
this](
auto&
msg){ callback1(
msg); });
532 callback1_counter_ = 0;
537 callback1_counter_ = 0;
540 if (callback1_counter_ < 450){
541 unsigned int nr = callback1_counter_;
543 for (
unsigned int i=0;
i<nr-1;
i++){
544 ROS_ERROR(
" - %u: time %f, delta time %f",
i, callback1_timing_[
i].toSec(), (callback1_timing_[
i+1]-callback1_timing_[
i]).toSec());
547 EXPECT_NEAR(callback1_counter_, 500, 50);
557 ros::Subscriber sub_js = node_.subscribe<sensor_msgs::JointState>(
"joint_states", 100,
558 [
this](
auto&
msg){ callbackJs(
msg); });
559 callback_js_counter_ = 0;
564 callback_js_counter_ = 0;
566 EXPECT_NEAR(callback_js_counter_, 1000, 40);
570 ros::Subscriber sub_ms = node_.subscribe<pr2_mechanism_msgs::MechanismStatistics>(
"mechanism_statistics", 100,
571 [
this](
auto&
msg){ callbackMs(
msg); });
572 callback_ms_counter_ = 0;
576 callback_ms_counter_ = 0;
578 EXPECT_NEAR(callback_ms_counter_, 10, 2);
588 ros::Subscriber sub_diagnostics = node_.subscribe<diagnostic_msgs::DiagnosticArray>(
"/diagnostics", 100,
589 [
this](
auto&
msg){ callbackDiagnostic(
msg); });
590 joint_diagnostic_counter_ = 0;
591 controller_diagnostic_counter_ = 0;
593 EXPECT_GT(joint_diagnostic_counter_, (
unsigned int)1);
594 EXPECT_GT(controller_diagnostic_counter_, (
unsigned int)1);
605 randomBombardment(1000);
612 boost::thread bomb1(boost::bind(&TestController_multithread_bombardment_Test::randomBombardment,
this, 500));
613 boost::thread bomb2(boost::bind(&TestController_multithread_bombardment_Test::randomBombardment,
this, 500));
614 boost::thread bomb3(boost::bind(&TestController_multithread_bombardment_Test::randomBombardment,
this, 500));
615 boost::thread bomb4(boost::bind(&TestController_multithread_bombardment_Test::randomBombardment,
this, 500));
626 int main(
int argc,
char** argv)
628 testing::InitGoogleTest(&argc, argv);
636 int res = RUN_ALL_TESTS();