Main MRPT website > C++ reference for MRPT 1.5.7
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 
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 ),
27  minStepInReturnedPath (0.4f),
28  robotRadius(0.35f)
29 {
30 }
31 
32 
33 /*---------------------------------------------------------------
34  computePath
35  ---------------------------------------------------------------*/
37  const COccupancyGridMap2D &theMap,
38  const CPose2D &origin_,
39  const CPose2D &target_,
40  std::deque<math::TPoint2D> &path,
41  bool &notFound,
42  float maxSearchPathLength ) const
43 {
44  #define CELL_ORIGIN 0x0000
45  #define CELL_EMPTY 0x8000
46  #define CELL_OBSTACLE 0xFFFF
47  #define CELL_TARGET 0xFFFE
48 
49  const TPoint2D origin = TPoint2D(origin_);
50  const TPoint2D target = TPoint2D(target_);
51 
52  std::vector<uint16_t> grid;
53  int size_x,size_y,i,n,m;
54  int x,y;
55  bool searching;
56  uint16_t minNeigh=CELL_EMPTY, maxNeigh=CELL_EMPTY, v=0, c;
57  int passCellFound_x = -1,
58  passCellFound_y = -1;
59  std::vector<uint16_t> pathcells_x,pathcells_y;
60 
61  // Check that origin and target falls inside the grid theMap!!
62  // -----------------------------------------------------------
63  ASSERT_(origin.x>theMap.getXMin() && origin.x<theMap.getXMax() &&
64  origin.y>theMap.getYMin() && origin.y<theMap.getYMax());
65  ASSERT_(target.x>theMap.getXMin() && target.x<theMap.getXMax() &&
66  target.y>theMap.getYMin() && target.y<theMap.getYMax() );
67 
68 
69  // Check for the special case of origin and target in the same cell:
70  // -----------------------------------------------------------------
71  if ( theMap.x2idx(origin.x) == theMap.x2idx(target.x) &&
72  theMap.y2idx(origin.y) == theMap.y2idx(target.y) )
73  {
74  path.clear();
75  path.push_back(TPoint2D(target.x,target.y));
76  notFound = false;
77  return;
78  }
79 
80  // Get the grid size:
81  // -----------------------------------------------------------
82  size_x = theMap.getSizeX();
83  size_y = theMap.getSizeY();
84 
85  // Fill the grid content with free-space and obstacles:
86  // -----------------------------------------------------------
87  grid.resize( size_x * size_y );
88  for (y=0;y<size_y;y++)
89  {
90  int row = y*size_x;
91  for (x=0;x<size_x;x++)
92  {
93  grid[x+row]=(theMap.getCell(x,y)>occupancyThreshold) ? CELL_EMPTY:CELL_OBSTACLE;
94  }
95  }
96 
97  // Enlarge obstacles with the robot radius:
98  // -----------------------------------------------------------
99  int obsEnlargement = (int)(ceil( robotRadius / theMap.getResolution() ));
100  for (int nEnlargements=0;nEnlargements<obsEnlargement;nEnlargements++)
101  {
102 // int size_y_1 = size_y-1;
103 // int size_x_1 = size_x-1;
104 
105  // For all cells(x,y)=EMPTY:
106  // -----------------------------
107  for (y=2;y<size_y-2;y++)
108  {
109  int row = y*size_x;
110  int row_1 = (y+1) * size_x;
111  int row__1 = (y-1) * size_x;
112 
113  for (x=2;x<size_x-2;x++)
114  {
115  uint16_t val = ( CELL_OBSTACLE - nEnlargements );
116 
117  // A cell near an obstacle found??
118  // -----------------------------------------------------
119  if (grid[x-1+row__1]>=val ||
120  grid[x+row__1]>=val ||
121  grid[x+1+row__1]>=val ||
122  grid[x-1+row]>=val ||
123  grid[x+1+row]>=val ||
124  grid[x-1+row_1]>=val ||
125  grid[x+row_1]>=val ||
126  grid[x+1+row_1]>=val )
127  {
128  grid[x+row] = max((uint16_t)grid[x+row], (uint16_t)(val-1));
129  }
130  }
131  }
132  }
133 
134  // Definitevely set new obstacles as obstacles
135  for (y=1;y<size_y-1;y++)
136  {
137  int row = y*size_x;
138  for (x=1;x<size_x-1;x++)
139  {
140  if (grid[x+row] > CELL_EMPTY )
141  grid[x+row] = CELL_OBSTACLE;
142  }
143  }
144 
145 
146  // Put the special cell codes for the origin and target:
147  // -----------------------------------------------------------
148  grid[ theMap.x2idx(origin.x) + size_x*theMap.y2idx(origin.y)] = CELL_ORIGIN;
149  grid[ theMap.x2idx(target.x) + size_x*theMap.y2idx(target.y)] = CELL_TARGET;
150 
151 
152  // The main path search loop:
153  // -----------------------------------------------------------
154  searching = true; // Will become false on path found.
155  notFound = false; // Will be set true inside the loop if a path is not found.
156 
157  int range_x_min = min( theMap.x2idx(origin.x)-1,theMap.x2idx(target.x)-1 );
158  int range_x_max = max( theMap.x2idx(origin.x)+1,theMap.x2idx(target.x)+1 );
159  int range_y_min = min( theMap.y2idx(origin.y)-1,theMap.y2idx(target.y)-1 );
160  int range_y_max = max( theMap.y2idx(origin.y)+1,theMap.y2idx(target.y)+1 );
161 
162  do
163  {
164  notFound = true;
165  bool wave1Found= false, wave2Found = false;
166  int size_y_1 = size_y-1;
167  int size_x_1 = size_x-1;
168  int longestPathInCellsMetric = 0;
169 
170  range_x_min= max(1, range_x_min - 1 );
171  range_x_max= min(size_x_1, range_x_max + 1 );
172  range_y_min= max(1, range_y_min - 1 );
173  range_y_max= min(size_y_1, range_y_max + 1 );
174 
175  // For all cells(x,y)=EMPTY:
176  // -----------------------------
177  for (y=range_y_min;y<range_y_max && passCellFound_x==-1;y++)
178  {
179  int row = y*size_x;
180  int row_1 = (y+1) * size_x;
181  int row__1 = (y-1) * size_x;
182  char metric; // =2 horz.vert, =3 diagonal <-- Since 3/2 ~= sqrt(2)
183 
184  for (x=range_x_min;x<range_x_max;x++)
185  {
186  if (grid[x+row] == CELL_EMPTY )
187  {
188  // Look in the neighboorhood:
189  // -----------------------------
190  minNeigh = maxNeigh = CELL_EMPTY;
191  metric = 2;
192  v = grid[x+row__1]; if (v+2<minNeigh)minNeigh=v+2; if (v-2>maxNeigh && v!=CELL_OBSTACLE)maxNeigh=v-2;
193  v = grid[x-1+row]; if (v+2<minNeigh)minNeigh=v+2; if (v-2>maxNeigh && v!=CELL_OBSTACLE)maxNeigh=v-2;
194  v = grid[x+1+row]; if (v+2<minNeigh)minNeigh=v+2; if (v-2>maxNeigh && v!=CELL_OBSTACLE)maxNeigh=v-2;
195  v = grid[x+row_1]; if (v+2<minNeigh)minNeigh=v+2; if (v-2>maxNeigh && v!=CELL_OBSTACLE)maxNeigh=v-2;
196 
197  v = grid[x-1+row__1]; if ((v+3)<minNeigh){metric=3;minNeigh=(v+3);} if ((v-3)>maxNeigh && v!=CELL_OBSTACLE) {metric=3;maxNeigh=v-3;}
198  v = grid[x+1+row__1]; if ((v+3)<minNeigh){metric=3;minNeigh=(v+3);} if ((v-3)>maxNeigh && v!=CELL_OBSTACLE) {metric=3;maxNeigh=v-3;}
199  v = grid[x-1+row_1]; if ((v+3)<minNeigh){metric=3;minNeigh=(v+3);} if ((v-3)>maxNeigh && v!=CELL_OBSTACLE) {metric=3;maxNeigh=v-3;}
200  v = grid[x+1+row_1]; if ((v+3)<minNeigh){metric=3;minNeigh=(v+3);} if ((v-3)>maxNeigh && v!=CELL_OBSTACLE) {metric=3;maxNeigh=v-3;}
201 
202  // Convergence cell found? = The shortest path found
203  // -----------------------------------------------------
204  if ( minNeigh<CELL_EMPTY && maxNeigh>CELL_EMPTY )
205  {
206  // Stop the search:
207  passCellFound_x = x;
208  passCellFound_y = y;
209  searching = false;
210  longestPathInCellsMetric = 0;
211  break;
212  }
213  else if (minNeigh<CELL_EMPTY)
214  {
215  wave1Found = true;
216 
217  // Cell in the expansion-wave from origin
218  grid[x+row] = minNeigh + metric;
219  ASSERT_(minNeigh+metric<CELL_EMPTY);
220 
221  longestPathInCellsMetric = max( longestPathInCellsMetric, CELL_EMPTY - minNeigh );
222  }
223  else if (maxNeigh>CELL_EMPTY)
224  {
225  wave2Found = true;
226 
227  // Cell in the expansion-wave from the target
228  grid[x+row] = maxNeigh - metric;
229  ASSERT_(maxNeigh-metric>CELL_EMPTY);
230 
231  longestPathInCellsMetric = max( longestPathInCellsMetric, maxNeigh - CELL_EMPTY);
232  }
233  else
234  { // Nothing to do: A free cell inside of all also free cells.
235  }
236  } // if cell empty
237  } // end for x
238  } // end for y
239 
240  notFound = !wave1Found && !wave2Found;
241 
242  // Exceeded the max. desired search length??
243  if (maxSearchPathLength>0)
244  if ( theMap.getResolution() * longestPathInCellsMetric * 0.5f > 1.5f*maxSearchPathLength )
245  {
246 // printf_debug("[PlannerSimple2D::computePath] Path exceeded desired length! (length=%f m)\n", theMap.getResolution() * longestPathInCellsMetric * 0.5f);
247  notFound = true;
248  }
249 
250 
251 
252  } while (!notFound && searching);
253 
254  // Path not found:
255  if (notFound)
256  return;
257 
258 
259  // Rebuild the optimal path from the two-waves convergence cell
260  // ----------------------------------------------------------------
261 
262  // STEP 1: Trace-back to origin
263  //-------------------------------------
264  pathcells_x.reserve( (minNeigh+1)+(CELL_TARGET-maxNeigh) ); // An (exact?) estimation of the final vector size:
265  pathcells_y.reserve( (minNeigh+1)+(CELL_TARGET-maxNeigh) );
266  x = passCellFound_x; y = passCellFound_y;
267 
268  while ( (v=grid[x+size_x*y]) != CELL_ORIGIN )
269  {
270  // Add cell to the path (in inverse order, now we go backward!!) Later is will be reversed
271  pathcells_x.push_back(x);
272  pathcells_y.push_back(y);
273 
274  // Follow the "negative gradient" toward the origin:
275  static signed char dx=0,dy=0;
276  if ((c=grid[x-1+size_x*y])<v) { v=c; dx=-1; dy= 0; }
277  if ((c=grid[x+1+size_x*y])<v) { v=c; dx= 1; dy= 0; }
278  if ((c=grid[x+size_x*(y-1)])<v) { v=c; dx= 0; dy=-1; }
279  if ((c=grid[x+size_x*(y+1)])<v) { v=c; dx= 0; dy= 1; }
280 
281  if ((c=grid[x-1+size_x*(y-1)])<v) { v=c; dx=-1; dy= -1; }
282  if ((c=grid[x+1+size_x*(y-1)])<v) { v=c; dx= 1; dy= -1; }
283  if ((c=grid[x-1+size_x*(y+1)])<v) { v=c; dx=-1; dy= 1; }
284  if ((c=grid[x+1+size_x*(y+1)])<v) { v=c; dx= 1; dy= 1; }
285 
286  ASSERT_(dx!=0 || dy!=0);
287  x+=dx;
288  y+=dy;
289  }
290 
291  // STEP 2: Reverse the path, since we want it from the origin
292  // toward the convergence cell
293  //--------------------------------------------------------------
294  n = pathcells_x.size(); m = n / 2;
295  for (i=0;i<m;i++)
296  {
297  v = pathcells_x[i];
298  pathcells_x[i] = pathcells_x[n-1-i];
299  pathcells_x[n-1-i] = v;
300 
301  v = pathcells_y[i];
302  pathcells_y[i] = pathcells_y[n-1-i];
303  pathcells_y[n-1-i] = v;
304  }
305 
306  // STEP 3: Trace-foward toward the target
307  //-------------------------------------
308  x = passCellFound_x; y = passCellFound_y;
309 
310  while ( (v=grid[x+size_x*y]) != CELL_TARGET )
311  {
312  // Add cell to the path
313  pathcells_x.push_back(x);
314  pathcells_y.push_back(y);
315 
316  // Follow the "positive gradient" toward the target:
317  static signed char dx=0,dy=0;
318  c=grid[x-1+size_x*y]; if (c>v && c!=CELL_OBSTACLE) { v=c; dx=-1; dy= 0; }
319  c=grid[x+1+size_x*y]; if (c>v && c!=CELL_OBSTACLE) { v=c; dx= 1; dy= 0; }
320  c=grid[x+size_x*(y-1)]; if (c>v && c!=CELL_OBSTACLE) { v=c; dx= 0; dy=-1; }
321  c=grid[x+size_x*(y+1)]; if (c>v && c!=CELL_OBSTACLE) { v=c; dx= 0; dy= 1; }
322 
323  c=grid[x-1+size_x*(y-1)]; if (c>v && c!=CELL_OBSTACLE) { v=c; dx=-1; dy= -1; }
324  c=grid[x+1+size_x*(y-1)]; if (c>v && c!=CELL_OBSTACLE) { v=c; dx= 1; dy= -1; }
325  c=grid[x-1+size_x*(y+1)]; if (c>v && c!=CELL_OBSTACLE) { v=c; dx=-1; dy= 1; }
326  c=grid[x+1+size_x*(y+1)]; if (c>v && c!=CELL_OBSTACLE) { v=c; dx= 1; dy= 1; }
327 
328  ASSERT_(dx!=0 || dy!=0);
329  x+=dx;
330  y+=dy;
331  }
332 
333 
334  // STEP 4: Translate the path-of-cells to a path-of-2d-points with subsampling
335  //-------------------------------------------------------------------------------
336  path.clear();
337  n = pathcells_x.size();
338  float xx,yy;
339  float last_xx = origin.x, last_yy = origin.y;
340  for (i=0;i<n;i++)
341  {
342  // Get cell coordinates:
343  xx = theMap.idx2x( pathcells_x[i] );
344  yy = theMap.idx2y( pathcells_y[i] );
345 
346 
347  // Enough distance??
348  if (sqrt(square(xx-last_xx)+square(yy-last_yy)) > minStepInReturnedPath)
349  {
350  // Add to the path:
351  path.push_back(CPoint2D(xx,yy));
352 
353  // For the next iteration:
354  last_xx = xx;
355  last_yy = yy;
356  }
357 
358  }
359 
360  // Add the target point:
361  path.push_back(CPoint2D(target.x,target.y));
362 
363  // That's all!! :-)
364 
365 }
const GLdouble * v
Definition: glew.h:1296
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
int x2idx(float x) const
Transform a coordinate value into a cell index.
GLenum GLenum GLvoid * row
Definition: glew.h:2903
double y
X,Y coordinates.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
#define min(a, b)
unsigned __int16 uint16_t
Definition: rptypes.h:46
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...
const GLfloat * c
Definition: glew.h:10088
STL namespace.
float robotRadius
The aproximate robot radius used in the planification. Default is 0.35m.
float getResolution() const
Returns the resolution of the grid map.
float getYMin() const
Returns the "y" coordinate of top side of grid map.
#define CELL_ORIGIN
float minStepInReturnedPath
The minimum distance between points in the returned found path (default=0.4); Notice that full grid r...
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
float idx2x(const size_t cx) const
Transform a cell index into a coordinate value.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
#define CELL_OBSTACLE
GLsizei n
Definition: glew.h:5051
float occupancyThreshold
The maximum occupancy probability to consider a cell as an obstacle, default=0.5. ...
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
float getXMin() const
Returns the "x" coordinate of left side of grid map.
typedef int(WINAPI *PFNWGLRELEASEPBUFFERDCARBPROC)(HPBUFFERARB hPbuffer
float getYMax() const
Returns the "y" coordinate of bottom side of grid map.
A class used to store a 2D point.
Definition: CPoint2D.h:36
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
#define CELL_TARGET
A class for storing an occupancy grid map.
GLenum target
Definition: glew.h:5023
float getXMax() const
Returns the "x" coordinate of right side of grid map.
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:36
float getCell(int x, int y) const
Read the real valued [0,1] contents of a cell, given its index.
#define ASSERT_(f)
const GLdouble * m
Definition: glew.h:5094
GLuint GLfloat * val
Definition: glew.h:7785
unsigned int getSizeX() const
Returns the horizontal size of grid map in cells count.
Lightweight 2D point.
#define CELL_EMPTY
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092
float idx2y(const size_t cy) const



Page generated by Doxygen 1.8.11 for MRPT 1.5.7 Git: 2190203 Tue May 15 02:01:15 2018 +0200 at miƩ may 16 12:40:16 CEST 2018