2D(单线)激光雷达PointCloud与LaserScan格式转换_2d激光雷达 转 点云

sensor_msgs::LaserScan转PointCloud

void LaserScanToPointCloud(sensor_msgs::LaserScan::ConstPtr _laser_scan, pcl::PointCloud<pcl::PointXYZI>& _pointcloud)
{
  _pointcloud.clear();
  pcl::PointXYZI newPoint;
  newPoint.z = 0.0;
  double newPointAngle;
 
  int beamNum = _laser_scan->ranges.size();
  for (int i = 0; i < beamNum; i++)
  {
      newPointAngle = _laser_scan->angle_min + _laser_scan->angle_increment * i;
      newPoint.x = _laser_scan->ranges[i] * cos(newPointAngle);
      newPoint.y = _laser_scan->ranges[i] * sin(newPointAngle);
      newPoint.intensity = _laser_scan->intensities[i];
      _pointcloud.push_back(newPoint);
  }
}

PointCloud转sensor_msgs::LaserScan

sensor_msgs::LaserScan PointCloudToLaserscan(pcl::PointCloud<pcl::PointXYZI>& _pointcloud)
{
  float angle_min, angle_max, range_min, range_max, angle_increment;
 
  //需要自行调整的参数
  angle_min = -3.14159;
  angle_max =  3.14159;
  range_min = 0.5;
  range_max = 20;
  //角度分辨率，分辨率越小，转换后的误差越小
  angle_increment = 0.005
 
  //计算扫描点个数
  unsigned int beam_size = ceil((angle_max - angle_min) / angle_increment);
 
  sensor_msgs::LaserScan output;
  output.header.stamp = ros::Time::now();
  output.header.frame_id = "laser";
  output.angle_min = angle_min;
  output.angle_max = angle_max;
  output.range_min = range_min;
  output.range_max = range_max;
  output.angle_increment = angle_increment;
  output.time_increment = 0.0;
  output.scan_time = 0.0;
 
  //先将所有数据用nan填充
  output.ranges.assign(beam_size, std::numeric_limits<float>::quiet_NaN());
  output.intensities.assign(beam_size, std::numeric_limits<float>::quiet_NaN());
 
  for (auto point : _pointcloud.points)
  {
    float range = hypot(point.x, point.y);
    float angle = atan2(point.y, point.x);
    int index = (int)((angle - output.angle_min) / output.angle_increment);
    if (index >= 0 && index < beam_size)
    {
      //如果当前内容为nan，则直接赋值
      if (isnan(output.ranges[index]))
      {
        output.ranges[index] = range;
      }
      //否则，只有距离小于当前值时，才可以重新赋值
      else
      {
        if (range < output.ranges[index])
        {
          output.ranges[index] = range;
        }
      }
      output.intensities[index] = point.intensity;
    }
  }
  return output;
}

PointCloud转sensor_msgs::PointCloud2

pcl::toROSMsg(pcl::PointCloud<pcl::PointXYZ> pointcloud_pcl,sensor_msgs::PointCloud2 pointcloud_msg);

sensor_msgs::PointCloud2转PointCloud

pcl::fromROSMsg(sensor_msgs::PointCloud2 pointcloud_msg,pcl::PointCloud<pcl::PointXYZ> pointcloud_pcl);