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


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019