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