从零开始搭二维激光SLAM --- 使用PCL进行雷达的消息类型转换

接下来的几篇文章，我将带大家使用PCL的ICP算法搭建出一个最简单的激光里程计，将涉及到PCL，TF，TF2等函数库，不会用不要紧，我将带着大家一步一步地学习。

由于我想搭的第一个激光里程计是通过ICP算法来实现，而ICP算法已经在PCL中实现好了，所以我们首先来看如何在ROS中引入PCL，并进行使用。

1 PCL

PCL（Point Cloud Library）作为我们接触的第一个外部库，可见其重要性。

PCL里实现了大量的处理点云相关的功能，实现了大量点云相关的通用算法和高效数据结构，涉及到点云获取、滤波、分割、配准、检索、特征提取、识别、追踪、曲面重建、可视化等。

我在这里不对PCL进行过多的介绍，希望读者朋友可以自己去PCL的官网，查看其能够实现的功能。

接下来，我将介绍如何在ROS中使用PCL。

2 在ros中使用PCL

首先，我将带大家从头新建一下lesson2这个包，以展示使用PCL需要的依赖以及需要更改哪些文件。

读者可以跟着我的步骤自己生成一下这个包，也可以直接更新到最新的提交，代码已经提交在github上了。

如果不知道github的地址的朋友, 请在我的公众号: 从零开始搭激光SLAM 中回复 开源地址 获得。

推荐使用 git clone 的方式进行下载, 因为代码是正处于更新状态的, git clone 下载的代码可以使用 git pull 很方便地进行更新.

2.1 新生成包

通过如下命令生成一个新的包，起名为lesson2。可见，需要的依赖有roscpp 、sensor_msgs 、pcl_ros 以及 pcl_conversions 。

cd ~/catkin_ws/src/Creating-2D-laser-slam-from-scratch
catkin_create_pkg lesson2 pcl_conversions pcl_ros roscpp sensor_msgs 

roscpp以及sensor_msgs在之前的文章说过了，roscpp是c++的依赖，sensor_msgs是雷达数据的消息类型的依赖。

pcl_conversions 以及 pcl_ros 是ROS官方为了在ROS中方便的使用PCL而写的包。

pcl_conversions 包含了一些方法，实现了 ROS的消息类型 与 PCL的消息类型 的转换。

pcl_ros 定义了一些其他的功能，如在ROS中使用标准的Publisher直接发布PCL的数据格式、将PCL的点云根据tf进行坐标变换、实现了一些常用的功能接口，使得可以直接在ROS中调用PCL的函数，例如进行体素滤波等等。

这两个依赖包的具体讲解请看我为了学习pcl_ros而写的另一篇博客。
https://blog.csdn.net/tiancailx/article/details/110816649

2.2 修改CMakeLists.txt

需要在CMakeLists.txt额外添加下面语句，代表我们需要依赖PCL。

find_package(PCL REQUIRED QUIET)
include_directories( 
  include
  ${catkin_INCLUDE_DIRS}
  ${PCL_INCLUDE_DIRS}
)

2.3 修改Package.xml

需要额外添加如下两句话，代表依赖PCL。

<build_depend>libpcl-all-devbuild_depend>
<exec_depend>libpcl-allexec_depend>

3 工程代码

上面部分讲了在ROS中使用PCL需要的依赖项，以及如何更改CMakeLists.txt与Package.xml。

这部分讲实际的代码。

3.1 头文件

在 lesson2/include/lesson2/ 文件夹下 生成头文件, 并取名为 scan_to_pointclod2_converter.h.

将如下内容复制进去

#ifndef LESSON2_SCAN_TO_POINTCLOUD2_CONVERTER_H
#define LESSON2_SCAN_TO_POINTCLOUD2_CONVERTER_H

// ros
#include 
#include 

// pcl_ros
#include     

// pcl
#include 
#include 


class ScanToPointCloud2Converter
{
    // 使用PCL中点的数据结构 pcl::PointXYZ
    typedef pcl::PointXYZ PointT;
    // 使用PCL中点云的数据结构 pcl::PointCloud
    typedef pcl::PointCloud<PointT> PointCloudT;

private:
    ros::NodeHandle node_handle_;           // ros中的句柄
    ros::NodeHandle private_node_;          // ros中的私有句柄
    ros::Subscriber laser_scan_subscriber_; // 声明一个Subscriber
    ros::Publisher pointcloud2_publisher_;  // 声明一个Publisher
    PointT invalid_point_;                  // 保存无效点的值,为nan
public:
    ScanToPointCloud2Converter();
    ~ScanToPointCloud2Converter();
    void ScanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_msg);
};

#endif // LESSON2_SCAN_TO_POINTCLOUD2_CONVERTER_H

代码的内容很简单，简单说明一下

首先，引入了3种头文件，分别是ros的，pcl_ros的（ros与PCL数据类型转换函数），以及PCL的基础数据类型。

其中我用了PCL的2个数据结构，分别为

• 数据点: pcl::PointXYZ
• 点云的数据结构: pcl::PointCloud< pcl::PointXYZ>

并将他们重命名，以减少代码量。

3.2 源文件

在 lesson2/src 文件夹下 生成源文件, 并取名为 scan_to_pointclod2_converter.cc

将如下内容复制进去

#include "lesson2/scan_to_pointclod2_converter.h"
#include 

ScanToPointCloud2Converter::ScanToPointCloud2Converter() : private_node_("~")
{
    // \033[1;32m，\033[0m 终端显示成绿色
    ROS_INFO_STREAM("\033[1;32m----> Scan to PointCloud2 Converter.\033[0m");

    laser_scan_subscriber_ = node_handle_.subscribe(
        "laser_scan", 1, &ScanToPointCloud2Converter::ScanCallback, this);
    
    // 注意，这里的发布器，发布的数据类型为 pcl::PointCloud
    // ros中自动做了 pcl::PointCloud 到 sensor_msgs/PointCloud2 的数据类型的转换
    pointcloud2_publisher_ = node_handle_.advertise<PointCloudT>(
        "pointcloud2_converted", 1, this);

    // 无效点的值设置为nan
    invalid_point_.x = std::numeric_limits<float>::quiet_NaN();
    invalid_point_.y = std::numeric_limits<float>::quiet_NaN();
    invalid_point_.z = std::numeric_limits<float>::quiet_NaN();
}

ScanToPointCloud2Converter::~ScanToPointCloud2Converter()
{
    ROS_INFO("Destroying ScanToPointCloud2Converter");
}

void ScanToPointCloud2Converter::ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
    // 声明一个 pcl::PointCloud 类型的指针
    PointCloudT::Ptr cloud_msg = boost::shared_ptr<PointCloudT>(new PointCloudT());

    // 对容器进行初始化
    cloud_msg->points.resize(scan_msg->ranges.size());

    for (unsigned int i = 0; i < scan_msg->ranges.size(); ++i)
    {
        // 首先声明一个 cloud_msg第i个点的 引用
        PointT & point_tmp = cloud_msg->points[i];
        // 获取scan的第i个点的距离值
        float range = scan_msg->ranges[i];

        // 将 inf 与 nan 点 设置为无效点
        if (!std::isfinite(range))
        {
            // std::cout << " " << i << " " << scan_msg->ranges[i];
            point_tmp = invalid_point_;
            continue;
        }

        // 有些雷达驱动会将无效点设置成 range_max+1
        // 所以要根据雷达的range_min与range_max进行有效值的判断
        if (range > scan_msg->range_min && range < scan_msg->range_max)
        {
            // 获取第i个点对应的角度
            float angle = scan_msg->angle_min + i * scan_msg->angle_increment;
            // 获取第i个点在笛卡尔坐标系下的坐标
            point_tmp.x = range * cos(angle);
            point_tmp.y = range * sin(angle);
            point_tmp.z = 0.0;
        }
        else
            // 无效点
            point_tmp = invalid_point_;
    }

    cloud_msg->width = scan_msg->ranges.size();
    cloud_msg->height = 1;
    cloud_msg->is_dense = false;    // contains nans
    // 将scan_msg的消息头 赋值到 PointCloudT的消息头
    pcl_conversions::toPCL(scan_msg->header, cloud_msg->header);

    // 由于ros中自动做了 pcl::PointCloud 到 sensor_msgs/PointCloud2 的数据类型的转换
    // 所以这里直接发布 pcl::PointCloud 即可
    pointcloud2_publisher_.publish(cloud_msg);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "lesson2_scan_to_cloud_converter_node"); // 节点的名字
    ScanToPointCloud2Converter scan_to_cloud_converter;

    ros::spin(); // 程序执行到此处时开始进行等待，每次订阅的消息到来都会执行一次ScanCallback()
    return 0;
}

接下来我将对重点语句进行说明

3.2.1

    pointcloud2_publisher_ = node_handle_.advertise<PointCloudT>(
        "pointcloud2_converted", 1, this);

构造函数中，我们将pointcloud2_publisher_初始化成了一个发布PointCloudT，也就是pcl::PointCloud，数据类型的的发布器。

有没有对这个很奇怪，在ros的发布器还可以发布非ros消息类型的消息？

答案：是因为ros官方在pcl_ros中做了Publisher的多态（和函数重载差不多，只不过多态形容的是类）。

使得ros的标准Publisher可以接收pcl::PointCloud< T>类型的消息，并自动转换成sensor_msgs::PointCloud2的消息类型，再发布出去。

也就是说，我们想要发布sensor_msgs::PointCloud2类型的数据，在我们只有pcl::PointCloud格式的数据时，不需要做转换，可以直接发布pcl::PointCloud格式，ros会自动帮我们转换，并发布sensor_msgs::PointCloud2类型的数据。

这块具体的实现代码请参看我的另一篇博客
perception_pcl理解 — pcl_conversions 与 pcl_ros

3.2.2

    // 无效点的值设置为nan
    invalid_point_.x = std::numeric_limits<float>::quiet_NaN();
    invalid_point_.y = std::numeric_limits<float>::quiet_NaN();
    invalid_point_.z = std::numeric_limits<float>::quiet_NaN();

将点赋值成nan的方法，需要引入 limits 头文件。

3.2.3

PointCloudT::Ptr cloud_msg = boost::shared_ptr<PointCloudT>(new PointCloudT());

声明了一个PointCloudT::Ptr的指针cloud_msg并进行初始化，PointCloudT::Ptr的数据类型为boost::shared_ptr。

3.2.4

        // 将 inf 与 nan 点 设置为无效点
        if (!std::isfinite(range))
        {
            // std::cout << " " << i << " " << scan_msg->ranges[i];
            point_tmp = invalid_point_;
            continue;
        }

将这段去掉也不会报错。貌似inf与nan直接进行数学运算不好报错。。。有待确认？？？

3.2.5

    cloud_msg->width = scan_msg->ranges.size();
    cloud_msg->height = 1;
    cloud_msg->is_dense = false; // contains nans
    // 将scan_msg的消息头 赋值到 PointCloudT的消息头
    pcl_conversions::toPCL(scan_msg->header, cloud_msg->header);

    // 由于ros中自动做了 pcl::PointCloud 到 sensor_msgs/PointCloud2 的数据类型的转换
    // 所以这里直接发布 pcl::PointCloud 即可
    pointcloud2_publisher_.publish(cloud_msg);

这段就是对点云数据的其他数据进行赋值

• width为scan的个数
• is_dense表示数据中是否全是有限值，当然不是，因为我们放入了nan
• pcl_conversions::toPCL()函数将scan_msg->header赋值到cloud_msg->header中

这样操作完之后，cloud_msg的数据就填充完整了，我们可以直接将其发布出去了。

3.3 CMakeLists.txt添加可执行

在文件末尾处添加如下语句

add_executable(${PROJECT_NAME}_scan_to_pointclod2_converter_node 
    src/scan_to_pointclod2_converter.cc)

add_dependencies(${PROJECT_NAME}_scan_to_pointclod2_converter_node 
    ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
    
target_link_libraries(${PROJECT_NAME}_scan_to_pointclod2_converter_node
  ${catkin_LIBRARIES}
)

3.4 launch文件

本篇文章对应的数据包, 请在我的公众号中回复 lesson1 获得，并将launch中的bag_filename更改成您实际的目录名。

<launch>

    <!-- bag的地址与名称 -->
    <arg name="bag_filename" default="/home/lx/bagfiles/lesson1.bag"/>

    <!-- 使用bag的时间戳 -->
    <param name="use_sim_time" value="true" />

    <!-- 启动节点 -->
    <node name="lesson2_scan_to_pointclod2_converter_node"
        pkg="lesson2" type="lesson2_scan_to_pointclod2_converter_node" output="screen" />
    
    <!-- launch rviz -->
    <node name="rviz" pkg="rviz" type="rviz" required="false"
        args="-d $(find lesson2)/launch/scan2pointcloud.rviz" />

    <!-- play bagfile -->
    <node name="playbag" pkg="rosbag" type="play"
        args="--clock $(arg bag_filename)" />

</launch>

3.5 运行

rospack profile
roslaunch lesson2 scan_to_pointcloud2_converter.launch

3.6 运行结果

由于已经配置好了rviz，所以将直接出现下边的图片。

这张图片展示的将sensor_msgs/LaserScan转换成的sensor_msgs/PointCloud2的数据。

接下来，将rviz左边的PointCloud2的对号点掉，在将laser_scan的对号点出来，将得到下边的图片。这就是sensor_msgs/LaserScan格式的数据，点云的形状肯定是一样的，因为我们没有做其他的操作。

接下来，新开一个终端，输入如下命令，将得到下图所示：

可以看到，我们发布的 /pointcloud2_converted ，这个topic的数据类型为sensor_msgs/PointCloud2。

4 总结

本篇文章首先我介绍了如何在ros中使用pcl，带着大家一步一步地进行配置。

之后，通过一小段程序，展示了如何将LaserScan转成成pcl::PointCloud，并发布sensor_msgs/PointCloud2消息的topic。

本节的代码不再和之前一样，只有一个.cc文件，本节的代码将类的定义放在头文件里。之后代码会越来越复杂，希望小白同学可以跟的上。

虽然代码已经写好了，但还是希望小白能够自己编写一遍代码，代码不自己写是不会理解的。

5 Next

本篇文章我们将数据类型转换好了，下一篇文章将使用PCL的ICP算法进行scan-to-scan的匹配，计算2帧scan间的位姿。

---

文章将在 公众号: 从零开始搭SLAM 进行同步更新，欢迎大家关注，以在文章更新的第一时间通知您。

同时，也希望您将这个公众号推荐给您身边做激光SLAM的人们，大家共同进步。

如果您对我写的文章有什么建议，或者想要看哪方面功能如何实现的，请直接在公众号中回复，我可以收到，并将认真考虑您的建议。