00001 #include <iostream> 00002 #include <gtest/gtest.h> 00003 #include "robodyn_utilities/RosMsgSynchronizer.h" 00004 #include "sensor_msgs/JointState.h" 00005 #include <ros/package.h> 00006 #include <stdexcept> 00007 #include <math.h> 00008 #include <fstream> 00009 00010 class RosMsgSynchronizerTest : public ::testing::Test 00011 { 00012 protected: 00013 virtual void SetUp() 00014 { 00015 ASSERT_NO_THROW(rms = new RosMsgSynchronizer<sensor_msgs::JointState>()); 00016 } 00017 00018 virtual void TearDown() 00019 { 00020 } 00021 00022 RosMsgSynchronizer<sensor_msgs::JointState>* rms; 00023 }; 00024 00025 00026 TEST_F(RosMsgSynchronizerTest, initialize) 00027 { 00028 EXPECT_ANY_THROW(rms->setWindowDelta(-1.)); 00029 EXPECT_TRUE(rms->empty()); 00030 EXPECT_EQ(0, rms->size()); 00031 EXPECT_EQ(0, rms->getBufferSize()); 00032 EXPECT_ANY_THROW(rms->front()); 00033 EXPECT_ANY_THROW(rms->back()); 00034 } 00035 00036 TEST_F(RosMsgSynchronizerTest, functional) 00037 { 00038 EXPECT_NO_THROW(rms->setWindowDelta(1.0)); 00039 sensor_msgs::JointState js; 00040 00041 js.header.frame_id = "source1A"; 00042 js.header.stamp = ros::Time(100, 10); 00043 rms->pushSource1(js); 00044 00045 EXPECT_TRUE(rms->empty()); 00046 EXPECT_EQ(0, rms->size()); 00047 EXPECT_EQ(1, rms->getBufferSize()); 00048 EXPECT_ANY_THROW(rms->front()); 00049 EXPECT_ANY_THROW(rms->back()); 00050 00051 js.header.frame_id = "source1B"; 00052 js.header.stamp = ros::Time(200, 20); 00053 rms->pushSource1(js); 00054 00055 EXPECT_TRUE(rms->empty()); 00056 EXPECT_EQ(0, rms->size()); 00057 EXPECT_EQ(2, rms->getBufferSize()); 00058 EXPECT_ANY_THROW(rms->front()); 00059 EXPECT_ANY_THROW(rms->back()); 00060 00061 js.header.frame_id = "source1C"; 00062 js.header.stamp = ros::Time(300, 30); 00063 rms->pushSource1(js); 00064 00065 EXPECT_TRUE(rms->empty()); 00066 EXPECT_EQ(0, rms->size()); 00067 EXPECT_EQ(3, rms->getBufferSize()); 00068 EXPECT_ANY_THROW(rms->front()); 00069 EXPECT_ANY_THROW(rms->back()); 00070 00071 js.header.frame_id = "source2D"; 00072 js.header.stamp = ros::Time(400, 40); 00073 rms->pushSource2(js); 00074 00075 EXPECT_TRUE(rms->empty()); 00076 EXPECT_EQ(0, rms->size()); 00077 EXPECT_EQ(4, rms->getBufferSize()); 00078 EXPECT_ANY_THROW(rms->front()); 00079 EXPECT_ANY_THROW(rms->back()); 00080 00081 js.header.frame_id = "source2E"; 00082 js.header.stamp = ros::Time(500, 50); 00083 rms->pushSource2(js); 00084 00085 EXPECT_TRUE(rms->empty()); 00086 EXPECT_EQ(0, rms->size()); 00087 EXPECT_EQ(5, rms->getBufferSize()); 00088 EXPECT_ANY_THROW(rms->front()); 00089 EXPECT_ANY_THROW(rms->back()); 00090 00091 js.header.frame_id = "source2C"; 00092 js.header.stamp = ros::Time(300, 30); 00093 rms->pushSource2(js); 00094 00095 EXPECT_FALSE(rms->empty()); 00096 EXPECT_EQ(1, rms->size()); 00097 EXPECT_EQ(4, rms->getBufferSize()); 00098 EXPECT_NO_THROW(rms->front()); 00099 EXPECT_NO_THROW(rms->back()); 00100 00101 std::pair<sensor_msgs::JointState, sensor_msgs::JointState> match; 00102 match = rms->front(); 00103 EXPECT_EQ(match.first.header.stamp, ros::Time(300, 30)); 00104 EXPECT_EQ(match.second.header.stamp, ros::Time(300, 30)); 00105 EXPECT_EQ(match.first.header.frame_id, "source1C"); 00106 EXPECT_EQ(match.second.header.frame_id, "source2C"); 00107 00108 match = rms->back(); 00109 EXPECT_EQ(match.first.header.stamp, ros::Time(300, 30)); 00110 EXPECT_EQ(match.second.header.stamp, ros::Time(300, 30)); 00111 EXPECT_EQ(match.first.header.frame_id, "source1C"); 00112 EXPECT_EQ(match.second.header.frame_id, "source2C"); 00113 00114 EXPECT_EQ(&rms->front(), &rms->back()); 00115 00116 js.header.frame_id = "source2B"; 00117 js.header.stamp = ros::Time(200, 0); 00118 rms->pushSource2(js); 00119 00120 EXPECT_FALSE(rms->empty()); 00121 EXPECT_EQ(2, rms->size()); 00122 EXPECT_EQ(3, rms->getBufferSize()); 00123 EXPECT_NO_THROW(rms->front()); 00124 EXPECT_NO_THROW(rms->back()); 00125 00126 match = rms->front(); 00127 EXPECT_EQ(match.first.header.stamp, ros::Time(200, 20)); 00128 EXPECT_EQ(match.second.header.stamp, ros::Time(200, 0)); 00129 EXPECT_EQ(match.first.header.frame_id, "source1B"); 00130 EXPECT_EQ(match.second.header.frame_id, "source2B"); 00131 00132 rms->purgeBuffer(ros::Time(100, 50)); 00133 00134 EXPECT_FALSE(rms->empty()); 00135 EXPECT_EQ(2, rms->size()); 00136 EXPECT_EQ(2, rms->getBufferSize()); 00137 EXPECT_NO_THROW(rms->front()); 00138 EXPECT_NO_THROW(rms->back()); 00139 00140 rms->purgeBuffer(); 00141 00142 EXPECT_FALSE(rms->empty()); 00143 EXPECT_EQ(2, rms->size()); 00144 EXPECT_EQ(0, rms->getBufferSize()); 00145 EXPECT_NO_THROW(rms->front()); 00146 EXPECT_NO_THROW(rms->back()); 00147 00148 rms->pop(); 00149 00150 EXPECT_FALSE(rms->empty()); 00151 EXPECT_EQ(1, rms->size()); 00152 EXPECT_EQ(0, rms->getBufferSize()); 00153 EXPECT_NO_THROW(rms->front()); 00154 EXPECT_NO_THROW(rms->back()); 00155 00156 rms->clear(); 00157 00158 EXPECT_TRUE(rms->empty()); 00159 EXPECT_EQ(0, rms->size()); 00160 EXPECT_EQ(0, rms->getBufferSize()); 00161 EXPECT_ANY_THROW(rms->front()); 00162 EXPECT_ANY_THROW(rms->back()); 00163 } 00164 00165 int main(int argc, char** argv) 00166 { 00167 testing::InitGoogleTest(&argc, argv); 00168 return RUN_ALL_TESTS(); 00169 }