使用ros和pcl点云库,调用松灵小车,并利用view函数显示。其中有可视化函数、滤波函数和回调函数,回调函数中主要有点云消息类型的转换。
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_types.h>//支持点类型头文件
#include <pcl/point_cloud.h>//支持点类型头文件
#include <pcl_conversions/pcl_conversions.h>//pcl消息头和ros消息头的转换
#include <pcl/filters/passthrough.h>//
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>//创建一个新的线程
//以上包含pcl的头文件
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("点云可视化"));
void visualizeCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud) {
//---------显示点云-----------------------
viewer->removeAllPointClouds(); // 移除当前所有点云
viewer->addPointCloud(cloud, "realtime pcl");
viewer->updatePointCloud(cloud,"realtime pcl");
viewer->spinOnce(0.001);
}
void filterCloudPoint(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_in,pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_out) {
//创建滤波器对象,pass对象
pcl::PassThrough<pcl::PointXYZI> pass;
//输入点云
pass.setInputCloud(cloud_in);
//设置在Z轴方向上进行滤波
pass.setFilterFieldName("z");
//设置滤波范围为0~1,在范围之外的点会被剪除
pass.setFilterLimits(0.0, 1000);
//是否反向过滤,也就是保留范围以外的点,默认为false
//pass.setFilterLimitsNegative (true);
pass.setFilterLimitsNegative(false);
//执行滤波
pass.filter(*cloud_out);
}
/**
* 回调函数(订阅者的回调函数)
*/
//回调函数调用
void laserCallback(sensor_msgs::PointCloud2ConstPtr lasercloudmsg){
//转换点云的数据类型
pcl::PointCloud<pcl::PointXYZ>::Ptr lasercloud_in (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr lasercloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
// 将接受到的雷达点云消息转化为接收点云数据类型
pcl::fromROSMsg(*lasercloudmsg,*lasercloud_in);
visualizeCloud(lasercloud_in);//进消息文件
}
int main(int argc,char *argv[])
{
// 初始化节点
ros::init(argc,argv,"passthrough");
// 创建句柄
ros::NodeHandle n;
// 创建订阅者,并订阅 rslidar_points 消息
// 传入回调函数
ros::Subscriber laser_sub = n.subscribe<sensor_msgs::PointCloud2>("/rslidar_points",10,laserCallback);
ros::spin();
return 0;
}