main.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Open Source Robotics Foundation
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #include <gtest/gtest.h>
36 
38 
39 TEST(sensor_msgs, PointCloud2Iterator)
40 {
41  // Create a dummy PointCloud2
42  int n_points = 4;
43  sensor_msgs::PointCloud2 cloud_msg_1, cloud_msg_2;
44  cloud_msg_1.height = n_points;
45  cloud_msg_1.width = 1;
46  sensor_msgs::PointCloud2Modifier modifier(cloud_msg_1);
47  modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
48  cloud_msg_2 = cloud_msg_1;
49 
50  // Fill 1 by hand
51  float point_data_raw[] = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0};
52  std::vector<float> point_data(point_data_raw, point_data_raw + 3*n_points);
53  // colors in RGB order
54  uint8_t color_data_raw[] = {40, 80, 120, 160, 200, 240, 20, 40, 60, 80, 100, 120};
55  std::vector<uint8_t> color_data(color_data_raw, color_data_raw + 3*n_points);
56 
57  float *data = reinterpret_cast<float*>(&cloud_msg_1.data.front());
58  for(size_t n=0, i=0; n<n_points; ++n) {
59  for(; i<3*(n+1); ++i)
60  *(data++) = point_data[i];
61  // Add an extra float of padding
62  ++data;
63  uint8_t *bgr = reinterpret_cast<uint8_t*>(data++);
64  // add the colors in order BGRA like PCL
65  size_t j_max = 2;
66  for(size_t j = 0; j <= j_max; ++j)
67  *(bgr++) = color_data[3*n+(j_max - j)];
68  // Add 3 extra floats of padding
69  data += 3;
70  }
71 
72  // Fill 2 using an iterator
73  sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg_2, "x");
74  sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(cloud_msg_2, "r");
75  sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(cloud_msg_2, "g");
76  sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(cloud_msg_2, "b");
77  for(size_t i=0; i<n_points; ++i, ++iter_x, ++iter_r, ++iter_g, ++iter_b) {
78  for(size_t j=0; j<3; ++j)
79  iter_x[j] = point_data[j+3*i];
80  *iter_r = color_data[3*i];
81  *iter_g = color_data[3*i+1];
82  *iter_b = color_data[3*i+2];
83  }
84 
85 
86  // Check the values using an iterator
87  sensor_msgs::PointCloud2ConstIterator<float> iter_const_1_x(cloud_msg_1, "x"), iter_const_2_x(cloud_msg_2, "x");
88  sensor_msgs::PointCloud2ConstIterator<float> iter_const_1_y(cloud_msg_1, "y"), iter_const_2_y(cloud_msg_2, "y");
89  sensor_msgs::PointCloud2ConstIterator<float> iter_const_1_z(cloud_msg_1, "z"), iter_const_2_z(cloud_msg_2, "z");
90  sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_const_1_r(cloud_msg_1, "r"), iter_const_2_r(cloud_msg_2, "r");
91  sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_const_1_g(cloud_msg_1, "g"), iter_const_2_g(cloud_msg_2, "g");
92  sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_const_1_b(cloud_msg_1, "b"), iter_const_2_b(cloud_msg_2, "b");
93 
94  size_t i=0;
95  for(; iter_const_1_x != iter_const_1_x.end(); ++i, ++iter_const_1_x, ++iter_const_2_x, ++iter_const_1_y, ++iter_const_2_y,
96  ++iter_const_1_z, ++iter_const_2_z, ++iter_const_1_r, ++iter_const_1_g, ++iter_const_1_b) {
97  EXPECT_EQ(*iter_const_1_x, *iter_const_2_x);
98  EXPECT_EQ(*iter_const_1_x, point_data[0+3*i]);
99  EXPECT_EQ(*iter_const_1_y, *iter_const_2_y);
100  EXPECT_EQ(*iter_const_1_y, point_data[1+3*i]);
101  EXPECT_EQ(*iter_const_1_z, *iter_const_2_z);
102  EXPECT_EQ(*iter_const_1_z, point_data[2+3*i]);
103  EXPECT_EQ(*iter_const_1_r, *iter_const_2_r);
104  EXPECT_EQ(*iter_const_1_r, color_data[3*i+0]);
105  EXPECT_EQ(*iter_const_1_g, *iter_const_2_g);
106  EXPECT_EQ(*iter_const_1_g, color_data[3*i+1]);
107  EXPECT_EQ(*iter_const_1_b, *iter_const_2_b);
108  EXPECT_EQ(*iter_const_1_b, color_data[3*i+2]);
109  // This is to test the different operators
110  ++iter_const_2_r;
111  iter_const_2_g += 1;
112  iter_const_2_b = iter_const_2_b + 1;
113  }
114  EXPECT_EQ(i, n_points);
115 }
116 
117 int main(int argc, char** argv)
118 {
119  ::testing::InitGoogleTest(&argc, argv);
120  return RUN_ALL_TESTS();
121 }
Same as a PointCloud2Iterator but for const data.
int main(int argc, char **argv)
Definition: main.cpp:117
Tools for manipulating sensor_msgs.
Class that can iterate over a PointCloud2.
Enables modifying a sensor_msgs::PointCloud2 like a container.
TEST(sensor_msgs, PointCloud2Iterator)
Definition: main.cpp:39
void setPointCloud2FieldsByString(int n_fields,...)
Function setting some fields in a PointCloud and adjusting the internals of the PointCloud2.


sensor_msgs
Author(s):
autogenerated on Fri Jun 7 2019 21:44:16