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