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 #include <gtest/gtest.h>
00031
00032 #include "laser_geometry/laser_geometry.h"
00033 #include "sensor_msgs/PointCloud.h"
00034 #include <math.h>
00035
00036
00037 #include <angles/angles.h>
00038
00039 #include "rostest/permuter.h"
00040
00041 #define PROJECTION_TEST_RANGE_MIN (0.23)
00042 #define PROJECTION_TEST_RANGE_MAX (40.0)
00043
00044
00045 class BuildScanException { };
00046
00047 sensor_msgs::LaserScan build_constant_scan(double range, double intensity,
00048 double ang_min, double ang_max, double ang_increment,
00049 ros::Duration scan_time)
00050 {
00051 if (((ang_max - ang_min) / ang_increment) < 0)
00052 throw (BuildScanException());
00053
00054 sensor_msgs::LaserScan scan;
00055 scan.header.stamp = ros::Time::now();
00056 scan.header.frame_id = "laser_frame";
00057 scan.angle_min = ang_min;
00058 scan.angle_max = ang_max;
00059 scan.angle_increment = ang_increment;
00060 scan.scan_time = scan_time.toSec();
00061 scan.range_min = PROJECTION_TEST_RANGE_MIN;
00062 scan.range_max = PROJECTION_TEST_RANGE_MAX;
00063 uint32_t i = 0;
00064 for(; ang_min + i * ang_increment < ang_max; i++)
00065 {
00066 scan.ranges.push_back(range);
00067 scan.intensities.push_back(intensity);
00068 }
00069
00070 scan.time_increment = scan_time.toSec()/(double)i;
00071
00072 return scan;
00073 };
00074
00075
00076 class TestProjection : public laser_geometry::LaserProjection
00077 {
00078 public:
00079 const boost::numeric::ublas::matrix<double>& getUnitVectors(double angle_min,
00080 double angle_max,
00081 double angle_increment,
00082 unsigned int length)
00083 {
00084 return getUnitVectors_(angle_min, angle_max, angle_increment, length);
00085 }
00086 };
00087
00088 void test_getUnitVectors(double angle_min, double angle_max, double angle_increment, unsigned int length)
00089 {
00090 double tolerance = 1e-12;
00091 TestProjection projector;
00092
00093 const boost::numeric::ublas::matrix<double> & mat = projector.getUnitVectors(angle_min, angle_max, angle_increment, length);
00094
00095
00096
00097 for (unsigned int i = 0; i < mat.size2(); i++)
00098 {
00099 EXPECT_NEAR(angles::shortest_angular_distance(atan2(mat(1,i), mat(0,i)),
00100 angle_min + i * angle_increment),
00101 0,
00102 tolerance);
00103 EXPECT_NEAR(1.0, mat(1,i)*mat(1,i) + mat(0,i)*mat(0,i), tolerance);
00104 }
00105 }
00106
00107 #if 0
00108
00109 TEST(laser_geometry, getUnitVectors)
00110 {
00111 double min_angle = -M_PI/2;
00112 double max_angle = M_PI/2;
00113 double angle_increment = M_PI/180;
00114
00115 std::vector<double> min_angles, max_angles, angle_increments;
00116
00117 rostest::Permuter permuter;
00118 min_angles.push_back(-M_PI);
00119 min_angles.push_back(-M_PI/1.5);
00120 min_angles.push_back(-M_PI/2);
00121 min_angles.push_back(-M_PI/4);
00122 min_angles.push_back(-M_PI/8);
00123 min_angles.push_back(M_PI);
00124 min_angles.push_back(M_PI/1.5);
00125 min_angles.push_back(M_PI/2);
00126 min_angles.push_back(M_PI/4);
00127 min_angles.push_back(M_PI/8);
00128 permuter.addOptionSet(min_angles, &min_angle);
00129
00130 max_angles.push_back(M_PI);
00131 max_angles.push_back(M_PI/1.5);
00132 max_angles.push_back(M_PI/2);
00133 max_angles.push_back(M_PI/4);
00134 max_angles.push_back(M_PI/8);
00135 max_angles.push_back(-M_PI);
00136 max_angles.push_back(-M_PI/1.5);
00137 max_angles.push_back(-M_PI/2);
00138 max_angles.push_back(-M_PI/4);
00139 max_angles.push_back(-M_PI/8);
00140 permuter.addOptionSet(max_angles, &max_angle);
00141
00142 angle_increments.push_back(M_PI/180);
00143 angle_increments.push_back(M_PI/360);
00144 angle_increments.push_back(M_PI/720);
00145 angle_increments.push_back(-M_PI/180);
00146 angle_increments.push_back(-M_PI/360);
00147 angle_increments.push_back(-M_PI/720);
00148 permuter.addOptionSet(angle_increments, &angle_increment);
00149
00150
00151 while (permuter.step())
00152 {
00153 if ((max_angle - min_angle) / angle_increment > 0.0)
00154 {
00155 unsigned int length = round((max_angle - min_angle)/ angle_increment);
00156 try
00157 {
00158 test_getUnitVectors(min_angle, max_angle, angle_increment, length);
00159 test_getUnitVectors(min_angle, max_angle, angle_increment, (max_angle - min_angle)/ angle_increment);
00160 test_getUnitVectors(min_angle, max_angle, angle_increment, (max_angle - min_angle)/ angle_increment + 1);
00161 }
00162 catch (BuildScanException &ex)
00163 {
00164 if ((max_angle - min_angle) / angle_increment > 0.0)
00165 FAIL();
00166 }
00167 }
00168
00169
00170 }
00171 }
00172
00173
00174 TEST(laser_geometry, projectLaser)
00175 {
00176 double tolerance = 1e-12;
00177 laser_geometry::LaserProjection projector;
00178
00179 double min_angle = -M_PI/2;
00180 double max_angle = M_PI/2;
00181 double angle_increment = M_PI/180;
00182
00183 double range = 1.0;
00184 double intensity = 1.0;
00185
00186 ros::Duration scan_time = ros::Duration(1/40);
00187 ros::Duration increment_time = ros::Duration(1/400);
00188
00189
00190 std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
00191 std::vector<ros::Duration> increment_times, scan_times;
00192
00193 rostest::Permuter permuter;
00194
00195 ranges.push_back(-1.0);
00196 ranges.push_back(1.0);
00197 ranges.push_back(2.0);
00198 ranges.push_back(3.0);
00199 ranges.push_back(4.0);
00200 ranges.push_back(5.0);
00201 ranges.push_back(100.0);
00202 permuter.addOptionSet(ranges, &range);
00203
00204 intensities.push_back(1.0);
00205 intensities.push_back(2.0);
00206 intensities.push_back(3.0);
00207 intensities.push_back(4.0);
00208 intensities.push_back(5.0);
00209 permuter.addOptionSet(intensities, &intensity);
00210
00211 min_angles.push_back(-M_PI);
00212 min_angles.push_back(-M_PI/1.5);
00213 min_angles.push_back(-M_PI/2);
00214 min_angles.push_back(-M_PI/4);
00215 min_angles.push_back(-M_PI/8);
00216 permuter.addOptionSet(min_angles, &min_angle);
00217
00218 max_angles.push_back(M_PI);
00219 max_angles.push_back(M_PI/1.5);
00220 max_angles.push_back(M_PI/2);
00221 max_angles.push_back(M_PI/4);
00222 max_angles.push_back(M_PI/8);
00223 permuter.addOptionSet(max_angles, &max_angle);
00224
00225
00226 angle_increments.push_back(M_PI/180);
00227 angle_increments.push_back(M_PI/360);
00228 angle_increments.push_back(M_PI/720);
00229 permuter.addOptionSet(angle_increments, &angle_increment);
00230
00231 scan_times.push_back(ros::Duration(1/40));
00232 scan_times.push_back(ros::Duration(1/20));
00233
00234 permuter.addOptionSet(scan_times, &scan_time);
00235
00236 while (permuter.step())
00237 {
00238 try
00239 {
00240
00241 sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
00242
00243 sensor_msgs::PointCloud cloud_out;
00244 projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index);
00245 EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
00246 projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity);
00247 EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
00248
00249 projector.projectLaser(scan, cloud_out, -1.0);
00250 EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
00251 projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
00252 EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
00253
00254 projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance);
00255 EXPECT_EQ(cloud_out.channels.size(), (unsigned int)3);
00256
00257 projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp);
00258 EXPECT_EQ(cloud_out.channels.size(), (unsigned int)4);
00259
00260 unsigned int valid_points = 0;
00261 for (unsigned int i = 0; i < scan.ranges.size(); i++)
00262 if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
00263 valid_points ++;
00264
00265 EXPECT_EQ(valid_points, cloud_out.points.size());
00266
00267 for (unsigned int i = 0; i < cloud_out.points.size(); i++)
00268 {
00269 EXPECT_NEAR(cloud_out.points[i].x , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
00270 EXPECT_NEAR(cloud_out.points[i].y , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
00271 EXPECT_NEAR(cloud_out.points[i].z, 0, tolerance);
00272 EXPECT_NEAR(cloud_out.channels[0].values[i], scan.intensities[i], tolerance);
00273 EXPECT_NEAR(cloud_out.channels[1].values[i], i, tolerance);
00274 EXPECT_NEAR(cloud_out.channels[2].values[i], scan.ranges[i], tolerance);
00275 EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);
00276 };
00277 }
00278 catch (BuildScanException &ex)
00279 {
00280 if ((max_angle - min_angle) / angle_increment > 0.0)
00281 FAIL();
00282 }
00283 }
00284 }
00285
00286 #endif
00287
00288
00289 TEST(laser_geometry, projectLaser2)
00290 {
00291 double tolerance = 1e-12;
00292 laser_geometry::LaserProjection projector;
00293
00294 double min_angle = -M_PI/2;
00295 double max_angle = M_PI/2;
00296 double angle_increment = M_PI/180;
00297
00298 double range = 1.0;
00299 double intensity = 1.0;
00300
00301 ros::Duration scan_time = ros::Duration(1/40);
00302 ros::Duration increment_time = ros::Duration(1/400);
00303
00304
00305 std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
00306 std::vector<ros::Duration> increment_times, scan_times;
00307
00308 rostest::Permuter permuter;
00309
00310 ranges.push_back(-1.0);
00311 ranges.push_back(1.0);
00312 ranges.push_back(2.0);
00313 ranges.push_back(3.0);
00314 ranges.push_back(4.0);
00315 ranges.push_back(5.0);
00316 ranges.push_back(100.0);
00317 permuter.addOptionSet(ranges, &range);
00318
00319 intensities.push_back(1.0);
00320 intensities.push_back(2.0);
00321 intensities.push_back(3.0);
00322 intensities.push_back(4.0);
00323 intensities.push_back(5.0);
00324 permuter.addOptionSet(intensities, &intensity);
00325
00326 min_angles.push_back(-M_PI);
00327 min_angles.push_back(-M_PI/1.5);
00328 min_angles.push_back(-M_PI/2);
00329 min_angles.push_back(-M_PI/4);
00330 min_angles.push_back(-M_PI/8);
00331 permuter.addOptionSet(min_angles, &min_angle);
00332
00333 max_angles.push_back(M_PI);
00334 max_angles.push_back(M_PI/1.5);
00335 max_angles.push_back(M_PI/2);
00336 max_angles.push_back(M_PI/4);
00337 max_angles.push_back(M_PI/8);
00338 permuter.addOptionSet(max_angles, &max_angle);
00339
00340
00341 angle_increments.push_back(M_PI/180);
00342 angle_increments.push_back(M_PI/360);
00343 angle_increments.push_back(M_PI/720);
00344 permuter.addOptionSet(angle_increments, &angle_increment);
00345
00346 scan_times.push_back(ros::Duration(1/40));
00347 scan_times.push_back(ros::Duration(1/20));
00348
00349 permuter.addOptionSet(scan_times, &scan_time);
00350
00351 while (permuter.step())
00352 {
00353 try
00354 {
00355
00356 sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
00357
00358 sensor_msgs::PointCloud2 cloud_out;
00359 projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index);
00360 EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
00361 projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity);
00362 EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
00363
00364 projector.projectLaser(scan, cloud_out, -1.0);
00365 EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
00366 projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
00367 EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
00368
00369 projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance);
00370 EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6);
00371
00372 projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp);
00373 EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
00374
00375 unsigned int valid_points = 0;
00376 for (unsigned int i = 0; i < scan.ranges.size(); i++)
00377 if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
00378 valid_points ++;
00379
00380 EXPECT_EQ(valid_points, cloud_out.width);
00381
00382 uint32_t x_offset = 0;
00383 uint32_t y_offset = 0;
00384 uint32_t z_offset = 0;
00385 uint32_t intensity_offset = 0;
00386 uint32_t index_offset = 0;
00387 uint32_t distance_offset = 0;
00388 uint32_t stamps_offset = 0;
00389 for (std::vector<sensor_msgs::PointField>::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++)
00390 {
00391 if (f->name == "x") x_offset = f->offset;
00392 if (f->name == "y") y_offset = f->offset;
00393 if (f->name == "z") z_offset = f->offset;
00394 if (f->name == "intensity") intensity_offset = f->offset;
00395 if (f->name == "index") index_offset = f->offset;
00396 if (f->name == "distances") distance_offset = f->offset;
00397 if (f->name == "stamps") stamps_offset = f->offset;
00398 }
00399
00400 for (unsigned int i = 0; i < cloud_out.width; i++)
00401 {
00402
00403 EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + x_offset] , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
00404 EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + y_offset] , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
00405 EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + z_offset] , 0, tolerance);
00406 EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + intensity_offset] , scan.intensities[i], tolerance);
00407 EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, tolerance);
00408 EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + distance_offset], scan.ranges[i], tolerance);
00409 EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + stamps_offset], (float)i * scan.time_increment, tolerance);
00410 };
00411 }
00412 catch (BuildScanException &ex)
00413 {
00414 if ((max_angle - min_angle) / angle_increment > 0.0)
00415 FAIL();
00416 }
00417 }
00418 }
00419
00420
00421 TEST(laser_geometry, transformLaserScanToPointCloud)
00422 {
00423
00424 tf::Transformer tf;
00425
00426 double tolerance = 1e-12;
00427 laser_geometry::LaserProjection projector;
00428
00429 double min_angle = -M_PI/2;
00430 double max_angle = M_PI/2;
00431 double angle_increment = M_PI/180;
00432
00433 double range = 1.0;
00434 double intensity = 1.0;
00435
00436 ros::Duration scan_time = ros::Duration(1/40);
00437 ros::Duration increment_time = ros::Duration(1/400);
00438
00439
00440 std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
00441 std::vector<ros::Duration> increment_times, scan_times;
00442
00443 rostest::Permuter permuter;
00444
00445 ranges.push_back(-1.0);
00446 ranges.push_back(1.0);
00447 ranges.push_back(2.0);
00448 ranges.push_back(3.0);
00449 ranges.push_back(4.0);
00450 ranges.push_back(5.0);
00451 ranges.push_back(100.0);
00452 permuter.addOptionSet(ranges, &range);
00453
00454 intensities.push_back(1.0);
00455 intensities.push_back(2.0);
00456 intensities.push_back(3.0);
00457 intensities.push_back(4.0);
00458 intensities.push_back(5.0);
00459 permuter.addOptionSet(intensities, &intensity);
00460
00461 min_angles.push_back(-M_PI);
00462 min_angles.push_back(-M_PI/1.5);
00463 min_angles.push_back(-M_PI/2);
00464 min_angles.push_back(-M_PI/4);
00465 min_angles.push_back(-M_PI/8);
00466
00467
00468 max_angles.push_back(M_PI);
00469 max_angles.push_back(M_PI/1.5);
00470 max_angles.push_back(M_PI/2);
00471 max_angles.push_back(M_PI/4);
00472 max_angles.push_back(M_PI/8);
00473
00474 permuter.addOptionSet(min_angles, &min_angle);
00475 permuter.addOptionSet(max_angles, &max_angle);
00476
00477 angle_increments.push_back(-M_PI/180);
00478 angle_increments.push_back(M_PI/180);
00479 angle_increments.push_back(M_PI/360);
00480 angle_increments.push_back(M_PI/720);
00481 permuter.addOptionSet(angle_increments, &angle_increment);
00482
00483 scan_times.push_back(ros::Duration(1/40));
00484 scan_times.push_back(ros::Duration(1/20));
00485
00486 permuter.addOptionSet(scan_times, &scan_time);
00487
00488 while (permuter.step())
00489 {
00490 try
00491 {
00492
00493 sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
00494 scan.header.frame_id = "laser_frame";
00495
00496 sensor_msgs::PointCloud cloud_out;
00497 projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Index);
00498 EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
00499 projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity);
00500 EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
00501
00502 projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf);
00503 EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
00504 projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
00505 EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
00506
00507 projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance);
00508 EXPECT_EQ(cloud_out.channels.size(), (unsigned int)3);
00509
00510 projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp);
00511 EXPECT_EQ(cloud_out.channels.size(), (unsigned int)4);
00512
00513 unsigned int valid_points = 0;
00514 for (unsigned int i = 0; i < scan.ranges.size(); i++)
00515 if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
00516 valid_points ++;
00517 EXPECT_EQ(valid_points, cloud_out.points.size());
00518
00519 for (unsigned int i = 0; i < cloud_out.points.size(); i++)
00520 {
00521 EXPECT_NEAR(cloud_out.points[i].x , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
00522 EXPECT_NEAR(cloud_out.points[i].y , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
00523 EXPECT_NEAR(cloud_out.points[i].z, 0, tolerance);
00524 EXPECT_NEAR(cloud_out.channels[0].values[i], scan.intensities[i], tolerance);
00525 EXPECT_NEAR(cloud_out.channels[1].values[i], i, tolerance);
00526 EXPECT_NEAR(cloud_out.channels[2].values[i], scan.ranges[i], tolerance);
00527 EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);
00528 };
00529 }
00530 catch (BuildScanException &ex)
00531 {
00532 if ((max_angle - min_angle) / angle_increment > 0.0)
00533 FAIL();
00534 }
00535 }
00536 }
00537
00538 TEST(laser_geometry, transformLaserScanToPointCloud2)
00539 {
00540
00541 tf::Transformer tf;
00542 tf2::BufferCore tf2;
00543
00544 double tolerance = 1e-12;
00545 laser_geometry::LaserProjection projector;
00546
00547 double min_angle = -M_PI/2;
00548 double max_angle = M_PI/2;
00549 double angle_increment = M_PI/180;
00550
00551 double range = 1.0;
00552 double intensity = 1.0;
00553
00554 ros::Duration scan_time = ros::Duration(1/40);
00555 ros::Duration increment_time = ros::Duration(1/400);
00556
00557 std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
00558 std::vector<ros::Duration> increment_times, scan_times;
00559
00560 rostest::Permuter permuter;
00561
00562 ranges.push_back(-1.0);
00563 ranges.push_back(1.0);
00564 ranges.push_back(2.0);
00565 ranges.push_back(3.0);
00566 ranges.push_back(4.0);
00567 ranges.push_back(5.0);
00568 ranges.push_back(100.0);
00569 permuter.addOptionSet(ranges, &range);
00570
00571 intensities.push_back(1.0);
00572 intensities.push_back(2.0);
00573 intensities.push_back(3.0);
00574 intensities.push_back(4.0);
00575 intensities.push_back(5.0);
00576 permuter.addOptionSet(intensities, &intensity);
00577
00578 min_angles.push_back(-M_PI);
00579 min_angles.push_back(-M_PI/1.5);
00580 min_angles.push_back(-M_PI/2);
00581 min_angles.push_back(-M_PI/4);
00582 min_angles.push_back(-M_PI/8);
00583
00584
00585 max_angles.push_back(M_PI);
00586 max_angles.push_back(M_PI/1.5);
00587 max_angles.push_back(M_PI/2);
00588 max_angles.push_back(M_PI/4);
00589 max_angles.push_back(M_PI/8);
00590
00591 permuter.addOptionSet(min_angles, &min_angle);
00592 permuter.addOptionSet(max_angles, &max_angle);
00593
00594 angle_increments.push_back(-M_PI/180);
00595 angle_increments.push_back(M_PI/180);
00596 angle_increments.push_back(M_PI/360);
00597 angle_increments.push_back(M_PI/720);
00598 permuter.addOptionSet(angle_increments, &angle_increment);
00599
00600 scan_times.push_back(ros::Duration(1/40));
00601 scan_times.push_back(ros::Duration(1/20));
00602
00603 permuter.addOptionSet(scan_times, &scan_time);
00604
00605 while (permuter.step())
00606 {
00607 try
00608 {
00609
00610 sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
00611 scan.header.frame_id = "laser_frame";
00612
00613 sensor_msgs::PointCloud2 cloud_out;
00614 projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::None);
00615 EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3);
00616 projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::None);
00617 EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3);
00618 projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Index);
00619 EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
00620 projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Index);
00621 EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
00622 projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity);
00623 EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
00624 projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity);
00625 EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
00626
00627 projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf);
00628 EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
00629 projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2);
00630 EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
00631 projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
00632 EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
00633 projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
00634 EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
00635
00636 projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance);
00637 EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6);
00638 projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance);
00639 EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6);
00640
00641 projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp);
00642 EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
00643 projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp);
00644 EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
00645
00646 EXPECT_EQ(cloud_out.is_dense, false);
00647
00648 unsigned int valid_points = 0;
00649 for (unsigned int i = 0; i < scan.ranges.size(); i++)
00650 if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
00651 valid_points ++;
00652 EXPECT_EQ(valid_points, cloud_out.width);
00653
00654 uint32_t x_offset = 0;
00655 uint32_t y_offset = 0;
00656 uint32_t z_offset = 0;
00657 uint32_t intensity_offset = 0;
00658 uint32_t index_offset = 0;
00659 uint32_t distance_offset = 0;
00660 uint32_t stamps_offset = 0;
00661 for (std::vector<sensor_msgs::PointField>::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++)
00662 {
00663 if (f->name == "x") x_offset = f->offset;
00664 if (f->name == "y") y_offset = f->offset;
00665 if (f->name == "z") z_offset = f->offset;
00666 if (f->name == "intensity") intensity_offset = f->offset;
00667 if (f->name == "index") index_offset = f->offset;
00668 if (f->name == "distances") distance_offset = f->offset;
00669 if (f->name == "stamps") stamps_offset = f->offset;
00670 }
00671
00672 for (unsigned int i = 0; i < cloud_out.width; i++)
00673 {
00674 EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + x_offset] , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
00675 EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + y_offset] , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
00676 EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + z_offset] , 0, tolerance);
00677 EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + intensity_offset] , scan.intensities[i], tolerance);
00678 EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, tolerance);
00679 EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + distance_offset], scan.ranges[i], tolerance);
00680 EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + stamps_offset], (float)i * scan.time_increment, tolerance);
00681
00682 };
00683 }
00684 catch (BuildScanException &ex)
00685 {
00686 if ((max_angle - min_angle) / angle_increment > 0.0)
00687 FAIL();
00688 }
00689 }
00690
00691 }
00692
00693
00694 int main(int argc, char **argv){
00695 ros::Time::init();
00696 testing::InitGoogleTest(&argc, argv);
00697 return RUN_ALL_TESTS();
00698 }