costmap2d 地图栅格代价值读取错误的问题分析

jucat

于 2023-11-21 23:00:51 发布

阅读量80

点赞数

分类专栏：调试记录文章标签：机器人算法自动驾驶

本文链接：https://blog.csdn.net/jucat/article/details/134541542

版权

调试记录专栏收录该内容

8 篇文章 0 订阅

订阅专栏

问题

局部规划器的 costmap2d 地图是在服务器端生成，通过指针传递给到控制器插件。在插件内部就可以通过该指针获取到局部代价地图，并在规划路径时候通过读取地图代价来给轨迹评分或者是做碰撞检测。

大概画了张不严谨的示意图

在实际使用过程中，遇到过规划路径时候，通过地图指针查阅地图是没有障碍物的，但是实际情况中地图是有障碍物的，发布出来的地图话题也是有障碍物的异常情况。使用 class Costmap2D 的拷贝函数，也会出现地图拷贝不全的情况，一边有信息，一边为空的情况，大概像下面情况：

分析

通过查看 costmap2d 源码看到 class Costmap2D 有一个接口是提供锁：

  typedef boost::recursive_mutex mutex_t;
  mutex_t* getMutex()
  {
    return access_;
  }

再通过这个 access_ 的锁找到了 class LayeredCostmap 中更新地图的函数

void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
{
  // Lock for the remainder of this function, some plugins (e.g. VoxelLayer)
  // implement thread unsafe updateBounds() functions.
  // 外部通过指针使用 costmap 的时候要记得上锁，最好是上锁拷贝一份地图
  boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));

  // if we're using a rolling buffer costmap... we need to update the origin using the robot's position
  // 机器移动局部地图跟着机器移动，这里拷贝了前后地图的重复部分，提高了点效率
  if (rolling_window_)
  {
    double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2;
    double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2;
    costmap_.updateOrigin(new_origin_x, new_origin_y);
  }

  if (plugins_.size() == 0)
    return;

  minx_ = miny_ = 1e30;
  maxx_ = maxy_ = -1e30;

  // 检查各个插件更新的最大边界范围，世界坐标，即地图的 Global_Frame 坐标系
  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
       ++plugin)
  {
    if(!(*plugin)->isEnabled())
      continue;
    double prev_minx = minx_;
    double prev_miny = miny_;
    double prev_maxx = maxx_;
    double prev_maxy = maxy_;
    (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
    if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
    {
      ROS_WARN_THROTTLE(1.0, "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
                        "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s",
                        prev_minx, prev_miny, prev_maxx , prev_maxy,
                        minx_, miny_, maxx_ , maxy_,
                        (*plugin)->getName().c_str());
    }
  }

  // 边界的世界坐标转地图数组坐标
  int x0, xn, y0, yn;
  costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
  costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn);

  x0 = std::max(0, x0);
  xn = std::min(int(costmap_.getSizeInCellsX()), xn + 1);
  y0 = std::max(0, y0);
  yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);

  ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);

  if (xn < x0 || yn < y0)
    return;

  costmap_.resetMap(x0, y0, xn, yn); // 这里会重置整个更新边界内的地图内容 ！
  // 各个插件更新边界内的地图代价值
  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
       ++plugin)
  {
    if((*plugin)->isEnabled())
      (*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
  }

  bx0_ = x0;
  bxn_ = xn;
  by0_ = y0;
  byn_ = yn;

  initialized_ = true;
}

大概结构如下：