Main MRPT website > C++ reference for MRPT 1.5.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 ),
27  minStepInReturnedPath (0.4f),
28  robotRadius(0.35f)
29 {
30 }
31 
32 
33 /*---------------------------------------------------------------
34  computePath
35  ---------------------------------------------------------------*/
36 void PlannerSimple2D::computePath(
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 }
mrpt::maps::COccupancyGridMap2D::getXMax
float getXMax() const
Returns the "x" coordinate of right side of grid map.
Definition: maps/COccupancyGridMap2D.h:227
mrpt::maps::COccupancyGridMap2D::getYMin
float getYMin() const
Returns the "y" coordinate of top side of grid map.
Definition: maps/COccupancyGridMap2D.h:230
mrpt::math::TPoint2D::y
double y
X,Y coordinates.
Definition: lightweight_geom_data.h:35
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
min
#define min(a, b)
Definition: rplidar_driver.cpp:43
uint16_t
unsigned __int16 uint16_t
Definition: rptypes.h:46
PlannerSimple2D.h
mrpt::maps::COccupancyGridMap2D::getResolution
float getResolution() const
Returns the resolution of the grid map.
Definition: maps/COccupancyGridMap2D.h:236
n
GLenum GLsizei n
Definition: glext.h:4618
std
STL namespace.
mrpt::nav::PlannerSimple2D::robotRadius
float robotRadius
The aproximate robot radius used in the planification. Default is 0.35m.
Definition: PlannerSimple2D.h:47
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:45
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:36
mrpt::math::square
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
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: maps/COccupancyGridMap2D.h:224
c
const GLubyte * c
Definition: glext.h:5590
mrpt::maps::COccupancyGridMap2D::idx2x
float idx2x(const size_t cx) const
Transform a cell index into a coordinate value.
Definition: maps/COccupancyGridMap2D.h:246
mrpt::nav::PlannerSimple2D::occupancyThreshold
float occupancyThreshold
The maximum occupancy probability to consider a cell as an obstacle, default=0.5. ...
Definition: PlannerSimple2D.h:39
val
int val
Definition: mrpt_jpeglib.h:953
mrpt::maps::COccupancyGridMap2D::getSizeX
unsigned int getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: maps/COccupancyGridMap2D.h:218
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: maps/COccupancyGridMap2D.h:62
v
const GLdouble * v
Definition: glext.h:3603
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
row
GLenum GLenum GLvoid * row
Definition: glext.h:3533
ASSERT_
#define ASSERT_(f)
Definition: maps/CColouredPointsMap.h:278
mrpt::maps::COccupancyGridMap2D::idx2y
float idx2y(const size_t cy) const
Definition: maps/COccupancyGridMap2D.h:247
mrpt::maps::COccupancyGridMap2D::getYMax
float getYMax() const
Returns the "y" coordinate of bottom side of grid map.
Definition: maps/COccupancyGridMap2D.h:233
y
GLenum GLint GLint y
Definition: glext.h:3516
x
GLenum GLint x
Definition: glext.h:3516
mrpt::maps::COccupancyGridMap2D::getSizeY
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: maps/COccupancyGridMap2D.h:221
mrpt::math::TPoint2D
Lightweight 2D point.
Definition: lightweight_geom_data.h:33
CELL_EMPTY
#define CELL_EMPTY
mrpt::maps::COccupancyGridMap2D::x2idx
int x2idx(float x) const
Transform a coordinate value into a cell index.
Definition: maps/COccupancyGridMap2D.h:239
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: maps/COccupancyGridMap2D.h:276


Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at mié abr 15 19:30:12 CEST 2020