system.cpp
Go to the documentation of this file.
1 // Copyright (C) 2018, 2019 Kevin Hallenbeck, Joshua Whitley
2 // All rights reserved.
3 //
4 // Software License Agreement (BSD License 2.0)
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions
8 // are met:
9 //
10 // * Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 // * Redistributions in binary form must reproduce the above
13 // copyright notice, this list of conditions and the following
14 // disclaimer in the documentation and/or other materials provided
15 // with the distribution.
16 // * Neither the name of {copyright_holder} nor the names of its
17 // contributors may be used to endorse or promote products derived
18 // from this software without specific prior written permission.
19 //
20 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 // POSSIBILITY OF SUCH DAMAGE.
32 
33 #include <gtest/gtest.h>
34 
35 #include <ros/ros.h>
36 #include <sensor_msgs/PointCloud2.h>
37 #include <sensor_msgs/LaserScan.h>
38 
39 #include <cstdlib>
40 #include <algorithm>
41 #include <vector>
42 
43 // Define our own PointCloud type for easy use
44 typedef struct
45 {
46  float x; // x
47  float y; // y
48  float z; // z
49  float i; // intensity
50  uint16_t r; // ring
51 }
52 Point;
53 
54 typedef struct
55 {
56  std_msgs::Header header;
57  std::vector<Point> points;
58 }
60 
61 // Global variables
64 sensor_msgs::LaserScan g_scan;
65 volatile bool g_scan_new = false;
66 
67 // Convert WallTime to Time
68 static inline ros::Time rosTime(const ros::WallTime &stamp)
69 {
70  return ros::Time(stamp.sec, stamp.nsec);
71 }
72 
73 // Subscriber receive callback
74 void recv(const sensor_msgs::LaserScanConstPtr& msg)
75 {
76  g_scan = *msg;
77  g_scan_new = true;
78 }
79 
80 // Wait for incoming LaserScan message
82 {
83  const ros::WallTime start = ros::WallTime::now();
84 
85  while (!g_scan_new)
86  {
87  if ((ros::WallTime::now() - start) > dur)
88  {
89  return false;
90  }
91 
92  ros::WallDuration(0.001).sleep();
93  ros::spinOnce();
94  }
95 
96  return true;
97 }
98 
99 // Build and publish PointCloud2 messages of various structures
100 void publishXYZIR1(const PointCloud &cloud)
101 {
102  g_scan_new = false;
103  const uint32_t POINT_STEP = 32;
104  sensor_msgs::PointCloud2 msg;
105  msg.header.frame_id = cloud.header.frame_id;
106  msg.header.stamp = cloud.header.stamp;
107  msg.fields.resize(5);
108  msg.fields[0].name = "x";
109  msg.fields[0].offset = 0;
110  msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
111  msg.fields[0].count = 1;
112  msg.fields[1].name = "y";
113  msg.fields[1].offset = 4;
114  msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
115  msg.fields[1].count = 1;
116  msg.fields[2].name = "z";
117  msg.fields[2].offset = 8;
118  msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
119  msg.fields[2].count = 1;
120  msg.fields[3].name = "intensity";
121  msg.fields[3].offset = 16;
122  msg.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
123  msg.fields[3].count = 1;
124  msg.fields[4].name = "ring";
125  msg.fields[4].offset = 20;
126  msg.fields[4].datatype = sensor_msgs::PointField::UINT16;
127  msg.fields[4].count = 1;
128  msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00);
129  msg.point_step = POINT_STEP;
130  msg.row_step = msg.data.size();
131  msg.height = 1;
132  msg.width = msg.row_step / POINT_STEP;
133  msg.is_bigendian = false;
134  msg.is_dense = true;
135  uint8_t *ptr = msg.data.data();
136 
137  for (size_t i = 0; i < cloud.points.size(); i++)
138  {
139  *(reinterpret_cast<float*>(ptr + 0)) = cloud.points[i].x;
140  *(reinterpret_cast<float*>(ptr + 4)) = cloud.points[i].y;
141  *(reinterpret_cast<float*>(ptr + 8)) = cloud.points[i].z;
142  *(reinterpret_cast<float*>(ptr + 16)) = cloud.points[i].i;
143  *(reinterpret_cast<uint16_t*>(ptr + 20)) = cloud.points[i].r;
144  ptr += POINT_STEP;
145  }
146 
147  g_pub.publish(msg);
148 }
149 
150 void publishXYZIR2(const PointCloud &cloud)
151 {
152  g_scan_new = false;
153  const uint32_t POINT_STEP = 19;
154  sensor_msgs::PointCloud2 msg;
155  msg.header.frame_id = cloud.header.frame_id;
156  msg.header.stamp = cloud.header.stamp;
157  msg.fields.resize(5);
158  msg.fields[0].name = "z";
159  msg.fields[0].offset = 4;
160  msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
161  msg.fields[0].count = 1;
162  msg.fields[1].name = "y";
163  msg.fields[1].offset = 8;
164  msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
165  msg.fields[1].count = 1;
166  msg.fields[2].name = "x";
167  msg.fields[2].offset = 12;
168  msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
169  msg.fields[2].count = 1;
170  msg.fields[3].name = "intensity";
171  msg.fields[3].offset = 0;
172  msg.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
173  msg.fields[3].count = 1;
174  msg.fields[4].name = "ring";
175  msg.fields[4].offset = 16;
176  msg.fields[4].datatype = sensor_msgs::PointField::UINT16;
177  msg.fields[4].count = 1;
178  msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00);
179  msg.point_step = POINT_STEP;
180  msg.row_step = msg.data.size();
181  msg.height = 1;
182  msg.width = msg.row_step / POINT_STEP;
183  msg.is_bigendian = false;
184  msg.is_dense = true;
185  uint8_t *ptr = msg.data.data();
186 
187  for (size_t i = 0; i < cloud.points.size(); i++)
188  {
189  *(reinterpret_cast<float*>(ptr + 0)) = cloud.points[i].i;
190  *(reinterpret_cast<float*>(ptr + 4)) = cloud.points[i].z;
191  *(reinterpret_cast<float*>(ptr + 8)) = cloud.points[i].y;
192  *(reinterpret_cast<float*>(ptr + 12)) = cloud.points[i].x;
193  *(reinterpret_cast<uint16_t*>(ptr + 16)) = cloud.points[i].r;
194  ptr += POINT_STEP;
195  }
196 
197  g_pub.publish(msg);
198 }
199 
200 void publishXYZR(const PointCloud &cloud)
201 {
202  g_scan_new = false;
203  const uint32_t POINT_STEP = 15;
204  sensor_msgs::PointCloud2 msg;
205  msg.header.frame_id = cloud.header.frame_id;
206  msg.header.stamp = cloud.header.stamp;
207  msg.fields.resize(4);
208  msg.fields[0].name = "x";
209  msg.fields[0].offset = 0;
210  msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
211  msg.fields[0].count = 1;
212  msg.fields[1].name = "y";
213  msg.fields[1].offset = 4;
214  msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
215  msg.fields[1].count = 1;
216  msg.fields[2].name = "z";
217  msg.fields[2].offset = 8;
218  msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
219  msg.fields[2].count = 1;
220  msg.fields[3].name = "ring";
221  msg.fields[3].offset = 12;
222  msg.fields[3].datatype = sensor_msgs::PointField::UINT16;
223  msg.fields[3].count = 1;
224  msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00);
225  msg.point_step = POINT_STEP;
226  msg.row_step = msg.data.size();
227  msg.height = 1;
228  msg.width = msg.row_step / POINT_STEP;
229  msg.is_bigendian = false;
230  msg.is_dense = true;
231  uint8_t *ptr = msg.data.data();
232 
233  for (size_t i = 0; i < cloud.points.size(); i++)
234  {
235  *(reinterpret_cast<float*>(ptr + 0)) = cloud.points[i].x;
236  *(reinterpret_cast<float*>(ptr + 4)) = cloud.points[i].y;
237  *(reinterpret_cast<float*>(ptr + 8)) = cloud.points[i].z;
238  *(reinterpret_cast<uint16_t*>(ptr + 12)) = cloud.points[i].r;
239  ptr += POINT_STEP;
240  }
241 
242  g_pub.publish(msg);
243 }
244 
245 void publishR(const PointCloud &cloud)
246 {
247  g_scan_new = false;
248  const uint32_t POINT_STEP = 2;
249  sensor_msgs::PointCloud2 msg;
250  msg.header.stamp = rosTime(ros::WallTime::now());
251  msg.fields.resize(1);
252  msg.fields[0].name = "ring";
253  msg.fields[0].offset = 0;
254  msg.fields[0].datatype = sensor_msgs::PointField::UINT16;
255  msg.fields[0].count = 1;
256  msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00);
257  msg.point_step = POINT_STEP;
258  msg.row_step = msg.data.size();
259  msg.height = 1;
260  msg.width = msg.row_step / POINT_STEP;
261  uint8_t *ptr = msg.data.data();
262 
263  for (size_t i = 0; i < cloud.points.size(); i++)
264  {
265  *(reinterpret_cast<uint16_t*>(ptr + 0)) = cloud.points[i].r;
266  ptr += POINT_STEP;
267  }
268 
269  g_pub.publish(msg);
270 }
271 
272 void publishXYZR32(const PointCloud &cloud)
273 {
274  g_scan_new = false;
275  const uint32_t POINT_STEP = 16;
276  sensor_msgs::PointCloud2 msg;
277  msg.header.frame_id = cloud.header.frame_id;
278  msg.header.stamp = cloud.header.stamp;
279  msg.fields.resize(4);
280  msg.fields[0].name = "x";
281  msg.fields[0].offset = 0;
282  msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
283  msg.fields[0].count = 1;
284  msg.fields[1].name = "y";
285  msg.fields[1].offset = 4;
286  msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
287  msg.fields[1].count = 1;
288  msg.fields[2].name = "z";
289  msg.fields[2].offset = 8;
290  msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
291  msg.fields[2].count = 1;
292  msg.fields[3].name = "ring";
293  msg.fields[3].offset = 12;
294  msg.fields[3].datatype = sensor_msgs::PointField::UINT32;
295  msg.fields[3].count = 1;
296  msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00);
297  msg.point_step = POINT_STEP;
298  msg.row_step = msg.data.size();
299  msg.height = 1;
300  msg.width = msg.row_step / POINT_STEP;
301  msg.is_bigendian = false;
302  msg.is_dense = true;
303  uint8_t *ptr = msg.data.data();
304 
305  for (size_t i = 0; i < cloud.points.size(); i++)
306  {
307  *(reinterpret_cast<float*>(ptr + 0)) = cloud.points[i].x;
308  *(reinterpret_cast<float*>(ptr + 4)) = cloud.points[i].y;
309  *(reinterpret_cast<float*>(ptr + 8)) = cloud.points[i].z;
310  *(reinterpret_cast<uint32_t*>(ptr + 12)) = cloud.points[i].r;
311  ptr += POINT_STEP;
312  }
313 
314  g_pub.publish(msg);
315 }
316 
317 void publishXYZ(const PointCloud &cloud)
318 {
319  g_scan_new = false;
320  const uint32_t POINT_STEP = 12;
321  sensor_msgs::PointCloud2 msg;
322  msg.header.stamp = rosTime(ros::WallTime::now());
323  msg.fields.resize(3);
324  msg.fields[0].name = "x";
325  msg.fields[0].offset = 0;
326  msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
327  msg.fields[0].count = 1;
328  msg.fields[1].name = "y";
329  msg.fields[1].offset = 4;
330  msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
331  msg.fields[1].count = 1;
332  msg.fields[2].name = "z";
333  msg.fields[2].offset = 8;
334  msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
335  msg.fields[2].count = 1;
336  msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00);
337  msg.point_step = POINT_STEP;
338  msg.row_step = msg.data.size();
339  msg.height = 1;
340  msg.width = msg.row_step / POINT_STEP;
341  uint8_t *ptr = msg.data.data();
342 
343  for (size_t i = 0; i < cloud.points.size(); i++)
344  {
345  *(reinterpret_cast<float*>(ptr + 0)) = cloud.points[i].x;
346  *(reinterpret_cast<float*>(ptr + 4)) = cloud.points[i].y;
347  *(reinterpret_cast<float*>(ptr + 8)) = cloud.points[i].z;
348  ptr += POINT_STEP;
349  }
350 
351  g_pub.publish(msg);
352 }
353 
355 {
356  g_scan_new = false;
357  const uint32_t POINT_STEP = 16;
358  sensor_msgs::PointCloud2 msg;
359  msg.header.stamp = rosTime(ros::WallTime::now());
360  msg.data.resize(1 * POINT_STEP, 0x00);
361  msg.point_step = POINT_STEP;
362  msg.row_step = msg.data.size();
363  msg.height = 1;
364  msg.width = msg.row_step / POINT_STEP;
365  g_pub.publish(msg);
366 }
367 
368 // Find the index of the point in the PointCloud with the shortest 2d distance to the point (x,y)
369 static inline float SQUARE(float x)
370 {
371  return x * x;
372 }
373 
374 size_t findClosestIndex(const PointCloud &cloud, uint16_t ring, float x, float y)
375 {
376  size_t index = SIZE_MAX;
377  float delta = INFINITY;
378 
379  for (size_t i = 0; i < cloud.points.size(); i++)
380  {
381  if (cloud.points[i].r == ring)
382  {
383  float dist = SQUARE(x - cloud.points[i].x) + SQUARE(y - cloud.points[i].y);
384 
385  if (dist < delta)
386  {
387  delta = dist;
388  index = i;
389  }
390  }
391  }
392 
393  return index;
394 }
395 
396 // Verify that all LaserScan header values are values are passed through, and other values are default
397 void verifyScanEmpty(const PointCloud &cloud, bool intensity = true)
398 {
399  ASSERT_EQ(cloud.header.stamp, g_scan.header.stamp);
400  EXPECT_EQ(cloud.header.frame_id, g_scan.header.frame_id);
401 
402  for (size_t i = 0; i < g_scan.ranges.size(); i++)
403  {
404  EXPECT_EQ(INFINITY, g_scan.ranges[i]);
405  }
406 
407  if (!intensity)
408  {
409  EXPECT_EQ(0, g_scan.intensities.size());
410  }
411  else
412  {
413  EXPECT_EQ(g_scan.ranges.size(), g_scan.intensities.size());
414 
415  for (size_t i = 0; i < g_scan.intensities.size(); i++)
416  {
417  EXPECT_EQ(0.0, g_scan.intensities[i]);
418  }
419  }
420 }
421 
422 // Verify that every PointCloud point made it to the LaserScan and other values are default
423 void verifyScanSparse(const PointCloud &cloud, uint16_t ring, uint16_t ring_count, bool intensity = true)
424 {
425  ASSERT_EQ(cloud.header.stamp, g_scan.header.stamp);
426  EXPECT_EQ(cloud.header.frame_id, g_scan.header.frame_id);
427  EXPECT_EQ(intensity ? g_scan.ranges.size() : 0, g_scan.intensities.size());
428  size_t count = 0;
429 
430  for (size_t i = 0; i < g_scan.ranges.size(); i++)
431  {
432  double r = g_scan.ranges[i];
433 
434  if (std::isfinite(r))
435  {
436  float a = g_scan.angle_min + i * g_scan.angle_increment;
437  float x = g_scan.ranges[i] * cosf(a);
438  float y = g_scan.ranges[i] * sinf(a);
439  float e = g_scan.ranges[i] * g_scan.angle_increment + static_cast<float>(1e-3); // allowable error
440  size_t index = findClosestIndex(cloud, ring, x, y);
441 
442  if (index < cloud.points.size())
443  {
444  count++;
445  EXPECT_NEAR(cloud.points[index].x, x, e);
446  EXPECT_NEAR(cloud.points[index].y, y, e);
447 
448  if (i < g_scan.intensities.size())
449  {
450  EXPECT_EQ(cloud.points[index].i, g_scan.intensities[i]);
451  }
452  }
453  else
454  {
455  EXPECT_TRUE(false); // LaserScan point not found in PointCloud
456  }
457  }
458  else
459  {
460  EXPECT_EQ(INFINITY, r);
461  }
462  }
463 
464  if (ring_count > 0)
465  {
466  EXPECT_EQ(cloud.points.size() / ring_count, count); // Make sure that all points were converted to ranges
467  }
468 }
469 
470 // Verify that every LaserScan point is not default, and every point came from the PointCloud
471 void verifyScanDense(const PointCloud &cloud, uint16_t ring, bool intensity = true)
472 {
473  ASSERT_EQ(cloud.header.stamp, g_scan.header.stamp);
474  EXPECT_EQ(cloud.header.frame_id, g_scan.header.frame_id);
475  EXPECT_EQ(intensity ? g_scan.ranges.size() : 0, g_scan.intensities.size());
476 
477  for (size_t i = 0; i < g_scan.ranges.size(); i++)
478  {
479  double r = g_scan.ranges[i];
480 
481  if (std::isfinite(r))
482  {
483  float a = g_scan.angle_min + i * g_scan.angle_increment;
484  float x = g_scan.ranges[i] * cosf(a);
485  float y = g_scan.ranges[i] * sinf(a);
486  float e = g_scan.ranges[i] * g_scan.angle_increment + static_cast<float>(1e-3); // allowable error
487  size_t index = findClosestIndex(cloud, ring, x, y);
488 
489  if (index < cloud.points.size())
490  {
491  EXPECT_NEAR(cloud.points[index].x, x, e);
492  EXPECT_NEAR(cloud.points[index].y, y, e);
493  // @TODO: Test for matching intensity
494  }
495  else
496  {
497  EXPECT_TRUE(false); // LaserScan point not found in PointCloud
498  }
499  }
500  else
501  {
502  EXPECT_TRUE(false); // Dense PointCloud should populate every range in LaserScan
503  }
504  }
505 }
506 
507 // Verify that no LaserScan is generated when the PointCloud2 message is missing required fields
508 TEST(System, missing_fields)
509 {
510  // Make sure system is connected
511  ASSERT_EQ(1, g_sub.getNumPublishers());
512  ASSERT_EQ(1, g_pub.getNumSubscribers());
513 
514  // Create PointCloud with 16 rings
515  PointCloud cloud;
516  cloud.points.resize(1);
517  cloud.points[0].x = 0.0;
518  cloud.points[0].y = 0.0;
519  cloud.points[0].z = 0.0;
520  cloud.points[0].i = 0.0;
521  cloud.points[0].r = 15;
522 
523  // Verify no LaserScan when PointCloud2 fields are empty
524  publishNone();
525  EXPECT_FALSE(waitForScan(ros::WallDuration(0.5)));
526 
527  // Verify no LaserScan when PointCloud2 fields are missing 'ring'
528  publishXYZ(cloud);
529  EXPECT_FALSE(waitForScan(ros::WallDuration(0.5)));
530 
531  // Verify no LaserScan when PointCloud2 field 'ring' is the incorrect type
532  publishXYZR32(cloud);
533  EXPECT_FALSE(waitForScan(ros::WallDuration(0.5)));
534 
535 
536  // Verify no LaserScan when PointCloud2 fields are missing 'x' and 'y'
537  publishR(cloud);
538  EXPECT_FALSE(waitForScan(ros::WallDuration(0.5)));
539 
540  // Verify that the node hasn't crashed by sending normal PointCloud2 fields
541  cloud.header.stamp = rosTime(ros::WallTime::now());
542  publishXYZIR1(cloud);
543  ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
544  ASSERT_EQ(cloud.header.stamp, g_scan.header.stamp);
545 }
546 
547 // Verify that non-point fields are passed through unmodified
548 TEST(System, empty_data)
549 {
550  // Make sure system is connected
551  ASSERT_EQ(1, g_sub.getNumPublishers());
552  ASSERT_EQ(1, g_pub.getNumSubscribers());
553 
554  // Create PointCloud with 16 rings
555  PointCloud cloud;
556  cloud.header.frame_id = "abcdefghijklmnopqrstuvwxyz";
557  cloud.points.resize(1);
558  cloud.points[0].x = 0.0;
559  cloud.points[0].y = 0.0;
560  cloud.points[0].z = 0.0;
561  cloud.points[0].i = 0.0;
562  cloud.points[0].r = 15;
563 
564  // Verify that all three PointCloud2 types create proper default values
565 
566  // PointXYZIR (expected format)
567  cloud.header.stamp = rosTime(ros::WallTime::now());
568  publishXYZIR1(cloud);
569  ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
570  verifyScanEmpty(cloud, true);
571 
572  // PointXYZIR (unexpected format with intensity)
573  cloud.header.stamp = rosTime(ros::WallTime::now());
574  publishXYZIR2(cloud);
575  ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
576  verifyScanEmpty(cloud, true);
577 
578  // PointXYZR (unexpected format without intensity)
579  cloud.header.stamp = rosTime(ros::WallTime::now());
580  publishXYZR(cloud);
581  ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
582  verifyScanEmpty(cloud, false);
583 }
584 
585 // Verify that every piece of a small amount of random data is passed through
586 TEST(System, random_data_sparse)
587 {
588  // Make sure system is connected
589  ASSERT_EQ(1, g_sub.getNumPublishers());
590  ASSERT_EQ(1, g_pub.getNumSubscribers());
591 
592  // Create PointCloud with sparse random data
593  PointCloud cloud;
594  cloud.header.frame_id = "velodyne";
595  const size_t RANGE_COUNT = 100;
596  const size_t RING_COUNT = 16;
597  const double RANGE_MAX = 20.0;
598  const double INTENSITY_MAX = 1.0;
599 
600  for (size_t i = 0; i < RANGE_COUNT; i++)
601  {
602  double angle_y = i * 1.99 * M_PI / RANGE_COUNT; // yaw
603 
604  for (size_t j = 0; j < RING_COUNT; j++)
605  {
606  double angle_p = j * 0.2 * M_PI / RING_COUNT - 0.1 * M_PI; // pitch
607  double range = std::rand() * (RANGE_MAX / RAND_MAX);
608  Point point;
609  point.x = range * cos(angle_p) * cos(angle_y);
610  point.y = range * cos(angle_p) * sin(angle_y);
611  point.z = range * sin(angle_p);
612  point.i = std::rand() * (INTENSITY_MAX / RAND_MAX);
613  point.r = j;
614  cloud.points.push_back(point);
615  }
616  }
617 
618  // Verify that all three PointCloud2 types are handled correctly
619 
620  // PointXYZIR (expected format)
621  cloud.header.stamp = rosTime(ros::WallTime::now());
622  publishXYZIR1(cloud);
623  ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
624  verifyScanSparse(cloud, 8, RING_COUNT, true);
625 
626  // PointXYZIR (unexpected format with intensity)
627  cloud.header.stamp = rosTime(ros::WallTime::now());
628  publishXYZIR2(cloud);
629  ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
630  verifyScanSparse(cloud, 8, RING_COUNT, true);
631 
632  // PointXYZR (unexpected format without intensity)
633  cloud.header.stamp = rosTime(ros::WallTime::now());
634  publishXYZR(cloud);
635  ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
636  verifyScanSparse(cloud, 8, RING_COUNT, false);
637 }
638 
639 // Verify that every LaserScan range is valid when given an extra large amount of random data
640 TEST(System, random_data_dense)
641 {
642  // Make sure system is connected
643  ASSERT_EQ(1, g_sub.getNumPublishers());
644  ASSERT_EQ(1, g_pub.getNumSubscribers());
645 
646  // Create PointCloud with dense random data
647  PointCloud cloud;
648  cloud.header.frame_id = "velodyne";
649  const size_t RANGE_COUNT = 2500;
650  const size_t RING_COUNT = 16;
651  const double RANGE_MAX = 20.0;
652  const double INTENSITY_MAX = 1.0;
653 
654  for (size_t i = 0; i < RANGE_COUNT; i++)
655  {
656  double angle_y = i * 2.0 * M_PI / RANGE_COUNT; // yaw
657 
658  for (size_t j = 0; j < RING_COUNT; j++)
659  {
660  double angle_p = j * 0.2 * M_PI / RING_COUNT - 0.1 * M_PI; // pitch
661  double range = std::rand() * (RANGE_MAX / RAND_MAX);
662  Point point;
663  point.x = range * cos(angle_p) * cos(angle_y);
664  point.y = range * cos(angle_p) * sin(angle_y);
665  point.z = range * sin(angle_p);
666  point.i = std::rand() * (INTENSITY_MAX / RAND_MAX);
667  point.r = j;
668  cloud.points.push_back(point);
669  }
670  }
671 
672  // Verify that all three PointCloud2 types are handled correctly
673 
674  // PointXYZIR (expected format)
675  cloud.header.stamp = rosTime(ros::WallTime::now());
676  publishXYZIR1(cloud);
677  ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
678  verifyScanDense(cloud, 8, true);
679 
680  // PointXYZIR (unexpected format with intensity)
681  cloud.header.stamp = rosTime(ros::WallTime::now());
682  publishXYZIR2(cloud);
683  ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
684  verifyScanDense(cloud, 8, true);
685 
686  // PointXYZR (unexpected format without intensity)
687  cloud.header.stamp = rosTime(ros::WallTime::now());
688  publishXYZR(cloud);
689  ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
690  verifyScanDense(cloud, 8, false);
691 }
692 
693 int main(int argc, char **argv)
694 {
695  testing::InitGoogleTest(&argc, argv);
696 
697  // Initialize ROS
698  ros::init(argc, argv, "test_lazy_subscriber");
699  ros::NodeHandle nh;
700 
701  // Setup publisher and subscriber
702  g_pub = nh.advertise<sensor_msgs::PointCloud2>("velodyne_points", 2);
703  g_sub = nh.subscribe("scan", 2, recv);
704 
705  // Wait for other nodes to startup
706  ros::WallDuration(1.0).sleep();
707  ros::spinOnce();
708 
709  // Run all the tests that were declared with TEST()
710  return RUN_ALL_TESTS();
711 }
TEST(System, missing_fields)
Definition: system.cpp:508
void verifyScanDense(const PointCloud &cloud, uint16_t ring, bool intensity=true)
Definition: system.cpp:471
ros::Subscriber g_sub
Definition: system.cpp:63
void verifyScanEmpty(const PointCloud &cloud, bool intensity=true)
Definition: system.cpp:397
void publishXYZ(const PointCloud &cloud)
Definition: system.cpp:317
void publish(const boost::shared_ptr< M > &message) const
void publishXYZIR1(const PointCloud &cloud)
Definition: system.cpp:100
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::vector< Point > points
Definition: system.cpp:57
static float SQUARE(float x)
Definition: system.cpp:369
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void publishXYZIR2(const PointCloud &cloud)
Definition: system.cpp:150
sensor_msgs::LaserScan g_scan
Definition: system.cpp:64
ros::Publisher g_pub
Definition: system.cpp:62
void publishR(const PointCloud &cloud)
Definition: system.cpp:245
bool sleep() const
float y
Definition: system.cpp:47
float x
Definition: system.cpp:46
float i
Definition: system.cpp:49
uint32_t getNumPublishers() const
static ros::Time rosTime(const ros::WallTime &stamp)
Definition: system.cpp:68
int main(int argc, char **argv)
Definition: system.cpp:693
size_t findClosestIndex(const PointCloud &cloud, uint16_t ring, float x, float y)
Definition: system.cpp:374
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void publishNone()
Definition: system.cpp:354
uint32_t getNumSubscribers() const
static WallTime now()
volatile bool g_scan_new
Definition: system.cpp:65
void recv(const sensor_msgs::LaserScanConstPtr &msg)
Definition: system.cpp:74
void publishXYZR32(const PointCloud &cloud)
Definition: system.cpp:272
Definition: system.cpp:44
float z
Definition: system.cpp:48
uint16_t r
Definition: system.cpp:50
std_msgs::Header header
Definition: system.cpp:56
void verifyScanSparse(const PointCloud &cloud, uint16_t ring, uint16_t ring_count, bool intensity=true)
Definition: system.cpp:423
ROSCPP_DECL void spinOnce()
bool waitForScan(ros::WallDuration dur)
Definition: system.cpp:81
void publishXYZR(const PointCloud &cloud)
Definition: system.cpp:200


velodyne_laserscan
Author(s): Micho Radovnikovich, Kevin Hallenbeck
autogenerated on Sun Sep 6 2020 03:25:25