Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include <string>
00037
00038 #include <gtest/gtest.h>
00039
00040 #include <time.h>
00041 #include <stdlib.h>
00042
00043 #include "ros/ros.h"
00044 #include <ros/callback_queue.h>
00045 #include <test_roscpp/TestArray.h>
00046
00047 int g_msg_count;
00048 ros::Duration g_dt;
00049 uint32_t g_options;
00050
00051 class Subscriptions : public testing::Test
00052 {
00053 public:
00054 bool success;
00055 bool failure;
00056 int msg_i;
00057 ros::Publisher pub_;
00058
00059 void messageCallback(const test_roscpp::TestArrayConstPtr& msg)
00060 {
00061 if(failure || success)
00062 return;
00063
00064
00065 if (msg_i == -1)
00066 {
00067 msg_i = msg->counter - 1;
00068 }
00069
00070 msg_i++;
00071
00072 if(msg_i != msg->counter)
00073 {
00074 failure = true;
00075 ROS_INFO("failed");
00076 }
00077 else if(msg_i == (g_msg_count-1))
00078 {
00079 success = true;
00080 ROS_INFO("success");
00081 }
00082 else
00083 {
00084 pub_.publish(msg);
00085 }
00086 }
00087
00088 void subscriberCallback(const ros::SingleSubscriberPublisher&)
00089 {
00090 test_roscpp::TestArray msg;
00091 msg.counter = 0;
00092 pub_.publish(msg);
00093 }
00094
00095 protected:
00096 void SetUp()
00097 {
00098 success = false;
00099 failure = false;
00100
00101 msg_i = -1;
00102 }
00103 void TearDown()
00104 {
00105
00106 }
00107 };
00108
00109 TEST_F(Subscriptions, subPub)
00110 {
00111 ros::NodeHandle nh;
00112 ros::Subscriber sub = nh.subscribe("test_roscpp/pubsub_test", 0, &Subscriptions::messageCallback, (Subscriptions*)this);
00113 ASSERT_TRUE(sub);
00114 pub_ = nh.advertise<test_roscpp::TestArray>("test_roscpp/subpub_test", 0, boost::bind(&Subscriptions::subscriberCallback, this, _1));
00115 ASSERT_TRUE(pub_);
00116 ros::Time t1(ros::Time::now()+g_dt);
00117
00118 while(ros::Time::now() < t1 && !success && !failure)
00119 {
00120 ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0.1));
00121 }
00122
00123 if(success)
00124 SUCCEED();
00125 else
00126 FAIL();
00127 }
00128
00129 #define USAGE "USAGE: sub_pub <count> <time>"
00130
00131 int
00132 main(int argc, char** argv)
00133 {
00134 testing::InitGoogleTest(&argc, argv);
00135 ros::init(argc, argv, "sub_pub");
00136
00137 if(argc != 3)
00138 {
00139 puts(USAGE);
00140 return -1;
00141 }
00142 g_msg_count = atoi(argv[1]);
00143 g_dt.fromSec(atof(argv[2]));
00144
00145 ros::NodeHandle nh;
00146
00147 return RUN_ALL_TESTS();
00148 }