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 
37 #define _GLIBCXX_DEBUG // Enable assertions in STL
38 
39 #include <sensor_msgs/point_cloud2_iterator.h>
40 
41 TEST(sensor_msgs, PointCloud2Iterator_Empty)
42 {
43  EXPECT_EXIT({
44  sensor_msgs::PointCloud2 cloud_msg;
45  cloud_msg.width = 1;
46  sensor_msgs::PointCloud2Modifier modifier(cloud_msg);
47  modifier.setPointCloud2FieldsByString(1, "xyz");
48 
49  sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg, "x");
50 
51  exit(0);
52  }, ::testing::ExitedWithCode(0), "");
53 }
54 
55 TEST(sensor_msgs, PointCloud2Iterator)
56 {
57  // Create a dummy PointCloud2
58  int n_points = 4;
59  sensor_msgs::PointCloud2 cloud_msg_1, cloud_msg_2;
60  cloud_msg_1.height = n_points;
61  cloud_msg_1.width = 1;
62  sensor_msgs::PointCloud2Modifier modifier(cloud_msg_1);
63  modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
64  cloud_msg_2 = cloud_msg_1;
65 
66  // Fill 1 by hand
67  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};
68  std::vector<float> point_data(point_data_raw, point_data_raw + 3*n_points);
69  // colors in RGB order
70  uint8_t color_data_raw[] = {40, 80, 120, 160, 200, 240, 20, 40, 60, 80, 100, 120};
71  std::vector<uint8_t> color_data(color_data_raw, color_data_raw + 3*n_points);
72 
73  float *data = reinterpret_cast<float*>(&cloud_msg_1.data.front());
74  for(size_t n=0, i=0; n<n_points; ++n) {
75  for(; i<3*(n+1); ++i)
76  *(data++) = point_data[i];
77  // Add an extra float of padding
78  ++data;
79  uint8_t *bgr = reinterpret_cast<uint8_t*>(data++);
80  // add the colors in order BGRA like PCL
81  size_t j_max = 2;
82  for(size_t j = 0; j <= j_max; ++j)
83  *(bgr++) = color_data[3*n+(j_max - j)];
84  // Add 3 extra floats of padding
85  data += 3;
86  }
87 
88  // Fill 2 using an iterator
89  sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg_2, "x");
90  sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(cloud_msg_2, "r");
91  sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(cloud_msg_2, "g");
92  sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(cloud_msg_2, "b");
93  for(size_t i=0; i<n_points; ++i, ++iter_x, ++iter_r, ++iter_g, ++iter_b) {
94  for(size_t j=0; j<3; ++j)
95  iter_x[j] = point_data[j+3*i];
96  *iter_r = color_data[3*i];
97  *iter_g = color_data[3*i+1];
98  *iter_b = color_data[3*i+2];
99  }
100 
101 
102  // Check the values using an iterator
103  sensor_msgs::PointCloud2ConstIterator<float> iter_const_1_x(cloud_msg_1, "x"), iter_const_2_x(cloud_msg_2, "x");
104  sensor_msgs::PointCloud2ConstIterator<float> iter_const_1_y(cloud_msg_1, "y"), iter_const_2_y(cloud_msg_2, "y");
105  sensor_msgs::PointCloud2ConstIterator<float> iter_const_1_z(cloud_msg_1, "z"), iter_const_2_z(cloud_msg_2, "z");
106  sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_const_1_r(cloud_msg_1, "r"), iter_const_2_r(cloud_msg_2, "r");
107  sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_const_1_g(cloud_msg_1, "g"), iter_const_2_g(cloud_msg_2, "g");
108  sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_const_1_b(cloud_msg_1, "b"), iter_const_2_b(cloud_msg_2, "b");
109 
110  size_t i=0;
111  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,
112  ++iter_const_1_z, ++iter_const_2_z, ++iter_const_1_r, ++iter_const_1_g, ++iter_const_1_b) {
113  EXPECT_EQ(*iter_const_1_x, *iter_const_2_x);
114  EXPECT_EQ(*iter_const_1_x, point_data[0+3*i]);
115  EXPECT_EQ(*iter_const_1_y, *iter_const_2_y);
116  EXPECT_EQ(*iter_const_1_y, point_data[1+3*i]);
117  EXPECT_EQ(*iter_const_1_z, *iter_const_2_z);
118  EXPECT_EQ(*iter_const_1_z, point_data[2+3*i]);
119  EXPECT_EQ(*iter_const_1_r, *iter_const_2_r);
120  EXPECT_EQ(*iter_const_1_r, color_data[3*i+0]);
121  EXPECT_EQ(*iter_const_1_g, *iter_const_2_g);
122  EXPECT_EQ(*iter_const_1_g, color_data[3*i+1]);
123  EXPECT_EQ(*iter_const_1_b, *iter_const_2_b);
124  EXPECT_EQ(*iter_const_1_b, color_data[3*i+2]);
125  // This is to test the different operators
126  ++iter_const_2_r;
127  iter_const_2_g += 1;
128  iter_const_2_b = iter_const_2_b + 1;
129  }
130  EXPECT_FALSE(iter_const_1_y != iter_const_1_y.end());
131  EXPECT_FALSE(iter_const_1_z != iter_const_1_z.end());
132  EXPECT_FALSE(iter_const_1_r != iter_const_1_r.end());
133  EXPECT_FALSE(iter_const_1_g != iter_const_1_g.end());
134  EXPECT_FALSE(iter_const_1_b != iter_const_1_b.end());
135  EXPECT_FALSE(iter_const_2_x != iter_const_2_x.end());
136  EXPECT_FALSE(iter_const_2_y != iter_const_2_y.end());
137  EXPECT_FALSE(iter_const_2_z != iter_const_2_z.end());
138  EXPECT_FALSE(iter_const_2_r != iter_const_2_r.end());
139  EXPECT_FALSE(iter_const_2_g != iter_const_2_g.end());
140  EXPECT_FALSE(iter_const_2_b != iter_const_2_b.end());
141  EXPECT_EQ(i, n_points);
142 }
143 
144 int main(int argc, char** argv)
145 {
146  ::testing::InitGoogleTest(&argc, argv);
147  return RUN_ALL_TESTS();
148 }
sensor_msgs::impl::PointCloud2IteratorBase< T, const T, const unsigned char, const sensor_msgs::PointCloud2, PointCloud2ConstIterator >::end
PointCloud2ConstIterator< T > end() const
Definition: impl/point_cloud2_iterator.h:346
main
int main(int argc, char **argv)
Definition: main.cpp:144
TEST
TEST(sensor_msgs, PointCloud2Iterator_Empty)
Definition: main.cpp:41
sensor_msgs::PointCloud2ConstIterator
Same as a PointCloud2Iterator but for const data.
Definition: point_cloud2_iterator.h:292
sensor_msgs::PointCloud2Iterator
Class that can iterate over a PointCloud2.
Definition: point_cloud2_iterator.h:281
point_cloud2_iterator.h
sensor_msgs::PointCloud2Modifier
Enables modifying a sensor_msgs::PointCloud2 like a container.
Definition: point_cloud2_iterator.h:105
sensor_msgs
Tools for manipulating sensor_msgs.
Definition: distortion_models.h:41
sensor_msgs::PointCloud2Modifier::setPointCloud2FieldsByString
void setPointCloud2FieldsByString(int n_fields,...)
Function setting some fields in a PointCloud and adjusting the internals of the PointCloud2.
Definition: impl/point_cloud2_iterator.h:198

sensor_msgs
Author(s): Tully Foote
autogenerated on Mon Apr 28 2025 02:25:37