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")
81 joint_diagnostic_counter_++;
82 if (!found_controller && msg->status[i].name.substr(0,10) ==
"Controller")
83 controller_diagnostic_counter_++;
88 void callbackJs(
const sensor_msgs::JointStateConstPtr& msg)
90 if (!msg->name.empty())
91 callback_js_counter_++;
94 void callbackMs(
const pr2_mechanism_msgs::MechanismStatisticsConstPtr& msg)
96 callback_ms_counter_++;
99 void callback1(
const sensor_msgs::JointStateConstPtr& msg)
101 if (callback1_counter_ < 1000){
104 callback1_name_ = msg->name[0];
105 callback1_effort_ = msg->effort[0];
106 callback1_counter_++;
109 void callback4(
const sensor_msgs::JointStateConstPtr& msg)
111 callback4_name_ = msg->name[0];
117 pr2_mechanism_msgs::LoadController srv_msg;
118 srv_msg.request.name = name;
119 if (!load_srv_.
call(srv_msg))
return false;
120 return srv_msg.response.ok;
126 pr2_mechanism_msgs::UnloadController srv_msg;
127 srv_msg.request.name = name;
128 if (!unload_srv_.
call(srv_msg))
return false;
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;
139 if (!switch_srv_.
call(srv_msg))
return false;
140 return srv_msg.response.ok;
146 pr2_mechanism_msgs::ListControllers srv_msg;
147 if (!list_srv_.
call(srv_msg))
return 0;;
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++)
201 switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT);
203 switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::BEST_EFFORT);
218 : callback1_timing_(1000)
235 load_srv_ = node_.
serviceClient<pr2_mechanism_msgs::LoadController>(
"pr2_controller_manager/load_controller");
236 unload_srv_ = node_.
serviceClient<pr2_mechanism_msgs::UnloadController>(
"pr2_controller_manager/unload_controller");
237 switch_srv_ = node_.
serviceClient<pr2_mechanism_msgs::SwitchController>(
"pr2_controller_manager/switch_controller");
238 list_srv_ = node_.
serviceClient<pr2_mechanism_msgs::ListControllers>(
"pr2_controller_manager/list_controllers");
382 std::vector<std::string>
start, stop;
383 start.push_back(
"controller1");
384 EXPECT_TRUE(
switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
388 start.push_back(
"controller2");
389 EXPECT_FALSE(
switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
395 EXPECT_TRUE(
switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
400 stop.push_back(
"controller2");
401 EXPECT_TRUE(
switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
406 stop.push_back(
"controller5");
407 EXPECT_FALSE(
switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
413 stop.push_back(
"controller4");
414 EXPECT_FALSE(
switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
422 EXPECT_TRUE(
switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
429 stop.push_back(
"controller3");
430 EXPECT_TRUE(
switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
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));
461 start.push_back(
"controller3");
462 start.push_back(
"controller6");
463 EXPECT_TRUE(
switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::BEST_EFFORT));
483 boost::bind(&TestController_service_and_realtime_publisher_Test::callback1,
this, _1));
485 boost::bind(&TestController_service_and_realtime_publisher_Test::callback4,
this, _1));
487 std::string not_started =
"not_started";
500 for (
unsigned int i=0; i<1000; i++){
505 std::string test_message =
"Hoe gaat het met Wim?";
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));
530 boost::bind(&TestController_publisher_hz_Test::callback1,
this, _1));
535 while (callback1_counter_ == 0 &&
ros::Time::now() - start < timeout)
537 callback1_counter_ = 0;
540 if (callback1_counter_ < 450){
543 for (
unsigned int i=0; i<nr-1; i++){
547 EXPECT_NEAR(callback1_counter_, 500, 50);
558 boost::bind(&TestController_manager_hz_Test::callbackJs,
this, _1));
562 while (callback_js_counter_ == 0 &&
ros::Time::now() - start < timeout)
564 callback_js_counter_ = 0;
566 EXPECT_NEAR(callback_js_counter_, 1000, 40);
571 boost::bind(&TestController_manager_hz_Test::callbackMs,
this, _1));
574 while (callback_ms_counter_ == 0 &&
ros::Time::now() - start < timeout)
576 callback_ms_counter_ = 0;
578 EXPECT_NEAR(callback_ms_counter_, 10, 2);
589 boost::bind(&TestController_manager_hz_Test::callbackDiagnostic,
this, _1));
593 EXPECT_GT(joint_diagnostic_counter_, (
unsigned int)1);
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);
634 boost::thread spinner(boost::bind(&
ros::spin));
636 int res = RUN_ALL_TESTS();
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
unsigned int joint_diagnostic_counter_
unsigned int controllerState(const std::string &name)
unsigned int controller_diagnostic_counter_
static const unsigned int _failure
ros::ServiceClient switch_srv_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
unsigned int nrControllers()
void callbackJs(const sensor_msgs::JointStateConstPtr &msg)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
std::vector< ros::Time > callback1_timing_
void callback4(const sensor_msgs::JointStateConstPtr &msg)
bool switchController(const std::vector< std::string > &start, const std::vector< std::string > &stop, int strictness)
ros::ServiceClient load_srv_
void callback1(const sensor_msgs::JointStateConstPtr &msg)
std::string callback4_name_
static bool bombardment_started_
~TestController()
Destructor.
unsigned int callback_ms_counter_
bool unloadController(const std::string &name)
TestController()
constructor
static const unsigned int _running
std::string callback1_name_
void callbackDiagnostic(const diagnostic_msgs::DiagnosticArrayConstPtr &msg)
ros::ServiceClient unload_srv_
static const unsigned int _unloaded
int main(int argc, char **argv)
void callbackMs(const pr2_mechanism_msgs::MechanismStatisticsConstPtr &msg)
TEST_F(TestController, spawner)
bool loadController(const std::string &name)
unsigned int callback_js_counter_
std::string randomController()
IMETHOD void random(Vector &a)
void randomBombardment(unsigned int times)
ROSCPP_DECL bool waitForService(const std::string &service_name, int32_t timeout)
ros::ServiceClient list_srv_
unsigned int callback1_counter_
static const unsigned int _stopped