test.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Wim Meeussen */
00036 
00037 #include <string>
00038 #include <gtest/gtest.h>
00039 #include <ros/ros.h>
00040 #include <boost/thread.hpp>
00041 #include <cstdlib>
00042 
00043 #include <pr2_mechanism_msgs/LoadController.h>
00044 #include <pr2_mechanism_msgs/UnloadController.h>
00045 #include <pr2_mechanism_msgs/ListControllers.h>
00046 #include <pr2_mechanism_msgs/SwitchController.h>
00047 #include <sensor_msgs/JointState.h>
00048 #include <pr2_mechanism_msgs/MechanismStatistics.h>
00049 #include <diagnostic_msgs/DiagnosticArray.h>
00050 
00051 static const unsigned int _failure = 0;
00052 static const unsigned int _unloaded = 1;
00053 static const unsigned int _stopped = 2;
00054 static const unsigned int _running = 3;
00055 
00056 
00057 int g_argc;
00058 char** g_argv;
00059 
00060 static bool bombardment_started_ = false;
00061 
00062 
00063 class TestController : public testing::Test
00064 {
00065 public:
00066   ros::NodeHandle node_;
00067   ros::ServiceClient load_srv_, unload_srv_, switch_srv_, list_srv_;
00068   std::string callback1_name_, callback4_name_;
00069   unsigned int callback1_counter_, callback_js_counter_, callback_ms_counter_;
00070   std::vector<ros::Time> callback1_timing_;
00071   unsigned int joint_diagnostic_counter_, controller_diagnostic_counter_;
00072   double callback1_effort_;
00073 
00074   void callbackDiagnostic(const diagnostic_msgs::DiagnosticArrayConstPtr& msg)
00075   {
00076     if (!msg->status.empty()){
00077       bool found_joint = false;
00078       bool found_controller = false;
00079       for (unsigned int i=0; i<msg->status.size(); i++){
00080         if (!found_joint && msg->status[i].name.substr(0,5) == "Joint")
00081           joint_diagnostic_counter_++;
00082         if (!found_controller && msg->status[i].name.substr(0,10) == "Controller")
00083           controller_diagnostic_counter_++;
00084       }
00085     }
00086   }
00087 
00088   void callbackJs(const sensor_msgs::JointStateConstPtr& msg)
00089   {
00090     if (!msg->name.empty())
00091       callback_js_counter_++;
00092   }
00093 
00094   void callbackMs(const pr2_mechanism_msgs::MechanismStatisticsConstPtr& msg)
00095   {
00096     callback_ms_counter_++;
00097   }
00098 
00099   void callback1(const sensor_msgs::JointStateConstPtr& msg)
00100   {
00101     if (callback1_counter_ < 1000){
00102       callback1_timing_[callback1_counter_] = ros::Time::now();
00103     }
00104     callback1_name_ = msg->name[0];
00105     callback1_effort_ = msg->effort[0];
00106     callback1_counter_++;
00107   }
00108 
00109   void callback4(const sensor_msgs::JointStateConstPtr& msg)
00110   {
00111     callback4_name_ = msg->name[0];
00112   }
00113 
00114   bool loadController(const std::string& name)
00115   {
00116 
00117     pr2_mechanism_msgs::LoadController srv_msg;
00118     srv_msg.request.name = name;
00119     if (!load_srv_.call(srv_msg)) return false;
00120     return srv_msg.response.ok;
00121   }
00122 
00123   bool unloadController(const std::string& name)
00124   {
00125 
00126     pr2_mechanism_msgs::UnloadController srv_msg;
00127     srv_msg.request.name = name;
00128     if (!unload_srv_.call(srv_msg)) return false;
00129     return srv_msg.response.ok;
00130   }
00131 
00132   bool switchController(const std::vector<std::string>& start, const std::vector<std::string>& stop, int strictness)
00133   {
00134 
00135     pr2_mechanism_msgs::SwitchController srv_msg;
00136     srv_msg.request.start_controllers = start;
00137     srv_msg.request.stop_controllers = stop;
00138     srv_msg.request.strictness = strictness;
00139     if (!switch_srv_.call(srv_msg)) return false;
00140     return srv_msg.response.ok;
00141   }
00142 
00143   unsigned int nrControllers()
00144   {
00145 
00146     pr2_mechanism_msgs::ListControllers srv_msg;
00147     if (!list_srv_.call(srv_msg)) return 0;;
00148     return srv_msg.response.controllers.size();
00149   }
00150 
00151   unsigned int controllerState(const std::string& name)
00152   {
00153 
00154     pr2_mechanism_msgs::ListControllers srv_msg;
00155     if (!list_srv_.call(srv_msg)) return _failure;
00156     for (unsigned int i=0; i<srv_msg.response.controllers.size(); i++){
00157       if (name == srv_msg.response.controllers[i]){
00158         if (srv_msg.response.state[i] == "running") return _running;
00159         else if (srv_msg.response.state[i] == "stopped") return _stopped;
00160         else return _failure;
00161       }
00162     }
00163     return _unloaded;
00164   }
00165 
00166   std::string randomController()
00167   {
00168       unsigned int random = rand();
00169       random = random % 10;
00170       std::stringstream s;
00171       s << random;
00172       return "controller" + s.str();
00173   }
00174 
00175   void randomBombardment(unsigned int times)
00176   {
00177     while (!bombardment_started_)
00178       ros::Duration(0.1).sleep();
00179 
00180     for (unsigned int i=0; i<times; i++){
00181       unsigned int random = rand();
00182       random = random % 4;
00183       switch (random){
00184       case 0:{
00185         loadController(randomController());
00186         break;
00187       }
00188       case 1:{
00189         unloadController(randomController());
00190         break;
00191       }
00192       case 2:{
00193         std::vector<std::string> start, stop;
00194         unsigned int start_size = rand() %10;
00195         unsigned int stop_size = rand() %10;
00196         for (unsigned int i=0; i<start_size; i++)
00197           start.push_back(randomController());
00198         for (unsigned int i=0; i<stop_size; i++)
00199           stop.push_back(randomController());
00200         if (rand() %1 == 0)
00201           switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT);
00202         else
00203           switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::BEST_EFFORT);
00204         break;
00205       }
00206       case 3:{
00207         controllerState(randomController());
00208         break;
00209       }
00210       }
00211     }
00212   }
00213   
00214 
00215 protected:
00217   TestController()
00218     : callback1_timing_(1000)
00219   {
00220   }
00221 
00223   ~TestController()
00224   {
00225   }
00226 
00227   void SetUp()
00228   {
00229     ros::service::waitForService("pr2_controller_manager/load_controller");
00230     ros::service::waitForService("pr2_controller_manager/unload_controller");
00231     ros::service::waitForService("pr2_controller_manager/switch_controller");
00232     ros::service::waitForService("pr2_controller_manager/list_controllers");
00233     ros::service::waitForService("pr2_controller_manager/list_controllers");
00234 
00235     load_srv_ = node_.serviceClient<pr2_mechanism_msgs::LoadController>("pr2_controller_manager/load_controller");
00236     unload_srv_ = node_.serviceClient<pr2_mechanism_msgs::UnloadController>("pr2_controller_manager/unload_controller");
00237     switch_srv_ = node_.serviceClient<pr2_mechanism_msgs::SwitchController>("pr2_controller_manager/switch_controller");
00238     list_srv_ = node_.serviceClient<pr2_mechanism_msgs::ListControllers>("pr2_controller_manager/list_controllers");
00239   }
00240 
00241   void TearDown()
00242   {
00243     load_srv_.shutdown();
00244   }
00245 };
00246 
00247 
00248 
00249 
00250 // test spawner
00251 TEST_F(TestController, spawner)
00252 {
00253   // wait until all controllers are loaded
00254   ros::Time start = ros::Time::now();
00255   ros::Duration timeout(60.0);
00256   while (nrControllers() != 6 && ros::Time::now() - start < timeout)
00257     ros::Duration(1.0).sleep();
00258   EXPECT_TRUE(ros::Time::now() - start < timeout);
00259 
00260   // this should be the controller state if spawner worked
00261   EXPECT_EQ(controllerState("controller1"), _running);
00262   EXPECT_EQ(controllerState("controller2"), _stopped);
00263   EXPECT_EQ(controllerState("controller3"), _running);
00264   EXPECT_EQ(controllerState("controller4"), _running);
00265   EXPECT_EQ(controllerState("controller5"), _stopped);
00266   EXPECT_EQ(controllerState("controller6"), _stopped);
00267   EXPECT_EQ(controllerState("controller7"), _unloaded);
00268   EXPECT_EQ(controllerState("controller8"), _unloaded);
00269   EXPECT_EQ(controllerState("controller9"), _unloaded);
00270   SUCCEED();
00271 }
00272 
00273 
00274 TEST_F(TestController, loading)
00275 {
00276   // check initial state
00277   EXPECT_EQ(controllerState("controller1"), _running);
00278   EXPECT_EQ(controllerState("controller2"), _stopped);
00279   EXPECT_EQ(controllerState("controller3"), _running);
00280   EXPECT_EQ(controllerState("controller4"), _running);
00281   EXPECT_EQ(controllerState("controller5"), _stopped);
00282   EXPECT_EQ(controllerState("controller6"), _stopped);
00283   EXPECT_EQ(controllerState("controller7"), _unloaded);
00284   EXPECT_EQ(controllerState("controller8"), _unloaded);
00285   EXPECT_EQ(controllerState("controller9"), _unloaded);
00286 
00287   // these are already loaded
00288   EXPECT_FALSE(loadController("controller1"));
00289   EXPECT_FALSE(loadController("controller2"));
00290   EXPECT_FALSE(loadController("controller3"));
00291   EXPECT_FALSE(loadController("controller4"));
00292   EXPECT_FALSE(loadController("controller5"));
00293   EXPECT_FALSE(loadController("controller6"));
00294 
00295   // this one is not loaded yet
00296   EXPECT_TRUE(loadController("controller7"));
00297 
00298   // this one is wrongly configured
00299   EXPECT_FALSE(loadController("controller9"));
00300   EXPECT_FALSE(loadController("controller11"));
00301   EXPECT_FALSE(loadController("controller12"));
00302   EXPECT_FALSE(loadController("controller13"));
00303   EXPECT_FALSE(loadController("controller14"));
00304   EXPECT_FALSE(loadController("controller15"));
00305   EXPECT_FALSE(loadController("controller16"));
00306   EXPECT_FALSE(loadController("controller17"));
00307   EXPECT_FALSE(loadController("controller18"));
00308   EXPECT_FALSE(loadController("controller19"));
00309 
00310   // this one is not configured
00311   EXPECT_FALSE(loadController("controller10"));
00312 
00313   // check end state
00314   EXPECT_EQ(controllerState("controller1"), _running);
00315   EXPECT_EQ(controllerState("controller2"), _stopped);
00316   EXPECT_EQ(controllerState("controller3"), _running);
00317   EXPECT_EQ(controllerState("controller4"), _running);
00318   EXPECT_EQ(controllerState("controller5"), _stopped);
00319   EXPECT_EQ(controllerState("controller6"), _stopped);
00320   EXPECT_EQ(controllerState("controller7"), _stopped);
00321   EXPECT_EQ(controllerState("controller8"), _unloaded);
00322   EXPECT_EQ(controllerState("controller9"), _unloaded);
00323 
00324   SUCCEED();
00325 }
00326 
00327 TEST_F(TestController, unloading)
00328 {
00329   // check initial state
00330   EXPECT_EQ(controllerState("controller1"), _running);
00331   EXPECT_EQ(controllerState("controller2"), _stopped);
00332   EXPECT_EQ(controllerState("controller3"), _running);
00333   EXPECT_EQ(controllerState("controller4"), _running);
00334   EXPECT_EQ(controllerState("controller5"), _stopped);
00335   EXPECT_EQ(controllerState("controller6"), _stopped);
00336   EXPECT_EQ(controllerState("controller7"), _stopped);
00337   EXPECT_EQ(controllerState("controller8"), _unloaded);
00338   EXPECT_EQ(controllerState("controller9"), _unloaded);
00339 
00340   // these are running, so unloading should fail
00341   EXPECT_FALSE(unloadController("controller1"));
00342   EXPECT_FALSE(unloadController("controller3"));
00343   EXPECT_FALSE(unloadController("controller4"));
00344 
00345   // these are stopped, so unloading should succeed
00346   EXPECT_TRUE(unloadController("controller2"));
00347   EXPECT_TRUE(unloadController("controller5"));
00348   EXPECT_TRUE(unloadController("controller6"));
00349   EXPECT_TRUE(unloadController("controller7"));
00350 
00351   // this one is not loaded, so unloading should fail
00352   EXPECT_FALSE(unloadController("controller8"));
00353 
00354   // check end state
00355   EXPECT_EQ(controllerState("controller1"), _running);
00356   EXPECT_EQ(controllerState("controller2"), _unloaded);
00357   EXPECT_EQ(controllerState("controller3"), _running);
00358   EXPECT_EQ(controllerState("controller4"), _running);
00359   EXPECT_EQ(controllerState("controller5"), _unloaded);
00360   EXPECT_EQ(controllerState("controller6"), _unloaded);
00361   EXPECT_EQ(controllerState("controller7"), _unloaded);
00362   EXPECT_EQ(controllerState("controller8"), _unloaded);
00363   EXPECT_EQ(controllerState("controller9"), _unloaded);
00364 
00365   SUCCEED();
00366 }
00367 
00368 TEST_F(TestController, start_stop_strict)
00369 {
00370   // check initial state
00371   EXPECT_EQ(controllerState("controller1"), _running);
00372   EXPECT_EQ(controllerState("controller2"), _unloaded);
00373   EXPECT_EQ(controllerState("controller3"), _running);
00374   EXPECT_EQ(controllerState("controller4"), _running);
00375   EXPECT_EQ(controllerState("controller5"), _unloaded);
00376   EXPECT_EQ(controllerState("controller6"), _unloaded);
00377   EXPECT_EQ(controllerState("controller7"), _unloaded);
00378   EXPECT_EQ(controllerState("controller8"), _unloaded);
00379   EXPECT_EQ(controllerState("controller9"), _unloaded);
00380 
00381   // starting already started controller
00382   std::vector<std::string> start, stop;
00383   start.push_back("controller1");
00384   EXPECT_TRUE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
00385   EXPECT_EQ(controllerState("controller1"), _running);
00386 
00387   // starting unloaded controller
00388   start.push_back("controller2");
00389   EXPECT_FALSE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
00390   EXPECT_EQ(controllerState("controller1"), _running);
00391   EXPECT_EQ(controllerState("controller2"), _unloaded);
00392 
00393   // starting one already stated, 1 stopped
00394   EXPECT_TRUE(loadController("controller2"));
00395   EXPECT_TRUE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
00396   EXPECT_EQ(controllerState("controller1"), _running);
00397   EXPECT_EQ(controllerState("controller2"), _running);
00398 
00399   // start and stop same controller
00400   stop.push_back("controller2");
00401   EXPECT_TRUE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
00402   EXPECT_EQ(controllerState("controller1"), _running);
00403   EXPECT_EQ(controllerState("controller2"), _running);
00404 
00405   // stop unloaded controller
00406   stop.push_back("controller5");
00407   EXPECT_FALSE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
00408   EXPECT_EQ(controllerState("controller1"), _running);
00409   EXPECT_EQ(controllerState("controller2"), _running);
00410   EXPECT_EQ(controllerState("controller5"), _unloaded);
00411 
00412   // stop unloaded and running controller
00413   stop.push_back("controller4");
00414   EXPECT_FALSE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
00415   EXPECT_EQ(controllerState("controller1"), _running);
00416   EXPECT_EQ(controllerState("controller2"), _running);
00417   EXPECT_EQ(controllerState("controller4"), _running);
00418   EXPECT_EQ(controllerState("controller5"), _unloaded);
00419 
00420   // stop running and stopped controller
00421   EXPECT_TRUE(loadController("controller5"));
00422   EXPECT_TRUE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
00423   EXPECT_EQ(controllerState("controller1"), _running);
00424   EXPECT_EQ(controllerState("controller2"), _running);
00425   EXPECT_EQ(controllerState("controller4"), _stopped);
00426   EXPECT_EQ(controllerState("controller5"), _stopped);
00427 
00428   // stop 2 stopped controllers, and 1 running controller
00429   stop.push_back("controller3");
00430   EXPECT_TRUE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
00431   EXPECT_EQ(controllerState("controller1"), _running);
00432   EXPECT_EQ(controllerState("controller2"), _running);
00433   EXPECT_EQ(controllerState("controller3"), _stopped);
00434   EXPECT_EQ(controllerState("controller4"), _stopped);
00435   EXPECT_EQ(controllerState("controller5"), _stopped);
00436 
00437   SUCCEED();
00438 }
00439 
00440 
00441 TEST_F(TestController, start_stop_best_effort)
00442 {
00443   // check initial state
00444   EXPECT_EQ(controllerState("controller1"), _running);
00445   EXPECT_EQ(controllerState("controller2"), _running);
00446   EXPECT_EQ(controllerState("controller3"), _stopped);
00447   EXPECT_EQ(controllerState("controller4"), _stopped);
00448   EXPECT_EQ(controllerState("controller5"), _stopped);
00449   EXPECT_EQ(controllerState("controller6"), _unloaded);
00450   EXPECT_EQ(controllerState("controller7"), _unloaded);
00451   EXPECT_EQ(controllerState("controller8"), _unloaded);
00452   EXPECT_EQ(controllerState("controller9"), _unloaded);
00453 
00454   // starting already started controller
00455   std::vector<std::string> start, stop;
00456   start.push_back("controller1");
00457   EXPECT_TRUE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::BEST_EFFORT));
00458   EXPECT_EQ(controllerState("controller1"), _running);
00459 
00460   // starting unloaded, started and stopped controller
00461   start.push_back("controller3");
00462   start.push_back("controller6");
00463   EXPECT_TRUE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::BEST_EFFORT));
00464   EXPECT_EQ(controllerState("controller1"), _running);
00465   EXPECT_EQ(controllerState("controller3"), _running);
00466   EXPECT_EQ(controllerState("controller6"), _unloaded);
00467 
00468   SUCCEED();
00469 }
00470 
00471 
00472 
00473 TEST_F(TestController, service_and_realtime_publisher)
00474 {
00475   EXPECT_EQ(controllerState("controller1"), _running);
00476   EXPECT_EQ(controllerState("controller4"), _stopped);
00477 
00478   callback1_timing_.resize(1000);
00479   callback1_counter_ = 0;
00480 
00481   // connect to topic
00482   ros::Subscriber sub1 = node_.subscribe<sensor_msgs::JointState>("controller1/my_topic", 100, 
00483                                                                   boost::bind(&TestController_service_and_realtime_publisher_Test::callback1, this, _1));
00484   ros::Subscriber sub4 = node_.subscribe<sensor_msgs::JointState>("controller4/my_topic", 100, 
00485                                                                   boost::bind(&TestController_service_and_realtime_publisher_Test::callback4, this, _1));
00486 
00487   std::string not_started = "not_started";
00488   callback1_name_ = not_started;
00489   callback4_name_ = not_started;
00490 
00491   ros::Time start = ros::Time::now();
00492   ros::Duration timeout(5.0);
00493   while (callback1_name_ == "not_started" && ros::Time::now() - start < timeout)
00494     ros::Duration(0.1).sleep();
00495   ros::Duration(1.0).sleep(); // avoid problem with simultanious access to callback1_name_
00496   EXPECT_EQ(callback1_name_, "");
00497   EXPECT_EQ(callback4_name_, not_started);
00498 
00499   // check for effort limits
00500   for (unsigned int i=0; i<1000; i++){
00501     EXPECT_LE(callback1_effort_, 0.0);
00502     ros::Duration(0.01).sleep();
00503   }
00504 
00505   std::string test_message = "Hoe gaat het met Wim?";
00506   ros::ServiceClient srv_client1 = node_.serviceClient<pr2_mechanism_msgs::LoadController>("controller1/my_service");
00507   ros::ServiceClient srv_client4 = node_.serviceClient<pr2_mechanism_msgs::LoadController>("controller1/my_service");
00508   pr2_mechanism_msgs::LoadController srv_msg;
00509   srv_msg.request.name = test_message;
00510   EXPECT_TRUE(srv_client1.call(srv_msg));
00511   EXPECT_TRUE(srv_client4.call(srv_msg));
00512 
00513   ros::Duration(1.0).sleep();
00514   EXPECT_EQ(callback1_name_, test_message);
00515   EXPECT_EQ(callback4_name_, not_started);
00516 
00517   sub1.shutdown();
00518   sub4.shutdown();
00519 
00520   SUCCEED();
00521 }
00522 
00523 
00524 TEST_F(TestController, publisher_hz)
00525 {
00526   EXPECT_EQ(controllerState("controller1"), _running);
00527 
00528   // connect to topic
00529   ros::Subscriber sub1 = node_.subscribe<sensor_msgs::JointState>("controller1/my_topic", 100, 
00530                                                                   boost::bind(&TestController_publisher_hz_Test::callback1, this, _1));
00531 
00532   callback1_counter_ = 0;
00533   ros::Time start = ros::Time::now();
00534   ros::Duration timeout(5.0);
00535   while (callback1_counter_ == 0 && ros::Time::now() - start < timeout)
00536     ros::Duration(0.01).sleep();
00537   callback1_counter_ = 0;
00538   ros::Duration(5.0).sleep();
00539   // gathering debugging info
00540   if (callback1_counter_ < 450){
00541     unsigned int nr = callback1_counter_;
00542     ROS_ERROR("Only received %u callbacks between start time %f and end time %f", nr, start.toSec(), ros::Time::now().toSec());
00543     for (unsigned int i=0; i<nr-1; i++){
00544       ROS_ERROR("  - %u:  time %f,  delta time %f", i, callback1_timing_[i].toSec(), (callback1_timing_[i+1]-callback1_timing_[i]).toSec());
00545     }
00546   }  
00547   EXPECT_NEAR(callback1_counter_, 500, 50);
00548 
00549   sub1.shutdown();
00550   SUCCEED();
00551 }
00552 
00553 
00554 TEST_F(TestController, manager_hz)
00555 {
00556   // connect to topic
00557   ros::Subscriber sub_js = node_.subscribe<sensor_msgs::JointState>("joint_states", 100, 
00558                                                                     boost::bind(&TestController_manager_hz_Test::callbackJs, this, _1));
00559   callback_js_counter_ = 0;
00560   ros::Time start = ros::Time::now();
00561   ros::Duration timeout(5.0);
00562   while (callback_js_counter_ == 0 && ros::Time::now() - start < timeout)
00563     ros::Duration(0.1).sleep();
00564   callback_js_counter_ = 0;
00565   ros::Duration(5.0).sleep();
00566   EXPECT_NEAR(callback_js_counter_, 1000, 40);
00567   sub_js.shutdown();
00568 
00569 
00570   ros::Subscriber sub_ms = node_.subscribe<pr2_mechanism_msgs::MechanismStatistics>("mechanism_statistics", 100, 
00571                                                                                     boost::bind(&TestController_manager_hz_Test::callbackMs, this, _1));
00572   callback_ms_counter_ = 0;
00573   start = ros::Time::now();
00574   while (callback_ms_counter_ == 0 && ros::Time::now() - start < timeout)
00575     ros::Duration(0.1).sleep();
00576   callback_ms_counter_ = 0;
00577   ros::Duration(5.0).sleep();
00578   EXPECT_NEAR(callback_ms_counter_, 10, 2);
00579   sub_ms.shutdown();
00580 
00581   SUCCEED();
00582 }
00583 
00584 
00585 TEST_F(TestController, diagnostic_hz)
00586 {
00587   // connect to topic
00588   ros::Subscriber sub_diagnostics = node_.subscribe<diagnostic_msgs::DiagnosticArray>("/diagnostics", 100, 
00589                                                                                       boost::bind(&TestController_manager_hz_Test::callbackDiagnostic, this, _1));
00590   joint_diagnostic_counter_ = 0;
00591   controller_diagnostic_counter_ = 0;
00592   ros::Duration(5.0).sleep();
00593   EXPECT_GT(joint_diagnostic_counter_, (unsigned int)1);
00594   EXPECT_GT(controller_diagnostic_counter_, (unsigned int)1);
00595   sub_diagnostics.shutdown();
00596 
00597   SUCCEED();
00598 }
00599 
00600 
00601 
00602 TEST_F(TestController, singlethread_bombardment)
00603 {
00604   bombardment_started_ = true;
00605   randomBombardment(1000);
00606   bombardment_started_ = false;
00607   SUCCEED();
00608 }
00609 
00610 TEST_F(TestController, multithread_bombardment)
00611 {
00612   boost::thread bomb1(boost::bind(&TestController_multithread_bombardment_Test::randomBombardment, this, 500));
00613   boost::thread bomb2(boost::bind(&TestController_multithread_bombardment_Test::randomBombardment, this, 500));
00614   boost::thread bomb3(boost::bind(&TestController_multithread_bombardment_Test::randomBombardment, this, 500));
00615   boost::thread bomb4(boost::bind(&TestController_multithread_bombardment_Test::randomBombardment, this, 500));
00616   bombardment_started_ = true;
00617   bomb1.join();
00618   bomb2.join();
00619   bomb3.join();
00620   bomb4.join();
00621 
00622   SUCCEED();
00623 }
00624 
00625 
00626 int main(int argc, char** argv)
00627 {
00628   testing::InitGoogleTest(&argc, argv);
00629   g_argc = argc;
00630   g_argv = argv;
00631 
00632   ros::init(g_argc, g_argv, "testControllers");
00633 
00634   boost::thread spinner(boost::bind(&ros::spin));
00635 
00636   int res = RUN_ALL_TESTS();
00637   spinner.interrupt();
00638   spinner.join();
00639 
00640   return res;
00641 }


pr2_controller_manager
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Thu Jun 6 2019 21:11:40