ROS Costmap 插件

背景

公司 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 边界，不管在哪里都是逆时针去 trace
  float 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,wy
            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 == 60 && x2 == 60)
          // Fov 分别与两条边框相交，分两段 trace
          // left 顺时针 trace x 到60
          for(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 增到到 60
          for(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 ++){ // 下方水平线，从左侧端点 trace
            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);
          }
          break;
        default: // left 与下方边框相交， right 与左侧边框相交 ( y1 == 0 && x2 == 0)
          // 顺时针 trace 意味着 x 是减小到0的
          for(unsigned xi = 0; xi < x1; xi ++){ // 下方水平线，从左侧端点 trace
            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);
          }
          // 逆时针 trace 意味着 y 是减小到0的
          for(unsigned yi = 0; yi < y2 ; yi++){ // 左侧竖直线，从下往上 trace
            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;
    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 到 0
            for(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 增到到 60
            for(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++){ // 左侧竖直线，从下往上 trace
              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 == 0 && y2 == 60)
            for(unsigned yi = y1; yi < 60 ; yi++){ // 下方水平线，从左侧端点 trace
              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 减小到 0
            for(unsigned xi = 0; xi < x2; xi ++){ // 左侧竖直线，从下往上 trace
              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;
        }
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257