17 #include <gtest/gtest.h>
21 #if MRPT_HAVE_PCL_CONVERSIONS
22 #include <pcl/common/common_headers.h>
23 #include <pcl/conversions.h>
24 #include <pcl/point_cloud.h>
25 #include <pcl_conversions/pcl_conversions.h>
27 TEST(PointCloud2, basicTest)
29 pcl::PointCloud<pcl::PointXYZ> point_cloud;
31 point_cloud.height = 10;
32 point_cloud.width = 10;
33 point_cloud.is_dense =
true;
35 int num_points = point_cloud.height * point_cloud.width;
36 point_cloud.points.resize(num_points);
39 for (
int i = 0; i < num_points; i++)
41 pcl::PointXYZ& point = point_cloud.points[i];
48 sensor_msgs::PointCloud2 point_cloud2_msg;
49 pcl::toROSMsg(point_cloud, point_cloud2_msg);
56 for (
int i = 0; i < num_points; i++)
58 float mrpt_x, mrpt_y, mrpt_z;
59 mrpt_pc.
getPoint(i, mrpt_x, mrpt_y, mrpt_z);
60 EXPECT_FLOAT_EQ(mrpt_x, i_f);
61 EXPECT_FLOAT_EQ(mrpt_y, -i_f);
62 EXPECT_FLOAT_EQ(mrpt_z, -2 * i_f);