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 #include <gtest/gtest.h>
00036 #include <laser_joint_processor/laser_joint_processor.h>
00037
00038 using namespace std;
00039 using namespace laser_joint_processor;
00040
00041 static const float eps = 1e-6;
00042
00043 class LaserJointProcessor_EasyTests : public ::testing::Test
00044 {
00045 protected:
00046 virtual void SetUp()
00047 {
00048 bool success;
00049
00050
00051 vector<string> names;
00052 names.push_back("JointA");
00053 names.push_back("JointB");
00054 processor_.setJointNames(names);
00055 processor_.setCacheSize(1000);
00056
00057
00058 for (unsigned int i=0; i<20; i++)
00059 {
00060 sensor_msgs::JointStatePtr joint_state(new sensor_msgs::JointState);
00061 joint_state->header.stamp = ros::Time(100+i, 0);
00062 joint_state->name.push_back("JointA");
00063 joint_state->name.push_back("JointB");
00064 joint_state->name.push_back("JointC");
00065 joint_state->position.push_back(i*1.0);
00066 joint_state->position.push_back(i*10.0);
00067 joint_state->position.push_back(i*20.0);
00068 joint_state->velocity.resize(3);
00069 joint_state->effort.resize(3);
00070
00071 success = processor_.addJointState(joint_state);
00072 ASSERT_TRUE(success);
00073 }
00074
00075
00076 snapshot_.angle_min = 10.0;
00077 snapshot_.angle_increment = 1.0;
00078 snapshot_.time_increment = .5;
00079 snapshot_.readings_per_scan = 4;
00080 snapshot_.num_scans = 3;
00081 snapshot_.ranges.resize(12);
00082 for (unsigned int i=0; i<snapshot_.ranges.size(); i++)
00083 snapshot_.ranges[i] = i*100;
00084 snapshot_.scan_start.resize(snapshot_.num_scans);
00085 snapshot_.scan_start[0] = ros::Time(105);
00086 snapshot_.scan_start[1] = ros::Time(110);
00087 snapshot_.scan_start[2] = ros::Time(115.1);
00088
00089
00090 features_.image_points.resize(P);
00091
00092
00093 features_.image_points[0] = Point(0,0);
00094 features_.image_points[1] = Point(3,0);
00095 features_.image_points[2] = Point(0,2);
00096 features_.image_points[3] = Point(3,2);
00097
00098 features_.image_points[4] = Point(2.5, .5);
00099 features_.image_points[5] = Point(2, 1);
00100 features_.image_points[6] = Point(1.25, 1.5);
00101 }
00102
00103 void testProcessing()
00104 {
00105 bool success;
00106
00107 success = processor_.processLaserData(snapshot_, features_, result_, ros::Duration(2,0));
00108 EXPECT_TRUE(success);
00109 }
00110
00111 geometry_msgs::Point Point(float x, float y)
00112 {
00113 geometry_msgs::Point point;
00114 point.x = x;
00115 point.y = y;
00116 return point;
00117 }
00118
00119 static const unsigned int P = 7;
00120 calibration_msgs::JointStateCalibrationPattern result_;
00121 calibration_msgs::CalibrationPattern features_;
00122 calibration_msgs::DenseLaserSnapshot snapshot_;
00123 LaserJointProcessor processor_;
00124 };
00125
00126 TEST_F(LaserJointProcessor_EasyTests, earlyTest)
00127 {
00128 EXPECT_FALSE(processor_.isSnapshotEarly(snapshot_));
00129 EXPECT_FALSE(processor_.isSnapshotLate(snapshot_));
00130 snapshot_.scan_start[2] = ros::Time(300);
00131 EXPECT_TRUE(processor_.isSnapshotEarly(snapshot_));
00132 EXPECT_FALSE(processor_.isSnapshotLate(snapshot_));
00133 }
00134
00135 TEST_F(LaserJointProcessor_EasyTests, lateTest)
00136 {
00137 EXPECT_FALSE(processor_.isSnapshotLate(snapshot_));
00138 EXPECT_FALSE(processor_.isSnapshotEarly(snapshot_));
00139 snapshot_.scan_start[0] = ros::Time(10);
00140 EXPECT_TRUE(processor_.isSnapshotLate(snapshot_));
00141 EXPECT_FALSE(processor_.isSnapshotEarly(snapshot_));
00142 }
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156 TEST_F(LaserJointProcessor_EasyTests, jointTest)
00157 {
00158 testProcessing();
00159
00160 ASSERT_EQ(result_.joint_points.size(), (unsigned int) P);
00161 for (unsigned int i=0; i<result_.joint_points.size(); i++)
00162 {
00163 ASSERT_EQ(result_.joint_points[i].name.size(), (unsigned int) 4);
00164 ASSERT_EQ(result_.joint_points[i].position.size(), (unsigned int) 4);
00165 EXPECT_EQ(result_.joint_points[i].name[0], "JointA");
00166 EXPECT_EQ(result_.joint_points[i].name[1], "JointB");
00167 }
00168
00169
00170
00171 EXPECT_NEAR(result_.joint_points[0].position[0], 5.0, eps);
00172 EXPECT_NEAR(result_.joint_points[0].position[1], 50.0, eps);
00173
00174
00175
00176 EXPECT_NEAR(result_.joint_points[1].position[0], 6.5, eps);
00177 EXPECT_NEAR(result_.joint_points[1].position[1], 65, eps);
00178
00179
00180
00181 EXPECT_NEAR(result_.joint_points[2].position[0], 15.1, eps);
00182 EXPECT_NEAR(result_.joint_points[2].position[1], 151, eps);
00183
00184
00185
00186 EXPECT_NEAR(result_.joint_points[3].position[0], 16.6, eps);
00187 EXPECT_NEAR(result_.joint_points[3].position[1], 166, eps);
00188
00189
00190 EXPECT_NEAR(result_.joint_points[4].position[0], 8.75, eps);
00191 EXPECT_NEAR(result_.joint_points[4].position[1], 87.5, eps);
00192
00193
00194 EXPECT_NEAR(result_.joint_points[5].position[0], 11, eps);
00195 EXPECT_NEAR(result_.joint_points[5].position[1], 110, eps);
00196
00197
00198 EXPECT_NEAR(result_.joint_points[6].position[0], 13.175, eps);
00199 EXPECT_NEAR(result_.joint_points[6].position[1], 131.75, eps);
00200 }
00201
00202 TEST_F(LaserJointProcessor_EasyTests, anglesTest)
00203 {
00204 testProcessing();
00205
00206 ASSERT_EQ(result_.joint_points.size(), (unsigned int) P);
00207 for (unsigned int i=0; i<P; i++)
00208 {
00209 ASSERT_EQ(result_.joint_points[i].name.size(), (unsigned int) 4);
00210 ASSERT_EQ(result_.joint_points[i].position.size(), (unsigned int) 4);
00211 EXPECT_EQ(result_.joint_points[i].name[2], "laser_angle_joint");
00212 }
00213
00214 EXPECT_NEAR(result_.joint_points[0].position[2], 10.0, eps);
00215 EXPECT_NEAR(result_.joint_points[1].position[2], 13.0, eps);
00216 EXPECT_NEAR(result_.joint_points[2].position[2], 10.0, eps);
00217 EXPECT_NEAR(result_.joint_points[3].position[2], 13.0, eps);
00218 EXPECT_NEAR(result_.joint_points[4].position[2], 12.5, eps);
00219 EXPECT_NEAR(result_.joint_points[5].position[2], 12.0, eps);
00220 EXPECT_NEAR(result_.joint_points[6].position[2], 11.25, eps);
00221 }
00222
00223 TEST_F(LaserJointProcessor_EasyTests, rangesTest)
00224 {
00225 testProcessing();
00226
00227 ASSERT_EQ(result_.joint_points.size(), (unsigned int) P);
00228 for (unsigned int i=0; i<P; i++)
00229 {
00230 ASSERT_EQ(result_.joint_points[i].name.size(), (unsigned int) 4);
00231 ASSERT_EQ(result_.joint_points[i].position.size(), (unsigned int) 4);
00232 EXPECT_EQ(result_.joint_points[i].name[3], "laser_range_joint");
00233 }
00234
00235 EXPECT_NEAR(result_.joint_points[0].position[3], 0.0, eps);
00236 EXPECT_NEAR(result_.joint_points[1].position[3], 300.0, eps);
00237 EXPECT_NEAR(result_.joint_points[2].position[3], 800.0, eps);
00238 EXPECT_NEAR(result_.joint_points[3].position[3], 1100.0, eps);
00239 EXPECT_NEAR(result_.joint_points[4].position[3], 450.0, eps);
00240 EXPECT_NEAR(result_.joint_points[5].position[3], 600.0, eps);
00241 EXPECT_NEAR(result_.joint_points[6].position[3], 725.0, eps);
00242 }
00243
00244 int main(int argc, char **argv){
00245 testing::InitGoogleTest(&argc, argv);
00246 return RUN_ALL_TESTS();
00247 }