- index/size量。它们都是一个整数。index量：在计算矩形行/列时，它表示的行、列都是有效的，最大值是长度减1，或高度减1。size量：表示尺寸，像矩形长度、高度。
生成代价地图 ---> 寻找最佳路径(findBestPath)
一、Costmap2D
Costmap2D封装了一个映射，映射是从world坐标系中的点坐标到该点的代价。具体来说，映射的数据结构是一个uint8_t数组costmap_，单元值uin8_t表示代价，索引则计算自该点在world坐标系的坐标。
Costmap2D只是封装了这个映射，要管理这映射，像修改costmap_尺寸，则是LayeredCostmap。
Costmap2D使用场景分为两处。
- 各costmap层从它派生，像静态层、障碍层。各种层用它存储自个层的私有成价。在updateCosts时，这私有代价会提交到最终代价，即LayeredCostmap.costmap_。注意，膨胀层没有继承Costmap2D，和静态、障碍地图这两层不同，它没有属于自己的栅格地图要维护，它是在updateCosts直接修改主代价地图。
- 作为LayeredCostmap的一个成员变量costmap_。这个costmap_是代价地图生成模块提交给下层模块的最终代价地图。
二、LayeredCostmap
2.1 LayeredCostmap::updateMap
Costmap2DROS会新建一个线程mapUpdateLoop，该线程以1秒为间隔，周期性调用LayeredCostmap::updateMap。1秒这值来自动态配置参数config.update_frequency。
- robot_x, robot_y, robot_yaw：代价地图必须关联一个坐标系，记为global，这三个变量是机器人中心点(base_footprint坐标系原点)现在在global坐标系下的位姿。
void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
{
// Lock for the remainder of this function, some plugins (e.g. VoxelLayer)
// implement thread unsafe updateBounds() functions.
boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
// if we're using a rolling buffer costmap... we need to update the origin using the robot's position
if (rolling_window_)
{
double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2;
double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2;
costmap_.updateOrigin(new_origin_x, new_origin_y);这是一个可滚动代价地图，先调用updateOrigin。updat两个作用。什么是可滚动地图？地图是动态了，随着时间推移，地图上值会发生变化。
一是重新计算原点origin_x_/y_，方法是微调new_origin_x、new_origin_y，确保新原点坐标能被resolution_(0.05)整除。
二是生成新的区域地图，新地图尺寸不变，前后两次重叠区域保持原值，其它部分则填上默认值default_value_。
}
if (plugins_.size() == 0)
return;
minx_ = miny_ = 1e30;
maxx_ = maxy_ = -1e30;
for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
if(!(*plugin)->isEnabled())
continue;
double prev_minx = minx_;
double prev_miny = miny_;
double prev_maxx = maxx_;
double prev_maxy = maxy_;
(*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);调用各层的updateBounds虚方法，得到此次时间片的代价地图边界。
minx_、miny_、maxx_、maxy_值基于world坐标系，它们都是index量。以下是虚方法updateBounds一般要做的工作。
- 从上次updateBounds后、到此次updateBounds之间，生成变化了的区域的私有代价地图。
- 把这个变化区域边界赋值给返回值minx_, miny_, maxx_, maxy_。注意maxx_、maxy_是index量。如果间隔中就没变化，即不改这个4个变量即可。
if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
{上面层只会保持和扩大区域，不会缩小。
ROS_WARN_THROTTLE(1.0, "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but " "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s", prev_minx, prev_miny, prev_maxx , prev_maxy, minx_, miny_, maxx_ , maxy_, (*plugin)->getName().c_str()); } } int x0, xn, y0, yn; costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0); costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn);
worldToMapEnforceBounds用于把world坐标转换为map坐标，会保征传下的world坐标都被转换。对x不在区域内的，如果小于最小值，就取最小值0，如果大于最大值，那就取最大值size_x - 1。y方向类似计算。
x0 = std::max(0, x0); xn = std::min(int(costmap_.getSizeInCellsX()), xn + 1); y0 = std::max(0, y0); yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);
{0, 0, costmap_.getSizeInCellsX(), costmap_.getSizeInCellsY()}是代价地图矩形，边界不可能超出这个形。上面worldToMapEnforceBounds会保证得到的x0, xn, y0, yn在区域地图内，不知这个是否还有必要？
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);把主代价地图中，{x0, y0, xn, yn}区域内点的代价复位为default_value_。这因为底下要对这部分点进行更新，有些点可能是所有层都不会碰到的，这些点保持着default_value_。注意，这里的xn、yn是size量。
for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
if((*plugin)->isEnabled())
(*plugin)->updateCosts(costmap_, x0, y0, xn, yn);更新主代价地图中，{x0, y0, xn, yn}区域内点的代价。注意，这里的xn、yn是size量。以下是虚方法updateCosts一般要做的工作。
- 把私有代价地合成到主代价地图。对怎么合成，每个层有各自定义，一般分为两种。一是覆盖，即把层的代价做为主代价地图的代码。二是取大值，即把私有代价、主地图代价的较大值作为最后填向主地图代价。
} bx0_ = x0; bxn_ = xn; by0_ = y0; byn_ = yn;
{bx0_, by0_, bxn_, byn_}存储最近一次更新过的地图区域。
initialized_ = true; }