激光SLAM放弃指南3：实现LaserScan数据转PCL和PiontCloud数据

参考链接：1.李太白lx 系列文章 从零开始搭二维激光SLAM link
2.岁月神偷小拳拳 link link

PCL数据类型

PointXYZ

PointXYZ是使用最常见的一个点数据类型，因为它只包含三维xyz坐标信息，这三个浮点数附加一个浮点数来满足存储对齐，用户可利用points[i].data[0]，或者points[i].x访问点的x坐标值。

union
{
     
   float data[4];
   struct
   {
     
      float x;
      float y;
      float z;
   };
};
;

PointCloud

在PCL中最基本的数据类型就是PointCloud了,它是一个C++类。
width(int)
指定无序点云中，点云的数量。
指定有序点云中，一行点云的数量。

height(int)
指定有序点云中，点云行的数量。
对于无序点云，将height设为1（它的width即为点云的大小），以此来区分点云是否有序。

points(std::vector)
包含所有存储的点云(Point)的数组。如果一个点云包含XYZ的数据，那么points的数据结构为pcl::PointXYZ

pcl::PointXYZ pointT;
pcl::PointCloud<pointT> pointcloudT;
std::vector<pcl::PointXYZ> data = cloud->points;

is_dense(bool)
指定点云中的所有数据都是有限的（true），还是其中的一些点不是有限的，它们的XYZ值可能包含inf/NaN 这样的值（false）。

sensor_origin_(Eigen::Vector4f)
指定传感器的采集位姿（origin/translation）这个成员通常是可选的，并且在PCL的主流算法中用不到。

sensor_orientaion_(Eigen::Quaternionf)
指定传感器的采集位姿（方向）。这个成员通常是可选的，并且在PCL的主流算法中用不到。
为了简化开发，PointCloud类包含许多帮助成员函数。举个例子，如果你想判断一个点云是否有序，不用检查height是否等于1，而可以使用isOrganized()函数来判断：

画重点

1.sensor_msgs/scan和sensor_msgs/PointCloud2是激光数据的两种表现形式，之间可以通过pcl相互转换
2.这里依赖pcl_ros 以及 pcl_conversions，将scan数据转化为 PointCloud2数据
3.这里ros发布PointCloud数据时，帮我们做了pcl::PointCloud --> sensor_msgs::PointCloud2，所以这里只需要做sensor_msgs::scan --> pcl::PointCloud
4.scan的数据形式类似极坐标，PointCloud的数据形式类似笛卡尔坐标

float range= scan_msg->ranges[i];
float angle =scan_msg->angle_increment*i+scan_msg->angle_min;
PointT & point_temp = cloud_msg->points[i];
point_temp.x=range * cos(angle);
point_temp.y=range * sin(angle);
point_temp.z=0.0;

float angle =scan_msg->angle_increment*i+scan_msg->angle_min;

同时注意：我们尽量在初始化的时候，使用私有命名空间下的句柄，这样可以用来获取从launch加载的参数，初始化为~，就是将他初始化为私有命名空间下的句柄。

ScanToPointCloud2Converter::ScanToPointCloud2Converter() : private_node_("~")

源代码

launch文件

<launch>
<arg name ="bag_name" default="/home/csy/desktop/test_code/slam_csdn/data/lesson1.bag"/>
 <!-- 使用bag的时间戳 -->
<param name = "use_sim_teme " value="true"/>
<node name = "lesson2" pkg = "lesson2" type = "lesson2" output = "screen">
 <node name = "rviz" pkg="rviz" type="rviz" required="false" args=" -d /home/csy/desktop/test_code/slam_csdn/data/lesson.rviz"/>
<node name = "playbag" pkg="rosbag" type="play"
args="--clock  $(arg bag_name)"/>
</launch>

.h文件

#include "ros/ros.h"
#include"float.h"
#include"sensor_msgs/LaserScan.h"
#include"pcl_ros/point_cloud.h"
#include <pcl/point_types.h>//包含一些PointT类型结构体的声明（本例中是pcl::PointXYZ）。
#include "pcl_conversions/pcl_conversions.h"
class scan_to_pointcloud2_converter
{
     
private:
typedef pcl::PointXYZ pointT;
typedef pcl::PointCloud<pointT> pointcloudT;
    ros::NodeHandle scan_nh_;
    ros::Subscriber laser_subscriber_;
    ros::Publisher point_publish_;
    pointT invalid_value_;
    void ScanCall(const sensor_msgs::LaserScan::ConstPtr scan_msg);
public:
    scan_to_pointcloud2_converter(/* args */);
    ~scan_to_pointcloud2_converter();
};

.cpp文件

#include "lesson2/scan_to_pointcloud2_converter.h"
#include "limits"

scan_to_pointcloud2_converter::scan_to_pointcloud2_converter(/* args */){
     
    ROS_INFO("open ");
    laser_subscriber_ = scan_nh_.subscribe("laser_scan",100,&scan_to_pointcloud2_converter::ScanCall,this);
    point_publish_ = scan_nh_.advertise<pointcloudT>( "PointCloud",100 );
    invalid_value_.x = std::numeric_limits<float>::quiet_NaN();
    invalid_value_.y = std::numeric_limits<float>::quiet_NaN();
    invalid_value_.z = std::numeric_limits<float>::quiet_NaN();
}

scan_to_pointcloud2_converter::~scan_to_pointcloud2_converter(){
     
}
void scan_to_pointcloud2_converter::ScanCall(const sensor_msgs::LaserScan::ConstPtr scan_msg){
     
    pointcloudT::Ptr cloud_msg=boost::shared_ptr<pointcloudT>(new pointcloudT());
    int scan_size = scan_msg->ranges.size();
    cloud_msg->points.resize(  scan_size );
    for(int i=0;i<scan_size;i++){
     
        pointT & point_temp = cloud_msg->points[i];
        float range = scan_msg->ranges[i];
        if( !std::isfinite(range)){
     
            point_temp = invalid_value_;
            continue;
        }else if( range>scan_msg-> range_min && range<scan_msg->range_max){
     
            float angle = scan_msg->angle_min + i*(scan_msg->angle_increment);
            point_temp.x = range*cos(angle);
            point_temp.y = range*sin(angle);
            point_temp.z = 0.0;
           
        }else{
     
              point_temp = invalid_value_;
        }
    }
    cloud_msg->height = 1;
    cloud_msg->width = scan_size;
    cloud_msg->is_bigendian = false;
    pcl_conversions::toPCL(scan_msg->header, cloud_msg->header );  //  pcl_conversions
    point_publish_.publish(cloud_msg);

}
int main(int argc,char ** argv){
     
    ros::init(argc,argv,"lesson3");
    scan_to_pointcloud2_converter test_scan_to_point;
    ros::spin();
    return 0;
}