Main MRPT website > C++ reference for MRPT 1.5.6
CPTG_DiffDrive_CollisionGridBased.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" // Precomp header
11 
13 
16 #include <mrpt/utils/CTicTac.h>
17 #include <mrpt/math/geometry.h>
20 
21 using namespace mrpt::nav;
22 
23 /** Constructor: possible values in "params":
24  * - ref_distance: The maximum distance in PTGs
25  * - resolution: The cell size
26  * - v_max, w_max: Maximum robot speeds.
27  */
29  V_MAX(.0), W_MAX(.0),
30  turningRadiusReference(.10),
31  m_resolution(0.05),
32  m_stepTimeDuration(0.01),
33  m_collisionGrid(-1,1,-1,1,0.5,this)
34 {
35 }
36 
38 {
41 
42  m_resolution = 0.10;
43  V_MAX = 1.0;
45 
46 }
47 
49 {
52 
53  MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(resolution ,double, m_resolution, cfg,sSection);
54  MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(v_max_mps ,double, V_MAX, cfg,sSection);
55  MRPT_LOAD_HERE_CONFIG_VAR_DEGREES_NO_DEFAULT(w_max_dps ,double, W_MAX, cfg,sSection);
56  MRPT_LOAD_CONFIG_VAR(turningRadiusReference ,double, cfg,sSection);
57 }
59 {
61  const int WN = 25, WV = 30;
62 
64 
65  cfg.write(sSection,"resolution",m_resolution, WN,WV, "Resolution of the collision-check look-up-table [m].");
66  cfg.write(sSection,"v_max_mps",V_MAX, WN,WV, "Maximum linear velocity for trajectories [m/s].");
67  cfg.write(sSection,"w_max_dps", mrpt::utils::RAD2DEG(W_MAX), WN,WV, "Maximum angular velocity for trajectories [deg/s].");
68  cfg.write(sSection,"turningRadiusReference",turningRadiusReference, WN,WV, "An approximate dimension of the robot (not a critical parameter) [m].");
69 
71 
72  MRPT_END
73 }
74 
75 
77 {
78  o << p.x<< p.y<< p.phi<< p.t<< p.dist<< p.v<< p.w;
79  return o;
80 }
82 {
83  i >> p.x>> p.y>> p.phi>> p.t>> p.dist>> p.v>> p.w;
84  return i;
85 }
86 
87 
88 
89 /*---------------------------------------------------------------
90  simulateTrajectories
91  Solve trajectories and fill cells.
92  ---------------------------------------------------------------*/
94  float max_time,
95  float max_dist,
96  unsigned int max_n,
97  float diferencial_t,
98  float min_dist,
99  float *out_max_acc_v,
100  float *out_max_acc_w)
101 {
102  using mrpt::math::square;
103 
104  internal_deinitialize(); // Free previous paths
105 
106  m_stepTimeDuration = diferencial_t;
107 
108  // Reserve the size in the buffers:
110 
111  const float radio_max_robot=1.0f; // Arbitrary "robot radius", only to determine the spacing of points under pure rotation
112 
113  // Aux buffer:
115 
116  float ult_dist, ult_dist1, ult_dist2;
117 
118  // For the grid:
119  float x_min = 1e3f, x_max = -1e3;
120  float y_min = 1e3f, y_max = -1e3;
121 
122  // Para averiguar las maximas ACELERACIONES lineales y angulares:
123  float max_acc_lin, max_acc_ang;
124 
125  max_acc_lin = max_acc_ang = 0;
126 
127  try
128  {
129  for (unsigned int k=0;k<m_alphaValuesCount;k++)
130  {
131  // Simulate / evaluate the trajectory selected by this "alpha":
132  // ------------------------------------------------------------
133  const float alpha = index2alpha( k );
134 
135  points.clear();
136  float t = .0f, dist = .0f, girado = .0f;
137  float x = .0f, y = .0f, phi = .0f, v = .0f, w = .0f, _x = .0f, _y = .0f, _phi = .0f;
138 
139  // Sliding window with latest movement commands (for the optional low-pass filtering):
140  float last_vs[2] = {.0f,.0f}, last_ws[2] = {.0f,.0f};
141 
142  // Add the first, initial point:
143  points.push_back( TCPoint( x,y,phi, t,dist, v,w ) );
144 
145  // Simulate until...
146  while ( t < max_time && dist < max_dist && points.size() < max_n && fabs(girado) < 1.95 * M_PI )
147  {
148  // Max. aceleraciones:
149  if (t>1)
150  {
151  float acc_lin = fabs( (last_vs[0]-last_vs[1])/diferencial_t);
152  float acc_ang = fabs( (last_ws[0]-last_ws[1])/diferencial_t);
153  mrpt::utils::keep_max(max_acc_lin, acc_lin);
154  mrpt::utils::keep_max(max_acc_ang, acc_ang);
155  }
156 
157  // Compute new movement command (v,w):
158  ptgDiffDriveSteeringFunction( alpha,t, x, y, phi, v,w );
159 
160  // History of v/w ----------------------------------
161  last_vs[1]=last_vs[0];
162  last_ws[1]=last_ws[0];
163  last_vs[0] = v;
164  last_ws[0] = w;
165  // -------------------------------------------
166 
167  // Finite difference equation:
168  x += cos(phi)* v * diferencial_t;
169  y += sin(phi)* v * diferencial_t;
170  phi+= w * diferencial_t;
171 
172  // Counters:
173  girado += w * diferencial_t;
174 
175  float v_inTPSpace = sqrt( square(v)+square(w*turningRadiusReference) );
176 
177  dist += v_inTPSpace * diferencial_t;
178 
179  t += diferencial_t;
180 
181  // Save sample if we moved far enough:
182  ult_dist1 = sqrt( square( _x - x )+square( _y - y ) );
183  ult_dist2 = fabs( radio_max_robot* ( _phi - phi ) );
184  ult_dist = std::max( ult_dist1, ult_dist2 );
185 
186  if (ult_dist > min_dist)
187  {
188  // Set the (v,w) to the last record:
189  points.back().v = v;
190  points.back().w = w;
191 
192  // And add the new record:
193  points.push_back( TCPoint( x,y,phi,t,dist,v,w) );
194 
195  // For the next iter:
196  _x = x;
197  _y = y;
198  _phi = phi;
199  }
200 
201  // for the grid:
202  x_min = std::min(x_min,x); x_max = std::max(x_max,x);
203  y_min = std::min(y_min,y); y_max = std::max(y_max,y);
204  }
205 
206  // Add the final point:
207  points.back().v = v;
208  points.back().w = w;
209  points.push_back( TCPoint( x,y,phi,t,dist,v,w) );
210 
211  // Save data to C-Space path structure:
212  m_trajectory[k] = points;
213 
214  } // end for "k"
215 
216  // Save accelerations
217  if (out_max_acc_v) *out_max_acc_v = max_acc_lin;
218  if (out_max_acc_w) *out_max_acc_w = max_acc_ang;
219 
220  // --------------------------------------------------------
221  // Build the speeding-up grid for lambda function:
222  // --------------------------------------------------------
223  const TCellForLambdaFunction defaultCell;
225  x_min-0.5f,x_max+0.5f,
226  y_min-0.5f,y_max+0.5f, 0.25f,
227  &defaultCell);
228 
229  for (uint16_t k=0;k<m_alphaValuesCount;k++)
230  {
231  const uint32_t M = static_cast<uint32_t>(m_trajectory[k].size());
232  for (uint32_t n=0;n<M;n++)
233  {
235  ASSERT_(cell)
236  // Keep limits:
237  mrpt::utils::keep_min(cell->k_min, k );
238  mrpt::utils::keep_max(cell->k_max, k );
239  mrpt::utils::keep_min(cell->n_min, n );
240  mrpt::utils::keep_max(cell->n_max, n );
241  }
242  }
243  }
244  catch(...)
245  {
246  std::cout << format("[CPTG_DiffDrive_CollisionGridBased::simulateTrajectories] Simulation aborted: unexpected exception!\n");
247  }
248 
249 }
250 
251 /** In this class, `out_action_cmd` contains: [0]: linear velocity (m/s), [1]: angular velocity (rad/s) */
252 mrpt::kinematics::CVehicleVelCmdPtr CPTG_DiffDrive_CollisionGridBased::directionToMotionCommand(uint16_t k) const
253 {
254  float v,w;
255  ptgDiffDriveSteeringFunction( index2alpha(k),0, 0, 0, 0, v, w );
256 
258  cmd->lin_vel = v;
259  cmd->ang_vel = w;
260  return mrpt::kinematics::CVehicleVelCmdPtr(cmd);
261 }
262 
263 /*---------------------------------------------------------------
264  getTPObstacle
265  ---------------------------------------------------------------*/
267  const float obsX, const float obsY) const
268 {
269  static const TCollisionCell emptyCell;
270  const TCollisionCell *cell = cellByPos(obsX,obsY);
271  return cell!=NULL ? *cell : emptyCell;
272 }
273 
274 /*---------------------------------------------------------------
275  Updates the info into a cell: It updates the cell only
276  if the distance d for the path k is lower than the previous value:
277  ---------------------------------------------------------------*/
279  const unsigned int icx,
280  const unsigned int icy,
281  const uint16_t k,
282  const float dist )
283 {
284  TCollisionCell *cell = cellByIndex(icx,icy);
285  if (!cell) return;
286 
287  // For such a small number of elements, brute-force search is not such a bad idea:
288  TCollisionCell::iterator itK = cell->end();
289  for (TCollisionCell::iterator it=cell->begin();it!=cell->end();++it)
290  if (it->first==k)
291  {
292  itK = it;
293  break;
294  }
295 
296  if (itK==cell->end())
297  { // New entry:
298  cell->push_back(std::make_pair(k,dist) );
299  }
300  else
301  { // Only update that "k" if the distance is shorter now:
302  if (dist<itK->second)
303  itK->second = dist;
304  }
305 }
306 
307 
308 /*---------------------------------------------------------------
309  Save to file
310  ---------------------------------------------------------------*/
311 bool CPTG_DiffDrive_CollisionGridBased::saveColGridsToFile( const std::string &filename, const mrpt::math::CPolygon & computed_robotShape ) const
312 {
313  try
314  {
316  if (!fo.fileOpenCorrectly()) return false;
317 
318  const uint32_t n = 1; // for backwards compatibility...
319  fo << n;
320  return m_collisionGrid.saveToFile(&fo, computed_robotShape);
321  }
322  catch (...)
323  {
324  return false;
325  }
326 }
327 
328 /*---------------------------------------------------------------
329  Load from file
330  ---------------------------------------------------------------*/
332 {
333  try
334  {
335  mrpt::utils::CFileGZInputStream fi(filename);
336  if (!fi.fileOpenCorrectly()) return false;
337 
338  uint32_t n;
339  fi >> n;
340  if (n!=1) return false; // Incompatible (old) format, just discard and recompute.
341 
342  return m_collisionGrid.loadFromFile(&fi,current_robotShape);
343  }
344  catch(...)
345  {
346  return false;
347  }
348 }
349 
350 const uint32_t COLGRID_FILE_MAGIC = 0xC0C0C0C3;
351 
352 /*---------------------------------------------------------------
353  Save to file
354  ---------------------------------------------------------------*/
356 {
357  try
358  {
359  if (!f) return false;
360 
361  const uint8_t serialize_version = 2; // v1: As of jun 2012, v2: As of dec-2013
362 
363  // Save magic signature && serialization version:
364  *f << COLGRID_FILE_MAGIC << serialize_version;
365 
366  // Robot shape:
367  *f << computed_robotShape;
368 
369  // and standard PTG data:
370  *f << m_parent->getDescription()
371  << m_parent->getAlphaValuesCount()
372  << static_cast<float>(m_parent->getMax_V())
373  << static_cast<float>(m_parent->getMax_W());
374 
375  *f << m_x_min << m_x_max << m_y_min << m_y_max;
376  *f << m_resolution;
377 
378  //v1 was: *f << m_map;
379  uint32_t N = m_map.size();
380  *f << N;
381  for (uint32_t i=0;i<N;i++)
382  {
383  uint32_t M = m_map[i].size();
384  *f << M;
385  for (uint32_t k=0;k<M;k++)
386  *f << m_map[i][k].first << m_map[i][k].second;
387  }
388 
389  return true;
390  }
391  catch(...)
392  {
393  return false;
394  }
395 }
396 
397 /*---------------------------------------------------------------
398  loadFromFile
399  ---------------------------------------------------------------*/
401 {
402  try
403  {
404  if (!f) return false;
405 
406  // Return false if the file contents doesn't match what we expected:
407  uint32_t file_magic;
408  *f >> file_magic;
409 
410  // It doesn't seem to be a valid file or was in an old format, just recompute the grid:
411  if (COLGRID_FILE_MAGIC!=file_magic)
412  return false;
413 
414  uint8_t serialized_version;
415  *f >> serialized_version;
416 
417  switch (serialized_version)
418  {
419  case 2:
420  {
421  mrpt::math::CPolygon stored_shape;
422  *f >> stored_shape;
423 
424  const bool shapes_match =
425  ( stored_shape.size()==current_robotShape.size() &&
426  std::equal(stored_shape.begin(),stored_shape.end(), current_robotShape.begin() ) );
427 
428  if (!shapes_match) return false; // Must recompute if the robot shape changed.
429  }
430  break;
431 
432  case 1:
433  default:
434  // Unknown version: Maybe we are loading a file from a more recent version of MRPT? Whatever, we can't read it: It's safer just to re-generate the PTG data
435  return false;
436  };
437 
438  // Standard PTG data:
439  const std::string expected_desc = m_parent->getDescription();
440  std::string desc;
441  *f >> desc;
442  if (desc!=expected_desc) return false;
443 
444  // and standard PTG data:
445 #define READ_UINT16_CHECK_IT_MATCHES_STORED(_VAR) { uint16_t ff; *f >> ff; if (ff!=_VAR) return false; }
446 #define READ_FLOAT_CHECK_IT_MATCHES_STORED(_VAR) { float ff; *f >> ff; if (std::abs(ff-_VAR)>1e-4f) return false; }
447 #define READ_DOUBLE_CHECK_IT_MATCHES_STORED(_VAR) { double ff; *f >> ff; if (std::abs(ff-_VAR)>1e-6) return false; }
448 
449  READ_UINT16_CHECK_IT_MATCHES_STORED(m_parent->getAlphaValuesCount())
450  READ_FLOAT_CHECK_IT_MATCHES_STORED(m_parent->getMax_V())
451  READ_FLOAT_CHECK_IT_MATCHES_STORED(m_parent->getMax_W())
452 
453  // Cell dimensions:
459 
460  // OK, all parameters seem to be exactly the same than when we precomputed the table: load it.
461  //v1 was: *f >> m_map;
462  uint32_t N;
463  *f >> N;
464  m_map.resize(N);
465  for (uint32_t i=0;i<N;i++)
466  {
467  uint32_t M;
468  *f >> M;
469  m_map[i].resize(M);
470  for (uint32_t k=0;k<M;k++)
471  *f >> m_map[i][k].first >> m_map[i][k].second;
472  }
473 
474  return true;
475  }
476  catch(std::exception &e)
477  {
478  std::cerr << "[CCollisionGrid::loadFromFile] " << e.what();
479  return false;
480  }
481  catch(...)
482  {
483  return false;
484  }
485 }
486 
487 bool CPTG_DiffDrive_CollisionGridBased::inverseMap_WS2TP(double x, double y, int &out_k, double &out_d, double tolerance_dist) const
488 {
489  using mrpt::math::square;
490 
491  ASSERTMSG_(m_alphaValuesCount>0, "Have you called simulateTrajectories() first?")
492 
493  // -------------------------------------------------------------------
494  // Optimization: (24-JAN-2007 @ Jose Luis Blanco):
495  // Use a "grid" to determine the range of [k,d] values to check!!
496  // If the point (x,y) is not found in the grid, then directly skip
497  // to the next step.
498  // -------------------------------------------------------------------
499  uint16_t k_min = 0, k_max = m_alphaValuesCount-1;
500  uint32_t n_min = 0, n_max = 0;
501  bool at_least_one = false;
502 
503  // Cell indexes:
504  int cx0 = m_lambdaFunctionOptimizer.x2idx(x);
505  int cy0 = m_lambdaFunctionOptimizer.y2idx(y);
506 
507  // (cx,cy)
508  for (int cx=cx0-1;cx<=cx0+1;cx++)
509  {
510  for (int cy=cy0-1;cy<=cy0+1;cy++)
511  {
512  const TCellForLambdaFunction *cell = m_lambdaFunctionOptimizer.cellByIndex(cx,cy);
513  if (cell && !cell->isEmpty())
514  {
515  if (!at_least_one)
516  {
517  k_min = cell->k_min; k_max = cell->k_max;
518  n_min = cell->n_min; n_max = cell->n_max;
519  at_least_one = true;
520  }
521  else
522  {
523  mrpt::utils::keep_min(k_min, cell->k_min);
524  mrpt::utils::keep_max(k_max, cell->k_max);
525 
526  mrpt::utils::keep_min(n_min, cell->n_min);
527  mrpt::utils::keep_max(n_max, cell->n_max);
528  }
529  }
530  }
531  }
532 
533  // Try to find a closest point to the paths:
534  // ----------------------------------------------
535  int selected_k = -1;
536  float selected_d= 0;
537  float selected_dist = std::numeric_limits<float>::max();
538 
539  if (at_least_one) // Otherwise, don't even lose time checking...
540  {
541  ASSERT_BELOW_(k_max, m_trajectory.size())
542  for (int k=k_min;k<=k_max;k++)
543  {
544  const size_t n_real = m_trajectory[k].size();
545  const uint32_t n_max_this = std::min( static_cast<uint32_t>(n_real ? n_real-1 : 0), n_max);
546 
547  for (uint32_t n = n_min;n<=n_max_this; n++)
548  {
549  const float dist_a_punto= square( m_trajectory[k][n].x - x ) + square( m_trajectory[k][n].y - y );
550  if (dist_a_punto<selected_dist)
551  {
552  selected_dist = dist_a_punto;
553  selected_k = k;
554  selected_d = m_trajectory[k][n].dist;
555  }
556  }
557  }
558  }
559 
560  if (selected_k!=-1)
561  {
562  out_k = selected_k;
563  out_d = selected_d / refDistance;
564  return (selected_dist <= square(tolerance_dist));
565  }
566 
567  // If not found, compute an extrapolation:
568 
569  // ------------------------------------------------------------------------------------
570  // Given a point (x,y), compute the "k_closest" whose extrapolation
571  // is closest to the point, and the associated "d_closest" distance,
572  // which can be normalized by "1/refDistance" to get TP-Space distances.
573  // ------------------------------------------------------------------------------------
574  selected_dist = std::numeric_limits<float>::max();
575  for (uint16_t k=0;k<m_alphaValuesCount;k++)
576  {
577  const int n = int (m_trajectory[k].size()) -1;
578  const float dist_a_punto = square( m_trajectory[k][n].dist ) + square( m_trajectory[k][n].x - x ) + square( m_trajectory[k][n].y - y );
579 
580  if (dist_a_punto<selected_dist)
581  {
582  selected_dist = dist_a_punto;
583  selected_k = k;
584  selected_d = dist_a_punto;
585  }
586  }
587 
588  selected_d = std::sqrt(selected_d);
589 
590  out_k = selected_k;
591  out_d = selected_d / refDistance;
592 
593  // If the target dist. > refDistance, then it's normal that we had to extrapolate.
594  // Otherwise, it may actually mean that the target is not reachable by this set of paths:
595  const float target_dist = std::sqrt( x*x+y*y );
596  return (target_dist>target_dist);
597 }
598 
600 {
601  ASSERTMSG_(m_trajectory.empty(), "Changing reference distance not allowed in this class after initialization!");
602  this->refDistance = refDist;
603 }
604 
606 {
607  ASSERTMSG_(m_trajectory.empty(), "Changing robot shape not allowed in this class after initialization!");
608 }
609 
611 {
612  m_trajectory.clear(); // Free trajectories
613 }
614 
615 void CPTG_DiffDrive_CollisionGridBased::internal_initialize(const std::string & cacheFilename, const bool verbose)
616 {
617  using namespace std;
618 
619  MRPT_START
620 
621  if (verbose)
622  cout << endl << "[CPTG_DiffDrive_CollisionGridBased::initialize] Starting... *** THIS MAY TAKE A WHILE, BUT MUST BE COMPUTED ONLY ONCE!! **" << endl;
623 
624  // Sanity checks:
625  ASSERTMSG_(!m_robotShape.empty(),"Robot shape was not defined");
626  ASSERTMSG_(m_robotShape.size()>=3,"Robot shape must have 3 or more vertices");
627  ASSERT_(refDistance>0);
628  ASSERT_(V_MAX>0);
629  ASSERT_(W_MAX>0);
631 
632  mrpt::utils::CTicTac tictac;
633  tictac.Tic();
634 
635  if (verbose) cout << "Initializing PTG '" << cacheFilename << "'...";
636 
637  // Simulate paths:
638  const float min_dist = 0.015f;
640  100, // max.tim,
641  refDistance, // max.dist,
642  10*refDistance/min_dist, // max.n,
643  0.0005f, // diferencial_t
644  min_dist // min_dist
645  );
646 
647  // Just for debugging, etc.
648  //debugDumpInFiles(n);
649 
650 
651  // Check for collisions between the robot shape and the grid cells:
652  // ----------------------------------------------------------------------------
654 
655  const size_t Ki = getAlphaValuesCount();
656  ASSERTMSG_(Ki>0, "The PTG seems to be not initialized!");
657 
658  // Load the cached version, if possible
659  if ( loadColGridsFromFile( cacheFilename, m_robotShape ) )
660  {
661  if (verbose)
662  cout << "loaded from file OK" << endl;
663  }
664  else
665  {
666  // BUGFIX: In case we start reading the file and in the end detected an error,
667  // we must make sure that there's space enough for the grid:
669 
670  const int grid_cx_max = m_collisionGrid.getSizeX()-1;
671  const int grid_cy_max = m_collisionGrid.getSizeY()-1;
672  const double half_cell = m_collisionGrid.getResolution()*0.5;
673 
674  const size_t nVerts = m_robotShape.verticesCount();
675  std::vector<mrpt::math::TPoint2D> transf_shape(nVerts); // The robot shape at each location
676 
677  // RECOMPUTE THE COLLISION GRIDS:
678  // ---------------------------------------
679  for (size_t k=0;k<Ki;k++)
680  {
681  const size_t nPoints = getPathStepCount(k);
682  ASSERT_(nPoints>1)
683 
684  for (size_t n=0;n<(nPoints-1);n++)
685  {
686  // Translate and rotate the robot shape at this C-Space pose:
688  getPathPose(k, n, p);
689 
690  mrpt::math::TPoint2D bb_min(std::numeric_limits<double>::max(),std::numeric_limits<double>::max());
691  mrpt::math::TPoint2D bb_max(-std::numeric_limits<double>::max(),-std::numeric_limits<double>::max());
692 
693  for (size_t m = 0;m<nVerts;m++)
694  {
695  transf_shape[m].x = p.x + cos(p.phi)*m_robotShape.GetVertex_x(m)-sin(p.phi)*m_robotShape.GetVertex_y(m);
696  transf_shape[m].y = p.y + sin(p.phi)*m_robotShape.GetVertex_x(m)+cos(p.phi)*m_robotShape.GetVertex_y(m);
697  mrpt::utils::keep_max( bb_max.x, transf_shape[m].x); mrpt::utils::keep_max( bb_max.y, transf_shape[m].y);
698  mrpt::utils::keep_min( bb_min.x, transf_shape[m].x); mrpt::utils::keep_min( bb_min.y, transf_shape[m].y);
699  }
700 
701  // Robot shape polygon:
702  const mrpt::math::TPolygon2D poly(transf_shape);
703 
704  // Get the range of cells that may collide with this shape:
705  const int ix_min = std::max(0,m_collisionGrid.x2idx(bb_min.x)-1);
706  const int iy_min = std::max(0,m_collisionGrid.y2idx(bb_min.y)-1);
707  const int ix_max = std::min(m_collisionGrid.x2idx(bb_max.x)+1,grid_cx_max);
708  const int iy_max = std::min(m_collisionGrid.y2idx(bb_max.y)+1,grid_cy_max);
709 
710  for (int ix=ix_min;ix<ix_max;ix++)
711  {
712  const double cx = m_collisionGrid.idx2x(ix) - half_cell;
713 
714  for (int iy=iy_min;iy<iy_max;iy++)
715  {
716  const double cy = m_collisionGrid.idx2y(iy) - half_cell;
717 
718  if ( poly.contains( mrpt::math::TPoint2D(cx,cy) ) )
719  {
720  // Collision!! Update cell info:
721  const float d = this->getPathDist(k, n);
722  m_collisionGrid.updateCellInfo(ix ,iy , k,d);
723  m_collisionGrid.updateCellInfo(ix-1,iy , k,d);
724  m_collisionGrid.updateCellInfo(ix ,iy-1, k,d);
725  m_collisionGrid.updateCellInfo(ix-1,iy-1, k,d);
726  }
727  } // for iy
728  } // for ix
729 
730  } // n
731 
732  if (verbose)
733  cout << k << "/" << Ki << ",";
734  } // k
735 
736  if (verbose)
737  cout << format("Done! [%.03f sec]\n",tictac.Tac() );
738 
739  // save it to the cache file for the next run:
740  saveColGridsToFile( cacheFilename, m_robotShape );
741 
742  } // "else" recompute all PTG
743 
744  MRPT_END
745 }
746 
748 {
749  ASSERT_(k<m_trajectory.size());
750 
751  return m_trajectory[k].size();
752 }
753 
755 {
756  ASSERT_(k<m_trajectory.size());
757  ASSERT_(step<m_trajectory[k].size());
758 
759  p.x = m_trajectory[k][step].x;
760  p.y = m_trajectory[k][step].y;
761  p.phi = m_trajectory[k][step].phi;
762 }
763 
765 {
766  ASSERT_(k<m_trajectory.size());
767  ASSERT_(step<m_trajectory[k].size());
768 
769  return m_trajectory[k][step].dist;
770 }
771 
773 {
774  ASSERT_(k<m_trajectory.size());
775  const size_t numPoints = m_trajectory[k].size();
776 
777  ASSERT_(numPoints > 0);
778 
779  for (size_t n=0; n < numPoints-1; n++)
780  {
781  if (m_trajectory[k][n + 1].dist >= dist)
782  {
783  out_step = n;
784  return true;
785  }
786  }
787 
788  out_step = numPoints-1;
789  return false;
790 }
791 
793  double ox, double oy,
794  std::vector<double> &tp_obstacles) const
795 {
796  ASSERTMSG_(!m_trajectory.empty(), "PTG has not been initialized!");
797  const TCollisionCell & cell = m_collisionGrid.getTPObstacle(ox, oy);
798  // Keep the minimum distance:
799  for (TCollisionCell::const_iterator i = cell.begin(); i != cell.end(); ++i) {
800  const double dist = i->second;
801  internal_TPObsDistancePostprocess(ox,oy,dist, tp_obstacles[i->first]);
802  }
803 }
804 
805 void CPTG_DiffDrive_CollisionGridBased::updateTPObstacleSingle(double ox, double oy, uint16_t k, double &tp_obstacle_k) const
806 {
807  ASSERTMSG_(!m_trajectory.empty(), "PTG has not been initialized!");
808  const TCollisionCell & cell = m_collisionGrid.getTPObstacle(ox, oy);
809  // Keep the minimum distance:
810  for (TCollisionCell::const_iterator i = cell.begin(); i != cell.end(); ++i)
811  if (i->first == k) {
812  const double dist = i->second;
813  internal_TPObsDistancePostprocess(ox,oy,dist, tp_obstacle_k);
814  }
815 }
816 
818 {
821 
823  in >> version;
824  switch (version)
825  {
826  case 0:
828  in >>V_MAX >> W_MAX
830  >> m_robotShape
831  >> m_resolution
832  >> m_trajectory;
833  break;
834  default:
836  };
837 }
838 
840 {
843 
844  const uint8_t version = 0;
845  out << version;
846 
847  out << V_MAX << W_MAX
849  << m_robotShape
850  << m_resolution
851  << m_trajectory;
852 }
853 
855 {
856  return mrpt::kinematics::CVehicleVelCmdPtr( new mrpt::kinematics::CVehicleVelCmd_DiffDriven() );
857 }
858 
860 {
861  return m_stepTimeDuration;
862 }
863 
void internal_TPObsDistancePostprocess(const double ox, const double oy, const double new_tp_obs_dist, double &inout_tp_obs) const
To be called by implementors of updateTPObstacle() and updateTPObstacleSingle() to honor the user set...
size_t getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: CDynamicGrid.h:223
const GLdouble * v
Definition: glew.h:1296
void updateTPObstacle(double ox, double oy, std::vector< double > &tp_obstacles) const MRPT_OVERRIDE
Updates the radial map of closest TP-Obstacles given a single obstacle point at (ox,oy)
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
double getPathDist(uint16_t k, uint32_t step) const MRPT_OVERRIDE
Access path k ([0,N-1]=>[-pi,pi] in alpha): traversed distance at discrete step step.
void internal_shape_saveToStream(mrpt::utils::CStream &out) const
double y
X,Y coordinates.
#define READ_DOUBLE_CHECK_IT_MATCHES_STORED(_VAR)
#define min(a, b)
unsigned __int16 uint16_t
Definition: rptypes.h:46
uint16_t getAlphaValuesCount() const
Get the number of different, discrete paths in this family.
Specifies the min/max values for "k" and "n", respectively.
double DEG2RAD(const double x)
Degrees to radians.
GLclampf GLclampf GLclampf alpha
Definition: glew.h:1425
virtual void internal_writeToStream(mrpt::utils::CStream &out) const
Trajectory points in C-Space for non-holonomic robots.
mrpt::utils::CDynamicGrid< TCellForLambdaFunction > m_lambdaFunctionOptimizer
This grid will contain indexes data for speeding-up the default, brute-force lambda function...
#define ASSERT_BELOW_(__A, __B)
Scalar * iterator
Definition: eigen_plugins.h:23
GLubyte GLubyte GLubyte GLubyte w
Definition: glew.h:1797
virtual void loadFromConfigFile(const mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) MRPT_OVERRIDE
Parameters accepted by this base class:
GLdouble GLdouble t
Definition: glew.h:1303
virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) const MRPT_OVERRIDE
This method saves the options to a ".ini"-like file or memory-stored string list. ...
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:25
bool saveToFile(mrpt::utils::CStream *fil, const mrpt::math::CPolygon &computed_robotShape) const
Save to file, true = OK.
std::vector< TCPoint > TCPointVector
#define M_PI
Definition: bits.h:78
const Scalar * const_iterator
Definition: eigen_plugins.h:24
void Tic()
Starts the stopwatch.
Definition: CTicTac.cpp:77
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: CDynamicGrid.h:220
void loadShapeFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section)
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
const uint32_t COLGRID_FILE_MAGIC
GLuint in
Definition: glew.h:7146
#define READ_FLOAT_CHECK_IT_MATCHES_STORED(_VAR)
This class allows loading and storing values and vectors of different types from a configuration text...
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
void internal_readFromStream(mrpt::utils::CStream &in) MRPT_OVERRIDE
unsigned char uint8_t
Definition: rptypes.h:43
TCollisionCell * cellByPos(double x, double y)
Returns a pointer to the contents of a cell given by its coordinates, or NULL if it is out of the map...
Definition: CDynamicGrid.h:183
int x2idx(double x) const
Transform a coordinate values into cell indexes.
Definition: CDynamicGrid.h:241
virtual void loadDefaultParams()
Loads a set of default parameters into the PTG.
bool getPathStepForDist(uint16_t k, double dist, uint32_t &out_step) const MRPT_OVERRIDE
Access path k ([0,N-1]=>[-pi,pi] in alpha): largest step count for which the traversed distance is < ...
GLuint GLenum GLsizei GLsizei GLboolean void * points
Definition: glew.h:7748
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
double RAD2DEG(const double x)
Radians to degrees.
GLsizei n
Definition: glew.h:5051
virtual void internal_readFromStream(mrpt::utils::CStream &in)
#define MRPT_END
void internal_shape_loadFromStream(mrpt::utils::CStream &in)
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Transparently opens a compressed "gz" file and reads uncompressed data from it.
This class implements a high-performance stopwatch.
Definition: CTicTac.h:24
virtual void loadFromConfigFile(const mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) MRPT_OVERRIDE
Possible values in "params" (those in CParameterizedTrajectoryGenerator, which is called internally...
CPTG_DiffDrive_CollisionGridBased()
Constructor: possible values in "params":
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) const MRPT_OVERRIDE
This method saves the options to a ".ini"-like file or memory-stored string list. ...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
void internal_processNewRobotShape() MRPT_OVERRIDE
Will be called whenever the robot shape is set / updated.
virtual void setRefDistance(const double refDist) MRPT_OVERRIDE
Launches an exception in this class: it is not allowed in numerical integration-based PTGs to change ...
bool loadColGridsFromFile(const std::string &filename, const mrpt::math::CPolygon &current_robotShape)
double idx2y(int cy) const
Definition: CDynamicGrid.h:254
int version
Definition: mrpt_jpeglib.h:898
double getPathStepDuration() const MRPT_OVERRIDE
Returns the duration (in seconds) of each "step".
GLfloat GLfloat p
Definition: glew.h:10113
typedef int(WINAPI *PFNWGLRELEASEPBUFFERDCARBPROC)(HPBUFFERARB hPbuffer
void simulateTrajectories(float max_time, float max_dist, unsigned int max_n, float diferencial_t, float min_dist, float *out_max_acc_v=NULL, float *out_max_acc_w=NULL)
Numerically solve the diferential equations to generate a family of trajectories. ...
double GetVertex_x(size_t i) const
Methods for accessing the vertices.
Definition: CPolygon.h:41
GLint * first
Definition: glew.h:1433
void updateCellInfo(const unsigned int icx, const unsigned int icy, const uint16_t k, const float dist)
Updates the info into a cell: It updates the cell only if the distance d for the path k is lower than...
std::vector< std::pair< uint16_t, float > > TCollisionCell
A list of all the pairs (alpha,distance) such as the robot collides at that cell. ...
size_t getPathStepCount(uint16_t k) const MRPT_OVERRIDE
Access path k ([0,N-1]=>[-pi,pi] in alpha): number of discrete "steps" along the trajectory.
void updateTPObstacleSingle(double ox, double oy, uint16_t k, double &tp_obstacle_k) const MRPT_OVERRIDE
Like updateTPObstacle() but for one direction only (k) in TP-Space.
double y
X,Y coordinates.
virtual mrpt::kinematics::CVehicleVelCmdPtr directionToMotionCommand(uint16_t k) const MRPT_OVERRIDE
In this class, out_action_cmd contains: [0]: linear velocity (m/s), [1]: angular velocity (rad/s)...
void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) const MRPT_OVERRIDE
This method saves the options to a ".ini"-like file or memory-stored string list. ...
GLsizeiptr size
Definition: glew.h:1586
#define MRPT_START
#define MRPT_LOAD_CONFIG_VAR(variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
bool contains(const TPoint2D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge). This works for concave...
#define CFileGZOutputStream
Saves data to a file and transparently compress the data using the given compression level...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
void write(const std::string &section, const std::string &name, const data_t &value, const int name_padding_width=-1, const int value_padding_width=-1, const std::string &comment=std::string())
bool loadFromFile(mrpt::utils::CStream *fil, const mrpt::math::CPolygon &current_robotShape)
Load from file, true = OK.
bool fileOpenCorrectly()
Returns true if the file was open without errors.
virtual bool inverseMap_WS2TP(double x, double y, int &out_k, double &out_d, double tolerance_dist=0.10) const MRPT_OVERRIDE
The default implementation in this class relies on a look-up-table.
T square(const T x)
Inline function for the square of a number.
bool saveColGridsToFile(const std::string &filename, const mrpt::math::CPolygon &computed_robotShape) const
Lightweight 2D pose.
#define ASSERT_(f)
int y2idx(double y) const
Definition: CDynamicGrid.h:242
::mrpt::utils::CStream & operator>>(mrpt::utils::CStream &in, CAbstractHolonomicReactiveMethodPtr &pObj)
void internal_initialize(const std::string &cacheFilename=std::string(), const bool verbose=true) MRPT_OVERRIDE
Must be called after setting all PTG parameters and before requesting converting obstacles to TP-Spac...
#define MRPT_LOAD_HERE_CONFIG_VAR_DEGREES_NO_DEFAULT(variableName, variableType, targetVariable, configFileObject, sectionNameStr)
void internal_writeToStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
#define READ_UINT16_CHECK_IT_MATCHES_STORED(_VAR)
const GLdouble * m
Definition: glew.h:5094
virtual void loadDefaultParams() MRPT_OVERRIDE
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
void setSize(const double x_min, const double x_max, const double y_min, const double y_max, const double resolution, const T *fill_value=NULL)
Changes the size of the grid, ERASING all previous contents.
Definition: CDynamicGrid.h:69
void getPathPose(uint16_t k, uint32_t step, mrpt::math::TPose2D &p) const MRPT_OVERRIDE
Access path k ([0,N-1]=>[-pi,pi] in alpha): pose of the vehicle at discrete step step.
virtual void ptgDiffDriveSteeringFunction(float alpha, float t, float x, float y, float phi, float &v, float &w) const =0
The main method to be implemented in derived classes: it defines the differential-driven differential...
mrpt::utils::CStream NAV_IMPEXP & operator<<(mrpt::utils::CStream &o, const mrpt::nav::TCPoint &p)
size_t verticesCount() const
Returns the vertices count in the polygon:
Definition: CPolygon.h:45
#define MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(variableName, variableType, targetVariable, configFileObject, sectionNameStr)
unsigned __int32 uint32_t
Definition: rptypes.h:49
void loadDefaultParams() MRPT_OVERRIDE
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
Lightweight 2D point.
double idx2x(int cx) const
Transform a cell index into a coordinate value of the cell central point.
Definition: CDynamicGrid.h:253
#define ASSERTMSG_(f, __ERROR_MSG)
void internal_deinitialize() MRPT_OVERRIDE
This must be called to de-initialize the PTG if some parameter is to be changed.
const TCollisionCell & getTPObstacle(const float obsX, const float obsY) const
For an obstacle (x,y), returns a vector with all the pairs (a,d) such as the robot collides...
double GetVertex_y(size_t i) const
Definition: CPolygon.h:42
double phi
Orientation (rads)
double index2alpha(uint16_t k) const
Alpha value for the discrete corresponding value.
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
virtual mrpt::kinematics::CVehicleVelCmdPtr getSupportedKinematicVelocityCommand() const MRPT_OVERRIDE
Returns an empty kinematic velocity command object of the type supported by this PTG.
Kinematic model for Ackermann-like or differential-driven vehicles.
double getResolution() const
Returns the resolution of the grid map.
Definition: CDynamicGrid.h:238
2D polygon, inheriting from std::vector<TPoint2D>.
uint16_t m_alphaValuesCount
The number of discrete values for "alpha" between -PI and +PI.
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018