pcl_ros读取bag激光雷达点云的实现

以下博文除了注释以外的部分均来自AdamShan大佬，本人只是对内容作了注释，还不一定对，只为自己学习，如有补充和错误，敬请提出，额，还有希望原文大佬看不见。
作者：AdamShan 
来源：CSDN 
原文：https://blog.csdn.net/AdamShan/article/details/82901295 
 

1.编写include/pcl_test_core.h

#pragma once

#include <ros/ros.h>      
//导入ROS系统包含核心公共头文件

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>   
//点类型头文件
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>  

#include <pcl/filters/voxel_grid.h>

#include <sensor_msgs/PointCloud2.h>
//这些 .h文件都是一系列模板文件，类似于python中的numpy和matplotlib等库文件，可以直接调用，简化编程。但具体每个库是干什么的，没查到。

class PclTestCore
{

  private:
    ros::Subscriber sub_point_cloud_;      //为接收点云信息创建了一个订阅节点

    ros::Publisher pub_filtered_points_;   //创建了一个发布滤波的节点

    void point_cb(const sensor_msgs::PointCloud2ConstPtr& in_cloud);
//void point_cb是声明一个函数，这里面设置了一个数据类型为sensor_msgs::PointCloud2ConstPtr& in_cloud形参，const在这里修饰函数中的参数。将点云格式sensor_mgs/pointcloud2转换为pcl/pointcloud

  public:
    PclTestCore(ros::NodeHandle &nh);    
    ~PclTestCore();
    void Spin();
};

//1、public修饰的成员变量
//在程序的任何地方都可以被访问，就是公共变量的意思，不需要通过成员函数就可以由类的实例直接访问
//2、private修饰的成员变量
//只有类内可直接访问，私有的，类的实例要通过成员函数才可以访问，这个可以起到信息隐藏

头文件定义了 类 PclTestCore  分为private和public。private中用ros建立了两个节点，为接受点云信息建立了订阅节点（输入），为滤波后的点云建立了发布节点（结果）。声明了一个数据类型

2.编写pcl_test_node.cpp

#include "pcl_test_core.h"

int main(int argc, char **argv)   //main函数，节点入口
{
    ros::init(argc, argv, "pcl_test");   //初始化节点，第三个参数  node_name，节点参数

    ros::NodeHandle nh;    //nh每个节点都对应一个句柄，实例化节点？

    PclTestCore core(nh); //   没查到，反正与后续的节点启动有关吧
    return 0;
}

这是main（）函数，是节点的入口。 

ros::init（） 初始化节点，pcl_test是节点名称

ros::NodeHandle nh("/my_node_handle_namespace");

nh代表句柄的意思，每一个node有一个句柄。实例化节点？

3.编写pcl_test_core.cpp

#include "pcl_test_core.h"

PclTestCore::PclTestCore(ros::NodeHandle &nh){
    sub_point_cloud_ = nh.subscribe("/velodyne_points",10, &PclTestCore::point_cb, this);  //这里的sub_point_cloud在pcl_test_core.h中的prviate

    pub_filtered_points_ = nh.advertise<sensor_msgs::PointCloud2>("/filtered_points", 10);

    ros::spin(); //回调函数
}

PclTestCore::~PclTestCore(){}

void PclTestCore::Spin(){
    
}
//可以看出以上三步呼应pcl_test_node.cpp，yeah!!!

void PclTestCore::point_cb(const sensor_msgs::PointCloud2ConstPtr & in_cloud_ptr){
//定义了两个点云指针，声明存储原始数据与滤波后的数据的点云的格式
    pcl::PointCloud<pcl::PointXYZI>::Ptr current_pc_ptr(new pcl::PointCloud<pcl::PointXYZI>);
//pcl::PointCloud<pcl::PointXYZ>::Ptr （在这断开的） cloud_Ptr(new pcl::PointCloud<pcl::PointXYZ>);  
//指的是pcl中指针和非指针的转换。其中new pcl::PointCloud<pcl::PointXYZ>为原始点云的数据格式。
//在函数返回指针时，经常会出现不知道的错误，不用返回指针，直接得到PointXYZ，再将其转化为Ptr。
    pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_pc_ptr(new pcl::PointCloud<pcl::PointXYZI>);
//new pcl::PointCloud<pcl::PointXYZI>，存储滤波后的数据格式。

    pcl::fromROSMsg(*in_cloud_ptr, *current_pc_ptr);
// ros转化为PCL中的点云的数据格式，sensor_msgs::PointCloud2转pcl::PointCloud<pcl::PointXYZ> 

    pcl::VoxelGrid<pcl::PointXYZI> vg;
//实例化滤波，vg为voxelgrid的缩写，前面全部的代替，为后面编程提供方便，相当于import numpy as np  np.xxx

    vg.setInputCloud(current_pc_ptr); //设置输入的滤波
    vg.setLeafSize(0.2f, 0.2f, 0.2f); //设置体素网格大小
    vg.filter(*filtered_pc_ptr);      //存储滤波后的点云


//再将滤波后的点云转换为ros下的数据格式发布出去。
    sensor_msgs::PointCloud2 pub_pc;   //声明输出的点云格式。
    pcl::toROSMsg(*filtered_pc_ptr, pub_pc);//将pcl点云格式转换为ros下的点云格式，pcl::PointCloud<pcl::PointXYZ>转sensor_msgs::PointCloud2。第一个参数是滤波后的pcl xyz格式点云，第二个参数是抓换后的ros pointcloud2格式，之后发布就发布该点云。

    pub_pc.header = in_cloud_ptr->header; 
//在此将in_cloude_ptr的索引赋给pub_pc.header？？？这个没查到。

    pub_filtered_points_.publish(pub_pc);   
// pub_filtered_points_为之前建立的发布节点，在此为该node赋值。
}


//从这里以下参考sitwangmin博主，感谢！
//作者：sitwangmin 
//来源：CSDN 
//原文：https://blog.csdn.net/u010284636/article/details/79214841 

//ROS转PCL数据格式
//sensor_msgs::PointCloud2转pcl::PCLPointCloud2 
//pcl_conversions::toPCL(sensor_msgs::PointCloud2, pcl::PCLPointCloud2)

//sensor_msgs::PointCloud2转pcl::PointCloud<pcl::PointXYZ> 
//pcl::fromROSMsg (sensor_msgs::PointCloud2, pcl::PointCloud<pcl::PointXYZ>);

//PCL转ROS数据
//pcl::PCLPointCloud2转sensor_msgs::PointCloud2
//pcl_conversions::fromPCL(pcl::PCLPointCloud2, sensor_msgs::PointCloud2);

//pcl::PointCloud<pcl::PointXYZ>转sensor_msgs::PointCloud2
//pcl::toROSMsg (pcl::PointCloud<pcl::PointXYZ>,sensor_msgs::PointCloud2);

//PCL中数据互转 
//pcl::PCLPointCloud2转pcl::PointCloud <pcl::PointXYZ>
//pcl::fromPCLPointCloud2(pcl::PCLPointCloud2,pcl::PointCloud<pcl::PointXYZ>);

//pcl::PointCloud<pcl::PointXYZ>转pcl::PCLPointCloud2
//pcl::toPCLPointCloud2(pcl::PointCloud<pcl::PointXYZ>, pcl::PCLPointCloud2);

！！！我的天啊，终于把上面三个文件捋了一遍，但是仍然有好多无法理解。还有很多问题没有解决。

4.写一个launch文件pcl_test.launch来启动这个节点：

<launch>
    <node pkg="pcl_test" type="pcl_test_node" name="pcl_test_node" output="screen"/>
</launch>
//pkg 包名称  type是什么？貌似和name是一个东东。

5.杂七杂八

1）回到workspace 目录，使用catkin_make 编译：

catkin_make

2）启动这个节点：

roslaunch pcl_test pcl_test.launch

3）新建终端，并运行我们的测试bag（测试bag下载链接：https://pan.baidu.com/s/1HOhs9StXUmZ_5sCALgKG3w）

rosbag play --clock test.bag

注意这里，输入这行命令一定是在bag所在位置，不然会报错。

4）打开第三个终端，启动Rviz：

rosrun rviz rviz 

5）配置Rviz的Frame为velodyne，并且加载原始点云和过滤以后的点云的display。

在frame中输入velodyne，在add中的topic里面添加原始点云和过滤后的点云。

原始点云：

降采样之后的点云（即我们的节点的输出）：