costmap_2d详解5：static_layer.cpp分析

关键变量

  std::string global_frame_;  //global_costmap时为map, local_costmap时为odom_combined
  std::string map_frame_; 
  bool subscribe_to_updates_;
  bool map_received_;
  bool has_updated_data_;

/*
*在接收到地图时，x_, y_被置为0,width_, height_ 被设为地图宽高
*/
  unsigned int x_, y_, width_, height_;
  bool track_unknown_space_; //默认没有配置，使用true
  bool use_maximum_;  //false
  bool first_map_only_;      //false
  bool trinary_costmap_; //true
  ros::Subscriber map_sub_, map_update_sub_;
  unsigned char* costmap_; //这个变量是从Costmap2D 类中继承来的,是一个指针
   //致命cost 阈值，使用默认的为100，未知的区域，cost 值为255
  unsigned char lethal_threshold_, unknown_cost_value_;
  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_;

关键代码

/*该函数是虚函数，是从Layer 类中继承来的，是在costmap_2d_ros.cpp 中

*plugin->initialize(layered_costmap_, name + "/" + pname, &tf_); 调用的

*/

void StaticLayer::onInitialize()
{
  ros::NodeHandle nh("~/" + name_), g_nh;
  current_ = true;
  //global_costmap时为map, local_costmap时为odom_combined
  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;

  //致命cost 阈值，使用默认的为100
  nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100));

  //未知的区域，cost 值为255
  nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1));

 //三倍的costmap??默认没有配置，为true
  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;

  // Only resubscribe if topic has changed
  if (map_sub_.getTopic() != ros::names::resolve(map_topic))
  {
//从/map中订阅地图数据，并在回调函数中使用
    map_sub_ = g_nh.subscribe(map_topic, 1, &StaticLayer::incomingMap, this);
    map_received_ = false;
    has_updated_data_ = false;


  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);

}

void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
{
//从地图消息中获取地图的宽和高
  unsigned int size_x = new_map->info.width, size_y = new_map->info.height;

  //判断master costmap地图是否发生变化
  Costmap2D* master = layered_costmap_->getCostmap();
  if (!layered_costmap_->isRolling() && (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 ||
      !layered_costmap_->isSizeLocked()))
  {

// 如果master costmap 层发生变化，就更新master costmap 范围
    layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x,new_map->info.origin.position.y, true);

  }
 //一开始size_x_为0,所以会进入并仅仅更新static costmap 层
  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)
  {
   //仅更新static costmap 层信息
    resizeMap(size_x, size_y, new_map->info.resolution,
              new_map->info.origin.position.x, new_map->info.origin.position.y);
  }
  /*size_x_，size_y_是从Costmap2D类中继承过来的
  * 此时size_x_，size_y_，resolution_，origin_x_，origin_y_
  * 都已经变成map 的地图的范围等信息了
  */
  unsigned int index = 0;

  //根据新地图中的数据来更新costmap 的值,
  //将图片的像素值转换成代价值了，具体转换可以看interpretValue函数
  for (unsigned int i = 0; i < size_y; ++i)
  {
    for (unsigned int j = 0; j < size_x; ++j)
    {
      //地图传来的值为-1(即255),0 和100
      unsigned char value = new_map->data[index];
      //这个变量是从Costmap2D 类中继承来的,是一个指针，具体指向哪里呢？
      costmap_[index] = interpretValue(value);
      ++index;
    }
  }
  map_frame_ = new_map->header.frame_id;
  x_ = y_ = 0;
  width_ = size_x_;  //这个是以像素坐标系(地图左下角)为参考的坐标，是int类型
  height_ = size_y_;
  map_received_ = true;
  has_updated_data_ = true;

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

 

unsigned char StaticLayer::interpretValue(unsigned char value)
{
  // check if the static value is above the unknown or lethal thresholds
  // 实际上就是高于一个阈值lethal_threshold那么就认为是障碍物
  // 没有信息的就认为是未探测的区域
  // 否则为自由空间，true, -1，
  if (track_unknown_space_ && value == unknown_cost_value_)
  {
    //map数据为-1 时会进入这里
    return NO_INFORMATION;
  }
  else if (!track_unknown_space_ && value == unknown_cost_value_)
    return FREE_SPACE;
  // map数据为100 时，>=100 就是致命的障碍物，会进入这里
  else if (value >= lethal_threshold_)
    return LETHAL_OBSTACLE;
  else if (trinary_costmap_)
    return FREE_SPACE;
  // map数据为0 时进入这里
  double scale = (double) value / lethal_threshold_;
  return scale * LETHAL_OBSTACLE;
}

 

/*作用：

 * 参数: robot_x, robot_y, robot_yaw

*min_x, min_y,max_x, max_y 初始值min_x=min_y=1e30，max_x=max_y=-1e30

*返回：min_x, min_y,max_x, max_y为地图的范围，是以世界坐标系(map)为参考的坐标，double类型

*/

void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,double* max_x, double* max_y)
{
  //传过来的参数 min_x=min_y=1e30，max_x=max_y=-1e30
  if( !layered_costmap_->isRolling() ){
    if (!map_received_ || !(has_updated_data_ || has_extra_bounds_))
      return;
  }
  //初始化边界
  useExtraBounds(min_x, min_y, max_x, max_y);
  double wx, wy;

  /*计算静态costmap地图左下角像素的坐标*/
  mapToWorld(x_, y_, wx, wy);
  *min_x = std::min(wx, *min_x);
  *min_y = std::min(wy, *min_y);

  /*计算静态costmap地图右上角像素的坐标*/
  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;

}

/*作用：用static costmap 的值更新master costmap 的值

*参数:master_grid： 是master costmap 地图

* min_i,min_j,max_i,max_j 是该master costmap 地图的范围，以像素坐标表示

*/
 

void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{

  if (!layered_costmap_->isRolling())
  {
    if (!use_maximum_)
/*
 *静态costmap 会进入这里面，会获取static costmap 的cost 值，然后更新master costmap
 *static costmap 的cost 值是在incomingMap函数中处理初始化的，该函数在costmap_2d 中实现
*/
      updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
    else
      updateWithMax(master_grid, min_i, min_j, max_i, max_j);

  }

}