引用头文件
首先，引用对应的头文件：
#include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/PointCloud.h> #include <sensor_msgs/point_cloud_conversion.h>
定义接听/发布
ros::Subscriber subMMWCloud; ros::Publisher pubLaserCloud; subMMWCloud = nh.subscribe<sensor_msgs::PointCloud>(mmwTopic, 2000, &preMMW::mmwHandler, this); pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>(laserTopic, 2000);
写回调函数
void mmwHandler(const sensor_msgs::PointCloudConstPtr& mmwCloudMsg) { mBuf.lock(); // rawMMWCloudQueue.push_back(mmwCloudMsg); sensor_msgs::PointCloud2 laserCloudMsg; convertPointCloudToPointCloud2(*mmwCloudMsg, laserCloudMsg); laserCloudMsg.header.stamp = mmwCloudMsg->header.stamp; laserCloudMsg.header.frame_id = lidarFrame; pubLaserCloud.publish(laserCloudMsg); // cout << "hello LASER: " << laserCloudMsg.header.stamp << endl; mBuf.unlock(); }
注意，其中最重要的是：convertPointCloudToPointCloud2这个函数。
