00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include <gtest/gtest.h>
00036
00037 #include <sensor_msgs/point_cloud2_iterator.h>
00038
00039 TEST(sensor_msgs, PointCloud2Iterator)
00040 {
00041
00042 int n_points = 4;
00043 sensor_msgs::PointCloud2 cloud_msg_1, cloud_msg_2;
00044 cloud_msg_1.height = n_points;
00045 cloud_msg_1.width = 1;
00046 sensor_msgs::PointCloud2Modifier modifier(cloud_msg_1);
00047 modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
00048 cloud_msg_2 = cloud_msg_1;
00049
00050
00051 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};
00052 std::vector<float> point_data(point_data_raw, point_data_raw + 3*n_points);
00053
00054 uint8_t color_data_raw[] = {40, 80, 120, 160, 200, 240, 20, 40, 60, 80, 100, 120};
00055 std::vector<uint8_t> color_data(color_data_raw, color_data_raw + 3*n_points);
00056
00057 float *data = reinterpret_cast<float*>(&cloud_msg_1.data.front());
00058 for(size_t n=0, i=0; n<n_points; ++n) {
00059 for(; i<3*(n+1); ++i)
00060 *(data++) = point_data[i];
00061
00062 ++data;
00063 uint8_t *bgr = reinterpret_cast<uint8_t*>(data++);
00064
00065 size_t j_max = 2;
00066 for(size_t j = 0; j <= j_max; ++j)
00067 *(bgr++) = color_data[3*n+(j_max - j)];
00068
00069 data += 3;
00070 }
00071
00072
00073 sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg_2, "x");
00074 sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(cloud_msg_2, "r");
00075 sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(cloud_msg_2, "g");
00076 sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(cloud_msg_2, "b");
00077 for(size_t i=0; i<n_points; ++i, ++iter_x, ++iter_r, ++iter_g, ++iter_b) {
00078 for(size_t j=0; j<3; ++j)
00079 iter_x[j] = point_data[j+3*i];
00080 *iter_r = color_data[3*i];
00081 *iter_g = color_data[3*i+1];
00082 *iter_b = color_data[3*i+2];
00083 }
00084
00085
00086
00087 sensor_msgs::PointCloud2ConstIterator<float> iter_const_1_x(cloud_msg_1, "x"), iter_const_2_x(cloud_msg_2, "x");
00088 sensor_msgs::PointCloud2ConstIterator<float> iter_const_1_y(cloud_msg_1, "y"), iter_const_2_y(cloud_msg_2, "y");
00089 sensor_msgs::PointCloud2ConstIterator<float> iter_const_1_z(cloud_msg_1, "z"), iter_const_2_z(cloud_msg_2, "z");
00090 sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_const_1_r(cloud_msg_1, "r"), iter_const_2_r(cloud_msg_2, "r");
00091 sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_const_1_g(cloud_msg_1, "g"), iter_const_2_g(cloud_msg_2, "g");
00092 sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_const_1_b(cloud_msg_1, "b"), iter_const_2_b(cloud_msg_2, "b");
00093
00094 size_t i=0;
00095 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,
00096 ++iter_const_1_z, ++iter_const_2_z, ++iter_const_1_r, ++iter_const_1_g, ++iter_const_1_b) {
00097 EXPECT_EQ(*iter_const_1_x, *iter_const_2_x);
00098 EXPECT_EQ(*iter_const_1_x, point_data[0+3*i]);
00099 EXPECT_EQ(*iter_const_1_y, *iter_const_2_y);
00100 EXPECT_EQ(*iter_const_1_y, point_data[1+3*i]);
00101 EXPECT_EQ(*iter_const_1_z, *iter_const_2_z);
00102 EXPECT_EQ(*iter_const_1_z, point_data[2+3*i]);
00103 EXPECT_EQ(*iter_const_1_r, *iter_const_2_r);
00104 EXPECT_EQ(*iter_const_1_r, color_data[3*i+0]);
00105 EXPECT_EQ(*iter_const_1_g, *iter_const_2_g);
00106 EXPECT_EQ(*iter_const_1_g, color_data[3*i+1]);
00107 EXPECT_EQ(*iter_const_1_b, *iter_const_2_b);
00108 EXPECT_EQ(*iter_const_1_b, color_data[3*i+2]);
00109
00110 ++iter_const_2_r;
00111 iter_const_2_g += 1;
00112 iter_const_2_b = iter_const_2_b + 1;
00113 }
00114 EXPECT_EQ(i, n_points);
00115 }
00116
00117 int main(int argc, char** argv)
00118 {
00119 ::testing::InitGoogleTest(&argc, argv);
00120 return RUN_ALL_TESTS();
00121 }