35 #include <gtest/gtest.h> 43 sensor_msgs::PointCloud2 cloud_msg_1, cloud_msg_2;
44 cloud_msg_1.height = n_points;
45 cloud_msg_1.width = 1;
48 cloud_msg_2 = cloud_msg_1;
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);
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);
57 float *data =
reinterpret_cast<float*
>(&cloud_msg_1.data.front());
58 for(
size_t n=0, i=0; n<n_points; ++n) {
60 *(data++) = point_data[i];
63 uint8_t *bgr =
reinterpret_cast<uint8_t*
>(data++);
66 for(
size_t j = 0; j <= j_max; ++j)
67 *(bgr++) = color_data[3*n+(j_max - j)];
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];
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]);
112 iter_const_2_b = iter_const_2_b + 1;
114 EXPECT_EQ(i, n_points);
117 int main(
int argc,
char** argv)
119 ::testing::InitGoogleTest(&argc, argv);
120 return RUN_ALL_TESTS();
Same as a PointCloud2Iterator but for const data.
int main(int argc, char **argv)
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)
void setPointCloud2FieldsByString(int n_fields,...)
Function setting some fields in a PointCloud and adjusting the internals of the PointCloud2.