25 PlannerSimple2D::PlannerSimple2D()
32 void PlannerSimple2D::computePath(
34 const CPose2D& target_, std::deque<math::TPoint2D>& path,
bool& notFound,
35 float maxSearchPathLength)
const
37 #define CELL_ORIGIN 0x0000
38 #define CELL_EMPTY 0x8000
39 #define CELL_OBSTACLE 0xFFFF
40 #define CELL_TARGET 0xFFFE
45 std::vector<uint16_t> grid;
46 int size_x, size_y, i, n, m;
50 int passCellFound_x = -1, passCellFound_y = -1;
51 std::vector<uint16_t> pathcells_x, pathcells_y;
68 path.emplace_back(target.
x, target.
y);
80 grid.resize(size_x * size_y);
81 for (y = 0; y < size_y; y++)
84 for (x = 0; x < size_x; x++)
86 grid[x + row] = (theMap.
getCell(x, y) > occupancyThreshold)
94 int obsEnlargement = (int)(ceil(robotRadius / theMap.
getResolution()));
95 for (
int nEnlargements = 0; nEnlargements < obsEnlargement; nEnlargements++)
102 for (y = 2; y < size_y - 2; y++)
104 int row = y * size_x;
105 int row_1 = (y + 1) * size_x;
106 int row__1 = (y - 1) * size_x;
108 for (x = 2; x < size_x - 2; x++)
114 if (grid[x - 1 + row__1] >=
val || grid[x + row__1] >=
val ||
115 grid[x + 1 + row__1] >=
val || grid[x - 1 + row] >=
val ||
116 grid[x + 1 + row] >=
val || grid[x - 1 + row_1] >=
val ||
117 grid[x + row_1] >=
val || grid[x + 1 + row_1] >=
val)
120 max((uint16_t)grid[x + row], (uint16_t)(
val - 1));
127 for (y = 1; y < size_y - 1; y++)
129 int row = y * size_x;
130 for (x = 1; x < size_x - 1; x++)
138 grid[theMap.
x2idx(origin.
x) + size_x * theMap.
y2idx(origin.
y)] =
140 grid[theMap.
x2idx(target.
x) + size_x * theMap.
y2idx(target.
y)] =
150 min(theMap.
x2idx(origin.
x) - 1, theMap.
x2idx(target.
x) - 1);
152 max(theMap.
x2idx(origin.
x) + 1, theMap.
x2idx(target.
x) + 1);
154 min(theMap.
y2idx(origin.
y) - 1, theMap.
y2idx(target.
y) - 1);
156 max(theMap.
y2idx(origin.
y) + 1, theMap.
y2idx(target.
y) + 1);
161 bool wave1Found =
false, wave2Found =
false;
162 int size_y_1 = size_y - 1;
163 int size_x_1 = size_x - 1;
164 int longestPathInCellsMetric = 0;
166 range_x_min = max(1, range_x_min - 1);
167 range_x_max = min(size_x_1, range_x_max + 1);
168 range_y_min = max(1, range_y_min - 1);
169 range_y_max = min(size_y_1, range_y_max + 1);
173 for (y = range_y_min; y < range_y_max && passCellFound_x == -1; y++)
175 int row = y * size_x;
176 int row_1 = (y + 1) * size_x;
177 int row__1 = (y - 1) * size_x;
180 for (x = range_x_min; x < range_x_max; x++)
188 v = grid[x + row__1];
189 if (v + 2 < minNeigh) minNeigh = v + 2;
192 v = grid[x - 1 + row];
193 if (v + 2 < minNeigh) minNeigh = v + 2;
196 v = grid[x + 1 + row];
197 if (v + 2 < minNeigh) minNeigh = v + 2;
201 if (v + 2 < minNeigh) minNeigh = v + 2;
205 v = grid[x - 1 + row__1];
206 if ((v + 3) < minNeigh)
216 v = grid[x + 1 + row__1];
217 if ((v + 3) < minNeigh)
227 v = grid[x - 1 + row_1];
228 if ((v + 3) < minNeigh)
238 v = grid[x + 1 + row_1];
239 if ((v + 3) < minNeigh)
252 if (minNeigh < CELL_EMPTY && maxNeigh >
CELL_EMPTY)
258 longestPathInCellsMetric = 0;
266 grid[x + row] = minNeigh + metric;
269 longestPathInCellsMetric = max(
270 longestPathInCellsMetric,
CELL_EMPTY - minNeigh);
277 grid[x + row] = maxNeigh - metric;
280 longestPathInCellsMetric = max(
281 longestPathInCellsMetric, maxNeigh -
CELL_EMPTY);
291 notFound = !wave1Found && !wave2Found;
294 if (maxSearchPathLength > 0)
295 if (theMap.
getResolution() * longestPathInCellsMetric * 0.5f >
296 1.5f * maxSearchPathLength)
305 }
while (!notFound && searching);
308 if (notFound)
return;
319 pathcells_y.reserve((minNeigh + 1) + (
CELL_TARGET - maxNeigh));
327 pathcells_x.push_back(x);
328 pathcells_y.push_back(y);
331 static signed char dx = 0, dy = 0;
332 if ((c = grid[x - 1 + size_x * y]) < v)
338 if ((c = grid[x + 1 + size_x * y]) < v)
344 if ((c = grid[x + size_x * (y - 1)]) < v)
350 if ((c = grid[x + size_x * (y + 1)]) < v)
357 if ((c = grid[x - 1 + size_x * (y - 1)]) < v)
363 if ((c = grid[x + 1 + size_x * (y - 1)]) < v)
369 if ((c = grid[x - 1 + size_x * (y + 1)]) < v)
375 if ((c = grid[x + 1 + size_x * (y + 1)]) < v)
390 n = pathcells_x.size();
392 for (i = 0; i < m; i++)
395 pathcells_x[i] = pathcells_x[n - 1 - i];
396 pathcells_x[n - 1 - i] = v;
399 pathcells_y[i] = pathcells_y[n - 1 - i];
400 pathcells_y[n - 1 - i] = v;
411 pathcells_x.push_back(x);
412 pathcells_y.push_back(y);
415 static signed char dx = 0, dy = 0;
416 c = grid[x - 1 + size_x * y];
423 c = grid[x + 1 + size_x * y];
430 c = grid[x + size_x * (y - 1)];
437 c = grid[x + size_x * (y + 1)];
445 c = grid[x - 1 + size_x * (y - 1)];
452 c = grid[x + 1 + size_x * (y - 1)];
459 c = grid[x - 1 + size_x * (y + 1)];
466 c = grid[x + 1 + size_x * (y + 1)];
483 n = pathcells_x.size();
485 float last_xx = origin.
x, last_yy = origin.
y;
486 for (i = 0; i < n; i++)
489 xx = theMap.
idx2x(pathcells_x[i]);
490 yy = theMap.
idx2y(pathcells_y[i]);
494 minStepInReturnedPath)
497 path.emplace_back(xx, yy);
506 path.emplace_back(target.
x, target.
y);