MRPT  2.0.4
PlannerSimple2D.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "nav-precomp.h" // Precompiled headers
11 //
12 #include <mrpt/math/TPose2D.h>
14 
15 
16 using namespace mrpt;
17 using namespace mrpt::maps;
18 using namespace mrpt::math;
19 using namespace mrpt::poses;
20 using namespace mrpt::nav;
21 using namespace std;
22 
23 /*---------------------------------------------------------------
24  computePath
25  ---------------------------------------------------------------*/
26 void PlannerSimple2D::computePath(
27  const COccupancyGridMap2D& theMap, const CPose2D& origin_,
28  const CPose2D& target_, std::deque<math::TPoint2D>& path, bool& notFound,
29  float maxSearchPathLength) const
30 {
31  using cell_t = int32_t;
32 
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;
37 
38  path.clear();
39 
40  const TPoint2D origin = TPoint2D(origin_.asTPose());
41  const TPoint2D target = TPoint2D(target_.asTPose());
42 
43  std::vector<cell_t> grid;
44  int size_x, size_y, i, n, m;
45  int x, y;
46  bool searching;
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;
50 
51  // Check that origin and target falls inside the grid theMap
52  // -----------------------------------------------------------
53  if (!((origin.x > theMap.getXMin() && origin.x < theMap.getXMax() &&
54  origin.y > theMap.getYMin() && origin.y < theMap.getYMax()) ||
55  !(target.x > theMap.getXMin() && target.x < theMap.getXMax() &&
56  target.y > theMap.getYMin() && target.y < theMap.getYMax())))
57  {
58  notFound = true;
59  return;
60  }
61 
62  // Check for the special case of origin and target in the same cell:
63  // -----------------------------------------------------------------
64  if (theMap.x2idx(origin.x) == theMap.x2idx(target.x) &&
65  theMap.y2idx(origin.y) == theMap.y2idx(target.y))
66  {
67  path.emplace_back(target.x, target.y);
68  notFound = false;
69  return;
70  }
71 
72  // Get the grid size:
73  // -----------------------------------------------------------
74  size_x = theMap.getSizeX();
75  size_y = theMap.getSizeY();
76 
77  // Fill the grid content with free-space and obstacles:
78  // -----------------------------------------------------------
79  grid.resize(size_x * size_y);
80  for (y = 0; y < size_y; y++)
81  {
82  int row = y * size_x;
83  for (x = 0; x < size_x; x++)
84  {
85  grid[x + row] = (theMap.getCell(x, y) > occupancyThreshold)
86  ? CELL_EMPTY
87  : CELL_OBSTACLE;
88  }
89  }
90 
91  // Enlarge obstacles with the robot radius:
92  // -----------------------------------------------------------
93  int obsEnlargement = (int)(ceil(robotRadius / theMap.getResolution()));
94  for (int nEnlargements = 0; nEnlargements < obsEnlargement; nEnlargements++)
95  {
96  // For all cells(x,y)=EMPTY:
97  // -----------------------------
98  for (y = 2; y < size_y - 2; y++)
99  {
100  int row = y * size_x;
101  int row_1 = (y + 1) * size_x;
102  int row__1 = (y - 1) * size_x;
103 
104  for (x = 2; x < size_x - 2; x++)
105  {
106  cell_t val = (CELL_OBSTACLE - nEnlargements);
107 
108  // A cell near an obstacle found??
109  // -----------------------------------------------------
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)
114  {
115  grid[x + row] = std::max(grid[x + row], val - 1);
116  }
117  }
118  }
119  }
120 
121  // Definitevely set new obstacles as obstacles
122  for (auto& cell : grid)
123  if (cell > CELL_EMPTY) cell = CELL_OBSTACLE;
124 
125  // Put the special cell codes for the origin and target:
126  // -----------------------------------------------------------
127  grid[theMap.x2idx(origin.x) + size_x * theMap.y2idx(origin.y)] =
128  CELL_ORIGIN;
129  grid[theMap.x2idx(target.x) + size_x * theMap.y2idx(target.y)] =
130  CELL_TARGET;
131 
132  // The main path search loop:
133  // -----------------------------------------------------------
134  searching = true; // Will become false on path found
135  notFound = false; // Will be true inside the loop if a path is not found
136 
137  int range_x_min =
138  std::min(theMap.x2idx(origin.x) - 1, theMap.x2idx(target.x) - 1);
139  int range_x_max =
140  std::max(theMap.x2idx(origin.x) + 1, theMap.x2idx(target.x) + 1);
141  int range_y_min =
142  std::min(theMap.y2idx(origin.y) - 1, theMap.y2idx(target.y) - 1);
143  int range_y_max =
144  std::max(theMap.y2idx(origin.y) + 1, theMap.y2idx(target.y) + 1);
145 
146  do
147  {
148  notFound = true;
149  bool wave1Found = false, wave2Found = false;
150  int size_y_1 = size_y - 1;
151  int size_x_1 = size_x - 1;
152 
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);
157 
158  // For all cells(x,y)=EMPTY:
159  // -----------------------------
160  for (y = range_y_min; y < range_y_max && passCellFound_x == -1; y++)
161  {
162  int row = y * size_x;
163  int row_1 = (y + 1) * size_x;
164  int row__1 = (y - 1) * size_x;
165  // metric: 2 horz.vert, =3 diagonal <-- Since 3/2 ~= sqrt(2)
166  cell_t metric;
167 
168  for (x = range_x_min; x < range_x_max; x++)
169  {
170  if (grid[x + row] != CELL_EMPTY) continue;
171 
172  // Look in the neighboorhood:
173  // -----------------------------
174  minNeigh = maxNeigh = CELL_EMPTY;
175  metric = 2;
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;
185  v = grid[x + row_1];
186  if (v + 2 < minNeigh) minNeigh = v + 2;
187  if (v - 2 > maxNeigh && v != CELL_OBSTACLE) maxNeigh = v - 2;
188 
189  v = grid[x - 1 + row__1];
190  if ((v + 3) < minNeigh)
191  {
192  metric = 3;
193  minNeigh = (v + 3);
194  }
195  if ((v - 3) > maxNeigh && v != CELL_OBSTACLE)
196  {
197  metric = 3;
198  maxNeigh = v - 3;
199  }
200  v = grid[x + 1 + row__1];
201  if ((v + 3) < minNeigh)
202  {
203  metric = 3;
204  minNeigh = (v + 3);
205  }
206  if ((v - 3) > maxNeigh && v != CELL_OBSTACLE)
207  {
208  metric = 3;
209  maxNeigh = v - 3;
210  }
211  v = grid[x - 1 + row_1];
212  if ((v + 3) < minNeigh)
213  {
214  metric = 3;
215  minNeigh = (v + 3);
216  }
217  if ((v - 3) > maxNeigh && v != CELL_OBSTACLE)
218  {
219  metric = 3;
220  maxNeigh = v - 3;
221  }
222  v = grid[x + 1 + row_1];
223  if ((v + 3) < minNeigh)
224  {
225  metric = 3;
226  minNeigh = (v + 3);
227  }
228  if ((v - 3) > maxNeigh && v != CELL_OBSTACLE)
229  {
230  metric = 3;
231  maxNeigh = v - 3;
232  }
233 
234  // Convergence cell found? = The shortest path found
235  // -----------------------------------------------------
236  if (minNeigh < CELL_EMPTY && maxNeigh > CELL_EMPTY)
237  {
238  // Stop the search:
239  passCellFound_x = x;
240  passCellFound_y = y;
241  searching = false;
242  break;
243  }
244  else if (minNeigh < CELL_EMPTY)
245  {
246  wave1Found = true;
247 
248  // Cell in the expansion-wave from origin
249  grid[x + row] = minNeigh + metric;
250  ASSERT_(minNeigh + metric < CELL_EMPTY);
251  }
252  else if (maxNeigh > CELL_EMPTY)
253  {
254  wave2Found = true;
255 
256  // Cell in the expansion-wave from the target
257  grid[x + row] = maxNeigh - metric;
258  ASSERT_(maxNeigh - metric > CELL_EMPTY);
259  }
260  else
261  { // Nothing to do: A free cell inside of all also free
262  // cells.
263  }
264  } // end for x
265  } // end for y
266 
267  notFound = !wave1Found && !wave2Found;
268 
269  // Check max. path:
270  const int estimPathLen = std::min(minNeigh + 1, CELL_TARGET - maxNeigh);
271 
272  if (maxSearchPathLength > 0 &&
273  estimPathLen * theMap.getResolution() > maxSearchPathLength)
274  {
275  notFound = true;
276  break;
277  }
278 
279  } while (!notFound && searching);
280 
281  // Path not found:
282  if (notFound) return;
283 
284  // Rebuild the optimal path from the two-waves convergence cell
285  // ----------------------------------------------------------------
286 
287  // STEP 1: Trace-back to origin
288  //-------------------------------------
289  x = passCellFound_x;
290  y = passCellFound_y;
291 
292  while ((v = grid[x + size_x * y]) != CELL_ORIGIN)
293  {
294  // Add cell to the path (in inverse order, now we go backward!!) Later
295  // is will be reversed
296  pathcells_x.push_back(x);
297  pathcells_y.push_back(y);
298 
299  // Follow the "negative gradient" toward the origin:
300  int8_t dx = 0, dy = 0;
301  if ((c = grid[x - 1 + size_x * y]) < v)
302  {
303  v = c;
304  dx = -1;
305  dy = 0;
306  }
307  if ((c = grid[x + 1 + size_x * y]) < v)
308  {
309  v = c;
310  dx = 1;
311  dy = 0;
312  }
313  if ((c = grid[x + size_x * (y - 1)]) < v)
314  {
315  v = c;
316  dx = 0;
317  dy = -1;
318  }
319  if ((c = grid[x + size_x * (y + 1)]) < v)
320  {
321  v = c;
322  dx = 0;
323  dy = 1;
324  }
325 
326  if ((c = grid[x - 1 + size_x * (y - 1)]) < v)
327  {
328  v = c;
329  dx = -1;
330  dy = -1;
331  }
332  if ((c = grid[x + 1 + size_x * (y - 1)]) < v)
333  {
334  v = c;
335  dx = 1;
336  dy = -1;
337  }
338  if ((c = grid[x - 1 + size_x * (y + 1)]) < v)
339  {
340  v = c;
341  dx = -1;
342  dy = 1;
343  }
344  if ((c = grid[x + 1 + size_x * (y + 1)]) < v)
345  {
346  v = c;
347  dx = 1;
348  dy = 1;
349  }
350 
351  ASSERT_(dx != 0 || dy != 0);
352  x += dx;
353  y += dy;
354  }
355 
356  // STEP 2: Reverse the path, since we want it from the origin
357  // toward the convergence cell
358  //--------------------------------------------------------------
359  n = pathcells_x.size();
360  m = n / 2;
361  for (i = 0; i < m; i++)
362  {
363  v = pathcells_x[i];
364  pathcells_x[i] = pathcells_x[n - 1 - i];
365  pathcells_x[n - 1 - i] = v;
366 
367  v = pathcells_y[i];
368  pathcells_y[i] = pathcells_y[n - 1 - i];
369  pathcells_y[n - 1 - i] = v;
370  }
371 
372  // STEP 3: Trace-foward toward the target
373  //-------------------------------------
374  x = passCellFound_x;
375  y = passCellFound_y;
376 
377  while ((v = grid[x + size_x * y]) != CELL_TARGET)
378  {
379  // Add cell to the path
380  pathcells_x.push_back(x);
381  pathcells_y.push_back(y);
382 
383  // Follow the "positive gradient" toward the target:
384  static signed char dx = 0, dy = 0;
385  c = grid[x - 1 + size_x * y];
386  if (c > v && c != CELL_OBSTACLE)
387  {
388  v = c;
389  dx = -1;
390  dy = 0;
391  }
392  c = grid[x + 1 + size_x * y];
393  if (c > v && c != CELL_OBSTACLE)
394  {
395  v = c;
396  dx = 1;
397  dy = 0;
398  }
399  c = grid[x + size_x * (y - 1)];
400  if (c > v && c != CELL_OBSTACLE)
401  {
402  v = c;
403  dx = 0;
404  dy = -1;
405  }
406  c = grid[x + size_x * (y + 1)];
407  if (c > v && c != CELL_OBSTACLE)
408  {
409  v = c;
410  dx = 0;
411  dy = 1;
412  }
413 
414  c = grid[x - 1 + size_x * (y - 1)];
415  if (c > v && c != CELL_OBSTACLE)
416  {
417  v = c;
418  dx = -1;
419  dy = -1;
420  }
421  c = grid[x + 1 + size_x * (y - 1)];
422  if (c > v && c != CELL_OBSTACLE)
423  {
424  v = c;
425  dx = 1;
426  dy = -1;
427  }
428  c = grid[x - 1 + size_x * (y + 1)];
429  if (c > v && c != CELL_OBSTACLE)
430  {
431  v = c;
432  dx = -1;
433  dy = 1;
434  }
435  c = grid[x + 1 + size_x * (y + 1)];
436  if (c > v && c != CELL_OBSTACLE)
437  {
438  v = c;
439  dx = 1;
440  dy = 1;
441  }
442 
443  ASSERT_(dx != 0 || dy != 0);
444  x += dx;
445  y += dy;
446  }
447 
448  // STEP 4: Translate the path-of-cells to a path-of-2d-points with
449  // subsampling
450  //-------------------------------------------------------------------------------
451  path.clear();
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);
457 
458  const auto minDistSqrCells = mrpt::round(
459  mrpt::square(minStepInReturnedPath / theMap.getResolution()));
460  double accumDist = 0;
461  for (i = 0; i < n; i++)
462  {
463  // Enough distance??
464  const auto distSqrCells =
465  square(pathcells_x[i] - last_cx) + square(pathcells_y[i] - last_cy);
466 
467  if (distSqrCells > minDistSqrCells)
468  {
469  // Get cell coordinates:
470  auto xx = theMap.idx2x(pathcells_x[i]);
471  auto yy = theMap.idx2y(pathcells_y[i]);
472 
473  // Add to the path:
474  path.emplace_back(xx, yy);
475 
476  accumDist += std::sqrt(square(xx - last_xx) + square(yy - last_yy));
477 
478  // For the next iteration:
479  last_cx = pathcells_x[i];
480  last_cy = pathcells_y[i];
481  last_xx = xx;
482  last_yy = yy;
483  }
484 
485  if (maxSearchPathLength > 0 && accumDist > maxSearchPathLength)
486  {
487  notFound = true;
488  path.clear();
489  return;
490  }
491  }
492 
493  // Add the target point:
494  path.emplace_back(target.x, target.y);
495 
496  // That's all!! :-)
497 }
nav-precomp.h
mrpt::poses::CPose2D::asTPose
mrpt::math::TPose2D asTPose() const
Definition: CPose2D.cpp:468
mrpt::math::TPoint2D_< double >
TPose2D.h
mrpt::nav
Definition: CAbstractHolonomicReactiveMethod.h:20
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::maps::COccupancyGridMap2D::getXMax
float getXMax() const
Returns the "x" coordinate of right side of grid map.
Definition: COccupancyGridMap2D.h:280
mrpt::maps::COccupancyGridMap2D::idx2y
float idx2y(const size_t cy) const
Definition: COccupancyGridMap2D.h:311
mrpt::maps::COccupancyGridMap2D::getCell
float getCell(int x, int y) const
Read the real valued [0,1] contents of a cell, given its index.
Definition: COccupancyGridMap2D.h:357
mrpt::maps::COccupancyGridMap2D::getSizeY
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: COccupancyGridMap2D.h:276
mrpt::maps::COccupancyGridMap2D::getYMax
float getYMax() const
Returns the "y" coordinate of bottom side of grid map.
Definition: COccupancyGridMap2D.h:284
val
int val
Definition: mrpt_jpeglib.h:957
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::maps::COccupancyGridMap2D::y2idx
int y2idx(float y) const
Definition: COccupancyGridMap2D.h:292
mrpt::maps::COccupancyGridMap2D::x2idx
int x2idx(float x) const
Transform a coordinate value into a cell index.
Definition: COccupancyGridMap2D.h:288
mrpt::maps::COccupancyGridMap2D::getYMin
float getYMin() const
Returns the "y" coordinate of top side of grid map.
Definition: COccupancyGridMap2D.h:282
mrpt::math::TPoint2D_data::y
T y
Definition: TPoint2D.h:25
mrpt::square
return_t square(const num_t x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:23
mrpt::maps::COccupancyGridMap2D::getResolution
float getResolution() const
Returns the resolution of the grid map.
Definition: COccupancyGridMap2D.h:286
mrpt::math::TPoint2D_data::x
T x
X,Y coordinates.
Definition: TPoint2D.h:25
mrpt::maps::COccupancyGridMap2D::getXMin
float getXMin() const
Returns the "x" coordinate of left side of grid map.
Definition: COccupancyGridMap2D.h:278
mrpt::maps::COccupancyGridMap2D::idx2x
float idx2x(const size_t cx) const
Transform a cell index into a coordinate value.
Definition: COccupancyGridMap2D.h:307
mrpt::math::TPoint2D
TPoint2D_< double > TPoint2D
Lightweight 2D point.
Definition: TPoint2D.h:213
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
PlannerSimple2D.h
mrpt::maps::COccupancyGridMap2D
A class for storing an occupancy grid map.
Definition: COccupancyGridMap2D.h:53
mrpt::maps::COccupancyGridMap2D::getSizeX
unsigned int getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: COccupancyGridMap2D.h:274
mrpt::maps
Definition: CBeacon.h:21



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 15:15:43 UTC 2020