【ROS-Navigation】Costmap2D代价地图源码解读-静态层StaticLayer

记录学习阅读ROS Navigation源码的理解，本文为Costmap2D代价地图源码学习记录，以文字总结、绘制结构图说明、代码注释为主。仍在学习过程中，有错误欢迎指正，共同进步。

本篇记录静态地图部分的内容，主要是开启地图的订阅，以及bound和cost的更新。静态地图默认只更新一次，地图的边界也只更新一次，对于rolling地图，需要随着机器人的运动位置不断更新附近的cost，而对于非rolling地图则不需要。

【结构示意图】

【相关文件】

• costmap_2d/src/costmap_2d_ros.cpp
• costmap_2d/src/costmap_2d.cpp
• costmap_2d/src/layered_costmap.cpp
• costmap_2d/src/costmap_layer.cpp
• costmap_2d/plugins/static_layer.cpp
• costmap_2d/plugins/obstale_layer.cpp
• costmap_2d/plugins/inflation_layer.cpp

本篇记录对StaticLayer类的阅读和理解。

【代码分析】

StaticLayer类

–目录–

StaticLayer::onInitialize | 初始化

StaticLayer::incomingMap | 回调函数

StaticLayer::interpretValue | 将接收到的地图“翻译”到本层地图

StaticLayer::updateBounds | 更新静态地图边界

StaticLayer::updateCosts | 更新静态地图代价

<1> 初始化 StaticLayer::onInitialize

Costmap2DROS的构造函数会调用各Layer中的initialize函数，而initialize函数会调用onInitialize函数，真正的初始化工作在这里完成。

void StaticLayer::onInitialize()
{
  ros::NodeHandle nh("~/" + name_), g_nh;
  current_ = true;

  global_frame_ = layered_costmap_->getGlobalFrameID();

从参数服务器加载订阅静态地图的话题名称；默认静态地图只接收一次，不进行更新；以及默认追踪未知区域（主地图默认关闭）

  //初始化以下参数
  std::string map_topic;
  nh.param("map_topic", map_topic, std::string("map"));
  nh.param("first_map_only", first_map_only_, false);
  nh.param("subscribe_to_updates", subscribe_to_updates_, false);

  nh.param("track_unknown_space", track_unknown_space_, true);
  nh.param("use_maximum", use_maximum_, false);

  int temp_lethal_threshold, temp_unknown_cost_value;
  nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100));
  nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1));
  nh.param("trinary_costmap", trinary_costmap_, true);

  lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
  unknown_cost_value_ = temp_unknown_cost_value;

订阅map_topic，回调函数：incomingMap，map_received_和has_updated_data_标志位设置为false（一旦收到地图，进入回调函数，它们会被置为真），没有接收到地图时，阻塞。

  //只有当话题改变后才重新订阅
  if (map_sub_.getTopic() != ros::names::resolve(map_topic))
  {
    ROS_INFO("Requesting the map...");
    map_sub_ = g_nh.subscribe(map_topic, 1, &StaticLayer::incomingMap, this);
    map_received_ = false;
    has_updated_data_ = false;

    ros::Rate r(10);
    //如果map_received_一直是false，则一直阻塞在这里，只有在接收到后进入回调函数更新为true后才会继续进行
    while (!map_received_ && g_nh.ok())
    {
      ros::spinOnce();
      r.sleep();
    }

    ROS_INFO("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution());

判断是否接收static map的更新（默认否），若接收，则开启topic对map_topic + "_updates"的更新，最后开启参数动态配置服务。

    if (subscribe_to_updates_)
    {
      ROS_INFO("Subscribing to updates");
      map_update_sub_ = g_nh.subscribe(map_topic + "_updates", 10, &StaticLayer::incomingUpdate, this);

    }
  }
  else
  {
    has_updated_data_ = true;
  }

  if (dsrv_)
  {
    delete dsrv_;
  }

  dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
      &StaticLayer::reconfigureCB, this, _1, _2);
  dsrv_->setCallback(cb);
}

<2> 回调函数 StaticLayer::incomingMap

获取接收到的静态地图的尺寸，当地图不随机器人移动时，若接收到的静态地图和主地图的尺寸/分辨率/起点不同，以接收到的地图为准，调整主地图的参数。

void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
{
  unsigned int size_x = new_map->info.width, size_y = new_map->info.height;

  ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution);

  //如果master map的尺寸、分辨率或原点与获取到的地图不匹配，重新设置master map
  Costmap2D* master = layered_costmap_->getCostmap();
  if (!layered_costmap_->isRolling() &&
      !layered_costmap_->isSizeLocked() &&
      (master->getSizeInCellsX() != size_x ||
       master->getSizeInCellsY() != size_y ||
       master->getResolution() != new_map->info.resolution ||
       master->getOriginX() != new_map->info.origin.position.x ||
       master->getOriginY() != new_map->info.origin.position.y))
  {
    // Update the size of the layered costmap (and all layers, including this one)
    ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
    layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x,
                                new_map->info.origin.position.y, true);
  }

若本层的数据和接收到的静态地图的参数不同时，继续以接收到的地图为准，调整本层参数。

  else if (size_x_ != size_x || size_y_ != size_y ||
           resolution_ != new_map->info.resolution ||
           origin_x_ != new_map->info.origin.position.x ||
           origin_y_ != new_map->info.origin.position.y)
  {
    // only update the size of the costmap stored locally in this layer
    ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
    resizeMap(size_x, size_y, new_map->info.resolution,
              new_map->info.origin.position.x, new_map->info.origin.position.y);
  }

将接收到的静态地图数据复制到本层地图，复制过程中调用interpretValue函数，进行“翻译”，内容后述。并设置好地图坐标系中的原点x_、y_，以及地图尺寸和标志位。

若first_map_only_开启，则在第一次接收到地图后，话题将不再接收消息。

  unsigned int index = 0;

  //用订阅到的map数据来初始化本层static map的数据成员costmap
  for (unsigned int i = 0; i < size_y; ++i)
  {
    for (unsigned int j = 0; j < size_x; ++j)
    {
      unsigned char value = new_map->data[index];
      costmap_[index] = interpretValue(value);
      ++index;
    }
  }
  map_frame_ = new_map->header.frame_id; //获取坐标系名称

  // we have a new map, update full size of map
  x_ = y_ = 0;
  width_ = size_x_;
  height_ = size_y_;
  map_received_ = true;
  has_updated_data_ = true;

  if (first_map_only_)
  {
    ROS_INFO("Shutting down the map subscriber. first_map_only flag is on");
    map_sub_.shutdown();
  }
}

<3> “翻译” StaticLayer::interpretValue

接收到的地图上：unknown_cost_value（默认为-1）为未知区域，lethal_cost_threshold（默认100）以上为致命障碍物

• 当接收到的地图上为-1时，若追踪未知区域，则本层地图上赋值NO_INFORMATION（255）；否则，在本层地图上赋值FREE_SPACE（0）；
• 当接收到的地图上>=100时，在本层地图上赋值LETHAL_OBSTACLE（254）；
• 若以上都不是，则按比例返回代价值。

unsigned char StaticLayer::interpretValue(unsigned char value)
{
  //如果追踪未知区域，且传入的参数代表未知，设置为NO_INFORMATION
  if (track_unknown_space_ && value == unknown_cost_value_)
    return NO_INFORMATION;
  //如果不追踪未知区域，且传入的参数代表未知，设置为FREE_SPACE
  else if (!track_unknown_space_ && value == unknown_cost_value_)
    return FREE_SPACE;
  //LETHAL_OBSTACLE
  else if (value >= lethal_threshold_)
    return LETHAL_OBSTACLE;
  //如果都不是，设置FREE_SPACE
  else if (trinary_costmap_)
    return FREE_SPACE;
bianjie
  //如果以上都不是，返回对应比例的障碍值
  double scale = (double) value / lethal_threshold_;
  return scale * LETHAL_OBSTACLE;
}

<4> 更新静态地图边界 StaticLayer::updateBounds

若非rolling地图，在有地图数据更新时更新边界，否则，根据静态层更新的区域的边界更新传入的边界。

void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
                               double* max_x, double* max_y)
{
  //若地图不是窗口滚动的
  if( !layered_costmap_->isRolling() ){
    if (!map_received_ || !(has_updated_data_ || has_extra_bounds_))
    //has_extra_bounds_初始值为false，而在第一次结束前，has_updated_data_被设置为false
    //且订阅话题在第一次接收到消息后关闭，故对于static layer，updateBounds只进行一次
    //Bounds的范围是整张地图的大小，在updateBounds过程中没有对静态地图层做任何的更新
      return;
  }
  //若有extrabounds，使用之
  useExtraBounds(min_x, min_y, max_x, max_y);

将map系中的起点（x_， y_）与终点（x_ + width_,， y_ + height_）转换到世界系，确保传入的bound能包含整个map在世界系中的范围。

  double wx, wy;

  mapToWorld(x_, y_, wx, wy);
  *min_x = std::min(wx, *min_x);
  *min_y = std::min(wy, *min_y);

  mapToWorld(x_ + width_, y_ + height_, wx, wy);
  *max_x = std::max(wx, *max_x);
  *max_y = std::max(wy, *max_y);

  has_updated_data_ = false;
}

由于默认不更新静态地图层，该层的bound将只更新一次。

<5> 更新静态地图代价 StaticLayer::updateCosts

这里将更新后的bound传入。

若不是rolling地图，那么直接将静态地图层bound范围内的内容合并到主地图，因为二者的尺寸也一样，updateWithTrueOverwrite和updateWithMax采用不同的合并策略，在上一篇有记录。

void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
  if (!map_received_)
    return;

  if (!enabled_)
    return;

  if (!layered_costmap_->isRolling())
  {
    //这里如果不是rolling选项，则直接将本层数据赋值到master map，因为他们尺寸也一样
    if (!use_maximum_)
      updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
    else
      updateWithMax(master_grid, min_i, min_j, max_i, max_j);
  }

若为rolling地图，则找到静态地图和global系之间的坐标转换，通过主地图→global→静态地图的转换过程，找到主地图的cell在静态地图上对应的cost，赋值给主地图。

  else
  { 
    //如果是rolling选项，master map就和该层数据坐标不一样
    unsigned int mx, my;
    double wx, wy;
    //首先获得map坐标系相对于global坐标系的位置，这个时候map坐标系随着机器人的运动而运动
    tf::StampedTransform transform;
    try
    {
      tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0), transform);
    }
    catch (tf::TransformException ex)
    {
      ROS_ERROR("%s", ex.what());
      return;
    }
    // Copy map data given proper transformations
    for (unsigned int i = min_i; i < max_i; ++i)
    {
      for (unsigned int j = min_j; j < max_j; ++j)
      {
        //master map坐标(i,j)→global坐标(wx,wy)
        layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
        tf::Point p(wx, wy, 0);
        p = transform(p);
        //global坐标p→静态地图坐标(mx, my)
        if (worldToMap(p.x(), p.y(), mx, my))
        {
          if (!use_maximum_)
            master_grid.setCost(i, j, getCost(mx, my));
          else
            master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
        }
      }
    }
  }
}

Neo 2020.3