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 "ros/ros.h"
00037 #include "topic_tools/shape_shifter.h"
00038 #include "std_msgs/String.h"
00039 #include "std_msgs/Int32.h"
00040
00041
00042 #include <gtest/gtest.h>
00043
00044 class ShapeShifterSubscriber : public testing::Test
00045 {
00046 public:
00047
00048 bool success;
00049
00050 void messageCallbackInt(const topic_tools::ShapeShifter::ConstPtr& msg)
00051 {
00052 try {
00053 std_msgs::Int32::Ptr s = msg->instantiate<std_msgs::Int32>();
00054 } catch (topic_tools::ShapeShifterException& e)
00055 {
00056 success = true;
00057 }
00058 }
00059
00060 void messageCallbackString(const topic_tools::ShapeShifter::ConstPtr& msg)
00061 {
00062 try {
00063 std_msgs::String::Ptr s = msg->instantiate<std_msgs::String>();
00064 if (s->data == "chatter")
00065 success = true;
00066 } catch (topic_tools::ShapeShifterException& e)
00067 {
00068
00069 }
00070 }
00071
00072 void messageCallbackLoopback(const topic_tools::ShapeShifter::ConstPtr& msg)
00073 {
00074 try {
00075 std_msgs::String::Ptr s = msg->instantiate<std_msgs::String>();
00076 printf("Got data: %s", s->data.c_str());
00077 if (s->data == "abc123")
00078 success = true;
00079 } catch (topic_tools::ShapeShifterException& e)
00080 {
00081 printf("Instantiate failed!\n");
00082 }
00083 }
00084
00085 protected:
00086 ShapeShifterSubscriber() {}
00087
00088 void SetUp()
00089 {
00090 success = false;
00091 }
00092
00093 void TearDown() {}
00094 };
00095
00096
00097 TEST_F(ShapeShifterSubscriber, testInstantiateString)
00098 {
00099 ros::NodeHandle nh;
00100 ros::Subscriber sub = nh.subscribe<topic_tools::ShapeShifter>("input",1,&ShapeShifterSubscriber::messageCallbackString, (ShapeShifterSubscriber*)this);
00101
00102 ros::Time t1(ros::Time::now()+ros::Duration(10.0));
00103
00104 while(ros::Time::now() < t1 && !success)
00105 {
00106 ros::WallDuration(0.01).sleep();
00107 ros::spinOnce();
00108 }
00109
00110 EXPECT_FALSE(topic_tools::ShapeShifter::uses_old_API_);
00111
00112 if(success)
00113 SUCCEED();
00114 else
00115 FAIL();
00116 }
00117
00118 TEST_F(ShapeShifterSubscriber, testInstantiateInt)
00119 {
00120 ros::NodeHandle nh;
00121 ros::Subscriber sub = nh.subscribe<topic_tools::ShapeShifter>("input",1,&ShapeShifterSubscriber::messageCallbackInt, (ShapeShifterSubscriber*)this);
00122
00123 ros::Time t1(ros::Time::now()+ros::Duration(10.0));
00124
00125 while(ros::Time::now() < t1 && !success)
00126 {
00127 ros::WallDuration(0.01).sleep();
00128 ros::spinOnce();
00129 }
00130
00131 EXPECT_FALSE(topic_tools::ShapeShifter::uses_old_API_);
00132
00133 if(success)
00134 SUCCEED();
00135 else
00136 FAIL();
00137 }
00138
00139 TEST_F(ShapeShifterSubscriber, testLoopback)
00140 {
00141 ros::NodeHandle nh;
00142 ros::Subscriber sub = nh.subscribe<topic_tools::ShapeShifter>("loopback",1,&ShapeShifterSubscriber::messageCallbackLoopback, (ShapeShifterSubscriber*)this);
00143
00144 ros::Time t1(ros::Time::now()+ros::Duration(10.0));
00145
00146 ros::Publisher pub = nh.advertise<std_msgs::String>("loopback", 1);
00147 std_msgs::String s;
00148 s.data = "abc123";
00149 pub.publish(s);
00150
00151 while(ros::Time::now() < t1 && !success)
00152 {
00153 ros::WallDuration(0.01).sleep();
00154 ros::spinOnce();
00155 }
00156
00157 EXPECT_FALSE(topic_tools::ShapeShifter::uses_old_API_);
00158
00159 if(success)
00160 SUCCEED();
00161 else
00162 FAIL();
00163 }
00164
00165 int main(int argc, char **argv){
00166 ros::init(argc, argv, "test_shapeshifter");
00167
00168 testing::InitGoogleTest(&argc, argv);
00169 return RUN_ALL_TESTS();
00170 }