MRPT  1.9.9
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-2018, 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/system/CTicTac.h>
17 #include <mrpt/math/geometry.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  : V_MAX(.0),
32  W_MAX(.0),
33  turningRadiusReference(.10),
34  m_resolution(0.05),
35  m_stepTimeDuration(0.01),
36  m_collisionGrid(-1, 1, -1, 1, 0.5, this)
37 {
38 }
39 
41 {
44 
45  m_resolution = 0.10;
46  V_MAX = 1.0;
47  W_MAX = mrpt::DEG2RAD(120);
48 }
49 
51  const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
52 {
55 
57  resolution, double, m_resolution, cfg, sSection);
59  v_max_mps, double, V_MAX, cfg, sSection);
61  w_max_dps, double, W_MAX, cfg, sSection);
62  MRPT_LOAD_CONFIG_VAR(turningRadiusReference, double, cfg, sSection);
63 }
65  mrpt::config::CConfigFileBase& cfg, const std::string& sSection) const
66 {
68  const int WN = 25, WV = 30;
69 
71 
72  cfg.write(
73  sSection, "resolution", m_resolution, WN, WV,
74  "Resolution of the collision-check look-up-table [m].");
75  cfg.write(
76  sSection, "v_max_mps", V_MAX, WN, WV,
77  "Maximum linear velocity for trajectories [m/s].");
78  cfg.write(
79  sSection, "w_max_dps", mrpt::RAD2DEG(W_MAX), WN, WV,
80  "Maximum angular velocity for trajectories [deg/s].");
81  cfg.write(
82  sSection, "turningRadiusReference", turningRadiusReference, WN, WV,
83  "An approximate dimension of the robot (not a critical parameter) "
84  "[m].");
85 
87 
88  MRPT_END
89 }
90 
93 {
94  o << p.x << p.y << p.phi << p.t << p.dist << p.v << p.w;
95  return o;
96 }
99 {
100  i >> p.x >> p.y >> p.phi >> p.t >> p.dist >> p.v >> p.w;
101  return i;
102 }
103 
104 /*---------------------------------------------------------------
105  simulateTrajectories
106  Solve trajectories and fill cells.
107  ---------------------------------------------------------------*/
109  float max_time, float max_dist, unsigned int max_n, float diferencial_t,
110  float min_dist, float* out_max_acc_v, float* out_max_acc_w)
111 {
112  using mrpt::square;
113 
114  internal_deinitialize(); // Free previous paths
115 
116  m_stepTimeDuration = diferencial_t;
117 
118  // Reserve the size in the buffers:
120 
121  const float radio_max_robot = 1.0f; // Arbitrary "robot radius", only to
122  // determine the spacing of points
123  // under pure rotation
124 
125  // Aux buffer:
127 
128  float ult_dist, ult_dist1, ult_dist2;
129 
130  // For the grid:
131  float x_min = 1e3f, x_max = -1e3;
132  float y_min = 1e3f, y_max = -1e3;
133 
134  // Para averiguar las maximas ACELERACIONES lineales y angulares:
135  float max_acc_lin, max_acc_ang;
136 
137  max_acc_lin = max_acc_ang = 0;
138 
139  try
140  {
141  for (unsigned int k = 0; k < m_alphaValuesCount; k++)
142  {
143  // Simulate / evaluate the trajectory selected by this "alpha":
144  // ------------------------------------------------------------
145  const float alpha = index2alpha(k);
146 
147  points.clear();
148  float t = .0f, dist = .0f, girado = .0f;
149  float x = .0f, y = .0f, phi = .0f, v = .0f, w = .0f, _x = .0f,
150  _y = .0f, _phi = .0f;
151 
152  // Sliding window with latest movement commands (for the optional
153  // low-pass filtering):
154  float last_vs[2] = {.0f, .0f}, last_ws[2] = {.0f, .0f};
155 
156  // Add the first, initial point:
157  points.push_back(TCPoint(x, y, phi, t, dist, v, w));
158 
159  // Simulate until...
160  while (t < max_time && dist < max_dist && points.size() < max_n &&
161  fabs(girado) < 1.95 * M_PI)
162  {
163  // Max. aceleraciones:
164  if (t > 1)
165  {
166  float acc_lin =
167  fabs((last_vs[0] - last_vs[1]) / diferencial_t);
168  float acc_ang =
169  fabs((last_ws[0] - last_ws[1]) / diferencial_t);
170  mrpt::keep_max(max_acc_lin, acc_lin);
171  mrpt::keep_max(max_acc_ang, acc_ang);
172  }
173 
174  // Compute new movement command (v,w):
176 
177  // History of v/w ----------------------------------
178  last_vs[1] = last_vs[0];
179  last_ws[1] = last_ws[0];
180  last_vs[0] = v;
181  last_ws[0] = w;
182  // -------------------------------------------
183 
184  // Finite difference equation:
185  x += cos(phi) * v * diferencial_t;
186  y += sin(phi) * v * diferencial_t;
187  phi += w * diferencial_t;
188 
189  // Counters:
190  girado += w * diferencial_t;
191 
192  float v_inTPSpace =
193  sqrt(square(v) + square(w * turningRadiusReference));
194 
195  dist += v_inTPSpace * diferencial_t;
196 
197  t += diferencial_t;
198 
199  // Save sample if we moved far enough:
200  ult_dist1 = sqrt(square(_x - x) + square(_y - y));
201  ult_dist2 = fabs(radio_max_robot * (_phi - phi));
202  ult_dist = std::max(ult_dist1, ult_dist2);
203 
204  if (ult_dist > min_dist)
205  {
206  // Set the (v,w) to the last record:
207  points.back().v = v;
208  points.back().w = w;
209 
210  // And add the new record:
211  points.push_back(TCPoint(x, y, phi, t, dist, v, w));
212 
213  // For the next iter:
214  _x = x;
215  _y = y;
216  _phi = phi;
217  }
218 
219  // for the grid:
220  x_min = std::min(x_min, x);
221  x_max = std::max(x_max, x);
222  y_min = std::min(y_min, y);
223  y_max = std::max(y_max, y);
224  }
225 
226  // Add the final point:
227  points.back().v = v;
228  points.back().w = w;
229  points.push_back(TCPoint(x, y, phi, t, dist, v, w));
230 
231  // Save data to C-Space path structure:
232  m_trajectory[k] = points;
233 
234  } // end for "k"
235 
236  // Save accelerations
237  if (out_max_acc_v) *out_max_acc_v = max_acc_lin;
238  if (out_max_acc_w) *out_max_acc_w = max_acc_ang;
239 
240  // --------------------------------------------------------
241  // Build the speeding-up grid for lambda function:
242  // --------------------------------------------------------
243  const TCellForLambdaFunction defaultCell;
245  x_min - 0.5f, x_max + 0.5f, y_min - 0.5f, y_max + 0.5f, 0.25f,
246  &defaultCell);
247 
248  for (uint16_t k = 0; k < m_alphaValuesCount; k++)
249  {
250  const uint32_t M = static_cast<uint32_t>(m_trajectory[k].size());
251  for (uint32_t n = 0; n < M; n++)
252  {
253  TCellForLambdaFunction* cell =
254  m_lambdaFunctionOptimizer.cellByPos(
255  m_trajectory[k][n].x, m_trajectory[k][n].y);
256  ASSERT_(cell);
257  // Keep limits:
258  mrpt::keep_min(cell->k_min, k);
259  mrpt::keep_max(cell->k_max, k);
260  mrpt::keep_min(cell->n_min, n);
261  mrpt::keep_max(cell->n_max, n);
262  }
263  }
264  }
265  catch (...)
266  {
267  std::cout << format(
268  "[CPTG_DiffDrive_CollisionGridBased::simulateTrajectories] "
269  "Simulation aborted: unexpected exception!\n");
270  }
271 }
272 
273 /** In this class, `out_action_cmd` contains: [0]: linear velocity (m/s), [1]:
274  * angular velocity (rad/s) */
277  uint16_t k) const
278 {
279  float v, w;
280  ptgDiffDriveSteeringFunction(index2alpha(k), 0, 0, 0, 0, v, w);
281 
284  cmd->lin_vel = v;
285  cmd->ang_vel = w;
287 }
288 
289 /*---------------------------------------------------------------
290  getTPObstacle
291  ---------------------------------------------------------------*/
294  const float obsX, const float obsY) const
295 {
296  static const TCollisionCell emptyCell;
297  const TCollisionCell* cell = cellByPos(obsX, obsY);
298  return cell != nullptr ? *cell : emptyCell;
299 }
300 
301 /*---------------------------------------------------------------
302  Updates the info into a cell: It updates the cell only
303  if the distance d for the path k is lower than the previous value:
304  ---------------------------------------------------------------*/
306  const unsigned int icx, const unsigned int icy, const uint16_t k,
307  const float dist)
308 {
309  TCollisionCell* cell = cellByIndex(icx, icy);
310  if (!cell) return;
311 
312  // For such a small number of elements, brute-force search is not such a bad
313  // idea:
314  TCollisionCell::iterator itK = cell->end();
315  for (TCollisionCell::iterator it = cell->begin(); it != cell->end(); ++it)
316  if (it->first == k)
317  {
318  itK = it;
319  break;
320  }
321 
322  if (itK == cell->end())
323  { // New entry:
324  cell->push_back(std::make_pair(k, dist));
325  }
326  else
327  { // Only update that "k" if the distance is shorter now:
328  if (dist < itK->second) itK->second = dist;
329  }
330 }
331 
332 /*---------------------------------------------------------------
333  Save to file
334  ---------------------------------------------------------------*/
336  const std::string& filename,
337  const mrpt::math::CPolygon& computed_robotShape) const
338 {
339  try
340  {
341  mrpt::io::CFileGZOutputStream fo(filename);
342  if (!fo.fileOpenCorrectly()) return false;
343 
344  const uint32_t n = 1; // for backwards compatibility...
345  auto arch = mrpt::serialization::archiveFrom(fo);
346  arch << n;
347  return m_collisionGrid.saveToFile(&arch, computed_robotShape);
348  }
349  catch (...)
350  {
351  return false;
352  }
353 }
354 
355 /*---------------------------------------------------------------
356  Load from file
357  ---------------------------------------------------------------*/
359  const std::string& filename, const mrpt::math::CPolygon& current_robotShape)
360 {
361  try
362  {
363  mrpt::io::CFileGZInputStream fi(filename);
364  if (!fi.fileOpenCorrectly()) return false;
365  auto arch = mrpt::serialization::archiveFrom(fi);
366 
367  uint32_t n;
368  arch >> n;
369  if (n != 1)
370  return false; // Incompatible (old) format, just discard and
371  // recompute.
372 
373  return m_collisionGrid.loadFromFile(&arch, current_robotShape);
374  }
375  catch (...)
376  {
377  return false;
378  }
379 }
380 
381 const uint32_t COLGRID_FILE_MAGIC = 0xC0C0C0C3;
382 
383 /*---------------------------------------------------------------
384  Save to file
385  ---------------------------------------------------------------*/
388  const mrpt::math::CPolygon& computed_robotShape) const
389 {
390  try
391  {
392  if (!f) return false;
393 
394  const uint8_t serialize_version =
395  2; // v1: As of jun 2012, v2: As of dec-2013
396 
397  // Save magic signature && serialization version:
398  *f << COLGRID_FILE_MAGIC << serialize_version;
399 
400  // Robot shape:
401  *f << computed_robotShape;
402 
403  // and standard PTG data:
404  *f << m_parent->getDescription() << m_parent->getAlphaValuesCount()
405  << static_cast<float>(m_parent->getMax_V())
406  << static_cast<float>(m_parent->getMax_W());
407 
408  *f << m_x_min << m_x_max << m_y_min << m_y_max;
409  *f << m_resolution;
410 
411  // v1 was: *f << m_map;
412  uint32_t N = m_map.size();
413  *f << N;
414  for (uint32_t i = 0; i < N; i++)
415  {
416  uint32_t M = m_map[i].size();
417  *f << M;
418  for (uint32_t k = 0; k < M; k++)
419  *f << m_map[i][k].first << m_map[i][k].second;
420  }
421 
422  return true;
423  }
424  catch (...)
425  {
426  return false;
427  }
428 }
429 
430 /*---------------------------------------------------------------
431  loadFromFile
432  ---------------------------------------------------------------*/
435  const mrpt::math::CPolygon& current_robotShape)
436 {
437  try
438  {
439  if (!f) return false;
440 
441  // Return false if the file contents doesn't match what we expected:
442  uint32_t file_magic;
443  *f >> file_magic;
444 
445  // It doesn't seem to be a valid file or was in an old format, just
446  // recompute the grid:
447  if (COLGRID_FILE_MAGIC != file_magic) return false;
448 
449  uint8_t serialized_version;
450  *f >> serialized_version;
451 
452  switch (serialized_version)
453  {
454  case 2:
455  {
456  mrpt::math::CPolygon stored_shape;
457  *f >> stored_shape;
458 
459  const bool shapes_match =
460  (stored_shape.size() == current_robotShape.size() &&
461  std::equal(
462  stored_shape.begin(), stored_shape.end(),
463  current_robotShape.begin()));
464 
465  if (!shapes_match)
466  return false; // Must recompute if the robot shape changed.
467  }
468  break;
469 
470  case 1:
471  default:
472  // Unknown version: Maybe we are loading a file from a more
473  // recent version of MRPT? Whatever, we can't read it: It's
474  // safer just to re-generate the PTG data
475  return false;
476  };
477 
478  // Standard PTG data:
479  const std::string expected_desc = m_parent->getDescription();
480  std::string desc;
481  *f >> desc;
482  if (desc != expected_desc) return false;
483 
484 // and standard PTG data:
485 #define READ_UINT16_CHECK_IT_MATCHES_STORED(_VAR) \
486  { \
487  uint16_t ff; \
488  *f >> ff; \
489  if (ff != _VAR) return false; \
490  }
491 #define READ_FLOAT_CHECK_IT_MATCHES_STORED(_VAR) \
492  { \
493  float ff; \
494  *f >> ff; \
495  if (std::abs(ff - _VAR) > 1e-4f) return false; \
496  }
497 #define READ_DOUBLE_CHECK_IT_MATCHES_STORED(_VAR) \
498  { \
499  double ff; \
500  *f >> ff; \
501  if (std::abs(ff - _VAR) > 1e-6) return false; \
502  }
503 
504  READ_UINT16_CHECK_IT_MATCHES_STORED(m_parent->getAlphaValuesCount())
505  READ_FLOAT_CHECK_IT_MATCHES_STORED(m_parent->getMax_V())
506  READ_FLOAT_CHECK_IT_MATCHES_STORED(m_parent->getMax_W())
507 
508  // Cell dimensions:
514 
515  // OK, all parameters seem to be exactly the same than when we
516  // precomputed the table: load it.
517  // v1 was: *f >> m_map;
518  uint32_t N;
519  *f >> N;
520  m_map.resize(N);
521  for (uint32_t i = 0; i < N; i++)
522  {
523  uint32_t M;
524  *f >> M;
525  m_map[i].resize(M);
526  for (uint32_t k = 0; k < M; k++)
527  *f >> m_map[i][k].first >> m_map[i][k].second;
528  }
529 
530  return true;
531  }
532  catch (std::exception& e)
533  {
534  std::cerr << "[CCollisionGrid::loadFromFile] " << e.what();
535  return false;
536  }
537  catch (...)
538  {
539  return false;
540  }
541 }
542 
544  double x, double y, int& out_k, double& out_d, double tolerance_dist) const
545 {
546  using mrpt::square;
547 
548  ASSERTMSG_(
549  m_alphaValuesCount > 0,
550  "Have you called simulateTrajectories() first?");
551 
552  // -------------------------------------------------------------------
553  // Optimization: (24-JAN-2007 @ Jose Luis Blanco):
554  // Use a "grid" to determine the range of [k,d] values to check!!
555  // If the point (x,y) is not found in the grid, then directly skip
556  // to the next step.
557  // -------------------------------------------------------------------
558  uint16_t k_min = 0, k_max = m_alphaValuesCount - 1;
559  uint32_t n_min = 0, n_max = 0;
560  bool at_least_one = false;
561 
562  // Cell indexes:
563  int cx0 = m_lambdaFunctionOptimizer.x2idx(x);
564  int cy0 = m_lambdaFunctionOptimizer.y2idx(y);
565 
566  // (cx,cy)
567  for (int cx = cx0 - 1; cx <= cx0 + 1; cx++)
568  {
569  for (int cy = cy0 - 1; cy <= cy0 + 1; cy++)
570  {
571  const TCellForLambdaFunction* cell =
572  m_lambdaFunctionOptimizer.cellByIndex(cx, cy);
573  if (cell && !cell->isEmpty())
574  {
575  if (!at_least_one)
576  {
577  k_min = cell->k_min;
578  k_max = cell->k_max;
579  n_min = cell->n_min;
580  n_max = cell->n_max;
581  at_least_one = true;
582  }
583  else
584  {
585  mrpt::keep_min(k_min, cell->k_min);
586  mrpt::keep_max(k_max, cell->k_max);
587 
588  mrpt::keep_min(n_min, cell->n_min);
589  mrpt::keep_max(n_max, cell->n_max);
590  }
591  }
592  }
593  }
594 
595  // Try to find a closest point to the paths:
596  // ----------------------------------------------
597  int selected_k = -1;
598  float selected_d = 0;
599  float selected_dist = std::numeric_limits<float>::max();
600 
601  if (at_least_one) // Otherwise, don't even lose time checking...
602  {
603  ASSERT_BELOW_(k_max, m_trajectory.size());
604  for (int k = k_min; k <= k_max; k++)
605  {
606  const size_t n_real = m_trajectory[k].size();
607  const uint32_t n_max_this =
608  std::min(static_cast<uint32_t>(n_real ? n_real - 1 : 0), n_max);
609 
610  for (uint32_t n = n_min; n <= n_max_this; n++)
611  {
612  const float dist_a_punto = square(m_trajectory[k][n].x - x) +
613  square(m_trajectory[k][n].y - y);
614  if (dist_a_punto < selected_dist)
615  {
616  selected_dist = dist_a_punto;
617  selected_k = k;
618  selected_d = m_trajectory[k][n].dist;
619  }
620  }
621  }
622  }
623 
624  if (selected_k != -1)
625  {
626  out_k = selected_k;
627  out_d = selected_d / refDistance;
628  return (selected_dist <= square(tolerance_dist));
629  }
630 
631  // If not found, compute an extrapolation:
632 
633  // ------------------------------------------------------------------------------------
634  // Given a point (x,y), compute the "k_closest" whose extrapolation
635  // is closest to the point, and the associated "d_closest" distance,
636  // which can be normalized by "1/refDistance" to get TP-Space distances.
637  // ------------------------------------------------------------------------------------
638  selected_dist = std::numeric_limits<float>::max();
639  for (uint16_t k = 0; k < m_alphaValuesCount; k++)
640  {
641  const int n = int(m_trajectory[k].size()) - 1;
642  const float dist_a_punto = square(m_trajectory[k][n].dist) +
643  square(m_trajectory[k][n].x - x) +
644  square(m_trajectory[k][n].y - y);
645 
646  if (dist_a_punto < selected_dist)
647  {
648  selected_dist = dist_a_punto;
649  selected_k = k;
650  selected_d = dist_a_punto;
651  }
652  }
653 
654  selected_d = std::sqrt(selected_d);
655 
656  out_k = selected_k;
657  out_d = selected_d / refDistance;
658 
659  // If the target dist. > refDistance, then it's normal that we had to
660  // extrapolate.
661  // Otherwise, it may actually mean that the target is not reachable by this
662  // set of paths:
663  const float target_dist = std::sqrt(x * x + y * y);
664  return (target_dist > target_dist);
665 }
666 
668 {
669  ASSERTMSG_(
670  m_trajectory.empty(),
671  "Changing reference distance not allowed in this class after "
672  "initialization!");
673  this->refDistance = refDist;
674 }
675 
677 {
678  ASSERTMSG_(
679  m_trajectory.empty(),
680  "Changing robot shape not allowed in this class after initialization!");
681 }
682 
684 {
685  m_trajectory.clear(); // Free trajectories
686 }
687 
689  const std::string& cacheFilename, const bool verbose)
690 {
691  using namespace std;
692 
693  MRPT_START
694 
695  if (verbose)
696  cout << endl
697  << "[CPTG_DiffDrive_CollisionGridBased::initialize] Starting... "
698  "*** THIS MAY TAKE A WHILE, BUT MUST BE COMPUTED ONLY ONCE!! **"
699  << endl;
700 
701  // Sanity checks:
702  ASSERTMSG_(!m_robotShape.empty(), "Robot shape was not defined");
703  ASSERTMSG_(
704  m_robotShape.size() >= 3, "Robot shape must have 3 or more vertices");
705  ASSERT_(refDistance > 0);
706  ASSERT_(V_MAX > 0);
707  ASSERT_(W_MAX > 0);
708  ASSERT_(m_resolution > 0);
709 
710  mrpt::system::CTicTac tictac;
711  tictac.Tic();
712 
713  if (verbose) cout << "Initializing PTG '" << cacheFilename << "'...";
714 
715  // Simulate paths:
716  const float min_dist = 0.015f;
718  100, // max.tim,
719  refDistance, // max.dist,
720  10 * refDistance / min_dist, // max.n,
721  0.0005f, // diferencial_t
722  min_dist // min_dist
723  );
724 
725  // Just for debugging, etc.
726  // debugDumpInFiles(n);
727 
728  // Check for collisions between the robot shape and the grid cells:
729  // ----------------------------------------------------------------------------
732 
733  const size_t Ki = getAlphaValuesCount();
734  ASSERTMSG_(Ki > 0, "The PTG seems to be not initialized!");
735 
736  // Load the cached version, if possible
737  if (loadColGridsFromFile(cacheFilename, m_robotShape))
738  {
739  if (verbose) cout << "loaded from file OK" << endl;
740  }
741  else
742  {
743  // BUGFIX: In case we start reading the file and in the end detected an
744  // error,
745  // we must make sure that there's space enough for the grid:
749 
750  const int grid_cx_max = m_collisionGrid.getSizeX() - 1;
751  const int grid_cy_max = m_collisionGrid.getSizeY() - 1;
752  const double half_cell = m_collisionGrid.getResolution() * 0.5;
753 
754  const size_t nVerts = m_robotShape.verticesCount();
755  std::vector<mrpt::math::TPoint2D> transf_shape(
756  nVerts); // The robot shape at each location
757 
758  // RECOMPUTE THE COLLISION GRIDS:
759  // ---------------------------------------
760  for (size_t k = 0; k < Ki; k++)
761  {
762  const size_t nPoints = getPathStepCount(k);
763  ASSERT_(nPoints > 1);
764  for (size_t n = 0; n < (nPoints - 1); n++)
765  {
766  // Translate and rotate the robot shape at this C-Space pose:
768  getPathPose(k, n, p);
769 
770  mrpt::math::TPoint2D bb_min(
771  std::numeric_limits<double>::max(),
772  std::numeric_limits<double>::max());
773  mrpt::math::TPoint2D bb_max(
774  -std::numeric_limits<double>::max(),
775  -std::numeric_limits<double>::max());
776 
777  for (size_t m = 0; m < nVerts; m++)
778  {
779  transf_shape[m].x =
780  p.x + cos(p.phi) * m_robotShape.GetVertex_x(m) -
781  sin(p.phi) * m_robotShape.GetVertex_y(m);
782  transf_shape[m].y =
783  p.y + sin(p.phi) * m_robotShape.GetVertex_x(m) +
784  cos(p.phi) * m_robotShape.GetVertex_y(m);
785  mrpt::keep_max(bb_max.x, transf_shape[m].x);
786  mrpt::keep_max(bb_max.y, transf_shape[m].y);
787  mrpt::keep_min(bb_min.x, transf_shape[m].x);
788  mrpt::keep_min(bb_min.y, transf_shape[m].y);
789  }
790 
791  // Robot shape polygon:
792  const mrpt::math::TPolygon2D poly(transf_shape);
793 
794  // Get the range of cells that may collide with this shape:
795  const int ix_min =
796  std::max(0, m_collisionGrid.x2idx(bb_min.x) - 1);
797  const int iy_min =
798  std::max(0, m_collisionGrid.y2idx(bb_min.y) - 1);
799  const int ix_max =
800  std::min(m_collisionGrid.x2idx(bb_max.x) + 1, grid_cx_max);
801  const int iy_max =
802  std::min(m_collisionGrid.y2idx(bb_max.y) + 1, grid_cy_max);
803 
804  for (int ix = ix_min; ix < ix_max; ix++)
805  {
806  const double cx = m_collisionGrid.idx2x(ix) - half_cell;
807 
808  for (int iy = iy_min; iy < iy_max; iy++)
809  {
810  const double cy = m_collisionGrid.idx2y(iy) - half_cell;
811 
812  if (poly.contains(mrpt::math::TPoint2D(cx, cy)))
813  {
814  // Collision!! Update cell info:
815  const float d = this->getPathDist(k, n);
816  m_collisionGrid.updateCellInfo(ix, iy, k, d);
817  m_collisionGrid.updateCellInfo(ix - 1, iy, k, d);
818  m_collisionGrid.updateCellInfo(ix, iy - 1, k, d);
820  ix - 1, iy - 1, k, d);
821  }
822  } // for iy
823  } // for ix
824 
825  } // n
826 
827  if (verbose) cout << k << "/" << Ki << ",";
828  } // k
829 
830  if (verbose) cout << format("Done! [%.03f sec]\n", tictac.Tac());
831 
832  // save it to the cache file for the next run:
833  saveColGridsToFile(cacheFilename, m_robotShape);
834 
835  } // "else" recompute all PTG
836 
837  MRPT_END
838 }
839 
841 {
842  ASSERT_(k < m_trajectory.size());
843 
844  return m_trajectory[k].size();
845 }
846 
848  uint16_t k, uint32_t step, mrpt::math::TPose2D& p) const
849 {
850  ASSERT_(k < m_trajectory.size());
851  ASSERT_(step < m_trajectory[k].size());
852 
853  p.x = m_trajectory[k][step].x;
854  p.y = m_trajectory[k][step].y;
855  p.phi = m_trajectory[k][step].phi;
856 }
857 
859  uint16_t k, uint32_t step) const
860 {
861  ASSERT_(k < m_trajectory.size());
862  ASSERT_(step < m_trajectory[k].size());
863 
864  return m_trajectory[k][step].dist;
865 }
866 
868  uint16_t k, double dist, uint32_t& out_step) const
869 {
870  ASSERT_(k < m_trajectory.size());
871  const size_t numPoints = m_trajectory[k].size();
872 
873  ASSERT_(numPoints > 0);
874 
875  for (size_t n = 0; n < numPoints - 1; n++)
876  {
877  if (m_trajectory[k][n + 1].dist >= dist)
878  {
879  out_step = n;
880  return true;
881  }
882  }
883 
884  out_step = numPoints - 1;
885  return false;
886 }
887 
889  double ox, double oy, std::vector<double>& tp_obstacles) const
890 {
891  ASSERTMSG_(!m_trajectory.empty(), "PTG has not been initialized!");
892  const TCollisionCell& cell = m_collisionGrid.getTPObstacle(ox, oy);
893  // Keep the minimum distance:
894  for (TCollisionCell::const_iterator i = cell.begin(); i != cell.end(); ++i)
895  {
896  const double dist = i->second;
897  internal_TPObsDistancePostprocess(ox, oy, dist, tp_obstacles[i->first]);
898  }
899 }
900 
902  double ox, double oy, uint16_t k, double& tp_obstacle_k) const
903 {
904  ASSERTMSG_(!m_trajectory.empty(), "PTG has not been initialized!");
905  const TCollisionCell& cell = m_collisionGrid.getTPObstacle(ox, oy);
906  // Keep the minimum distance:
907  for (TCollisionCell::const_iterator i = cell.begin(); i != cell.end(); ++i)
908  if (i->first == k)
909  {
910  const double dist = i->second;
911  internal_TPObsDistancePostprocess(ox, oy, dist, tp_obstacle_k);
912  }
913 }
914 
917 {
920 
921  uint8_t version;
922  in >> version;
923  switch (version)
924  {
925  case 0:
929  break;
930  default:
932  };
933 }
934 
937 {
940 
941  const uint8_t version = 0;
942  out << version;
943 
946 }
947 
950  const
951 {
954 }
955 
957 {
958  return m_stepTimeDuration;
959 }
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:90
Scalar * iterator
Definition: eigen_plugins.h:26
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3525
virtual 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:262
int y2idx(double y) const
Definition: CDynamicGrid.h:270
#define READ_DOUBLE_CHECK_IT_MATCHES_STORED(_VAR)
GLdouble GLdouble t
Definition: glext.h:3689
#define min(a, b)
unsigned __int16 uint16_t
Definition: rptypes.h:44
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 < ...
virtual 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. ...
double x
X,Y coordinates.
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.
virtual 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)
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:165
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:3827
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:264
bool loadFromFile(mrpt::serialization::CArchive *fil, const mrpt::math::CPolygon &current_robotShape)
Load from file, true = OK.
GLenum GLsizei n
Definition: glext.h:5074
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:289
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:4178
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:293
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:5339
virtual 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)...
unsigned char uint8_t
Definition: rptypes.h:41
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:90
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:555
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:82
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:113
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...
virtual 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:211
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:101
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: CDynamicGrid.h:252
GLsizei const GLchar ** string
Definition: glext.h:4101
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:254
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:3678
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:52
std::vector< TCPoint > TCPointVector
virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Parameters accepted by this base class:
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
#define MRPT_END
Definition: exceptions.h:266
virtual void internal_readFromStream(mrpt::serialization::CArchive &in)
GLuint in
Definition: glext.h:7274
Lightweight 2D pose.
int x2idx(double x) const
Transform a coordinate values into cell indexes.
Definition: CDynamicGrid.h:266
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:3538
#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)
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:3923
double GetVertex_y(size_t i) const
Definition: CPolygon.h:39
virtual void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
GLenum GLint x
Definition: glext.h:3538
Saves data to a file and transparently compress the data using the given compression level...
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:79
std::shared_ptr< CVehicleVelCmd > Ptr
virtual 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)
unsigned __int32 uint32_t
Definition: rptypes.h:47
Lightweight 2D point.
GLfloat GLfloat p
Definition: glext.h:6305
virtual mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand() const override
Returns an empty kinematic velocity command object of the type supported by this PTG.
const Scalar * const_iterator
Definition: eigen_plugins.h:27
Kinematic model for Ackermann-like or differential-driven vehicles.
2D polygon, inheriting from std::vector<TPoint2D>.
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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020