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



Page generated by Doxygen 1.8.14 for MRPT 2.0.2 Git: 9efc2a654 Mon Apr 6 11:24:47 2020 +0200 at lun abr 6 11:30:12 CEST 2020