8 #include <mrpt/ros1bridge/point_cloud2.h> 9 #include <mrpt/maps/CSimplePointsMap.h> 10 #include <pcl/conversions.h> 11 #include <pcl/point_cloud.h> 13 #include <gtest/gtest.h> 15 TEST(PointCloud2, basicTest)
17 pcl::PointCloud<pcl::PointXYZ> point_cloud;
19 point_cloud.height = 10;
20 point_cloud.width = 10;
21 point_cloud.is_dense =
true;
23 int num_points = point_cloud.height * point_cloud.width;
24 point_cloud.points.resize(num_points);
27 for (
int i = 0; i < num_points; i++)
29 pcl::PointXYZ& point = point_cloud.points[i];
36 sensor_msgs::PointCloud2 point_cloud2_msg;
40 mrpt::maps::CSimplePointsMap mrpt_pc;
43 mrpt::ros1bridge::fromROS(point_cloud2_msg, mrpt_pc);
46 for (
int i = 0; i < num_points; i++)
48 float mrpt_x, mrpt_y, mrpt_z;
49 mrpt_pc.getPoint(i, mrpt_x, mrpt_y, mrpt_z);
#define EXPECT_FLOAT_EQ(a, b)
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
TEST(PointCloud2, basicTest)