ROS中激光雷达数据类型传递转换及自定义点云数据类型介绍

目录

一、ROS中激光雷达数据类型传递转换；

二、点云数据解析;

三、自定义点云数据类型;

一、ROS中激光雷达数据类型传递转换；

     ROS中涉及激光雷达传递的消息类型有两种，一种是针对2D雷达:sensor_msgs::LaserScan;一种是针对3D雷达的即点云数据的：sensor_msgs::PointCloud2.

(1) 2D激光雷达LaserScan;

ROS中LaserScan.msg定义类型如下：

File: sensor_msgs/LaserScan.msg
Raw Message Definition
# Single scan from a planar laser range-finder
#
# If you have another ranging device with different behavior (e.g. a sonar
# array), please find or create a different message, since applications
# will make fairly laser-specific assumptions about this data

Header header            # timestamp in the header is the acquisition time of 
                         # the first ray in the scan.
                         #
                         # in frame frame_id, angles are measured around 
                         # the positive Z axis (counterclockwise, if Z is up)
                         # with zero angle being forward along the x axis
                         
float32 angle_min        # start angle of the scan [rad]
float32 angle_max        # end angle of the scan [rad]
float32 angle_increment  # angular distance between measurements [rad]

float32 time_increment   # time between measurements [seconds] - if your scanner
                         # is moving, this will be used in interpolating position
                         # of 3d points
float32 scan_time        # time between scans [seconds]

float32 range_min        # minimum range value [m]
float32 range_max        # maximum range value [m]

float32[] ranges         # range data [m] (Note: values < range_min or > range_max should be discarded)
float32[] intensities    # intensity data [device-specific units].  If your
                         # device does not provide intensities, please leave
                         # the array empty.

发布： 

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

int main(int argc, char** argv){

  ros::init(argc, argv, "laser_scan_publisher");
  ros::NodeHandle n;

  ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan", 50);

  unsigned int num_readings = 1000;
  double laser_frequency = 50;
  double ranges[num_readings];
  double intensities[num_readings];
  int count = 0;
  ros::Rate r(10.0);

  while(n.ok()){
    //generate some fake data for our laser scan
    for(unsigned int i = 0; i < num_readings; ++i){
      ranges[i] = count;
      intensities[i] = 100 + count;
    }

    ros::Time scan_time = ros::Time::now();

    //populate the LaserScan message
    sensor_msgs::LaserScan scan;
    scan.header.stamp = scan_time;
    scan.header.frame_id = "laser_frame";
    scan.angle_min = -1.57;           //雷达扫描起始角度;
    scan.angle_max = 1.57;            //雷达扫描结束角度;
    scan.angle_increment = 3.14 / num_readings;     //水平角度分辨率;
    scan.time_increment = (1 / laser_frequency) / (num_readings);   //相邻点时间间隔
    scan.range_min = 0.0;            //雷达有效测量最小值;
    scan.range_max = 100.0;          //雷达有效测量最大值;
    scan.ranges.resize(num_readings);   // num_readings = 扫描角 / 角度分辨率;根据雷达参数计算;
    scan.intensities.resize(num_readings);
    //实际中，距离值根据激光雷达传感器测得;
    for(unsigned int i = 0; i < num_readings; ++i){
      scan.ranges[i] = ranges[i];
      scan.intensities[i] = intensities[i];
    }
    scan_pub.publish(scan);

    r.sleep();
  }

}

接收：

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

#define PI 3.1415926

void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
   std::vector<float> ranges = msg->ranges;
   
   //转换到二维XY平面坐标系下;
   for(int i=0; i< ranges.size(); i++)
   {
     double angle = msg.angle_min + i * msg.angle_increment;
     double X = ranges[i] * cos(angle);
     double Y = ranges[i] * sin(angle);
     float intensity = msg.intensities[i];
     std::cout << ranges[i] << " , " << std::endl;
   }
}

int main(int argc, char **argv)
{
   ros::init(argc, argv, "laser_listener");
   ros::NodeHandle nh;

   ros::Subscriber sub = nh.subscribe("/scan", 10, laserCallback);

   ros::spin();

   return 0;
}

(2) 3D点云数据PointCloud2;

     接收：

#include <stdio.h>
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types_conversion.h>
#include <sensor_msgs/PointCloud2.h>

//点云数据接收回调函数;
void PointCloud_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg)
{
  const auto& stamp = points_msg->header.stamp;
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZI>());             //

  pcl::fromROSMsg(*points_msg, *cloud_in);

}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "point_cloud_subscriber");
  ros::NodeHandle node;

  ros::Subscriber points_sub = node.subscribe("/points_cloud", 5, PointCloud_callback);
  
  ros::spin();

  return 0;
}

     发布：

#include <stdio.h>
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types_conversion.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/filters/voxel_grid.h>
 
ros::Subscriber points_sub;   //订阅者;
ros::Publisher Map_pub;       //发布者;

//降采样函数;
void DownSample_Filter(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_in,
                       pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud_downsize,
                       double downsize){
   pcl::VoxelGrid<pcl::PointXYZI> downsample_filter;
   downsample_filter.setLeafSize(downsize, downsize, downsize);
   downsample_filter.setDownsampleAllData(true);    //对全字段进行下采样;
   downsample_filter.setInputCloud(cloud_in);
   downsample_filter.filter(*cloud_downsize);
}

//点云数据发布;
void publish_result(pcl::PointCloud<pcl::PointXYZI>::Ptr fliter_map){
if(Map_pub.getNumSubscribers() != 0){
   sensor_msgs::PointCloud2 cloud_out;
   pcl::toROSMsg(*fliter_map, cloud_out);
   cloud_out.header.frame_id = "map";
   cloud_out.header.stamp = stamp_now;
   Map_pub.publish(cloud_out);
   std::cout << " map size: " << fliter_map->points.size() << std::endl;
  }
}

//点云数据接收回调函数;
void PointCloud_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg)
{
  const auto& stamp = points_msg->header.stamp;
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZI>());             //

  pcl::fromROSMsg(*points_msg, *cloud_in);

  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filter(new pcl::PointCloud<pcl::PointXYZI>());

  DownSample_Filter(cloud_in, cloud_filter, 0.15);

  publish_result(cloud_filter);

}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "point_cloud_subscriber");
  ros::NodeHandle node;

  points_sub = node.subscribe("/points_cloud", 5, PointCloud_callback);
  
  Map_pub= node.advertise<sensor_msgs::PointCloud2>("/filtered_map", 5, true);

  ros::spin();

  return 0;
}

（3）pointcloud  to laserscan 转换;

    pointcloud类型可以转化为 laserScan，github有专门的代码包：

    https://github.com/ros-perception/pointcloud_to_laserscan

二、点云数据解析;

       在进行ROS消息类型和PCL库点云类型之间转换时，除了可以利用PCL自带的转换函数，也可以自己解析。参考代码如下：

  void velodyPointCloundCallback(const sensor_msgs::PointCloud2ConstPtr &point_clound)
  {
     std::string frame_id = point_clound->header.frame_id;
     ros::Time received_time = point_clound->header.stamp;

      std::vector<pcl::PointXYZI> *gridpoints;
      gridpoints = new std::vector<pcl::PointXYZI>[laser_rings];
      pcl::PointXYZI point;

      //通过迭代器获取参数；
     for (sensor_msgs::PointCloud2ConstIterator<float> its(*point_clound, "x"); its != its.end(); ++its)
      {
        const uint16_t r = *((const uint16_t*)(&its[5])); // ring
        float x_p = its[0]; // x
        float y_p = its[1]; // y
        float z_p = its[2]; // z
        float in_p = its[4]; // intensity

        point.x = x_p;
        point.y = y_p;
        point.z = z_p;
        point.intensity = in_p;

        gridpoints[r].push_back(point);
    }
 }

三、自定义点云数据类型;

     在PCL点云库中，提供了自定义模板扩展，用户可以自己定义点云的数据类型。PCL提供了常见的点云类型，包括：pcl::PointXYZ, pcl::PointXYZI,pcl::PointXYZL等。pcl提供的点云数据类型

    如果需要自己重新添加自定义的点云类型，可参考如下：

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

struct PointXYZIRPYT
{
    PCL_ADD_POINT4D
    PCL_ADD_INTENSITY;
    float roll;
    float pitch;
    float yaw;
    double time;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,
                                   (float, x, x) (float, y, y)
                                   (float, z, z) (float, intensity, intensity)
                                   (float, roll, roll) (float, pitch, pitch)
                                   (float, yaw, yaw)
                                   (double, time, time)
)

typedef PointXYZIRPYT PointTypePose;

#include <stdint.h>

#define PCL_NO_PRECOMPILE
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/impl/instantiate.hpp>

struct _Kitti_PointExtended {
  inline _Kitti_PointExtended(const _Kitti_PointExtended &p)
    : data { p.x, p.y, p.z, 1.0f }, i(p.i){
  }

  inline _Kitti_PointExtended()
    : data { 0.0f, 0.0f, 0.0f, 1.0f }, i(0.0f) {
  }

  friend std::ostream& operator << (std::ostream& os, const _Kitti_PointExtended& p) {
    return os << "x: "<< p.x << ", y: " << p.y << ", z: " << p.z
        << ", intensity : " << p.i;
  }
  PCL_ADD_POINT4D;
  union {
    struct {
      float i;
    };
    float data_c[4];
  };
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}EIGEN_ALIGN16;       // 强制SSE对齐

// Register the point type.
POINT_CLOUD_REGISTER_POINT_STRUCT (_Kitti_PointExtended,
                                   (float, x, x)
                                   (float, y, y)
                                   (float, z, z)
                                   (float, i, i)
)

typedef _Kitti_PointExtended PointKittiExtended;