26 void PlannerSimple2D::computePath(
28 const CPose2D& target_, std::deque<math::TPoint2D>& path,
bool& notFound,
29 float maxSearchPathLength)
const
31 using cell_t = int32_t;
33 constexpr cell_t CELL_ORIGIN = 0;
34 constexpr cell_t CELL_EMPTY = 0x8000000;
35 constexpr cell_t CELL_OBSTACLE = 0xfffffff;
36 constexpr cell_t CELL_TARGET = 0xffffffe;
43 std::vector<cell_t> grid;
44 int size_x, size_y, i, n, m;
47 cell_t minNeigh = CELL_EMPTY, maxNeigh = CELL_EMPTY, v = 0, c;
48 int passCellFound_x = -1, passCellFound_y = -1;
49 std::vector<cell_t> pathcells_x, pathcells_y;
67 path.emplace_back(target.
x, target.
y);
79 grid.resize(size_x * size_y);
80 for (y = 0; y < size_y; y++)
83 for (x = 0; x < size_x; x++)
85 grid[x + row] = (theMap.
getCell(x, y) > occupancyThreshold)
93 int obsEnlargement = (int)(ceil(robotRadius / theMap.
getResolution()));
94 for (
int nEnlargements = 0; nEnlargements < obsEnlargement; nEnlargements++)
98 for (y = 2; y < size_y - 2; y++)
100 int row = y * size_x;
101 int row_1 = (y + 1) * size_x;
102 int row__1 = (y - 1) * size_x;
104 for (x = 2; x < size_x - 2; x++)
106 cell_t
val = (CELL_OBSTACLE - nEnlargements);
110 if (grid[x - 1 + row__1] >=
val || grid[x + row__1] >=
val ||
111 grid[x + 1 + row__1] >=
val || grid[x - 1 + row] >=
val ||
112 grid[x + 1 + row] >=
val || grid[x - 1 + row_1] >=
val ||
113 grid[x + row_1] >=
val || grid[x + 1 + row_1] >=
val)
115 grid[x + row] = std::max(grid[x + row],
val - 1);
122 for (
auto& cell : grid)
123 if (cell > CELL_EMPTY) cell = CELL_OBSTACLE;
127 grid[theMap.
x2idx(origin.
x) + size_x * theMap.
y2idx(origin.
y)] =
129 grid[theMap.
x2idx(target.
x) + size_x * theMap.
y2idx(target.
y)] =
138 std::min(theMap.
x2idx(origin.
x) - 1, theMap.
x2idx(target.
x) - 1);
140 std::max(theMap.
x2idx(origin.
x) + 1, theMap.
x2idx(target.
x) + 1);
142 std::min(theMap.
y2idx(origin.
y) - 1, theMap.
y2idx(target.
y) - 1);
144 std::max(theMap.
y2idx(origin.
y) + 1, theMap.
y2idx(target.
y) + 1);
149 bool wave1Found =
false, wave2Found =
false;
150 int size_y_1 = size_y - 1;
151 int size_x_1 = size_x - 1;
153 range_x_min = std::max(1, range_x_min - 1);
154 range_x_max = std::min(size_x_1, range_x_max + 1);
155 range_y_min = std::max(1, range_y_min - 1);
156 range_y_max = std::min(size_y_1, range_y_max + 1);
160 for (y = range_y_min; y < range_y_max && passCellFound_x == -1; y++)
162 int row = y * size_x;
163 int row_1 = (y + 1) * size_x;
164 int row__1 = (y - 1) * size_x;
168 for (x = range_x_min; x < range_x_max; x++)
170 if (grid[x + row] != CELL_EMPTY)
continue;
174 minNeigh = maxNeigh = CELL_EMPTY;
176 v = grid[x + row__1];
177 if (v + 2 < minNeigh) minNeigh = v + 2;
178 if (v - 2 > maxNeigh && v != CELL_OBSTACLE) maxNeigh = v - 2;
179 v = grid[x - 1 + row];
180 if (v + 2 < minNeigh) minNeigh = v + 2;
181 if (v - 2 > maxNeigh && v != CELL_OBSTACLE) maxNeigh = v - 2;
182 v = grid[x + 1 + row];
183 if (v + 2 < minNeigh) minNeigh = v + 2;
184 if (v - 2 > maxNeigh && v != CELL_OBSTACLE) maxNeigh = v - 2;
186 if (v + 2 < minNeigh) minNeigh = v + 2;
187 if (v - 2 > maxNeigh && v != CELL_OBSTACLE) maxNeigh = v - 2;
189 v = grid[x - 1 + row__1];
190 if ((v + 3) < minNeigh)
195 if ((v - 3) > maxNeigh && v != CELL_OBSTACLE)
200 v = grid[x + 1 + row__1];
201 if ((v + 3) < minNeigh)
206 if ((v - 3) > maxNeigh && v != CELL_OBSTACLE)
211 v = grid[x - 1 + row_1];
212 if ((v + 3) < minNeigh)
217 if ((v - 3) > maxNeigh && v != CELL_OBSTACLE)
222 v = grid[x + 1 + row_1];
223 if ((v + 3) < minNeigh)
228 if ((v - 3) > maxNeigh && v != CELL_OBSTACLE)
236 if (minNeigh < CELL_EMPTY && maxNeigh > CELL_EMPTY)
244 else if (minNeigh < CELL_EMPTY)
249 grid[x + row] = minNeigh + metric;
250 ASSERT_(minNeigh + metric < CELL_EMPTY);
252 else if (maxNeigh > CELL_EMPTY)
257 grid[x + row] = maxNeigh - metric;
258 ASSERT_(maxNeigh - metric > CELL_EMPTY);
267 notFound = !wave1Found && !wave2Found;
270 const int estimPathLen = std::min(minNeigh + 1, CELL_TARGET - maxNeigh);
272 if (maxSearchPathLength > 0 &&
279 }
while (!notFound && searching);
282 if (notFound)
return;
292 while ((v = grid[x + size_x * y]) != CELL_ORIGIN)
296 pathcells_x.push_back(x);
297 pathcells_y.push_back(y);
300 int8_t dx = 0, dy = 0;
301 if ((c = grid[x - 1 + size_x * y]) < v)
307 if ((c = grid[x + 1 + size_x * y]) < v)
313 if ((c = grid[x + size_x * (y - 1)]) < v)
319 if ((c = grid[x + size_x * (y + 1)]) < v)
326 if ((c = grid[x - 1 + size_x * (y - 1)]) < v)
332 if ((c = grid[x + 1 + size_x * (y - 1)]) < v)
338 if ((c = grid[x - 1 + size_x * (y + 1)]) < v)
344 if ((c = grid[x + 1 + size_x * (y + 1)]) < v)
359 n = pathcells_x.size();
361 for (i = 0; i < m; i++)
364 pathcells_x[i] = pathcells_x[n - 1 - i];
365 pathcells_x[n - 1 - i] = v;
368 pathcells_y[i] = pathcells_y[n - 1 - i];
369 pathcells_y[n - 1 - i] = v;
377 while ((v = grid[x + size_x * y]) != CELL_TARGET)
380 pathcells_x.push_back(x);
381 pathcells_y.push_back(y);
384 static signed char dx = 0, dy = 0;
385 c = grid[x - 1 + size_x * y];
386 if (c > v && c != CELL_OBSTACLE)
392 c = grid[x + 1 + size_x * y];
393 if (c > v && c != CELL_OBSTACLE)
399 c = grid[x + size_x * (y - 1)];
400 if (c > v && c != CELL_OBSTACLE)
406 c = grid[x + size_x * (y + 1)];
407 if (c > v && c != CELL_OBSTACLE)
414 c = grid[x - 1 + size_x * (y - 1)];
415 if (c > v && c != CELL_OBSTACLE)
421 c = grid[x + 1 + size_x * (y - 1)];
422 if (c > v && c != CELL_OBSTACLE)
428 c = grid[x - 1 + size_x * (y + 1)];
429 if (c > v && c != CELL_OBSTACLE)
435 c = grid[x + 1 + size_x * (y + 1)];
436 if (c > v && c != CELL_OBSTACLE)
452 n = pathcells_x.size();
453 double last_xx = origin.
x;
454 double last_yy = origin.
y;
455 auto last_cx = theMap.
x2idx(origin.
x);
456 auto last_cy = theMap.
y2idx(origin.
y);
460 double accumDist = 0;
461 for (i = 0; i < n; i++)
464 const auto distSqrCells =
465 square(pathcells_x[i] - last_cx) +
square(pathcells_y[i] - last_cy);
467 if (distSqrCells > minDistSqrCells)
470 auto xx = theMap.
idx2x(pathcells_x[i]);
471 auto yy = theMap.
idx2y(pathcells_y[i]);
474 path.emplace_back(xx, yy);
476 accumDist += std::sqrt(
square(xx - last_xx) +
square(yy - last_yy));
479 last_cx = pathcells_x[i];
480 last_cy = pathcells_y[i];
485 if (maxSearchPathLength > 0 && accumDist > maxSearchPathLength)
494 path.emplace_back(target.
x, target.
y);