SLAM笔记-在ROS中使用PCL

http://t.csdn.cn/xQhgS从零开始搭二维激光SLAM --- 使用PCL进行雷达的消息类型转换

第一步

修改CMake.list 和 package.xml

find_package(PCL REQUIRED QUIET) include_directories( include ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ) 在CMake.list中添加依赖

<build_depend>libpcl-all-dev</build_depend> <exec_depend>libpcl-all</exec_depend>

在package.xml中添加的依赖

第二步

生成头文件

#ifndef LESSON2_SCAN_TO_POINTCLOUD2_CONVERTER_H
#define LESSON2_SCAN_TO_POINTCLOUD2_CONVERTER_H

// ros
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>

// pcl_ros
#include <pcl_ros/point_cloud.h>    

// pcl
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>


class ScanToPointCloud2Converter
{
    // 使用PCL中点的数据结构 pcl::PointXYZ
    typedef pcl::PointXYZ PointT;
    // 使用PCL中点云的数据结构 pcl::PointCloud<pcl::PointXYZ>
    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

http://t.csdn.cn/85ITj这个链接也是这个博主的，具体讲pcl_ros的

当中引用了PCL的2个数据结构，分别为

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

生成源文件

#include "lesson2/scan_to_pointclod2_converter.h"
#include <limits>

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<PointT>
    // ros中自动做了 pcl::PointCloud<PointT> 到 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<PointT> 类型的指针
    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<PointT> 到 sensor_msgs/PointCloud2 的数据类型的转换
    // 所以这里直接发布 pcl::PointCloud<PointT> 即可
    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;
}

添加可执行文件

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}
)
 

launch文件

<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>

运行结果如下