背景

公司 AVP demo 需要把 Tof 得到的点云放到 costmap 里避障用，但是出现了“残影”， 研究了一下应该是 raytrace 部分出现了问题，ROS 自带的 obtacle layer 里的 raytrace 不适合我们的 Tof，需要自己弄一个插件，又重新看了一下 navigation 的代码。

继承关系

move_base 里会有一个成员
costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;
这是一个 ROS 节点，这个节点的主要工作就是维护这个成员
LayeredCostmap* layered_costmap_;
而 LayeredCostmap 里有一个成员，用来装各种插件
std::vector > plugins_;
这些插件用来更新这个成员
Costmap2D costmap_;
这个就是我们最后要用到的代价地图
接下啦就是看每一个插件是怎么运行的, 这里以 obstaclelayer 为例
class ObstacleLayer : public CostmapLayer
他是继承自 CostmapLayer, ros 自己带的几个插件都是继承自这个类，这个类又继承自两个东西
class CostmapLayer : public Layer, public Costmap2D
其中 layer 规定了层的行为， Costmap2D 是存数据的

调用关系

Costmap2DROS 实例化以后会跑一个线程 mapUpdateLoop，这个线程里不断执行 updateMap() 这个函数
这个函数会首先更新机器人的位姿，然后调用 layered_costmap_ 的 updateMap
layered_costmap_ 里的 updateMap 就是遍历所有插件，
首先进行 updateBounds
这个过程中每个插件都会计算出自己要更新的 grid 的值（有的还会进行 raytrace 之类的操作）。和要更新的范围(不是整个地图都要更新，只更新一块 min max xy)
之后对综合所有插件得到的需要更新的 bound 里的代价值进行更新 updateCosts
把上一步里插件算出来的值更新到 costmap_ 成员里， 这里还可以选择更新方式，例如overwrite等，这些都是可以自定义的

修改

之前的雷达更新逻辑是观测数据末端到观测点连线之间的所有 grid 都会被置为 FREE, 但是障碍物移动出我们的 Tof 的 FOV 以后就没有观测数据了，就没法 raytrace 了。
这里直接把这个 raytrace 改成 FOV 的左右两条线和 localcostmap 外围框的围成的区域都 raytrace 掉， 然后在把当前帧看到的物体放到代价地图里。
比较绕的就是怎么算这个区域，自己整理了一个 FOV 小于 90° 时可以用的方法。
代码里用了许多 magic number，根据自己情况修改。

void TofObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,double* max_x, double* max_y, double robot_yaw)
{double ox = clearing_observation.origin_.x;double oy = clearing_observation.origin_.y;  pcl::PointCloud < pcl::PointXYZ > cloud = *(clearing_observation.cloud_);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;}double origin_x = origin_x_, origin_y = origin_y_;double map_end_x = origin_x + size_x_ * resolution_;double map_end_y = origin_y + size_y_ * resolution_;touch(ox, oy, min_x, min_y, max_x, max_y);// for each point in the cloud, we want to trace a line from the origin and clear obstacles along it// printf("getting points num: %d\n", cloud.points.size());// for (unsigned int i = 0; i < cloud.points.size(); ++i)// {//   // 世界坐标//   double wx = cloud.points[i].x;//   double wy = cloud.points[i].y;//   // now we also need to make sure that the enpoint we're raytracing//   // to isn't off the costmap and scale if necessary//   double a = wx - ox; // 点到原点差值//   double b = wy - oy;//   // the minimum value to raytrace from is the origin//   // 这里就是把地图外的点截到地图边界上//   if (wx < origin_x) // origin-x 应该是局部地图的起点（世界坐标）//   {//     double t = (origin_x - ox) / a;//     wx = origin_x;//     wy = oy + b * t;//   }//   if (wy < origin_y)//   {//     double t = (origin_y - oy) / b;//     wx = ox + a * t;//     wy = origin_y;//   }//   // 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;//   }//   // now that the vector is scaled correctly... we'll get the map coordinates of its endpoint//   unsigned int x1, y1;//   // check for legality just in case//   if (!worldToMap(wx, wy, x1, y1)) // 这里是要知道到 map 是局部的还是全局的//     continue;unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);MarkCell marker(costmap_, FREE_SPACE);// and finally... we can execute our trace to clear obstacles along that line// raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); // 传感器原点x0 y0, 到观测点 x1 y1，进行 raytrace，都是地图坐标(格子)//   updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);// }// 1. 根据 yaw 算出两个交点// float half_fov = horizontal_fov_ / 2;float left_angle = robot_yaw + horizontal_fov_ / 2;float right_angle = robot_yaw - horizontal_fov_ / 2;// 算出left的交点，我的地图用的是 60x60， 29 + 31 = 60， 左下角是 （-29，-29） 右上角是 （31,31）float x1_f, y1_f;float left_line_a = tan(left_angle);if(left_angle > 0.785 && left_angle <2.355){ // case 1 pi/4 至 pi 3/4 与 y = 31 相交x1_f = 31 / left_line_a;y1_f = 31;}else if(left_angle > -0.785 && left_angle < 0.785){ // case 2 -pi/4 至 +pi/4  与 x = 31 相交x1_f = 31;y1_f = 31 * left_line_a;}else if(left_angle < -0.785 && left_angle > -2.355){ // case3  -pi/4 至 -3pi/4 与 y = -29 相交x1_f = -29 / left_angle;y1_f = -29;}else if(left_angle > 2.355 || left_angle < -2.355){// 因为 yaw 计数规则，这个象限被拆开了，与 x = -29 相交 x1_f = -29;y1_f = -29 * left_line_a;}// (注意要平移 + 29)x1_f = x1_f + 29;y1_f = y1_f + 29;unsigned int x1 = (unsigned int)x1_f;unsigned int y1 = (unsigned int)y1_f;// 机器人右边 FOV 边界，不管在哪里都是逆时针去 tracefloat x2_f, y2_f;float right_line_a = tan(right_angle);if(right_angle > 0.785 && right_angle <2.355){ // pi/4 至 pi 3/4 与 y = 31 相交x2_f = 31 / right_line_a;y2_f = 31;}else if(right_angle > -0.785 && right_angle < 0.785){ // -pi/4 至 +pi/4  与 x = 31 相交x2_f = 31 ;y2_f = 31 * right_line_a;}else if(right_angle < -0.785 && right_angle > -2.355){ // -pi/4 至 -3pi/4 与 y = -29 相交x2_f = -29 / right_line_a;y2_f = -29;}else if(right_angle > 2.355 ||  right_angle < -2.355){ // 与 x = -29 相交 x2_f = -29;y2_f = -29 * right_line_a;}// (注意要平移 + 29)x2_f = x2_f + 29;y2_f = y2_f + 29;unsigned int x2 = (unsigned int)((int)x2_f);unsigned int y2 = (unsigned int)y2_f;// 以left和哪条线相交作为判断起点switch (y1){case 60: // left 与上方边框相交switch (y2){case 60: // left right 都与上方边框相交 （y1 == 60 && y2 == 60）// 那么从 x1 trace 到 x2, 因为 x2 在 x1 顺时针方向, x2 大for(unsigned xi = x1; xi < x2; xi ++){raytraceLine(marker, x0, y0, xi, y1, cell_raytrace_range);// 这里还要计算出 xi,y1 对应的世界坐标，替换 wx,wydouble wx = origin_x +  xi * resolution_;double wy = origin_y +  y1 * resolution_;updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);}break;default: // left 与上方边框相交，right 与右侧边框相交 (y1 == 60 && x2 == 60)// Fov 分别与两条边框相交，分两段 trace// left 顺时针 trace x 到60for(unsigned xi = x1; xi < 60; xi ++){raytraceLine(marker, x0, y0, xi, y1, cell_raytrace_range);double wx = origin_x +  xi * resolution_;double wy = origin_y +  y1 * resolution_;updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);}// right 逆时针 trace 意味着 y 增到到 60for(unsigned yi = y2; yi < 60; yi++){raytraceLine(marker, x0, y0, x2, yi, cell_raytrace_range);double wx = origin_x +  x2 * resolution_;double wy = origin_y +  yi * resolution_;updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);} break;}break;case 0: // left 与下方边框相交switch (y2){case 0: // left right 都与下方边框相交 ( y1 == 0 && y2 == 0)// 那么从 x1 trace 到 x2, 因为 x2 在 x1 顺时针方向, x2 小for(unsigned xi = x2; xi < x1; xi ++){ // 下方水平线，从左侧端点 traceraytraceLine(marker, x0, y0, xi, y1, cell_raytrace_range);double wx = origin_x +  xi * resolution_;double wy = origin_y +  y1 * resolution_;updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);}break;default: // left 与下方边框相交， right 与左侧边框相交 ( y1 == 0 && x2 == 0)// 顺时针 trace 意味着 x 是减小到0的for(unsigned xi = 0; xi < x1; xi ++){ // 下方水平线，从左侧端点 traceraytraceLine(marker, x0, y0, xi, y1, cell_raytrace_range);double wx = origin_x +  xi * resolution_;double wy = origin_y +  y1 * resolution_;updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);}// 逆时针 trace 意味着 y 是减小到0的for(unsigned yi = 0; yi < y2 ; yi++){ // 左侧竖直线，从下往上 traceraytraceLine(marker, x0, y0, x2, yi, cell_raytrace_range);double wx = origin_x +  x2 * resolution_;double wy = origin_y +  yi * resolution_;updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);}break;}break;default: // left 与左右侧边框相交switch (x1){case 60: // left 与右侧边框相交switch (x2){case 60: // left right 都与右侧边框相交 (x1 == 60 && x2 == 60)for(unsigned yi = y2; yi < y1; yi++){raytraceLine(marker, x0, y0, x2, yi, cell_raytrace_range);double wx = origin_x +  x2 * resolution_;double wy = origin_y +  yi * resolution_;updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);}break;default: // left 与右侧边框相交，right 与下方边框相交 (x1 == 60 && y2 == 0)// Fov 分别与两条边框相交，分两段 trace// left 顺时针 trace y 到 0for(unsigned yi = 0; yi < y1 ; yi++){ raytraceLine(marker, x0, y0, x1, yi, cell_raytrace_range);double wx = origin_x +  x1 * resolution_;double wy = origin_y +  yi * resolution_;updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);}// right 逆时针 trace x 增到到 60for(unsigned xi = x2; xi < 60; xi ++){raytraceLine(marker, x0, y0, xi, y2, cell_raytrace_range);double wx = origin_x +  xi * resolution_;double wy = origin_y +  y2 * resolution_;updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);}break;}break;case 0: // left 与左侧边框相交switch (x2){case 0: // left right 都与左侧边框相交 (x1 == 0 && x2 == 0)for(unsigned yi = y1; yi < y2 ; yi++){ // 左侧竖直线，从下往上 traceraytraceLine(marker, x0, y0, x2, yi, cell_raytrace_range);double wx = origin_x +  x2 * resolution_;double wy = origin_y +  yi * resolution_;updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);}break;default: // left 与左侧边框相交, right 与上方边框相交 (x1 == 0 && y2 == 60)for(unsigned yi = y1; yi < 60 ; yi++){ // 下方水平线，从左侧端点 traceraytraceLine(marker, x0, y0, x1, yi, cell_raytrace_range);double wx = origin_x +  x1 * resolution_;double wy = origin_y +  yi * resolution_;updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);}// right 逆时针 trace  意味着 x 减小到 0for(unsigned xi = 0; xi < x2; xi ++){ // 左侧竖直线，从下往上 traceraytraceLine(marker, x0, y0, xi, y2, cell_raytrace_range);double wx = origin_x +  xi * resolution_;double wy = origin_y +  y2 * resolution_;updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);}break;}break;}}
}