test_pointcloud2.cpp
Go to the documentation of this file.
1 /*
2  * test_pose_conversions.cpp
3  *
4  * Created on: Mar 15, 2012
5  * Author: Pablo IƱigo Blasco
6  */
7 
9 
10 #include <mrpt/version.h>
11 #include <mrpt/maps/CSimplePointsMap.h>
12 #include <pcl/conversions.h>
13 #include <pcl/point_cloud.h>
15 #include <gtest/gtest.h>
16 
17 TEST(PointCloud2, basicTest)
18 {
19  pcl::PointCloud<pcl::PointXYZ> point_cloud;
20 
21  point_cloud.height = 10;
22  point_cloud.width = 10;
23  point_cloud.is_dense = true;
24 
25  int num_points = point_cloud.height * point_cloud.width;
26  point_cloud.points.resize(num_points);
27 
28  float i_f = 0;
29  for (int i = 0; i < num_points; i++)
30  {
31  pcl::PointXYZ& point = point_cloud.points[i];
32  point.x = i_f;
33  point.y = -i_f;
34  point.z = -2 * i_f;
35  i_f += 1.0;
36  }
37 
38  sensor_msgs::PointCloud2 point_cloud2_msg;
39  // pcl_conversions::fromPCL(point_cloud, point_cloud2_msg);
40  pcl::toROSMsg(point_cloud, point_cloud2_msg);
41 
43 
44  // printf("step 3\n");
45  mrpt_bridge::copy(point_cloud2_msg, mrpt_pc);
46 
47  i_f = 0;
48  for (int i = 0; i < num_points; i++)
49  {
50  float mrpt_x, mrpt_y, mrpt_z;
51  mrpt_pc.getPoint(i, mrpt_x, mrpt_y, mrpt_z);
52  EXPECT_FLOAT_EQ(mrpt_x, i_f);
53  EXPECT_FLOAT_EQ(mrpt_y, -i_f);
54  EXPECT_FLOAT_EQ(mrpt_z, -2 * i_f);
55 
56  i_f += 1.0;
57  }
58  //;
59 }
PointCloud2_< std::allocator< void > > PointCloud2
Definition: point_cloud2.h:28
bool copy(const sensor_msgs::PointCloud2 &msg, mrpt::maps::CSimplePointsMap &obj)
unsigned long getPoint(size_t index, float &x, float &y, float &z) const
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
#define EXPECT_FLOAT_EQ(val1, val2)
TEST(PointCloud2, basicTest)


mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Fri Feb 28 2020 03:22:14