- /map坐标系。建图、导航时，相应node发布的tf基坐标系（rviz中Fixed Frame）。加上前缀“/”，是为区分这里要说的map坐标系。
- rolling_window。代价地图的一个参数，表示该代价地图的world坐标系原点(origin_x_, origin_y_)是否要随着机器人移动而改变。换句话说，world坐标系原点要时刻“跟随”机器人中心，并保持着一个固定偏移(-getSizeInMetersX() / 2, -getSizeInMetersY() / 2)。rolling_window是true时，一旦机器人移动了，要以机器人中心坐标为参数调用updateOrigin修改出代价地图原点。对全局代价地图，world坐标系原点是机器人起始点，因而值是false。对局部代价地图，是一个以机器人中心为中心、半径1.5米的矩形，因而值是true。
- updateOrigin执行两个任务。一是重新计算原点origin_x_/y_，方法是微调参数new_origin_x、new_origin_y，确保新原点坐标能被resolution_(0.05)整除。二是生成新的区域地图，新地图尺寸不变，前后两次重叠区域保持原值，其它部分则填上默认值default_value_。
world坐标系
不论方向还是数值，world坐标系就是数学上的直角坐标系。只是ros用它来表示真实世界地图，像nav_msgs::OccupancyGrid中的栅格地图，Costmap2D中的区域地图，于是有了地图这层物理意义。图1中的坐标系就是world坐标系。
- world坐标系原点一般和/map坐标系的原点重合，对应真实世界中机器人中心在开始时刻落在的那个位置。
- world坐标值是此刻机器人中心点距离初始位置的偏移。机器人向东移动，x坐标向右，值变大；机器人向南移动，y坐标向下，坐标绝对值变大，但值是变小。其它方向同理。
- world坐标单元往往叫栅格，一个单元表示着“resolution x resolution”的真实世界面积。resolution默认0.05，即每个栅格对应“0.05mx0.05m”面积。
- world坐标单元中的值，往往和该栅格上存在障碍物的概率相关。
- 要由world坐标系生成地图图像。生成图像时，图像原点是在左下角，不是经常看到的左上角，像OpenCV。图像内存中第一个数据表示该地图西南角栅格的值，第二个数据是和该栅格相邻的东边栅格，存储完该行后，开始南边第二行的最西栅格，依此类推，一直到最北那一行。
- 地图图像原点在左下角，缺点是会增加理解难度，好处是在生成图像时，y可以“+1”增，符合真实世界中y向北表示增加。要画出图像时，须要上下颠倒，y往往要换成“(map.info.height - y - 1)”。
一、Costmap2D::updateOrigin
void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y) { // project the new origin into the grid int cell_ox, cell_oy; cell_ox = int((new_origin_x - origin_x_) / resolution_); cell_oy = int((new_origin_y - origin_y_) / resolution_); // Nothing to update if (cell_ox == 0 && cell_oy == 0) return; // compute the associated world coordinates for the origin cell // because we want to keep things grid-aligned // 任务一：微调new_origin_x、new_origin_y，确保新原点坐标能被resolution_(0.05)整除。 double new_grid_ox, new_grid_oy; new_grid_ox = origin_x_ + cell_ox * resolution_; new_grid_oy = origin_y_ + cell_oy * resolution_; // To save casting from unsigned int to int a bunch of times int size_x = size_x_; int size_y = size_y_; // we need to compute the overlap of the new and existing windows int lower_left_x, lower_left_y, upper_right_x, upper_right_y; lower_left_x = min(max(cell_ox, 0), size_x); lower_left_y = min(max(cell_oy, 0), size_y); upper_right_x = min(max(cell_ox + size_x, 0), size_x); upper_right_y = min(max(cell_oy + size_y, 0), size_y); unsigned int cell_size_x = upper_right_x - lower_left_x; unsigned int cell_size_y = upper_right_y - lower_left_y; // we need a map to store the obstacles in the window temporarily unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y]; // copy the local window in the costmap to the local map // 任务二：两个copyMapRegion以及restMaps，生成新的区域地图。 copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y); // now we'll set the costmap to be completely unknown if we track unknown space resetMaps(); // update the origin with the appropriate world coordinates origin_x_ = new_grid_ox; origin_y_ = new_grid_oy; // compute the starting cell location for copying data back in int start_x = lower_left_x - cell_ox; int start_y = lower_left_y - cell_oy; // now we want to copy the overlapping information back into the map, but in its new location copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y); // make sure to clean up delete[] local_map; }
使用场景是这样的，机器人发生了移动，中心点在world坐标系中的坐标变了，因为这改变，须要改变一些代价地图的实时区域。参数new_origin_x、new_origin_y指示此次机器人移动后希望的新区域原点。以下是一个调用示例。
void ObstacleLayer::updateBounds(double robot_x, double robot_y, ...) { ... if (rolling_window_) updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); }
从传入参数计算方法，可看出新区域原点是机器人中心的西南角、距半区域尺寸的位置。机器人中心和区域原点之间位置关系参考图1中绿色、紫色线条描述的轨迹。
updateOrigin主要两个任务，一是微调new_origin_x、new_origin_y，确保新原点坐标能被resolution_(0.05)整除。二是用两个copyMapRegion以及restMaps，生成新的区域地图。具体怎么个过程，图1给了个机器人中心变动了三次的示例，图2是各次移动时的相关变量值。
图1中蓝色矩形是初始化后Costmap2D中区域地图，origin_是(0, 0)，一直到这时其实没存在过机器人，按origin_反推机器人中心，大概在(1.45, 1.45)。接下过来第一次“移动”，此时机器人其实还没发生过移动，但中心点和/map坐标系原点重合，机器人中心点坐标要从(1.45, 1.45)变成(0, 0)，须要执行Costmap2D::updateOrigin，此个updateOrigin逻辑和处理真正有发生过移动是一样的。这里让深入看下怎么处理两个任务。
1.1 微调new_origin_x、new_origin_y，确保新原点坐标能被resolution_(0.05)整除
new_grid_ox = origin_x_ + cell_ox * resolution_; new_grid_oy = origin_y_ + cell_oy * resolution_;
cell_ox、cell_oy是前后两次origin、用“int”表示的偏差，计算时已经向下取整。于是原来原点加上由它乘以resolution_，就确保了新原点坐标能被resolution_整除。
1.2 用两个copyMapRegion以及restMaps，生成新的区域地图
- 第一个copyMapRegion把蓝色、红色交叉部分（白框）复制到新建的临时内存块local_map。复制时，蓝色地图图像是源，即costmap_，costmap_的跨距size_x_；local_map是目标，它的宽度等于源中要复制的宽度cell_size_x，因为每栅格值一个字节，这个宽度也等于跨距。理解copyMapRegion参数时，注意这是world坐标系，图像原点在左下角。
- 复制出重叠块后，用restMaps把整个costmap_置为默认值default_value_。
- 第二个copyMapRegion把暂存在local_map中的数据复制回costmap_，此时costmap_已指到红色块。复制时，local_map变成了源，costmap_变成了目标。
综上所述，经过updateOrigin后，前后两次重叠区域保持原值，其它部分则填上默认值default_value_。
二、map坐标系
图3中，黑色是world坐标系，绿色是map坐标系。map坐标系总是关联某个区域。
- map坐标系原点在该区域左下角坐标(origin_x_, origin_y_)附近。具体是在(origin_x_ + resolution/2, origin_x_ + resolution/2)。
- map坐标系坐标单元是栅格，只有整数才有意义。坐标值变化方向类似world坐标系，x坐标向右，值变大；y坐标向下，坐标绝对值变大，但值是变小。
- 由于一个栅格表示一个resolutin x resolution面积的真实区域，栅格坐标对应的world坐标是在该栅格的中心点坐标。在图3，蓝色块是map坐标系原点栅格，它的map坐标系坐标是(0, 0)，对应的world坐标系坐标是红色表示的(-1.425, -1.425)，它位在栅格中心。
- 虽然map坐标也可以是负数、很大的正数，但一般来说，只有在区域内的才有意义，即x范围是[0, size_x_-1]，y范围是[0, size_y_-1]。
- 在一次导航过程中，world坐标系只有一个，原点固定。map坐标则随着区域出现而出现。
知道map坐标系、world坐标系特点后，让看几个两标系间的坐标转换函数。
2.1 mapToWorld
void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const
{
wx = origin_x_ + (mx + 0.5) * resolution_;
wy = origin_y_ + (my + 0.5) * resolution_;
}mapToWorld用于把map坐标转换为world坐标。正如上面所说的，栅格坐标对应的world坐标是在该栅格的中心点坐标，“+0.5”目的是从左下角算出中心点。以图1为例。
(mx, my): (0, 0); (origin_x_, origin_y_): (-1.45, -1.45); resolution_: 0.05; ==> (wx, xy): (-1.425, -1.425)，即图1红色箭头标注的坐标。
2.2 worldToMap
bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const
{
if (wx < origin_x_ || wy < origin_y_)
return false;
mx = (int)((wx - origin_x_) / resolution_);
my = (int)((wy - origin_y_) / resolution_);
if (mx < size_x_ && my < size_y_)
return true;
return false;
}worldToMap用于把区域内world坐标转换为map坐标。理论上说，在world坐标系中，只有位在区域内world坐标算出的map坐标才有意义，因而在起始点、尺寸方向作了滤除。由于做了滤除，该函数只会计算区域内的map坐标。该函数同时具有判断该world坐标是否在区域内功能，true表示在区域内，否则区域外。
2.3 worldToMapNoBounds
void Costmap2D::worldToMapNoBounds(double wx, double wy, int& mx, int& my) const
{
mx = (int)((wx - origin_x_) / resolution_);
my = (int)((wy - origin_y_) / resolution_);
}worldToMap用于把world坐标转换为map坐标。正如函数名所写，它和worldToMap区别就是不受区域限制。
2.3 worldToMapEnforceBounds
void Costmap2D::worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) const
{
if (wx < origin_x_) {
// 区域内x最小map坐标：0
mx = 0;
} else if (wx > resolution_ * size_x_ + origin_x_) {
// 区域内x最大map坐标：size_x_ - 1
mx = size_x_ - 1;
} else {
mx = (int)((wx - origin_x_) / resolution_);
}
...
}worldToMapEnforceBounds用于把world坐标转换为map坐标，会保征传下的world坐标都被转换。对x不在区域内的，如果小于最小值，就取最小值0，如果大于最大值，那就取最大值size_x - 1。y方向类似计算。
三、WorldModel::footprintCost
WorldModel是个工具类，它的footprintCost方法提供一种把机器人中心放到特定坐标(x, y)，并且转特定角度(theat)后，计算机器人足迹代价的方法。它只约定返回值意义，对如何计算代价，由派生类自个决定。
- >=0时，值是什么意义由派生类决定。像派生类CostmapModel，值表示机器人这个凸边形，N条边涉及到栅格中最大的cost值。
- 是负数，只能出现下面三种。
- -1。足迹中至少含有一个LETHAL_OBSTACLE(254)的栅格。
- -2。足迹中至少含有一个LETHAL_OBSTACLE(NO_INFORMATION(255))的栅格。
- -3。足迹中至少有一个栅格落在代价地图外面。
<libros>/include/base_local_planner/world_model.h ------ class WorldModel{ /** * @brief Subclass will implement this method to check a footprint at a given position and orientation for legality * in the world * @param position The position of the robot in world coordinates * @param footprint The specification of the footprint of the robot in world coordinates * @param inscribed_radius The radius of the inscribed circle of the robot * @param circumscribed_radius The radius of the circumscribed circle of the robot * @return Positive if all the points lie outside the footprint, negative otherwise: * -1 if footprint covers at least a lethal obstacle cell, or * -2 if footprint covers at least a no-information cell, or * -3 if footprint is partially or totally outside of the map */ virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint, double inscribed_radius, double circumscribed_radius) = 0; double footprintCost(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec, double inscribed_radius = 0.0, double circumscribed_radius=0.0){
WorldModel希望caller首先调用这函数，由它再调用上面的纯虚footprintCost。
double cos_th = cos(theta); double sin_th = sin(theta); std::vector<geometry_msgs::Point> oriented_footprint; for(unsigned int i = 0; i < footprint_spec.size(); ++i){ geometry_msgs::Point new_pt; new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th); new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th); oriented_footprint.push_back(new_pt); }
oriented_footprint存储着把机器人中心放在(x, y)，并且角度是th后，size()个凸点在x、y所属的坐标系下的坐标。costmap_2d::transformFootprint(...)有着一样代码。
geometry_msgs::Point robot_position; robot_position.x = x; robot_position.y = y; if(inscribed_radius==0.0){ costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius, circumscribed_radius); inscribed_radius等于0，指示footprintCost要负责计算内切圆、外切圆半径。 } return footprintCost(robot_position, oriented_footprint, inscribed_radius, circumscribed_radius);
调用派生类的footprintCost(const geometry_msgs::Point& position, ...)，该函数的返回值就是本函数的返回值。
} };
3.1 CostmapModel::footprintCost
为提高效率，它不计算足迹所有栅格的代价，认为只要计算N条边涉及到的栅格，就可计算出最大代价。注：它不计算中心点(x, y)栅格的代价。返回值中，最大值是253(INSCRIBED_INFLATED_OBSTACLE)，因为比253大的LETHAL_OBSTACLE(254)、NO_INFORMATION(255)会分别返回-1、-2，表示不可行。
对于world_model->footprintCost的计算，是对footprint中的每个线段进行独立计算代价的。首先尝试获取线段的地图坐标，如果获取失败则返回-1。然后，计算利用lineCost函数计算线段代价值，并用footprint_cost参数保存最大的线段代价值，最终如果正常，则返回的也是footprint_cost。但是，如果某个线段的代价值小于0，则world_model->footprintCost直接返回-1。
对于线段的代价计算函数lineCost，其实是对线段中的所有像素点（通过bresenham算法得到）进行代价计算，如果像素点的代价为LETHAL_OBSTACLE或者NO_INFORMATION，则该点代价为-1。如果某个点的代价为-1，则该线段的代价也为-1，否则则取所有点中最大的代价作为线段的代价。
四、显示地图，内用坐标和显示坐标
引入“内用”这个词，就是为区分“显示”，“显示”是展示出来给人看，“内用”就是看不到的条种内部计算。以下是图4中蓝色路径对应的栅格。
[0/38](-0.025, 0.024) --> (28, 29) [1/38](0.024, -0.025) --> (29, 28) [2/38](0.074, -0.075) --> (30, 27) [3/38](0.124, -0.124) --> (31, 26) [4/38](0.174, -0.075) --> (32, 27) ... [33/38](0.975, 0.924) --> (48, 47) [34/38](1.024, 0.975) --> (49, 48) [35/38](1.024, 1.024) --> (49, 49) [36/38](0.975, 1.075) --> (48, 50) [37/38](0.924, 1.124) --> (47, 51)
对内用坐标和显示坐标，x一样，y相互颠倒。当一个是y时，另一个是”height - 1 -y“。
world系和map坐标都属内用坐标。要理解内用坐标，直观就想map坐标系，坐标系原点在左下角。除要显示外，涉及到的计算都以着内用坐标规则去使用。
显示坐标系原点在左上角，这是要和电脑屏一致。对图4，由于内用坐标是越下面值越小，而显示时y是颠倒的，有人会认为要显示左侧y刻度值应该是越下值值越大，这是错误的。左侧y刻度值变化方向和内用一样。