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


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST