projection_test.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <gtest/gtest.h>
31 
32 #include "laser_geometry/laser_geometry.h"
33 #include "sensor_msgs/PointCloud.h"
34 #include <math.h>
35 
36 
37 #include <angles/angles.h>
38 
39 #include "rostest/permuter.h"
40 
41 #define PROJECTION_TEST_RANGE_MIN (0.23)
42 #define PROJECTION_TEST_RANGE_MAX (40.0)
43 
44 
45 class BuildScanException { };
46 
47 sensor_msgs::LaserScan build_constant_scan(double range, double intensity,
48  double ang_min, double ang_max, double ang_increment,
49  ros::Duration scan_time)
50 {
51  if (((ang_max - ang_min) / ang_increment) < 0)
52  throw (BuildScanException());
53 
54  sensor_msgs::LaserScan scan;
55  scan.header.stamp = ros::Time::now();
56  scan.header.frame_id = "laser_frame";
57  scan.angle_min = ang_min;
58  scan.angle_max = ang_max;
59  scan.angle_increment = ang_increment;
60  scan.scan_time = scan_time.toSec();
61  scan.range_min = PROJECTION_TEST_RANGE_MIN;
62  scan.range_max = PROJECTION_TEST_RANGE_MAX;
63  uint32_t i = 0;
64  for(; ang_min + i * ang_increment < ang_max; i++)
65  {
66  scan.ranges.push_back(range);
67  scan.intensities.push_back(intensity);
68  }
69 
70  scan.time_increment = scan_time.toSec()/(double)i;
71 
72  return scan;
73 };
74 
75 
76 class TestProjection : public laser_geometry::LaserProjection
77 {
78 public:
79  const boost::numeric::ublas::matrix<double>& getUnitVectors(double angle_min,
80  double angle_max,
81  double angle_increment,
82  unsigned int length)
83  {
84  return getUnitVectors_(angle_min, angle_max, angle_increment, length);
85  }
86 };
87 
88 void test_getUnitVectors(double angle_min, double angle_max, double angle_increment, unsigned int length)
89 {
90  double tolerance = 1e-12;
91  TestProjection projector;
92 
93  const boost::numeric::ublas::matrix<double> & mat = projector.getUnitVectors(angle_min, angle_max, angle_increment, length);
94 
95 
96 
97  for (unsigned int i = 0; i < mat.size2(); i++)
98  {
99  EXPECT_NEAR(angles::shortest_angular_distance(atan2(mat(1,i), mat(0,i)),
100  angle_min + i * angle_increment),
101  0,
102  tolerance); // check expected angle
103  EXPECT_NEAR(1.0, mat(1,i)*mat(1,i) + mat(0,i)*mat(0,i), tolerance); //make sure normalized
104  }
105 }
106 
107 #if 0
108 
109 TEST(laser_geometry, getUnitVectors)
110 {
111  double min_angle = -M_PI/2;
112  double max_angle = M_PI/2;
113  double angle_increment = M_PI/180;
114 
115  std::vector<double> min_angles, max_angles, angle_increments;
116 
117  rostest::Permuter permuter;
118  min_angles.push_back(-M_PI);
119  min_angles.push_back(-M_PI/1.5);
120  min_angles.push_back(-M_PI/2);
121  min_angles.push_back(-M_PI/4);
122  min_angles.push_back(-M_PI/8);
123  min_angles.push_back(M_PI);
124  min_angles.push_back(M_PI/1.5);
125  min_angles.push_back(M_PI/2);
126  min_angles.push_back(M_PI/4);
127  min_angles.push_back(M_PI/8);
128  permuter.addOptionSet(min_angles, &min_angle);
129 
130  max_angles.push_back(M_PI);
131  max_angles.push_back(M_PI/1.5);
132  max_angles.push_back(M_PI/2);
133  max_angles.push_back(M_PI/4);
134  max_angles.push_back(M_PI/8);
135  max_angles.push_back(-M_PI);
136  max_angles.push_back(-M_PI/1.5);
137  max_angles.push_back(-M_PI/2);
138  max_angles.push_back(-M_PI/4);
139  max_angles.push_back(-M_PI/8);
140  permuter.addOptionSet(max_angles, &max_angle);
141 
142  angle_increments.push_back(M_PI/180); // one degree
143  angle_increments.push_back(M_PI/360); // half degree
144  angle_increments.push_back(M_PI/720); // quarter degree
145  angle_increments.push_back(-M_PI/180); // -one degree
146  angle_increments.push_back(-M_PI/360); // -half degree
147  angle_increments.push_back(-M_PI/720); // -quarter degree
148  permuter.addOptionSet(angle_increments, &angle_increment);
149 
150 
151  while (permuter.step())
152  {
153  if ((max_angle - min_angle) / angle_increment > 0.0)
154  {
155  unsigned int length = round((max_angle - min_angle)/ angle_increment);
156  try
157  {
158  test_getUnitVectors(min_angle, max_angle, angle_increment, length);
159  test_getUnitVectors(min_angle, max_angle, angle_increment, (max_angle - min_angle)/ angle_increment);
160  test_getUnitVectors(min_angle, max_angle, angle_increment, (max_angle - min_angle)/ angle_increment + 1);
161  }
162  catch (BuildScanException &ex)
163  {
164  if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
165  FAIL();
166  }
167  }
168  //else
169  //printf("%f\n", (max_angle - min_angle) / angle_increment);
170  }
171 }
172 
173 
174 TEST(laser_geometry, projectLaser)
175 {
176  double tolerance = 1e-12;
177  laser_geometry::LaserProjection projector;
178 
179  double min_angle = -M_PI/2;
180  double max_angle = M_PI/2;
181  double angle_increment = M_PI/180;
182 
183  double range = 1.0;
184  double intensity = 1.0;
185 
186  ros::Duration scan_time = ros::Duration(1/40);
187  ros::Duration increment_time = ros::Duration(1/400);
188 
189 
190  std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
191  std::vector<ros::Duration> increment_times, scan_times;
192 
193  rostest::Permuter permuter;
194 
195  ranges.push_back(-1.0);
196  ranges.push_back(1.0);
197  ranges.push_back(2.0);
198  ranges.push_back(3.0);
199  ranges.push_back(4.0);
200  ranges.push_back(5.0);
201  ranges.push_back(100.0);
202  permuter.addOptionSet(ranges, &range);
203 
204  intensities.push_back(1.0);
205  intensities.push_back(2.0);
206  intensities.push_back(3.0);
207  intensities.push_back(4.0);
208  intensities.push_back(5.0);
209  permuter.addOptionSet(intensities, &intensity);
210 
211  min_angles.push_back(-M_PI);
212  min_angles.push_back(-M_PI/1.5);
213  min_angles.push_back(-M_PI/2);
214  min_angles.push_back(-M_PI/4);
215  min_angles.push_back(-M_PI/8);
216  permuter.addOptionSet(min_angles, &min_angle);
217 
218  max_angles.push_back(M_PI);
219  max_angles.push_back(M_PI/1.5);
220  max_angles.push_back(M_PI/2);
221  max_angles.push_back(M_PI/4);
222  max_angles.push_back(M_PI/8);
223  permuter.addOptionSet(max_angles, &max_angle);
224 
225  // angle_increments.push_back(-M_PI/180); // -one degree
226  angle_increments.push_back(M_PI/180); // one degree
227  angle_increments.push_back(M_PI/360); // half degree
228  angle_increments.push_back(M_PI/720); // quarter degree
229  permuter.addOptionSet(angle_increments, &angle_increment);
230 
231  scan_times.push_back(ros::Duration(1/40));
232  scan_times.push_back(ros::Duration(1/20));
233 
234  permuter.addOptionSet(scan_times, &scan_time);
235 
236  while (permuter.step())
237  {
238  try
239  {
240  // printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
241  sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
242 
243  sensor_msgs::PointCloud cloud_out;
244  projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index);
245  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
246  projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity);
247  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
248 
249  projector.projectLaser(scan, cloud_out, -1.0);
250  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
251  projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
252  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
253 
254  projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance);
255  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)3);
256 
257  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);
258  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)4);
259 
260  unsigned int valid_points = 0;
261  for (unsigned int i = 0; i < scan.ranges.size(); i++)
262  if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
263  valid_points ++;
264 
265  EXPECT_EQ(valid_points, cloud_out.points.size());
266 
267  for (unsigned int i = 0; i < cloud_out.points.size(); i++)
268  {
269  EXPECT_NEAR(cloud_out.points[i].x , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
270  EXPECT_NEAR(cloud_out.points[i].y , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
271  EXPECT_NEAR(cloud_out.points[i].z, 0, tolerance);
272  EXPECT_NEAR(cloud_out.channels[0].values[i], scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order
273  EXPECT_NEAR(cloud_out.channels[1].values[i], i, tolerance);//index
274  EXPECT_NEAR(cloud_out.channels[2].values[i], scan.ranges[i], tolerance);//ranges
275  EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps
276  };
277  }
278  catch (BuildScanException &ex)
279  {
280  if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
281  FAIL();
282  }
283  }
284 }
285 
286 #endif
287 
288 
289 TEST(laser_geometry, projectLaser2)
290 {
291  double tolerance = 1e-12;
292  laser_geometry::LaserProjection projector;
293 
294  double min_angle = -M_PI/2;
295  double max_angle = M_PI/2;
296  double angle_increment = M_PI/180;
297 
298  double range = 1.0;
299  double intensity = 1.0;
300 
301  ros::Duration scan_time = ros::Duration(1/40);
302  ros::Duration increment_time = ros::Duration(1/400);
303 
304 
305  std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
306  std::vector<ros::Duration> increment_times, scan_times;
307 
308  rostest::Permuter permuter;
309 
310  ranges.push_back(-1.0);
311  ranges.push_back(1.0);
312  ranges.push_back(2.0);
313  ranges.push_back(3.0);
314  ranges.push_back(4.0);
315  ranges.push_back(5.0);
316  ranges.push_back(100.0);
317  permuter.addOptionSet(ranges, &range);
318 
319  intensities.push_back(1.0);
320  intensities.push_back(2.0);
321  intensities.push_back(3.0);
322  intensities.push_back(4.0);
323  intensities.push_back(5.0);
324  permuter.addOptionSet(intensities, &intensity);
325 
326  min_angles.push_back(-M_PI);
327  min_angles.push_back(-M_PI/1.5);
328  min_angles.push_back(-M_PI/2);
329  min_angles.push_back(-M_PI/4);
330  min_angles.push_back(-M_PI/8);
331  permuter.addOptionSet(min_angles, &min_angle);
332 
333  max_angles.push_back(M_PI);
334  max_angles.push_back(M_PI/1.5);
335  max_angles.push_back(M_PI/2);
336  max_angles.push_back(M_PI/4);
337  max_angles.push_back(M_PI/8);
338  permuter.addOptionSet(max_angles, &max_angle);
339 
340  // angle_increments.push_back(-M_PI/180); // -one degree
341  angle_increments.push_back(M_PI/180); // one degree
342  angle_increments.push_back(M_PI/360); // half degree
343  angle_increments.push_back(M_PI/720); // quarter degree
344  permuter.addOptionSet(angle_increments, &angle_increment);
345 
346  scan_times.push_back(ros::Duration(1/40));
347  scan_times.push_back(ros::Duration(1/20));
348 
349  permuter.addOptionSet(scan_times, &scan_time);
350 
351  while (permuter.step())
352  {
353  try
354  {
355  // printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
356  sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
357 
358  sensor_msgs::PointCloud2 cloud_out;
359  projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index);
360  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
361  projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity);
362  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
363 
364  projector.projectLaser(scan, cloud_out, -1.0);
365  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
366  projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
367  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
368 
369  projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance);
370  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6);
371 
372  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);
373  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
374 
375  unsigned int valid_points = 0;
376  for (unsigned int i = 0; i < scan.ranges.size(); i++)
377  if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
378  valid_points ++;
379 
380  EXPECT_EQ(valid_points, cloud_out.width);
381 
382  uint32_t x_offset = 0;
383  uint32_t y_offset = 0;
384  uint32_t z_offset = 0;
385  uint32_t intensity_offset = 0;
386  uint32_t index_offset = 0;
387  uint32_t distance_offset = 0;
388  uint32_t stamps_offset = 0;
389  for (std::vector<sensor_msgs::PointField>::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++)
390  {
391  if (f->name == "x") x_offset = f->offset;
392  if (f->name == "y") y_offset = f->offset;
393  if (f->name == "z") z_offset = f->offset;
394  if (f->name == "intensity") intensity_offset = f->offset;
395  if (f->name == "index") index_offset = f->offset;
396  if (f->name == "distances") distance_offset = f->offset;
397  if (f->name == "stamps") stamps_offset = f->offset;
398  }
399 
400  for (unsigned int i = 0; i < cloud_out.width; i++)
401  {
402 
403  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);
404  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);
405  EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + z_offset] , 0, tolerance);
406  EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + intensity_offset] , scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order
407  EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, tolerance);//index
408  EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + distance_offset], scan.ranges[i], tolerance);//ranges
409  EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + stamps_offset], (float)i * scan.time_increment, tolerance);//timestamps
410  };
411  }
412  catch (BuildScanException &ex)
413  {
414  if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
415  FAIL();
416  }
417  }
418 }
419 
420 
421 TEST(laser_geometry, transformLaserScanToPointCloud)
422 {
423 
424  tf::Transformer tf;
425 
426  double tolerance = 1e-12;
427  laser_geometry::LaserProjection projector;
428 
429  double min_angle = -M_PI/2;
430  double max_angle = M_PI/2;
431  double angle_increment = M_PI/180;
432 
433  double range = 1.0;
434  double intensity = 1.0;
435 
436  ros::Duration scan_time = ros::Duration(1/40);
437  ros::Duration increment_time = ros::Duration(1/400);
438 
439 
440  std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
441  std::vector<ros::Duration> increment_times, scan_times;
442 
443  rostest::Permuter permuter;
444 
445  ranges.push_back(-1.0);
446  ranges.push_back(1.0);
447  ranges.push_back(2.0);
448  ranges.push_back(3.0);
449  ranges.push_back(4.0);
450  ranges.push_back(5.0);
451  ranges.push_back(100.0);
452  permuter.addOptionSet(ranges, &range);
453 
454  intensities.push_back(1.0);
455  intensities.push_back(2.0);
456  intensities.push_back(3.0);
457  intensities.push_back(4.0);
458  intensities.push_back(5.0);
459  permuter.addOptionSet(intensities, &intensity);
460 
461  min_angles.push_back(-M_PI);
462  min_angles.push_back(-M_PI/1.5);
463  min_angles.push_back(-M_PI/2);
464  min_angles.push_back(-M_PI/4);
465  min_angles.push_back(-M_PI/8);
466 
467 
468  max_angles.push_back(M_PI);
469  max_angles.push_back(M_PI/1.5);
470  max_angles.push_back(M_PI/2);
471  max_angles.push_back(M_PI/4);
472  max_angles.push_back(M_PI/8);
473 
474  permuter.addOptionSet(min_angles, &min_angle);
475  permuter.addOptionSet(max_angles, &max_angle);
476 
477  angle_increments.push_back(-M_PI/180); // -one degree
478  angle_increments.push_back(M_PI/180); // one degree
479  angle_increments.push_back(M_PI/360); // half degree
480  angle_increments.push_back(M_PI/720); // quarter degree
481  permuter.addOptionSet(angle_increments, &angle_increment);
482 
483  scan_times.push_back(ros::Duration(1/40));
484  scan_times.push_back(ros::Duration(1/20));
485 
486  permuter.addOptionSet(scan_times, &scan_time);
487 
488  while (permuter.step())
489  {
490  try
491  {
492  //printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
493  sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
494  scan.header.frame_id = "laser_frame";
495 
496  sensor_msgs::PointCloud cloud_out;
497  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Index);
498  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
499  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity);
500  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
501 
502  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf);
503  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
504  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
505  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
506 
507  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);
508  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)3);
509 
510  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);
511  EXPECT_EQ(cloud_out.channels.size(), (unsigned int)4);
512 
513  unsigned int valid_points = 0;
514  for (unsigned int i = 0; i < scan.ranges.size(); i++)
515  if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
516  valid_points ++;
517  EXPECT_EQ(valid_points, cloud_out.points.size());
518 
519  for (unsigned int i = 0; i < cloud_out.points.size(); i++)
520  {
521  EXPECT_NEAR(cloud_out.points[i].x , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
522  EXPECT_NEAR(cloud_out.points[i].y , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
523  EXPECT_NEAR(cloud_out.points[i].z, 0, tolerance);
524  EXPECT_NEAR(cloud_out.channels[0].values[i], scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order
525  EXPECT_NEAR(cloud_out.channels[1].values[i], i, tolerance);//index
526  EXPECT_NEAR(cloud_out.channels[2].values[i], scan.ranges[i], tolerance);//ranges
527  EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps
528  };
529  }
530  catch (BuildScanException &ex)
531  {
532  if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
533  FAIL();
534  }
535  }
536 }
537 
538 TEST(laser_geometry, transformLaserScanToPointCloud2)
539 {
540 
541  tf::Transformer tf;
542  tf2::BufferCore tf2;
543 
544  double tolerance = 1e-12;
545  laser_geometry::LaserProjection projector;
546 
547  double min_angle = -M_PI/2;
548  double max_angle = M_PI/2;
549  double angle_increment = M_PI/180;
550 
551  double range = 1.0;
552  double intensity = 1.0;
553 
554  ros::Duration scan_time = ros::Duration(1/40);
555  ros::Duration increment_time = ros::Duration(1/400);
556 
557  std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
558  std::vector<ros::Duration> increment_times, scan_times;
559 
560  rostest::Permuter permuter;
561 
562  ranges.push_back(-1.0);
563  ranges.push_back(1.0);
564  ranges.push_back(2.0);
565  ranges.push_back(3.0);
566  ranges.push_back(4.0);
567  ranges.push_back(5.0);
568  ranges.push_back(100.0);
569  permuter.addOptionSet(ranges, &range);
570 
571  intensities.push_back(1.0);
572  intensities.push_back(2.0);
573  intensities.push_back(3.0);
574  intensities.push_back(4.0);
575  intensities.push_back(5.0);
576  permuter.addOptionSet(intensities, &intensity);
577 
578  min_angles.push_back(-M_PI);
579  min_angles.push_back(-M_PI/1.5);
580  min_angles.push_back(-M_PI/2);
581  min_angles.push_back(-M_PI/4);
582  min_angles.push_back(-M_PI/8);
583 
584 
585  max_angles.push_back(M_PI);
586  max_angles.push_back(M_PI/1.5);
587  max_angles.push_back(M_PI/2);
588  max_angles.push_back(M_PI/4);
589  max_angles.push_back(M_PI/8);
590 
591  permuter.addOptionSet(min_angles, &min_angle);
592  permuter.addOptionSet(max_angles, &max_angle);
593 
594  angle_increments.push_back(-M_PI/180); // -one degree
595  angle_increments.push_back(M_PI/180); // one degree
596  angle_increments.push_back(M_PI/360); // half degree
597  angle_increments.push_back(M_PI/720); // quarter degree
598  permuter.addOptionSet(angle_increments, &angle_increment);
599 
600  scan_times.push_back(ros::Duration(1/40));
601  scan_times.push_back(ros::Duration(1/20));
602 
603  permuter.addOptionSet(scan_times, &scan_time);
604 
605  while (permuter.step())
606  {
607  try
608  {
609  //printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
610  sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
611  scan.header.frame_id = "laser_frame";
612 
613  sensor_msgs::PointCloud2 cloud_out;
614  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::None);
615  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3);
616  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::None);
617  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3);
618  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Index);
619  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
620  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Index);
621  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
622  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity);
623  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
624  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity);
625  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
626 
627  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf);
628  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
629  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2);
630  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
631  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
632  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
633  projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
634  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
635 
636  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);
637  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6);
638  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);
639  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6);
640 
641  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);
642  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
643  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);
644  EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
645 
646  EXPECT_EQ(cloud_out.is_dense, false);
647 
648  unsigned int valid_points = 0;
649  for (unsigned int i = 0; i < scan.ranges.size(); i++)
650  if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
651  valid_points ++;
652  EXPECT_EQ(valid_points, cloud_out.width);
653 
654  uint32_t x_offset = 0;
655  uint32_t y_offset = 0;
656  uint32_t z_offset = 0;
657  uint32_t intensity_offset = 0;
658  uint32_t index_offset = 0;
659  uint32_t distance_offset = 0;
660  uint32_t stamps_offset = 0;
661  for (std::vector<sensor_msgs::PointField>::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++)
662  {
663  if (f->name == "x") x_offset = f->offset;
664  if (f->name == "y") y_offset = f->offset;
665  if (f->name == "z") z_offset = f->offset;
666  if (f->name == "intensity") intensity_offset = f->offset;
667  if (f->name == "index") index_offset = f->offset;
668  if (f->name == "distances") distance_offset = f->offset;
669  if (f->name == "stamps") stamps_offset = f->offset;
670  }
671 
672  for (unsigned int i = 0; i < cloud_out.width; i++)
673  {
674  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);
675  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);
676  EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + z_offset] , 0, tolerance);
677  EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + intensity_offset] , scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order
678  EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, tolerance);//index
679  EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + distance_offset], scan.ranges[i], tolerance);//ranges
680  EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + stamps_offset], (float)i * scan.time_increment, tolerance);//timestamps
681 
682  };
683  }
684  catch (BuildScanException &ex)
685  {
686  if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
687  FAIL();
688  }
689  }
690 
691 }
692 
693 
694 int main(int argc, char **argv){
695  ros::Time::init();
696  testing::InitGoogleTest(&argc, argv);
697  return RUN_ALL_TESTS();
698 }
laser_geometry::LaserProjection::projectLaser
void projectLaser(const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud &cloud_out, double range_cutoff=-1.0, int channel_options=channel_option::Default)
Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud.
Definition: laser_geometry.h:139
laser_geometry::LaserProjection
A Class to Project Laser Scan.
Definition: laser_geometry.h:113
f
f
laser_geometry::channel_option::Index
Enable "index" channel.
Definition: laser_geometry.h:83
build_constant_scan
sensor_msgs::LaserScan build_constant_scan(double range, double intensity, double ang_min, double ang_max, double ang_increment, ros::Duration scan_time)
Definition: projection_test.cpp:47
TEST
TEST(laser_geometry, projectLaser2)
Definition: projection_test.cpp:289
PROJECTION_TEST_RANGE_MIN
#define PROJECTION_TEST_RANGE_MIN
Definition: projection_test.cpp:41
laser_geometry
Definition: laser_geometry.h:64
laser_geometry::channel_option::None
Enable no channels.
Definition: laser_geometry.h:81
PROJECTION_TEST_RANGE_MAX
#define PROJECTION_TEST_RANGE_MAX
Definition: projection_test.cpp:42
ros::Time::init
static void init()
laser_geometry::channel_option::Timestamp
Enable "stamps" channel.
Definition: laser_geometry.h:85
TestProjection::getUnitVectors
const boost::numeric::ublas::matrix< double > & getUnitVectors(double angle_min, double angle_max, double angle_increment, unsigned int length)
Definition: projection_test.cpp:79
test_getUnitVectors
void test_getUnitVectors(double angle_min, double angle_max, double angle_increment, unsigned int length)
Definition: projection_test.cpp:88
laser_geometry::channel_option::Intensity
Enable "intensities" channel.
Definition: laser_geometry.h:82
laser_geometry.h
ros::Time::now
static Time now()
length
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
laser_geometry::channel_option::Distance
Enable "distances" channel.
Definition: laser_geometry.h:84
laser_geometry::LaserProjection::transformLaserScanToPointCloud
void transformLaserScanToPointCloud(const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud &cloud_out, tf::Transformer &tf, double range_cutoff, int channel_options=channel_option::Default)
Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud in target frame.
Definition: laser_geometry.h:193
angles::shortest_angular_distance
def shortest_angular_distance(from_angle, to_angle)
main
int main(int argc, char **argv)
Definition: projection_test.cpp:694

laser_geometry
Author(s): Tully Foote, Radu Bogdan Rusu
autogenerated on Tue Jul 2 2019 19:09:14