ROS Navigation的costmap_2d类继承关系与实现方法

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.0obstacles:    observation_sources: base_scan    base_scan: {data_type: LaserScan, sensor_frame: /base_laser_link, clearing: true, marking: true, topic: /base_scan}