基于rslidar_16的栅格地图建立（1）

订阅激光雷达发布的LaserScan并获取角度和距离信息

1、用容器把激光的信息转化为为inf的数值信息存在ranges_a这个vector里面。

double lidar_err = 0.05;
  std::vector<double> ranges_a;
  for (int j = 0; j < msg->ranges.size(); j++)
  {
    if (msg->ranges[j] >= lidar_err)
    {
      ranges_a.push_back(msg->ranges[j]);
    }
  }

2、把激光雷达的角度数据转化为需要的角度。

/获取每一帧的激光雷达、机器人位姿数据
    float range = ranges_a[i];
    float angle_laser = msg->angle_min + msg->angle_increment * i;

建成栅格图的原理

栅格图就像把一张图划分成很小的一个板块，每个版块赋值到底为多少，-1为未知，值小于50为空闲，大于50为击中。而关键点在于怎么赋值，这就需要激光雷达数据来体现，通过找到某扫描激光点的终点坐标和机器人坐标之间途径的空闲坐标来实现的。
主要核心代码：

bool CacheLaserScan(const sensor_msgs::LaserScan::ConstPtr &laserScanMsg)
{

  if (first_scan_)
  {
    first_scan_ = false;
  }
  scan_count_ = laserScanMsg->ranges.size();
  //std::cout << "scan_count_:" << scan_count_ << std::endl;
  laser_queue_.push_back(*laserScanMsg);
  //if (laser_queue_.size() < 3)
  //return false;
  //std::cout << "laser_queue_size:" << laser_queue_.size() << std::endl;

  current_laserscan_ = laser_queue_.front();
  laser_queue_.pop_front();
  current_laserscan_header_ = current_laserscan_.header;
  current_scan_time_start_ = current_laserscan_header_.stamp.toSec();
  current_scan_time_increment_ = current_laserscan_.time_increment;
  current_scan_time_end_ = current_scan_time_start_ + current_scan_time_increment_ * (scan_count_ - 1);
  return true;
}
void OccupanyMapping(const sensor_msgs::LaserScan::ConstPtr &msg)
{

  if (!CacheLaserScan(msg))
  {
    return;
  }
  //遍历该时刻所有帧激光雷达数据

  for (int i = 0; i < scan_count_; i++)
  { //获取每一帧的激光雷达、机器人位姿数据
    if (!std::isfinite(current_laserscan_.ranges[i]) ||
        current_laserscan_.ranges[i] < current_laserscan_.range_min ||
        current_laserscan_.ranges[i] > current_laserscan_.range_max)
      continue;
    double range = current_laserscan_.ranges[i];
    double angle_laser = current_laserscan_.angle_min + current_laserscan_.angle_increment * i;

    //获取该帧机器人位姿的栅格序号
    GridIndex robotIndex = ConvertWorld2GridIndex(state.x, state.y); //（state.x，state.y）为在world坐标系下的坐标点;
    //判断该帧机器人位姿的栅格序号，是否在自己设定的栅格地图范围内
    if (isValidGridIndex(robotIndex) == false)
    {
      continue;
    }

    //遍历该帧激光雷达数据所有扫描点
    //取出该激光雷达扫描点的距离和角度
    double dist = range;
    double angle = angle_laser;
    //机器人航向角，机器人x轴与世界坐标系x轴夹角
    double theta = M_PI / 2;
    //在旋转过后（与世界坐标系（像素坐标系下）平行）的激光雷达坐标系下的坐标x,y
    //该开始一直不理解这个为啥laser_y要加一个负号
    //明确激光雷达数据的角度逆时针变化
    //明确机器人航向角与世界坐标系x轴呈逆时针变化
    //明确这里的世界坐标系world_x，不是真实的世界坐标系，而是像素坐标系，y轴与真实的世界坐标系相反，这样是laser_y加负号的原因
    double laser_x = dist * cos(angle + theta);
    double laser_y = -dist * sin(angle + theta);

    //得到该激光扫描点，在世界坐标系下（像素坐标系下）的位置
    double world_x = laser_x + state.x; //5表示机器人的x坐标。
    double world_y = laser_y + state.y;

    //将该激光扫描点在世界坐标系下的位置，转化为栅格序号
    GridIndex mapIndex = ConvertWorld2GridIndex(world_x, world_y);

    //判断该激光扫描点的栅格序号，是否在自己设定的栅格地图900x900范围内，如果不在则跳过

    if (isValidGridIndex(mapIndex) == false)
    {
      continue;
    }

    //从机器人的栅格序号到该激光扫描点的栅格序号划线
    //目的：找到两点之间途径的空闲栅格，将栅格序号存入std::vector<GridIndex>中
    std::vector<GridIndex> freeIndex = TraceLine(robotIndex.x, robotIndex.y, mapIndex.x, mapIndex.y);
    //std::cout << freeIndex.size() << std::endl;
    //遍历该扫描激光点通过的所有空闲栅格
    for (int k = 0; k < freeIndex.size(); k++)
    {

      GridIndex tmpIndex = freeIndex[k];
      //将空闲栅格的栅格序号，转化到数组序号,该数组用于存储每一个栅格的数据
      int linearIndex = GridIndexToLinearIndex(tmpIndex);
      //取出该栅格代表的数据
      int data = pMap[linearIndex];
      //根据栅格空闲规则，执行data += mapParams.log_free;
      if (data > 0)                 //默认data=50
        data += mapParams.log_free; //log_free=-1，data将变小
      else
        data = 0;
      //给该空闲栅格赋新值，最小为0
      pMap[linearIndex] = data;
    }

    //更新该激光扫描点集中的栅格，
    int tmpIndex = GridIndexToLinearIndex(mapIndex);
    int data = pMap[tmpIndex];
    //根据栅格击中规则，执行data += mapParams.log_occ;
    if (data < 100)              //默认data=50
      data += mapParams.log_occ; //log_occ=2，data将变大
    else
      data = 100;
    //给击中的栅格赋新值，最大100
    pMap[tmpIndex] = data;

    //到这里，对一个位姿下的一个激光扫描数据经过的空闲栅格和击中栅格的pMap进行了重新赋值
  }
  //到这里，对一个位姿下的一帧激光扫描数据经的过的空闲栅格和击中栅格进行了重新赋值
  std::cout << "finish loop" << std::endl;
}

结果展示

白色的就是表示占据的珊格。

下一步规划

本篇的机器人的坐标系是随机编写的，下一步希望实现机器人的坐标系实时传输。
除此以外，还需要把栅格地图的坐标中心建立在机器人的位置上，随着机器人的位置动，珊格地图也实时更新。