雷达无法及时清除代价地图中的障碍

使用代价地图时，会出现由于激光雷达测距的局限性，代价地图中的障碍会不能及时清除。 比如有行人走过会拖出一个条状障碍或者雷达扫描到障碍仍不能清除。问题根源在于某些激光雷达测的距离超过极限，会没有相对应的数据。

不同的激光雷达的情况不一样。所以costmap的源码不可能处理所有的情况。 在测不到数据时， sick雷达，返回的是inf。国内一些雷达，在未测到数据时，返回的是0.0 或者默认的最大值。

代码在障碍层的ObstacleLayer::laserScanValidInfCallback

for (size_t i = 0; i < message.ranges.size(); i++)
{
  float range = message.ranges[ i ];
  if ( (!std::isfinite(range) && range > 0)  )
  {
    message.ranges[ i ] = message.range_max - epsilon;
  }
}

上面的代码处理了每一个激光的数据，如果是激光的点是最大的距离(不是 finite)，那么将这个点设置为比最大距离小十分之一毫米。看来程序作者也考虑到了这个问题，当激光的距离等于最大的距离的时候会出现障碍物无法清除的现象，但是作者不可能对所有雷达出现数据invalid的情况都做判断，这就需要自己加了。

有两种已知的雷达数据invalid的情况:

• 雷达在超出量程或没数据时返回0，上面的if判断加入 || range ==0.0，

• 雷达的数据超出量程后设为大于range_max的值，而不是inf，比如量程位30m的HOKUYO激光雷达，超量程后会返回65.33m。这样也不在上面的if判断里，加入range >= range_max

最后还要在通用代价地图中设置inf_is_valid参数为true

但是这只适用于单线雷达，如果是多线雷达或相机的深度点云，回调函数就是 pointCloud2Callback，不能用这个方法了。

地图分辨率高于激光雷达分辨率

在costmap_common_params.yaml中有两个配置参数

obstacle_range: 2.5  //只有障碍物在这个范围内才会被标记
raytrace_range: 3    //这个范围内不存在的障碍物都会被清除

当raytrace_range = 3时，假设激光雷达的角分辨率是1，即360度每一度一个激光点时，每个激光点之间的距离大约是0.052(1x3.14/180 x3), 如果此时地图的分辨率是0.01，在靠近激光点附近有一个障碍物，但是始终在激光雷达两条射线之间的话，也就是始终没有扫描到的话，那这个障碍始终无法被清除掉。所以参数raytrace_range不是越大越好

我们知道代价地图实际上是一定分辨率的方格图，两条射线如下图所示，根据bresenham2D算法找到经过的蓝色方格，这些方格的代价值会置为FREE_SPACE。但实际上黄色方格也应该置为FREE_SPACE，但是因为角分辨率不足而没有扫射到，这就是问题的由来。

解决方法首先是买好雷达，角分辨率一定要好。

清除障碍物是ObstacleLayer::raytraceFreespace，
raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); 会将所有在(x0,y0)>>(x1,y1)之间的所有cell标记为FREE_SPACE

updateRaytraceBounds会根据测量的距离，更新扩张(min_x, min_y, max_x, max_y)

updateBounds 在根据测量数据完成 clear 操作之后，就开始了mark 操作，对每个测量到的点，标记为obstacle

修改清除代价值的规则，即ObstacleLayer::raytraceFreespace，把一个激光点做十字形扩展：

for (unsigned int i = 0; i < cloud.points.size(); ++i)
{
    double wx = cloud.points[i].x;
    double wy = cloud.points[i].y;

    //ROS_INFO("laser scan wx = %.2f, wy = %.2f", wx, wy);
    //在检测到的点周围生成6x6的点，
    double inflate_dx = 0.01, inflate_dy = 0.01; //在原来点的位置膨胀的尺度
    std::vector< std::pair<double,double> > inflate_pts;
    inflate_pts.push_back(std::make_pair(wx +    0      , wy +     0     ));
    inflate_pts.push_back(std::make_pair(wx -    0      , wy - inflate_dy));
    inflate_pts.push_back(std::make_pair(wx - inflate_dx, wy -     0     ));
    inflate_pts.push_back(std::make_pair(wx + 0         , wy + inflate_dy));
    inflate_pts.push_back(std::make_pair(wx + inflate_dx, wy +     0      ));
    inflate_pts.push_back(std::make_pair(wx -    0        , wy - 2*inflate_dy));
    inflate_pts.push_back(std::make_pair(wx - 2*inflate_dx, wy -     0     ));
    inflate_pts.push_back(std::make_pair(wx +    0        , wy + 2*inflate_dy));
    inflate_pts.push_back(std::make_pair(wx + 2*inflate_dx, wy +     0      ));
    inflate_pts.push_back(std::make_pair(wx -    0        , wy - 3*inflate_dy));
    inflate_pts.push_back(std::make_pair(wx - 3*inflate_dx, wy -     0     ));
    inflate_pts.push_back(std::make_pair(wx +    0        , wy + 3*inflate_dy));
    inflate_pts.push_back(std::make_pair(wx + 3*inflate_dx, wy +     0      ));  

    // 实质上增加一个循环
    std::vector< std::pair<double,double> >::iterator inflate_iter;
    for(inflate_iter = inflate_pts.begin(); inflate_iter != inflate_pts.end(); inflate_iter++)
    {
      wx = (*inflate_iter).first;
      wy = (*inflate_iter).second;
            ......
      MarkCell marker(costmap_, FREE_SPACE);
      // and finally... we can execute our trace to clear obstacles along that line
      //最终raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); 
      //会将所有在(x0,y0)>>(x1,y1)之间的所有cell标记为FREE_SPACE。
      raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);
      //而updateRaytraceBounds 会根据测量的距离，更新扩张（min_x, min_y, max_x, max_y）。
      updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
  }
}

move_base中自动清除代价地图

MoveBase的状态机，在全局规划失败达到上限时，会进入恢复行为，清除代价地图。这样的效率比较低，而且清除的方法不是我们需要的。所以修改MoveBase::makePlan，在多次找不到全局路径时(次数较少)，清除代价地图，其实跟clear_costmaps服务端的代码是一样的。一开始我还在move_base里定义了客户端函数，在规划失败太多时发起service请求move_base/clear_costmaps，后来发现服务端的代码就在move_base里，就两行：

if(!planner_->makePlan(start, goal, plan) || plan.empty())
{
  ROS_DEBUG_NAMED("move_base","Failed to find a  plan to point (%.2f, %.2f)", 
    goal.pose.position.x, goal.pose.position.y);

  failed_num++;
  if(failed_num > min_clear_ )
  {
    controller_costmap_ros_->resetLayers();
    planner_costmap_ros_->resetLayers();
    ROS_INFO("\033[44;37m clear costmaps done ! \033[0m");
  }
  return false;
}

min_clear_是我自定义的参数min_clear的赋值，默认是4。

failed_num在构造函数中初始化为0，这是当然的。 另外注意failed_num在 MoveBase::executeCb开头置为0，因为这里是收到新目标的地方。

参考：激光点做圆形扩展

客户端发起清除代价地图的请求

有时全局代价地图还是不能及时清除，但是又不能使用上面自动清除的方法。考虑在客户端(python 程序)发目标点时，先发起move_base/clear_costmaps服务，让move_base清除一次，这样效果好多了。我开始是修改planThread和executeCb函数，但是这两个函数在路径规划时并不是只运行一次，多次运行会清除过多，会撞障碍；如果清除后执行sleep，又使机器人表现卡顿。