障碍物地图（二）
上一篇大致看完了障碍物地图的初始化内容以及对于传感器数据的处理，我们知道在该部分算法维护了一个ObservationBuffer，其中存储了一段时间内的点云数据。每次新的数据进来后，还会根据设定的时间参数observation_keep_time抛弃比较久远的障碍物点云。但是在看的过程中，我们也产生了一些疑惑，比如ObservationBuffer本身维护的是一系列世界坐标系下的pointcloud，它跟栅格地图之间是如何结合的？又比如在维护ObservationBuffer时，每一帧点云都是通过Observation类函数完成的。在其中传入了当时点云产生时其传感器在世界坐标系下的位置origin，以及还有两个似乎一直没用过的参数obstacle_range与raytrace_range。带着这些疑惑我们重新回到obstacle_layer这个类函数中，继续学习其中剩下的内容。
1、updateBounds
在obstacle_layer类函数中，我们会发现它除了初始化与点云函数的订阅外，还有一个很重要的函数updateBounds。这个函数首先确定了是否需要更新地图的原点：
if (rolling_window_) updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
这行代码比较有意思，感觉它相当于是一个用来决定当前地图是否锁定机器人视角的参数，如果是的话地图的中心会始终随着机器人的位置变化。正常来说它应该是false，静态地图本身也不会随着机器人的位置变化而变化。
然后是获取了下地图的上下界：
useExtraBounds(min_x, min_y, max_x, max_y);
接下来的函数是获取了两个vector:
bool current = true; std::vector<Observation> observations, clearing_observations; // get the marking observations current = current && getMarkingObservations(observations); // get the clearing observations current = current && getClearingObservations(clearing_observations); // update the global current status current_ = current;
这里维护的是两个Observation类的数据。这两个之间咋一看似乎是互斥的，但是实际上它们的数据来源是一样的。为什么这么说呢，首先看一下getMarkingObservations这个函数，它的作用呢是返回marking_buffers_的值，这个buffer的值本身是在上一篇中初始化函数时执行的这么一个步骤：
// create an observation buffer observation_buffers_.push_back( boost::shared_ptr < ObservationBuffer > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_, sensor_frame, transform_tolerance))); // check if we'll add this buffer to our marking observation buffers if (marking) marking_buffers_.push_back(observation_buffers_.back()); // check if we'll also add this buffer to our clearing observation buffers if (clearing) clearing_buffers_.push_back(observation_buffers_.back());
在代码创建完成ObservationBuffer指针后，marking_buffers_实际上存储了该指针，所以操作marking_buffers_其实约等于操作了ObservationBuffer。因为marking_buffers_和 observation_buffers_共享了相同的ObservationBuffer 对象。注意到下面的clearing_buffers_也进行了相同的操作。
然后我们再回到上面的代码中，再看一下getClearingObservations这个函数，而这个函数其实也就是返回了clearing_buffers_中的所有点云值。所以说这两个函数其实殊涂同归，最终都是执行的ObservationBuffer::getObservations这个函数。那么为什么这里要获取两个相同的东西呢？继续往下看。
下面一个函数跟我们解释了clearing_observations中数据的使用方式
// raytrace freespace for (unsigned int i = 0; i < clearing_observations.size(); ++i) { raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y); }
这个地方调用了一个raytraceFreespace函数，这个函数的详细解释在第三部分，它的作用是清理出传感器到被测物体之间的栅格，设置为空闲。
在根据clearing_observations将传感器到点云之间的栅格清理完毕后，下一步自然是要操作这些障碍物点云了，所以这里就用到了observations的点云，算法在后面对其进行了遍历，取出每一帧点云，首先判断它到传感器之间的距离，注意到这里用到了一个参数obstacle_range_，这个上一章中提到Observations类时提到了，但是当时没有使用到，后面在这里我们发现它被使用了，它的作用是过滤掉那些点云距离传感器原点超过该值的点。然后对剩下的点求取其在地图下的栅格位置index，最后将该栅格的值设置为LETHAL_OBSTACLE（254：障碍物）。
// place the new obstacles into a priority queue... each with a priority of zero to begin with for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it) { const Observation& obs = *it; const sensor_msgs::PointCloud2& cloud = *(obs.cloud_); double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_; sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x"); sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y"); sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z"); for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { double px = *iter_x, py = *iter_y, pz = *iter_z; // if the obstacle is too high or too far away from the robot we won't add it if (pz > max_obstacle_height_) { ROS_DEBUG("The point is too high"); continue; } // compute the squared distance from the hitpoint to the pointcloud's origin double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y) + (pz - obs.origin_.z) * (pz - obs.origin_.z); // if the point is far enough away... we won't consider it if (sq_dist >= sq_obstacle_range) { ROS_DEBUG("The point is too far away"); continue; } // now we need to compute the map coordinates for the observation unsigned int mx, my; if (!worldToMap(px, py, mx, my)) { ROS_DEBUG("Computing map coords failed"); continue; } unsigned int index = getIndex(mx, my); costmap_[index] = LETHAL_OBSTACLE; touch(px, py, min_x, min_y, max_x, max_y); } }
注意到后面使用到的
costmap_[index] = LETHAL_OBSTACLE;
这里已经是在处理地图层面了，本身updateBounds这个函数就是在LayeredCostmap::updateMap中被调用，而这个函数的作用也就是为了更新地图。因此，这里就将障碍物点云与地图信息结合起来了。
这个函数的最后一步，是计算了机器人所占的栅格位置：
updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
函数本身不长，但是涉及到一些细节问题，具体函数实现参看第四部分。自此该函数结束。
2、updateCosts
这个函数是障碍物地图中除了updateBounds外最重要的一个函数了，它的作用是将机器人足迹范围内设置为FREE_SPACE，并且在bound范围内将本层障碍地图的内容合并到主地图上。
这个函数一共就两部分，首先看一下它如何将机器人足迹范围内设置为FREE_SPACE：
if (footprint_clearing_enabled_) { setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE); }
它本身调用的是Costmap2D下的setConvexPolygonCost函数，这个函数需要传递两个参数：足迹所在的位置以及空闲的栅格值。
首先，函数对传入的机器人多边形进行位置判断，判断它是否在地图上：
std::vector<MapLocation> map_polygon; for (unsigned int i = 0; i < polygon.size(); ++i) { MapLocation loc; if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y)) { // ("Polygon lies outside map bounds, so we can't fill it"); return false; } map_polygon.push_back(loc); }
对于在地图上的点，它们会被存储到一个map_polygon这么一个容器中，然后下一步调用convexFillCells函数，该函数通过机器人顶点坐标数组map_polygon得到多边形边缘及内部的全部cell，存放在polygon_cells中：
// get the cells that fill the polygon convexFillCells(map_polygon, polygon_cells);
碍于篇幅就不展开了，知道是这么个功能就行，然后下一步则是对这些栅格进行赋值：
// set the cost of those cells for (unsigned int i = 0; i < polygon_cells.size(); ++i) { unsigned int index = getIndex(polygon_cells[i].x, polygon_cells[i].y); costmap_[index] = cost_value; }
可以看到这里对于所有栅格在机器人多边形包含范围内的数据，其概率值都被设置为0了，也就是空闲。这个就是这个函数的功能与具体执行过程。
然后回到该节开始部分，我们提到updateCosts执行了两个功能，一个是将机器人足迹范围内设置为FREE_SPACE，剩下的是将本层障碍地图的内容合并到主地图上，第一个已经讲完，看一下第二部分：
switch (combination_method_) { case 0: // Overwrite updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); break; case 1: // Maximum updateWithMax(master_grid, min_i, min_j, max_i, max_j); break; default: // Nothing break; }
这边是一个switch语句，根据combination_method_决定使用的是哪个函数，这个参数默认是0，所以这里我们简单看一下updateWithOverwrite函数。这个函数本身是定义在CostmapLayer类中的，它的功能其实很简单，就是赋值：
void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { if (!enabled_) return; unsigned char* master = master_grid.getCharMap(); unsigned int span = master_grid.getSizeInCellsX(); for (int j = min_j; j < max_j; j++) { unsigned int it = span*j+min_i; for (int i = min_i; i < max_i; i++) { if (costmap_[it] != NO_INFORMATION) master[it] = costmap_[it]; it++; } } }
这个函数相当于是对地图所有栅格执行了一次遍历，然后对于costmap_这张地图中栅格状态已知的点进行赋值给master_grid。这里的master_grid是updateCosts函数的传参，来自于LayeredCostmap这个类的私有变量：
private: //这是一个普通的`Costmap2D`类型的对象定义。它是通过值传递的方式进行操作，即在创建副本时对原始对象进行复制。因此，对该变量的修改不会影响到原始对象。 Costmap2D costmap_;
{ if((*plugin)->isEnabled()) (*plugin)->updateCosts(costmap_, x0, y0, xn, yn); }
所以这张地图应该是维护的全局地图？而函数中的costmap_则是来自于Costmap2D类：
//`const Costmap2D& costmap_`：这是一个引用类型的常量变量定义。它引用了一个`Costmap2D`类型的对象，并且该对象在定义时被声明为常量，即不可修改。 //通过引用，对该变量的任何修改都会直接影响到原始对象。 unsigned char* costmap_;
这里涉及到一些非三张地图函数的调用，仅看着一块感觉不是很清楚。我的理解是：Costmap2D类中维护的是一张临时性的地图，操作障碍物点云时存储的是这里，而LayeredCostmap这里则是实际对外使用的地图，例如全局路径规划，所以在LayeredCostmap中会需要调用到ObstacleLayer下的updateCosts函数，将局部的障碍物添加到全局地图中。
3、raytraceFreespace
这个函数的作用是清理出传感器到被测物体之间的栅格，设置为空闲。具体过程如下：
首先该函数获取了一帧数据中的origin参数，这个参数在上一篇中我们知道它代表的是当时记录数据时传感器在地图坐标系下的位置，以及设置了一个指针指向了点云：
double ox = clearing_observation.origin_.x; double oy = clearing_observation.origin_.y; const sensor_msgs::PointCloud2 &cloud = *(clearing_observation.cloud_);
然后计算了这个坐标点是否在地图上，对于静态地图而言，其实应该用不着这步，能记录下来的基本肯定在地图里面：
// get the map coordinates of the origin of the sensor unsigned int x0, y0; if (!worldToMap(ox, oy, x0, y0)) { ROS_WARN_THROTTLE( 1.0, "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.", ox, oy); return; }
然后还是一个范围限定函数：
// 保证传感器原点在bound范围内 touch(ox, oy, min_x, min_y, max_x, max_y);
这个函数的作用感觉不是很明确，它同样是限定了传感器原点，但是又没有给出超过这个范围该怎么处理，感觉可有可无。
接下来，正餐开始！
前面一些乱七八糟的前奏处理完了，接下来自然是开始处理实际的点云数据。
我们首先从点云中遍历每一个点，拿到它的世界坐标系下的坐标，同时拿到传感器的位置，然后XY轴的距离：
//对于点云中的每个点，我们要追踪原点和clear obstacles之间的一条线 for (; iter_x != iter_x.end(); ++iter_x, ++iter_y) { //wx wy是当前点云中的点的坐标 double wx = *iter_x; double wy = *iter_y; // now we also need to make sure that the enpoint we're raytracing // to isn't off the costmap and scale if necessary //a、b是该点跟传感器原点的距离 double a = wx - ox; double b = wy - oy;
接下来的这一步，是为了对那些超出地图的点云的处理：
// the minimum value to raytrace from is the origin //如果当前点x方向比地图原点还小 if (wx < origin_x) { //t（比例）=（地图原点-传感器原点）/（点云中的该点-传感器原点） double t = (origin_x - ox) / a; //当前点x = 地图原点x wx = origin_x; //当前点y = //实际上还是把点云点和传感器连线之间清空，只是通过相似三角形丢弃了超出地图原点范围外的部分，下面三个判断结构同理 wy = oy + b * t; } if (wy < origin_y) { double t = (origin_y - oy) / b; wx = ox + a * t; wy = origin_y; }
这部分该怎么理解呢，在看栅格地图这一章节的时候，我们知道了地图的原点实际上是在地图的左下角的。所以对于一个栅格而言，它的XY的值其实应该是一定大于地图原点的。所以对于任何一个方向小于地图原点的点，它本身其实都是超出了地图的。而对于这些点，这里并没有直接丢弃掉，而是根据相似三角形原理截取了它在地图范围内的一部分，这就是上述部分代码的原理。
当然，判断了XY很小的值，同样也需要判断一下超过上限的部分，原理相同，不重复叙述：
// the maximum value to raytrace to is the end of the map if (wx > map_end_x) { double t = (map_end_x - ox) / a; wx = map_end_x - .001; wy = oy + b * t; } if (wy > map_end_y) { double t = (map_end_y - oy) / b; wx = ox + a * t; wy = map_end_y - .001; }
然后，我们在这里设置了一个参数：
unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
这个参数的来源是最开始我们初始化Observation时传递的raytrace_range参数，这里根据分辨率转换出来了一个栅格距离，也就是它大概经过了多少个栅格。
这个距离是干嘛的呢？它其实是用于清理传感器原点和障碍物点之间的cell的：
MarkCell marker(costmap_, FREE_SPACE);
raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);
updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
这里首先调用了一个MarkCell，它需要两个参数，一个是地图的原始数据costmap，一个是某一个栅格的值value。同时MarkCell维护了一个operator重载函数，它只有一个作用，对某一个栅格赋值：
inline void operator()(unsigned int offset) { costmap_[offset] = value_; }
然后是调用了两个不同的函数，第一个函数是画出了每一个点云到传感器原点之间的连线所经过的栅格，底层使用的是bresenham算法，关于这个算法的原理可以参考《【计算机图形学】画线算法——Bresenham算法(任意斜率)》一文，这里不展开详细叙述了。重要的是在这里面对每一个起点与终点之间的栅格调用了重载函数，也就是经过这种方式将所有传感器原点到障碍物之间的点云都转化成了空闲的状态。而updateRaytraceBounds函数跟前面提到的touch函数比较类似，只是上面限定的是传感器的原点，这里限制的是画线的点。没太看懂有啥用。到此raytraceFreespace函数结束。
4、updateFootprint
这个函数内容不多，就是几行代码：
//它的作用是基于机器人当前位置确定该位置下的足迹 void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y) { if (!footprint_clearing_enabled_) return; transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_); for (unsigned int i = 0; i < transformed_footprint_.size(); i++) { touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y); } }
单独摘出来讲主要是为了理一下其中使用到的transformFootprint函数与getFootprint函数。首先我们可以先看一下getFootprint这个函数，它本身是返回的一组点：
const std::vector<geometry_msgs::Point>& Layer::getFootprint() const { return layered_costmap_->getFootprint(); }
/** @brief Returns the latest footprint stored with setFootprint(). */ const std::vector<geometry_msgs::Point>& getFootprint() { return footprint_; }
在layered_costmap_中footprint_定义为：
std::vector<geometry_msgs::Point> footprint_;
可以看到它是一组点。其实这组点代表的是机器人的外轮廓。对于多边形机器人而言，自然是好理解的，主要问题在于如何处理圆形呢？这部分主要在Costmap2DROS这个类中进行了处理，在函数初始化时，调用了一个setUnpaddedRobotFootprint函数，这个函数负责存储footprint_多边形，在机器人类型为圆形时，其传参时调用了makeFootprintFromRadius函数：
setUnpaddedRobotFootprint(makeFootprintFromRadius(new_config.robot_radius));
而makeFootprintFromRadius函数的主要工作就是根据圆的半径生成出16个圆上的点：
std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius) { std::vector<geometry_msgs::Point> points; // Loop over 16 angles around a circle making a point each time int N = 16; geometry_msgs::Point pt; for (int i = 0; i < N; ++i) { double angle = i * 2 * M_PI / N; pt.x = cos(angle) * radius; pt.y = sin(angle) * radius; points.push_back(pt); } return points; }
因此，对于圆形的机器人，其footprint_是使用的16个圆周上的点表示的。这就是getFootprint函数中足迹的来源。
然后我们再看一下transformFootprint函数，它对于这16个点做了一个操作：
void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec, std::vector<geometry_msgs::Point>& oriented_footprint) { // build the oriented footprint at a given location oriented_footprint.clear(); double cos_th = cos(theta); double sin_th = sin(theta); 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); } }
对于这块比较敏感的人其实应该一眼能看出来了哈，这个函数其实本身就是实现了点的转换。将原来的16个点投影到了地图上。因为最原始的足迹我们可以看到它是以机器人为中心的16个坐标点，但是通过根据机器人在世界坐标系下的位置与角度关系，我们就将这16个点转换到了世界坐标系下。而这个，就是updateFootprint函数的主要工作。
总结
通过对上述几个函数的简单翻阅，我们大致理解了障碍物地图中上一节遇到的一些问题，例如ObservationBuffer本身维护的是一系列世界坐标系下的pointcloud，它跟栅格地图之间是如何结合的？又比如在维护ObservationBuffer时，每一帧点云都是通过Observation类函数完成的。在其中传入了当时点云产生时其传感器在世界坐标系下的位置origin，以及还有两个似乎一直没用过的参数obstacle_range与raytrace_range。这些在这部分函数中都得到了一定的解答，也因此对算法本身有了一个更加深刻的认知。总体来说这部分代码大概实现的所有东西就在这两节内容中了，唯一一个让我目前还比较疑惑的地方是：当时静态地图初始化时获取的全局地图属于LayeredCostmap类，与这里的第二节更新的地图似乎是同一个？但总感觉应该不是，这里有个地方应该漏掉了，做个笔记，后面如果看到了再补充。
参考：
《【ROS-Navigation】Costmap2D代价地图源码解读-障碍层ObstacleLayer》
《障碍层ObstacleLayer》
