MRPT  1.9.9
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-2019, 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:
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):
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  << static_cast<float>(m_parent->getMax_V())
400  << static_cast<float>(m_parent->getMax_W());
401 
402  *f << m_x_min << m_x_max << m_y_min << m_y_max;
403  *f << m_resolution;
404 
405  // v1 was: *f << m_map;
406  uint32_t N = m_map.size();
407  *f << N;
408  for (uint32_t i = 0; i < N; i++)
409  {
410  uint32_t M = m_map[i].size();
411  *f << M;
412  for (uint32_t k = 0; k < M; k++)
413  *f << m_map[i][k].first << m_map[i][k].second;
414  }
415 
416  return true;
417  }
418  catch (...)
419  {
420  return false;
421  }
422 }
423 
424 /*---------------------------------------------------------------
425  loadFromFile
426  ---------------------------------------------------------------*/
429  const mrpt::math::CPolygon& current_robotShape)
430 {
431  try
432  {
433  if (!f) return false;
434 
435  // Return false if the file contents doesn't match what we expected:
436  uint32_t file_magic;
437  *f >> file_magic;
438 
439  // It doesn't seem to be a valid file or was in an old format, just
440  // recompute the grid:
441  if (COLGRID_FILE_MAGIC != file_magic) return false;
442 
443  uint8_t serialized_version;
444  *f >> serialized_version;
445 
446  switch (serialized_version)
447  {
448  case 2:
449  {
450  mrpt::math::CPolygon stored_shape;
451  *f >> stored_shape;
452 
453  const bool shapes_match =
454  (stored_shape.size() == current_robotShape.size() &&
455  std::equal(
456  stored_shape.begin(), stored_shape.end(),
457  current_robotShape.begin()));
458 
459  if (!shapes_match)
460  return false; // Must recompute if the robot shape changed.
461  }
462  break;
463 
464  case 1:
465  default:
466  // Unknown version: Maybe we are loading a file from a more
467  // recent version of MRPT? Whatever, we can't read it: It's
468  // safer just to re-generate the PTG data
469  return false;
470  };
471 
472  // Standard PTG data:
473  const std::string expected_desc = m_parent->getDescription();
474  std::string desc;
475  *f >> desc;
476  if (desc != expected_desc) return false;
477 
478 // and standard PTG data:
479 #define READ_UINT16_CHECK_IT_MATCHES_STORED(_VAR) \
480  { \
481  uint16_t ff; \
482  *f >> ff; \
483  if (ff != _VAR) return false; \
484  }
485 #define READ_FLOAT_CHECK_IT_MATCHES_STORED(_VAR) \
486  { \
487  float ff; \
488  *f >> ff; \
489  if (std::abs(ff - _VAR) > 1e-4f) return false; \
490  }
491 #define READ_DOUBLE_CHECK_IT_MATCHES_STORED(_VAR) \
492  { \
493  double ff; \
494  *f >> ff; \
495  if (std::abs(ff - _VAR) > 1e-6) return false; \
496  }
497 
498  READ_UINT16_CHECK_IT_MATCHES_STORED(m_parent->getAlphaValuesCount())
499  READ_FLOAT_CHECK_IT_MATCHES_STORED(m_parent->getMax_V())
500  READ_FLOAT_CHECK_IT_MATCHES_STORED(m_parent->getMax_W())
501 
502  // Cell dimensions:
508 
509  // OK, all parameters seem to be exactly the same than when we
510  // precomputed the table: load it.
511  // v1 was: *f >> m_map;
512  uint32_t N;
513  *f >> N;
514  m_map.resize(N);
515  for (uint32_t i = 0; i < N; i++)
516  {
517  uint32_t M;
518  *f >> M;
519  m_map[i].resize(M);
520  for (uint32_t k = 0; k < M; k++)
521  *f >> m_map[i][k].first >> m_map[i][k].second;
522  }
523 
524  return true;
525  }
526  catch (const std::exception& e)
527  {
528  std::cerr << "[CCollisionGrid::loadFromFile] " << e.what();
529  return false;
530  }
531  catch (...)
532  {
533  return false;
534  }
535 }
536 
538  double x, double y, int& out_k, double& out_d, double tolerance_dist) const
539 {
540  using mrpt::square;
541 
542  ASSERTMSG_(
543  m_alphaValuesCount > 0,
544  "Have you called simulateTrajectories() first?");
545 
546  // -------------------------------------------------------------------
547  // Optimization: (24-JAN-2007 @ Jose Luis Blanco):
548  // Use a "grid" to determine the range of [k,d] values to check!!
549  // If the point (x,y) is not found in the grid, then directly skip
550  // to the next step.
551  // -------------------------------------------------------------------
552  uint16_t k_min = 0, k_max = m_alphaValuesCount - 1;
553  uint32_t n_min = 0, n_max = 0;
554  bool at_least_one = false;
555 
556  // Cell indexes:
557  int cx0 = m_lambdaFunctionOptimizer.x2idx(x);
558  int cy0 = m_lambdaFunctionOptimizer.y2idx(y);
559 
560  // (cx,cy)
561  for (int cx = cx0 - 1; cx <= cx0 + 1; cx++)
562  {
563  for (int cy = cy0 - 1; cy <= cy0 + 1; cy++)
564  {
565  const TCellForLambdaFunction* cell =
566  m_lambdaFunctionOptimizer.cellByIndex(cx, cy);
567  if (cell && !cell->isEmpty())
568  {
569  if (!at_least_one)
570  {
571  k_min = cell->k_min;
572  k_max = cell->k_max;
573  n_min = cell->n_min;
574  n_max = cell->n_max;
575  at_least_one = true;
576  }
577  else
578  {
579  mrpt::keep_min(k_min, cell->k_min);
580  mrpt::keep_max(k_max, cell->k_max);
581 
582  mrpt::keep_min(n_min, cell->n_min);
583  mrpt::keep_max(n_max, cell->n_max);
584  }
585  }
586  }
587  }
588 
589  // Try to find a closest point to the paths:
590  // ----------------------------------------------
591  int selected_k = -1;
592  float selected_d = 0;
593  float selected_dist = std::numeric_limits<float>::max();
594 
595  if (at_least_one) // Otherwise, don't even lose time checking...
596  {
597  ASSERT_BELOW_(k_max, m_trajectory.size());
598  for (int k = k_min; k <= k_max; k++)
599  {
600  const size_t n_real = m_trajectory[k].size();
601  const uint32_t n_max_this =
602  std::min(static_cast<uint32_t>(n_real ? n_real - 1 : 0), n_max);
603 
604  for (uint32_t n = n_min; n <= n_max_this; n++)
605  {
606  const float dist_a_punto = square(m_trajectory[k][n].x - x) +
607  square(m_trajectory[k][n].y - y);
608  if (dist_a_punto < selected_dist)
609  {
610  selected_dist = dist_a_punto;
611  selected_k = k;
612  selected_d = m_trajectory[k][n].dist;
613  }
614  }
615  }
616  }
617 
618  if (selected_k != -1)
619  {
620  out_k = selected_k;
621  out_d = selected_d / refDistance;
622  return (selected_dist <= square(tolerance_dist));
623  }
624 
625  // If not found, compute an extrapolation:
626 
627  // ------------------------------------------------------------------------------------
628  // Given a point (x,y), compute the "k_closest" whose extrapolation
629  // is closest to the point, and the associated "d_closest" distance,
630  // which can be normalized by "1/refDistance" to get TP-Space distances.
631  // ------------------------------------------------------------------------------------
632  selected_dist = std::numeric_limits<float>::max();
633  for (uint16_t k = 0; k < m_alphaValuesCount; k++)
634  {
635  const int n = int(m_trajectory[k].size()) - 1;
636  const float dist_a_punto = square(m_trajectory[k][n].dist) +
637  square(m_trajectory[k][n].x - x) +
638  square(m_trajectory[k][n].y - y);
639 
640  if (dist_a_punto < selected_dist)
641  {
642  selected_dist = dist_a_punto;
643  selected_k = k;
644  selected_d = dist_a_punto;
645  }
646  }
647 
648  selected_d = std::sqrt(selected_d);
649 
650  out_k = selected_k;
651  out_d = selected_d / refDistance;
652 
653  // If the target dist. > refDistance, then it's normal that we had to
654  // extrapolate.
655  // Otherwise, it may actually mean that the target is not reachable by this
656  // set of paths:
657  const float target_dist = std::sqrt(x * x + y * y);
658  return (target_dist > target_dist);
659 }
660 
662 {
663  ASSERTMSG_(
664  m_trajectory.empty(),
665  "Changing reference distance not allowed in this class after "
666  "initialization!");
667  this->refDistance = refDist;
668 }
669 
671 {
672  ASSERTMSG_(
673  m_trajectory.empty(),
674  "Changing robot shape not allowed in this class after initialization!");
675 }
676 
678 {
679  m_trajectory.clear(); // Free trajectories
680 }
681 
683  const std::string& cacheFilename, const bool verbose)
684 {
685  using namespace std;
686 
687  MRPT_START
688 
689  if (verbose)
690  cout << endl
691  << "[CPTG_DiffDrive_CollisionGridBased::initialize] Starting... "
692  "*** THIS MAY TAKE A WHILE, BUT MUST BE COMPUTED ONLY ONCE!! **"
693  << endl;
694 
695  // Sanity checks:
696  ASSERTMSG_(!m_robotShape.empty(), "Robot shape was not defined");
697  ASSERTMSG_(
698  m_robotShape.size() >= 3, "Robot shape must have 3 or more vertices");
699  ASSERT_(refDistance > 0);
700  ASSERT_(V_MAX > 0);
701  ASSERT_(W_MAX > 0);
702  ASSERT_(m_resolution > 0);
703 
704  mrpt::system::CTicTac tictac;
705  tictac.Tic();
706 
707  if (verbose) cout << "Initializing PTG '" << cacheFilename << "'...";
708 
709  // Simulate paths:
710  const float min_dist = 0.015f;
712  100, // max.tim,
713  refDistance, // max.dist,
714  10 * refDistance / min_dist, // max.n,
715  0.0005f, // diferencial_t
716  min_dist // min_dist
717  );
718 
719  // Just for debugging, etc.
720  // debugDumpInFiles(n);
721 
722  // Check for collisions between the robot shape and the grid cells:
723  // ----------------------------------------------------------------------------
726 
727  const size_t Ki = getAlphaValuesCount();
728  ASSERTMSG_(Ki > 0, "The PTG seems to be not initialized!");
729 
730  // Load the cached version, if possible
731  if (loadColGridsFromFile(cacheFilename, m_robotShape))
732  {
733  if (verbose) cout << "loaded from file OK" << endl;
734  }
735  else
736  {
737  // BUGFIX: In case we start reading the file and in the end detected an
738  // error,
739  // we must make sure that there's space enough for the grid:
743 
744  const int grid_cx_max = m_collisionGrid.getSizeX() - 1;
745  const int grid_cy_max = m_collisionGrid.getSizeY() - 1;
746  const double half_cell = m_collisionGrid.getResolution() * 0.5;
747 
748  const size_t nVerts = m_robotShape.verticesCount();
749  std::vector<mrpt::math::TPoint2D> transf_shape(
750  nVerts); // The robot shape at each location
751 
752  // RECOMPUTE THE COLLISION GRIDS:
753  // ---------------------------------------
754  for (size_t k = 0; k < Ki; k++)
755  {
756  const size_t nPoints = getPathStepCount(k);
757  ASSERT_(nPoints > 1);
758  for (size_t n = 0; n < (nPoints - 1); n++)
759  {
760  // Translate and rotate the robot shape at this C-Space pose:
762  getPathPose(k, n, p);
763 
765  std::numeric_limits<double>::max(),
766  std::numeric_limits<double>::max());
768  -std::numeric_limits<double>::max(),
769  -std::numeric_limits<double>::max());
770 
771  for (size_t m = 0; m < nVerts; m++)
772  {
773  transf_shape[m].x =
774  p.x + cos(p.phi) * m_robotShape.GetVertex_x(m) -
775  sin(p.phi) * m_robotShape.GetVertex_y(m);
776  transf_shape[m].y =
777  p.y + sin(p.phi) * m_robotShape.GetVertex_x(m) +
778  cos(p.phi) * m_robotShape.GetVertex_y(m);
779  mrpt::keep_max(bb_max.x, transf_shape[m].x);
780  mrpt::keep_max(bb_max.y, transf_shape[m].y);
781  mrpt::keep_min(bb_min.x, transf_shape[m].x);
782  mrpt::keep_min(bb_min.y, transf_shape[m].y);
783  }
784 
785  // Robot shape polygon:
786  const mrpt::math::TPolygon2D poly(transf_shape);
787 
788  // Get the range of cells that may collide with this shape:
789  const int ix_min =
790  std::max(0, m_collisionGrid.x2idx(bb_min.x) - 1);
791  const int iy_min =
792  std::max(0, m_collisionGrid.y2idx(bb_min.y) - 1);
793  const int ix_max =
794  std::min(m_collisionGrid.x2idx(bb_max.x) + 1, grid_cx_max);
795  const int iy_max =
796  std::min(m_collisionGrid.y2idx(bb_max.y) + 1, grid_cy_max);
797 
798  for (int ix = ix_min; ix < ix_max; ix++)
799  {
800  const double cx = m_collisionGrid.idx2x(ix) - half_cell;
801 
802  for (int iy = iy_min; iy < iy_max; iy++)
803  {
804  const double cy = m_collisionGrid.idx2y(iy) - half_cell;
805 
806  if (poly.contains(mrpt::math::TPoint2D(cx, cy)))
807  {
808  // Collision!! Update cell info:
809  const float d = this->getPathDist(k, n);
810  m_collisionGrid.updateCellInfo(ix, iy, k, d);
811  m_collisionGrid.updateCellInfo(ix - 1, iy, k, d);
812  m_collisionGrid.updateCellInfo(ix, iy - 1, k, d);
814  ix - 1, iy - 1, k, d);
815  }
816  } // for iy
817  } // for ix
818 
819  } // n
820 
821  if (verbose) cout << k << "/" << Ki << ",";
822  } // k
823 
824  if (verbose) cout << format("Done! [%.03f sec]\n", tictac.Tac());
825 
826  // save it to the cache file for the next run:
827  saveColGridsToFile(cacheFilename, m_robotShape);
828 
829  } // "else" recompute all PTG
830 
831  MRPT_END
832 }
833 
835 {
836  ASSERT_(k < m_trajectory.size());
837 
838  return m_trajectory[k].size();
839 }
840 
842  uint16_t k, uint32_t step, mrpt::math::TPose2D& p) const
843 {
844  ASSERT_(k < m_trajectory.size());
845  ASSERT_(step < m_trajectory[k].size());
846 
847  p.x = m_trajectory[k][step].x;
848  p.y = m_trajectory[k][step].y;
849  p.phi = m_trajectory[k][step].phi;
850 }
851 
853  uint16_t k, uint32_t step) const
854 {
855  ASSERT_(k < m_trajectory.size());
856  ASSERT_(step < m_trajectory[k].size());
857 
858  return m_trajectory[k][step].dist;
859 }
860 
862  uint16_t k, double dist, uint32_t& out_step) const
863 {
864  ASSERT_(k < m_trajectory.size());
865  const size_t numPoints = m_trajectory[k].size();
866 
867  ASSERT_(numPoints > 0);
868 
869  for (size_t n = 0; n < numPoints - 1; n++)
870  {
871  if (m_trajectory[k][n + 1].dist >= dist)
872  {
873  out_step = n;
874  return true;
875  }
876  }
877 
878  out_step = numPoints - 1;
879  return false;
880 }
881 
883  double ox, double oy, std::vector<double>& tp_obstacles) const
884 {
885  ASSERTMSG_(!m_trajectory.empty(), "PTG has not been initialized!");
886  const TCollisionCell& cell = m_collisionGrid.getTPObstacle(ox, oy);
887  // Keep the minimum distance:
888  for (const auto& i : cell)
889  {
890  const double dist = i.second;
891  internal_TPObsDistancePostprocess(ox, oy, dist, tp_obstacles[i.first]);
892  }
893 }
894 
896  double ox, double oy, uint16_t k, double& tp_obstacle_k) const
897 {
898  ASSERTMSG_(!m_trajectory.empty(), "PTG has not been initialized!");
899  const TCollisionCell& cell = m_collisionGrid.getTPObstacle(ox, oy);
900  // Keep the minimum distance:
901  for (const auto& i : cell)
902  if (i.first == k)
903  {
904  const double dist = i.second;
905  internal_TPObsDistancePostprocess(ox, oy, dist, tp_obstacle_k);
906  }
907 }
908 
911 {
914 
915  uint8_t version;
916  in >> version;
917  switch (version)
918  {
919  case 0:
923  break;
924  default:
926  };
927 }
928 
931 {
934 
935  const uint8_t version = 0;
936  out << version;
937 
940 }
941 
944  const
945 {
948 }
949 
951 {
952  return m_stepTimeDuration;
953 }
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
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3529
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)
GLdouble GLdouble t
Definition: glext.h:3695
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
double RAD2DEG(const double x)
Radians to degrees.
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.
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
#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...
GLint * first
Definition: glext.h:3833
double DEG2RAD(const double x)
Degrees to radians.
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.
GLenum GLsizei n
Definition: glext.h:5136
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
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 internal_shape_saveToStream(mrpt::serialization::CArchive &out) const
bool fileOpenCorrectly() const
Returns true if the file was open without errors.
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4199
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...
GLsizei const GLfloat * points
Definition: glext.h:5414
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:591
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
T square(const T x)
Inline function for the square of a number.
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)
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.
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...
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.
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
GLsizei const GLchar ** string
Definition: glext.h:4116
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
uint16_t getAlphaValuesCount() const
Get the number of different, discrete paths in this family.
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...
bool contains(const TPoint2D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge).
const GLdouble * v
Definition: glext.h:3684
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:
#define MRPT_END
Definition: exceptions.h:245
virtual void internal_readFromStream(mrpt::serialization::CArchive &in)
GLuint in
Definition: glext.h:7391
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. ...
GLenum GLint GLint y
Definition: glext.h:3542
#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...
GLsizeiptr size
Definition: glext.h:3934
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.
GLenum GLint x
Definition: glext.h:3542
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)
Lightweight 2D point.
Definition: TPoint2D.h:31
GLfloat GLfloat p
Definition: glext.h:6398
mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand() const override
Returns an empty kinematic velocity command object of the type supported by this PTG.
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 1.9.9 Git: abb8b1a1e Fri Oct 18 14:19:12 2019 +0200 at vie oct 18 14:20:13 CEST 2019