ros开发之pointcloud_to_laserscan功能包浅析

ROS智能移动小车开发过程中需要slam建图定位,我这边具备的硬件条件为:Robosense 16线激光雷达,笔记本电脑(ubuntu16.04,Ros-Kinetic),当前不借助里程计完成SLAM建图定位的只有二维建图的hector-slam算法,需要将激光雷达三维点云转换为二维点云数据,功能包的配置实现过程可以参考:https://blog.csdn.net/geerniya/article/details/84880514

接下来主要记录pointcloud_to_laserscan功能包三维点云转二维原理:

原理实现主要在pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp中的void PointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)中:

输出数据类型为sensor_msgs/LaserScan,终端输入rosmsg show sensor_msgs/LaserScan得到具体格式如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
float32 angle_min
float32 angle_max
float32 angle_increment
float32 time_increment
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges
float32[] intensities

输出数据变量名为:sensor_msgs::LaserScan output;

变量赋值如下:

output.header = cloud_msg->header;
  if (!target_frame_.empty())
  {
    output.header.frame_id = target_frame_;
  }

  output.angle_min = angle_min_;
  output.angle_max = angle_max_;
  output.angle_increment = angle_increment_;
  output.time_increment = 0.0;
  output.scan_time = scan_time_;
  output.range_min = range_min_;
  output.range_max = range_max_;

关键赋值如下:

double range = hypot(*iter_x, *iter_y);

int index = (angle - output.angle_min) / output.angle_increment;
    if (range < output.ranges[index])
    {
      output.ranges[index] = range;//将RANGE值去高度
    }

数据发布如下:

ros::Publisher pub_;

pub_ = nh_.advertise("scan", 10, boost::bind(&PointCloudToLaserScanNodelet::connectCb, this),
                                               boost::bind(&PointCloudToLaserScanNodelet::disconnectCb, this));

pub_.publish(output);