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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 5887d2b31 Wed Apr 24 13:03:27 2019 +0200 at miƩ abr 24 13:10:13 CEST 2019