main.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Open Source Robotics Foundation
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 #include <gtest/gtest.h>
00036 
00037 #include <sensor_msgs/point_cloud2_iterator.h>
00038 
00039 TEST(sensor_msgs, PointCloud2Iterator)
00040 {
00041   // Create a dummy PointCloud2
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   // Fill 1 by hand
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   // colors in RGB order
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     // Add an extra float of padding
00062     ++data;
00063     uint8_t *bgr = reinterpret_cast<uint8_t*>(data++);
00064     // add the colors in order BGRA like PCL
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     // Add 3 extra floats of padding
00069     data += 3;
00070   }
00071 
00072   // Fill 2 using an iterator
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   // Check the values using an iterator
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     // This is to test the different operators
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 }


sensor_msgs
Author(s):
autogenerated on Wed Jun 14 2017 04:10:20