costmap_2d

costmap的计算

costmap表示的是地图中栅格的cost,cost的计算方法是:

上图将地图中栅格的cost分为五部分，其中红色多边形表示机器人的轮廓:
（1） Lethal （致命的):机器人的中心与该网格的中心重合，此时机器人必然与障碍物冲突。
（2） Inscribed（内切圆):网格的外切圆与机器人的轮廓内切，此时机器人也必然与障碍物冲突。
（3） Possibly circumscribed（外切圆):栅格与机器人的轮廓外切，此时机器人相当于靠在障碍物附近，所以不一定冲突。
（4） Freespace（自由空间）：没有障碍物的空间。
（5） Unknown（未知）：未知的空间。
对应的cost的计算:exp(-1.0 * cost_scaling_factor * (distance_from_obstacle - inscribed_radius)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)

// costmap_2d/inflation_layer.h
  inline unsigned char computeCost(double distance) const
  {
    unsigned char cost = 0;
    if (distance == 0)
      cost = LETHAL_OBSTACLE;
    else if (distance * resolution_ <= inscribed_radius_)
      cost = INSCRIBED_INFLATED_OBSTACLE;                //254
    else
    {
      // make sure cost falls off by Euclidean distance cost 0~255
      double euclidean_distance = distance * resolution_;
      double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
      cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
    }
    return cost;
  }

costmap由使用的是layeredMap的方式，如下图:

图中红色栅格表示障碍物，绿色表示障碍物根据机器人的内切圆膨胀形成的，红色多边形表示机器人。机器人需要保证机器人的多边形轮廓不与红色栅格区域相交、机器人的中心不到达蓝色膨胀区域。

costmap_2d类之间关系

costmap_2d中的类继承关系:

David Lu的《Layered Costmaps for Context-Sensitive Navigation》

move_base中的使用

在move_base的使用中，首先分别定义了planner_costmap_ros_,controller_costmap_ros_分别对应gloabl_costmap,local_costmap,在move_base的构造函数中，初始化这两个对象。这两个对象使用的是costmap_2d包中封装之后的costmap_2d::Costmap2DROS类，在这个类中包含对plugin的载入。分别如下:

    move_base中的初始化:move_base/src/move_base.cpp
    //create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
    planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
    planner_costmap_ros_->pause();
    //initialize the global planner
    try {
      planner_ = bgp_loader_.createInstance(global_planner);
      planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
    } catch (const pluginlib::PluginlibException& ex) {
      ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
      exit(1);
    }
    //create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
    controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
    controller_costmap_ros_->pause();
    //create a local planner
    try {
      tc_ = blp_loader_.createInstance(local_planner);
      ROS_INFO("Created local_planner %s", local_planner.c_str());
      tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
    } catch (const pluginlib::PluginlibException& ex) {
      ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());
      exit(1);
    }
    // Start actively updating costmaps based on sensor data
    planner_costmap_ros_->start();
    controller_costmap_ros_->start();

在costmap2DROS中初始化对象layered_costmap_，然后向其中添加plugin:layered_costmap_->addPlugin(plugin)

  // costmap_2d/src/costmap_2d_ros.cpp
  layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);  
  if (!private_nh.hasParam("plugins"))   // load the default configure
  { //函数 最后一行:nh.setParam("plugins",super_array); super_array是构造的默认plugin
    //函数 起始一行:ROS_INFO("Loading from pre-hydro paramter sytle");
    //也可以看出navigation的设计，是plugin的一种方式
    resetOldParameters(private_nh);      
  }
  if (private_nh.hasParam("plugins"))    // load the plugins
  {
    XmlRpc::XmlRpcValue my_list;
    private_nh.getParam("plugins", my_list);
    for (int32_t i = 0; i < my_list.size(); ++i)
    {
      std::string pname = static_cast<std::string>(my_list[i]["name"]);
      std::string type = static_cast<std::string>(my_list[i]["type"]);
      ROS_INFO("Using plugin \"%s\"", pname.c_str());
      boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type);
      layered_costmap_->addPlugin(plugin);
      plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
    }
  }

完成每一个plugin的初始化之后，每一个地图的updateMap中先后执行updateBounds,updataCosts,updateBounds：这个阶段会更新每个Layer的更新区域，这样在每个运行周期内减少了数据拷贝的操作时间。StaticLayer的Static map只在第一次做更新，Bounds 范围是整张Map的大小，而且在UpdateBounds过程中没有对Static Map层的数据做过任何的更新。ObstacleLayer在这个阶段主要的操作是更新Obstacles Map层的数据，然后更新Bounds。InflationLayer则保持上一次的Bounds。 updateCosts：这个阶段将各层数据逐一拷贝到Master Map，上图中展示了Master Map的产生过程。

void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw);
  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
       ++plugin)
  {
    ………… 
    (*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(……);
    }
  }
  …………
  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)
  {
    (*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
  }
  …………
  initialized_ = true;

运用plugins
plugins在costmap中的运用ROS中有介绍Configuring Layered Costmaps,也可以添加一些新的plugin对应新的costmap，比如navigation_layers。

plugins: 
    - {name: static_map,       type: "costmap_2d::StaticLayer"}
    - {name: obstacles,        type: "costmap_2d::VoxelLayer"}
publish_frequency: 1.0
obstacles:
    observation_sources: base_scan
    base_scan: {data_type: LaserScan, sensor_frame: /base_laser_link, clearing: true, marking: true, topic: /base_scan}