机器人开发笔记之ros-pcl学习（4）

机器人开发笔记之ros-pcl学习（4）

---

前言

本文主要为ROS中使用体素滤波的相关内容，一些ros-pcl基础操作请见前几篇。

一、代码：

#include <ros/ros.h>
#include <tf/transform_listener.h>

// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
 #include<pcl/io/pcd_io.h>

//filters specific includes
#include <pcl/PCLPointCloud2.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/statistical_outlier_removal.h>


//publisher
ros::Publisher pub;

//callback function
void 
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
// ROS_INFO("hahahahah");
 // 声明存储原始数据与滤波后的数据的点云的格式
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; //原始的点云的数据格式
  pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
  pcl::PCLPointCloud2 cloud_filtered;                   //存储滤波后的数据格式
  pcl_conversions::toPCL(*cloud_msg, *cloud);        //采集的点云数据转化为PCL中的点云的数据格式
  std::cout << cloud->header.frame_id << std::endl;
  pcl::VoxelGrid<pcl::PCLPointCloud2> filter;       //创建滤波对象
  filter.setInputCloud (cloudPtr);                  //设置输入的滤波，将需要过滤的点云给滤波对象
  filter.setLeafSize(0.1,0.1,0.1);                   //设置滤波时创建的体素大小为xxxm的立方体
  filter.filter (cloud_filtered);                    //执行滤波处理，存储输出cloud_filtered               
//再将滤波后的点云的数据格式转换为ROS下的数据格式发布出去
  sensor_msgs::PointCloud2 output;                                             //声明的输出的点云的格式
  pcl_conversions::moveFromPCL(cloud_filtered, output);//第一个参数是输入，后面的是输出
  pub.publish (output);
}
int main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_voxel");
 
    ros::NodeHandle nh;
    ros::Subscriber bat_pub = nh.subscribe ("/camera/depth/color/points", 10,cloud_cb);
    pub =nh.advertise<sensor_msgs::PointCloud2>("pcl_output_filtered", 1);
    ros::spin ();
    return 0;
}

二、注意：

1.更改自己的topic。
2.如果你没有写Tf坐标转换的话，应该在代码中直接加入对于输出点云的坐标系定义。

output.header.frame_id = "odom";

上面这个是定义了发布使用全局坐标系。
3.补充一个后期遇到的问题，后面在使用tf建立坐标系树的时候出现了如下报错，会导致tf树在rviz里灰掉。

Leaf size is too small for the input dataset. Integer indices would overflow

此问题是因为点云数据量过大导致数据溢出。通过修改滤波参数得以解决。如下：

  filter.setLeafSize(0.1,0.1,0.1);      //设置滤波时创建的体素大小为xxxm的立方体

之前是0.02f改为了0.1。

总结

ros-pcl中的操作主要分为以下几步：
1.添加相关头文件。
2.订阅与发布相关话题。
3.收到与处理完成分别有一次数据类型转换，其他基本就是和pcl中的操作一样。