Main MRPT website > C++ reference for MRPT 1.9.9
CHolonomicFullEval.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 
14 #include <mrpt/core/round.h>
15 #include <mrpt/math/geometry.h>
18 #include <cmath>
19 
20 using namespace mrpt;
21 using namespace mrpt::math;
22 using namespace mrpt::nav;
23 using namespace std;
24 
29 
30 const unsigned int INVALID_K = std::numeric_limits<unsigned int>::max();
31 
33  const mrpt::config::CConfigFileBase* INI_FILE)
35  m_last_selected_sector(std::numeric_limits<unsigned int>::max())
36 {
37  if (INI_FILE != nullptr) initialize(*INI_FILE);
38 }
39 
40 void CHolonomicFullEval::saveConfigFile(mrpt::config::CConfigFileBase& c) const
41 {
42  options.saveToConfigFile(c, getConfigFileSectionName());
43 }
44 
45 void CHolonomicFullEval::initialize(const mrpt::config::CConfigFileBase& c)
46 {
47  options.loadFromConfigFile(c, getConfigFileSectionName());
48 }
49 
50 struct TGap
51 {
52  int k_from, k_to;
53  double min_eval, max_eval;
55  /** Direction with the best evaluation inside the gap. */
57 
58  TGap()
59  : k_from(-1),
60  k_to(-1),
61  min_eval(std::numeric_limits<double>::max()),
62  max_eval(-std::numeric_limits<double>::max()),
63  contains_target_k(false),
64  k_best_eval(-1)
65  {
66  }
67 };
68 
69 CHolonomicFullEval::EvalOutput::EvalOutput() : best_k(INVALID_K), best_eval(.0)
70 {
71 }
72 
74  unsigned int target_idx, const NavInput& ni, EvalOutput& eo)
75 {
76  ASSERT_(target_idx < ni.targets.size());
77  const auto target = ni.targets[target_idx];
78 
79  using mrpt::square;
80 
81  eo = EvalOutput();
82 
83  const auto ptg = getAssociatedPTG();
84  const double ptg_ref_dist = ptg ? ptg->getRefDistance() : 1.0;
85  const size_t nDirs = ni.obstacles.size();
86 
87  const double target_dir = ::atan2(target.y, target.x);
88  const unsigned int target_k =
90  const double target_dist = target.norm();
91 
92  m_dirs_scores.resize(nDirs, options.factorWeights.size() + 2);
93 
94  // TP-Obstacles in 2D:
95  std::vector<mrpt::math::TPoint2D> obstacles_2d(nDirs);
96 
98  sp.aperture = 2.0 * M_PI;
99  sp.nRays = nDirs;
100  sp.rightToLeft = true;
101  const auto& sc_lut = m_sincos_lut.getSinCosForScan(sp);
102 
103  for (unsigned int i = 0; i < nDirs; i++)
104  {
105  obstacles_2d[i].x = ni.obstacles[i] * sc_lut.ccos[i];
106  obstacles_2d[i].y = ni.obstacles[i] * sc_lut.csin[i];
107  }
108 
109  const int NUM_FACTORS = 5;
110 
111  ASSERT_(options.factorWeights.size() == NUM_FACTORS);
112 
113  for (unsigned int i = 0; i < nDirs; i++)
114  {
115  double scores[NUM_FACTORS]; // scores for each criterion
116 
117  if (ni.obstacles[i] < options.TOO_CLOSE_OBSTACLE &&
118  !(i == target_k &&
119  ni.obstacles[i] > 1.02 * target_dist)) // Too close to obstacles?
120  // (unless target is in
121  // between obstacles and
122  // the robot)
123  {
124  for (int l = 0; l < NUM_FACTORS; l++) m_dirs_scores(i, l) = .0;
125  continue;
126  }
127 
128  const double d = std::min(ni.obstacles[i], 0.95 * target_dist);
129 
130  // The TP-Space representative coordinates for this direction:
131  const double x = d * sc_lut.ccos[i];
132  const double y = d * sc_lut.csin[i];
133 
134  // Factor #1: collision-free distance
135  // -----------------------------------------------------
136  if (mrpt::abs_diff(i, target_k) <= 1 &&
137  target_dist < 1.0 - options.TOO_CLOSE_OBSTACLE &&
138  ni.obstacles[i] > 1.05 * target_dist)
139  {
140  // Don't count obstacles ahead of the target.
141  scores[0] =
142  std::max(target_dist, ni.obstacles[i]) / (target_dist * 1.05);
143  }
144  else
145  {
146  scores[0] =
147  std::max(0.0, ni.obstacles[i] - options.TOO_CLOSE_OBSTACLE);
148  }
149 
150  // Discount "circular loop aparent free distance" here, but don't count
151  // it for clearance, since those are not real obstacle points.
152  if (ptg != nullptr)
153  {
154  const double max_real_freespace =
155  ptg->getActualUnloopedPathLength(i);
156  const double max_real_freespace_norm =
157  max_real_freespace / ptg->getRefDistance();
158 
159  mrpt::keep_min(scores[0], max_real_freespace_norm);
160  }
161 
162  // Factor #2: Closest approach to target along straight line (Euclidean)
163  // -------------------------------------------
165  sg.point1.x = 0;
166  sg.point1.y = 0;
167  sg.point2.x = x;
168  sg.point2.y = y;
169 
170  // Range of attainable values: 0=passes thru target. 2=opposite
171  // direction
172  double min_dist_target_along_path = sg.distance(target);
173 
174  // Idea: if this segment is taking us *away* from target, don't make the
175  // segment to start at (0,0), since all
176  // paths "running away" will then have identical minimum distances to
177  // target. Use the middle of the segment instead:
178  const double endpt_dist_to_target = (target - TPoint2D(x, y)).norm();
179  const double endpt_dist_to_target_norm =
180  std::min(1.0, endpt_dist_to_target);
181 
182  if ((endpt_dist_to_target_norm > target_dist &&
183  endpt_dist_to_target_norm >= 0.95 * target_dist) &&
184  min_dist_target_along_path >
185  1.05 * std::min(
186  target_dist,
187  endpt_dist_to_target_norm) // the path does not get
188  // any closer to trg
189  //|| (ni.obstacles[i]<0.95*target_dist)
190  )
191  {
192  // path takes us away or way blocked:
193  sg.point1.x = x * 0.5;
194  sg.point1.y = y * 0.5;
195  min_dist_target_along_path = sg.distance(target);
196  }
197 
198  scores[1] = 1.0 / (1.0 + square(min_dist_target_along_path));
199 
200  // Factor #3: Distance of end collision-free point to target (Euclidean)
201  // -----------------------------------------------------
202  {
203  scores[2] = std::sqrt(
204  1.01 - endpt_dist_to_target_norm); // the 1.01 instead of 1.0
205  // is to be 100% sure we
206  // don't get a domain error
207  // in sqrt()
208  }
209 
210  // Factor #4: Stabilizing factor (hysteresis) to avoid quick switch
211  // among very similar paths:
212  // ------------------------------------------------------------------------------------------
213  if (m_last_selected_sector != std::numeric_limits<unsigned int>::max())
214  {
215  const unsigned int hist_dist = mrpt::abs_diff(
217  i); // It's fine here to consider that -PI is far from +PI.
218 
219  if (hist_dist >= options.HYSTERESIS_SECTOR_COUNT)
220  scores[3] = square(
221  1.0 -
222  (hist_dist - options.HYSTERESIS_SECTOR_COUNT) /
223  double(nDirs));
224  else
225  scores[3] = 1.0;
226  }
227  else
228  {
229  scores[3] = 1.0;
230  }
231 
232  // Factor #5: clearance to nearest obstacle along path
233  // ------------------------------------------------------------------------------------------
234  {
235  const double query_dist_norm = std::min(0.99, target_dist * 0.95);
236  const double avr_path_clearance = ni.clearance->getClearance(
237  i /*path index*/, query_dist_norm, true /*interpolate path*/);
238  const double point_clearance = ni.clearance->getClearance(
239  i /*path index*/, query_dist_norm, false /*interpolate path*/);
240  scores[4] = 0.5 * (avr_path_clearance + point_clearance);
241  }
242 
243  // Save stats for debugging:
244  for (int l = 0; l < NUM_FACTORS; l++) m_dirs_scores(i, l) = scores[l];
245  }
246 
247  // Normalize factors?
248  ASSERT_(options.factorNormalizeOrNot.size() == NUM_FACTORS);
249  for (int l = 0; l < NUM_FACTORS; l++)
250  {
251  if (!options.factorNormalizeOrNot[l]) continue;
252 
253  const double mmax = m_dirs_scores.col(l).maxCoeff();
254  const double mmin = m_dirs_scores.col(l).minCoeff();
255  const double span = mmax - mmin;
256  if (span <= .0) continue;
257 
258  m_dirs_scores.col(l).array() -= mmin;
259  m_dirs_scores.col(l).array() /= span;
260  }
261 
262  // Phase 1: average of PHASE1_FACTORS and thresholding:
263  // ----------------------------------------------------------------------
264  const unsigned int NUM_PHASES = options.PHASE_FACTORS.size();
265  ASSERT_(NUM_PHASES >= 1);
266 
267  std::vector<double> weights_sum_phase(NUM_PHASES, .0),
268  weights_sum_phase_inv(NUM_PHASES);
269  for (unsigned int i = 0; i < NUM_PHASES; i++)
270  {
271  for (unsigned int l : options.PHASE_FACTORS[i])
272  weights_sum_phase[i] += options.factorWeights[l];
273  ASSERT_(weights_sum_phase[i] > .0);
274  weights_sum_phase_inv[i] = 1.0 / weights_sum_phase[i];
275  }
276 
277  eo.phase_scores = std::vector<std::vector<double>>(
278  NUM_PHASES, std::vector<double>(nDirs, .0));
279  auto& phase_scores = eo.phase_scores; // shortcut
280  double last_phase_threshold = -1.0; // don't threshold for the first phase
281 
282  for (unsigned int phase_idx = 0; phase_idx < NUM_PHASES; phase_idx++)
283  {
284  double phase_min = std::numeric_limits<double>::max(), phase_max = .0;
285 
286  for (unsigned int i = 0; i < nDirs; i++)
287  {
288  double this_dir_eval = 0;
289 
290  if (ni.obstacles[i] <
291  options.TOO_CLOSE_OBSTACLE || // Too close to obstacles ?
292  (phase_idx > 0 &&
293  phase_scores[phase_idx - 1][i] <
294  last_phase_threshold) // thresholding of the previous
295  // phase
296  )
297  {
298  this_dir_eval = .0;
299  }
300  else
301  {
302  // Weighted avrg of factors:
303  for (unsigned int l : options.PHASE_FACTORS[phase_idx])
304  this_dir_eval +=
306  std::log(std::max(1e-6, m_dirs_scores(i, l)));
307 
308  this_dir_eval *= weights_sum_phase_inv[phase_idx];
309  this_dir_eval = std::exp(this_dir_eval);
310  }
311  phase_scores[phase_idx][i] = this_dir_eval;
312 
313  mrpt::keep_max(phase_max, phase_scores[phase_idx][i]);
314  mrpt::keep_min(phase_min, phase_scores[phase_idx][i]);
315 
316  } // for each direction
317 
318  ASSERT_(options.PHASE_THRESHOLDS.size() == NUM_PHASES);
319  ASSERT_(
320  options.PHASE_THRESHOLDS[phase_idx] > .0 &&
321  options.PHASE_THRESHOLDS[phase_idx] < 1.0);
322 
323  last_phase_threshold =
324  options.PHASE_THRESHOLDS[phase_idx] * phase_max +
325  (1.0 - options.PHASE_THRESHOLDS[phase_idx]) * phase_min;
326  } // end for each phase
327 
328  // Give a chance for a derived class to manipulate the final evaluations:
329  auto& dirs_eval = *phase_scores.rbegin();
330 
331  postProcessDirectionEvaluations(dirs_eval, ni, target_idx);
332 
333  // Recalculate the threshold just in case the postProcess function above
334  // changed things:
335  {
336  double phase_min = std::numeric_limits<double>::max(), phase_max = .0;
337  for (unsigned int i = 0; i < nDirs; i++)
338  {
339  mrpt::keep_max(phase_max, phase_scores[NUM_PHASES - 1][i]);
340  mrpt::keep_min(phase_min, phase_scores[NUM_PHASES - 1][i]);
341  }
342  last_phase_threshold =
343  options.PHASE_THRESHOLDS[NUM_PHASES - 1] * phase_max +
344  (1.0 - options.PHASE_THRESHOLDS[NUM_PHASES - 1]) * phase_min;
345  }
346 
347  // Search for best direction:
348 
349  // Of those directions above "last_phase_threshold", keep the GAP with the
350  // largest maximum value within;
351  // then pick the MIDDLE point as the final selection.
352  std::vector<TGap> gaps;
353  int best_gap_idx = -1;
354  int gap_idx_for_target_dir = -1;
355  {
356  bool inside_gap = false;
357  for (unsigned int i = 0; i < nDirs; i++)
358  {
359  const double val = dirs_eval[i];
360  if (val < last_phase_threshold)
361  {
362  if (inside_gap)
363  {
364  // We just ended a gap:
365  auto& active_gap = *gaps.rbegin();
366  active_gap.k_to = i - 1;
367  inside_gap = false;
368  }
369  }
370  else
371  {
372  // higher or EQUAL to the treshold (equal is important just in
373  // case we have a "flat" diagram...)
374  if (!inside_gap)
375  {
376  // We just started a gap:
377  TGap new_gap;
378  new_gap.k_from = i;
379  gaps.emplace_back(new_gap);
380  inside_gap = true;
381  }
382  }
383 
384  if (inside_gap)
385  {
386  auto& active_gap = *gaps.rbegin();
387  if (val >= active_gap.max_eval)
388  {
389  active_gap.k_best_eval = i;
390  }
391  mrpt::keep_max(active_gap.max_eval, val);
392  mrpt::keep_min(active_gap.min_eval, val);
393 
394  if (target_k == i)
395  {
396  active_gap.contains_target_k = true;
397  gap_idx_for_target_dir = gaps.size() - 1;
398  }
399 
400  if (best_gap_idx == -1 || val > gaps[best_gap_idx].max_eval)
401  {
402  best_gap_idx = gaps.size() - 1;
403  }
404  }
405  } // end for i
406 
407  // Handle the case where we end with an open, active gap:
408  if (inside_gap)
409  {
410  auto& active_gap = *gaps.rbegin();
411  active_gap.k_to = nDirs - 1;
412  }
413  }
414 
415  ASSERT_(!gaps.empty());
416  ASSERT_(best_gap_idx >= 0 && best_gap_idx < int(gaps.size()));
417 
418  const TGap& best_gap = gaps[best_gap_idx];
419 
420  eo.best_eval = best_gap.max_eval;
421 
422  // Different qualitative situations:
423  if (best_gap_idx == gap_idx_for_target_dir) // Gap contains target, AND
424  {
425  // the way seems to have clearance enought:
426  const auto cl_left =
427  mrpt::abs_diff(target_k, (unsigned int)best_gap.k_from);
428  const auto cl_right =
429  mrpt::abs_diff(target_k, (unsigned int)best_gap.k_to);
430 
431  const auto smallest_clearance_in_k_units = std::min(cl_left, cl_right);
432  const unsigned int clearance_threshold =
434 
435  const unsigned int gap_width = best_gap.k_to - best_gap.k_from;
436  const unsigned int width_threshold =
438 
439  // Move straight to target?
440  if (smallest_clearance_in_k_units >= clearance_threshold &&
441  gap_width >= width_threshold &&
442  ni.obstacles[target_k] > target_dist * 1.01)
443  {
444  eo.best_k = target_k;
445  }
446  }
447 
448  if (eo.best_k == INVALID_K) // did not fulfill conditions above
449  {
450  // Not heading to target: go thru the "middle" of the gap to maximize
451  // clearance
452  eo.best_k = mrpt::round(0.5 * (best_gap.k_to + best_gap.k_from));
453  }
454 
455  // Alternative, simpler method to decide motion:
456  // If target can be reached without collision *and* with a minimum of
457  // clearance,
458  // then select that direction, with the score as computed with the regular
459  // formulas above
460  // (even if that score was not the maximum!).
461  if (target_dist < 0.99 &&
462  (
463  /* No obstacles to target + enough clearance: */
464  (ni.obstacles[target_k] > target_dist * 1.01 &&
466  target_k /*path index*/, std::min(0.99, target_dist * 0.95),
467  true /*interpolate path*/) > options.TOO_CLOSE_OBSTACLE) ||
468  /* Or: no obstacles to target with extra margin, target is really
469  near, dont check clearance: */
470  (ni.obstacles[target_k] >
471  (target_dist + 0.15 /*meters*/ / ptg_ref_dist) &&
472  target_dist < (1.5 /*meters*/ / ptg_ref_dist))) &&
473  dirs_eval[target_k] >
474  0 /* the direct target direction has at least a minimum score */
475  )
476  {
477  eo.best_k = target_k;
478  eo.best_eval = dirs_eval[target_k];
479 
480  // Reflect this decision in the phase score plots:
481  phase_scores[NUM_PHASES - 1][target_k] += 2.0;
482  }
483 }
484 
486 {
487  using mrpt::square;
488 
489  ASSERT_(ni.clearance != nullptr);
490  ASSERT_(!ni.targets.empty());
491 
492  // Create a log record for returning data.
494  mrpt::make_aligned_shared<CLogFileRecord_FullEval>();
495  no.logRecord = log;
496 
497  const size_t numTrgs = ni.targets.size();
498 
499  std::vector<EvalOutput> evals(numTrgs);
500  double best_eval = .0;
501  unsigned int best_trg_idx = 0;
502 
503  for (unsigned int trg_idx = 0; trg_idx < numTrgs; trg_idx++)
504  {
505  auto& eo = evals[trg_idx];
506  evalSingleTarget(trg_idx, ni, eo);
507 
508  if (eo.best_eval >=
509  best_eval) // >= because we prefer the most advanced targets...
510  {
511  best_eval = eo.best_eval;
512  best_trg_idx = trg_idx;
513  }
514  }
515 
516  // Prepare NavigationOutput data:
517  if (best_eval == .0)
518  {
519  // No way found!
520  no.desiredDirection = 0;
521  no.desiredSpeed = 0;
522  }
523  else
524  {
525  // A valid movement:
526  const auto ptg = getAssociatedPTG();
527  const double ptg_ref_dist = ptg ? ptg->getRefDistance() : 1.0;
528 
530  evals[best_trg_idx].best_k, ni.obstacles.size());
531 
532  // Speed control: Reduction factors
533  // ---------------------------------------------
534  const double targetNearnessFactor =
536  ? std::min(
537  1.0, ni.targets[best_trg_idx].norm() /
539  ptg_ref_dist))
540  : 1.0;
541 
542  const double obs_dist =
543  ni.obstacles[evals[best_trg_idx].best_k]; // Was: min with
544  // obs_clearance too.
545  const double obs_dist_th = std::max(
547  (options.OBSTACLE_SLOW_DOWN_DISTANCE / ptg_ref_dist) *
548  ni.maxObstacleDist);
549  double riskFactor = 1.0;
550  if (obs_dist <= options.TOO_CLOSE_OBSTACLE)
551  {
552  riskFactor = 0.0;
553  }
554  else if (
555  obs_dist < obs_dist_th && obs_dist_th > options.TOO_CLOSE_OBSTACLE)
556  {
557  riskFactor = (obs_dist - options.TOO_CLOSE_OBSTACLE) /
558  (obs_dist_th - options.TOO_CLOSE_OBSTACLE);
559  }
560  no.desiredSpeed =
561  ni.maxRobotSpeed * std::min(riskFactor, targetNearnessFactor);
562  }
563 
564  m_last_selected_sector = evals[best_trg_idx].best_k;
565 
566  // LOG --------------------------
567  if (log)
568  {
569  log->selectedTarget = best_trg_idx;
570  log->selectedSector = evals[best_trg_idx].best_k;
571  log->evaluation = evals[best_trg_idx].best_eval;
572  log->dirs_eval = evals[best_trg_idx].phase_scores;
573 
575  {
576  log->dirs_scores = m_dirs_scores;
577  }
578  }
579 }
580 
582  const double a, const unsigned int N)
583 {
584  const int idx = round(0.5 * (N * (1 + mrpt::math::wrapToPi(a) / M_PI) - 1));
585  if (idx < 0)
586  return 0;
587  else
588  return static_cast<unsigned int>(idx);
589 }
590 
592  : selectedSector(0), evaluation(.0), dirs_scores(), selectedTarget(0)
593 {
594 }
595 
599 {
601  << evaluation << selectedTarget /*v3*/;
602 }
603 
604 /*---------------------------------------------------------------
605  readFromStream
606  ---------------------------------------------------------------*/
609 {
610  switch (version)
611  {
612  case 0:
613  case 1:
614  case 2:
615  case 3:
616  {
617  if (version >= 2)
618  {
620  }
621  else
622  {
625  if (version >= 1)
626  {
628  }
629  }
631  if (version >= 3)
632  {
633  in >> selectedTarget;
634  }
635  else
636  {
637  selectedTarget = 0;
638  }
639  }
640  break;
641  default:
643  };
644 }
645 
646 /*---------------------------------------------------------------
647  TOptions
648  ---------------------------------------------------------------*/
650  : // Default values:
651  TOO_CLOSE_OBSTACLE(0.15),
652  TARGET_SLOW_APPROACHING_DISTANCE(0.60),
653  OBSTACLE_SLOW_DOWN_DISTANCE(0.15),
654  HYSTERESIS_SECTOR_COUNT(5),
655  factorWeights{0.1, 0.5, 0.5, 0.01, 1},
656  factorNormalizeOrNot{0, 0, 0, 0, 1},
657  PHASE_FACTORS{{1, 2}, {4}, {0, 2}},
658  PHASE_THRESHOLDS{0.5, 0.6, 0.7},
659  LOG_SCORE_MATRIX(false),
660  clearance_threshold_ratio(0.05),
661  gap_width_ratio_threshold(0.25)
662 {
663 }
664 
667 {
668  MRPT_START
669 
670  // Load from config text:
671  MRPT_LOAD_CONFIG_VAR(TOO_CLOSE_OBSTACLE, double, c, s);
672  MRPT_LOAD_CONFIG_VAR(TARGET_SLOW_APPROACHING_DISTANCE, double, c, s);
673  MRPT_LOAD_CONFIG_VAR(OBSTACLE_SLOW_DOWN_DISTANCE, double, c, s);
674  MRPT_LOAD_CONFIG_VAR(HYSTERESIS_SECTOR_COUNT, double, c, s);
675  MRPT_LOAD_CONFIG_VAR(LOG_SCORE_MATRIX, bool, c, s);
676  MRPT_LOAD_CONFIG_VAR(clearance_threshold_ratio, double, c, s);
677  MRPT_LOAD_CONFIG_VAR(gap_width_ratio_threshold, double, c, s);
678 
679  c.read_vector(
680  s, "factorWeights", std::vector<double>(), factorWeights, true);
681  ASSERT_(factorWeights.size() == 5);
682 
683  c.read_vector(
684  s, "factorNormalizeOrNot", factorNormalizeOrNot, factorNormalizeOrNot);
685  ASSERT_(factorNormalizeOrNot.size() == factorWeights.size());
686 
687  // Phases:
688  int PHASE_COUNT = 0;
689  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(PHASE_COUNT, int, c, s);
690 
691  PHASE_FACTORS.resize(PHASE_COUNT);
692  PHASE_THRESHOLDS.resize(PHASE_COUNT);
693  for (int i = 0; i < PHASE_COUNT; i++)
694  {
695  c.read_vector(
696  s, mrpt::format("PHASE%i_FACTORS", i + 1), PHASE_FACTORS[i],
697  PHASE_FACTORS[i], true);
698  ASSERT_(!PHASE_FACTORS[i].empty());
699 
700  PHASE_THRESHOLDS[i] = c.read_double(
701  s, mrpt::format("PHASE%i_THRESHOLD", i + 1), .0, true);
702  ASSERT_(PHASE_THRESHOLDS[i] >= .0 && PHASE_THRESHOLDS[i] <= 1.0);
703  }
704 
705  MRPT_END
706 }
707 
710 {
711  MRPT_START;
712 
713  const int WN = mrpt::config::MRPT_SAVE_NAME_PADDING(),
715 
717  TOO_CLOSE_OBSTACLE,
718  "Directions with collision-free distances below this threshold are not "
719  "elegible.");
721  TARGET_SLOW_APPROACHING_DISTANCE,
722  "Start to reduce speed when closer than this to target.");
724  OBSTACLE_SLOW_DOWN_DISTANCE,
725  "Start to reduce speed when clearance is below this value ([0,1] ratio "
726  "wrt obstacle reference/max distance)");
728  HYSTERESIS_SECTOR_COUNT,
729  "Range of `sectors` (directions) for hysteresis over successive "
730  "timesteps");
732  LOG_SCORE_MATRIX, "Save the entire score matrix in log files");
734  clearance_threshold_ratio,
735  "Ratio [0,1], times path_count, gives the minimum number of paths at "
736  "each side of a target direction to be accepted as desired direction");
738  gap_width_ratio_threshold,
739  "Ratio [0,1], times path_count, gives the minimum gap width to accept "
740  "a direct motion towards target.");
741 
742  ASSERT_EQUAL_(factorWeights.size(), 5);
743  c.write(
744  s, "factorWeights",
745  mrpt::system::sprintf_container("%.2f ", factorWeights), WN, WV,
746  "[0]=Free space, [1]=Dist. in sectors, [2]=Closer to target "
747  "(Euclidean), [3]=Hysteresis, [4]=clearance along path");
748  c.write(
749  s, "factorNormalizeOrNot",
750  mrpt::system::sprintf_container("%u ", factorNormalizeOrNot), WN, WV,
751  "Normalize factors or not (1/0)");
752 
753  c.write(
754  s, "PHASE_COUNT", PHASE_FACTORS.size(), WN, WV,
755  "Number of evaluation phases to run (params for each phase below)");
756 
757  for (unsigned int i = 0; i < PHASE_FACTORS.size(); i++)
758  {
759  c.write(
760  s, mrpt::format("PHASE%u_THRESHOLD", i + 1), PHASE_THRESHOLDS[i],
761  WN, WV,
762  "Phase scores must be above this relative range threshold [0,1] to "
763  "be considered in next phase (Default:`0.75`)");
764  c.write(
765  s, mrpt::format("PHASE%u_FACTORS", i + 1),
766  mrpt::system::sprintf_container("%d ", PHASE_FACTORS[i]), WN, WV,
767  "Indices of the factors above to be considered in this phase");
768  }
769 
770  MRPT_END;
771 }
772 
775 {
776  // Params:
778  << options.PHASE_FACTORS << // v3
780  << options.PHASE_THRESHOLDS // v3
785  ;
786  // State:
787  out << m_last_selected_sector;
788 }
791 {
792  switch (version)
793  {
794  case 0:
795  case 1:
796  case 2:
797  case 3:
798  case 4:
799  {
800  // Params:
802 
803  if (version >= 3)
804  {
806  }
807  else
808  {
809  options.PHASE_THRESHOLDS.resize(2);
811  }
814 
815  if (version >= 3)
816  {
818  }
819  else
820  {
821  options.PHASE_THRESHOLDS.resize(1);
823  }
824 
825  if (version >= 1) in >> options.OBSTACLE_SLOW_DOWN_DISTANCE;
826  if (version >= 2) in >> options.factorNormalizeOrNot;
827 
828  if (version >= 4)
829  {
832  }
833 
834  // State:
836  }
837  break;
838  default:
840  };
841 }
842 
844  std::vector<double>& dir_evals, const NavInput& ni, unsigned int trg_idx)
845 {
846  // Default: do nothing
847 }
mrpt::keep_min
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.
Definition: core/include/mrpt/core/bits_math.h:124
mrpt::nav::CLogFileRecord_FullEval
A class for storing extra information about the execution of CHolonomicFullEval navigation.
Definition: CHolonomicFullEval.h:165
ops_containers.h
mrpt::nav::CAbstractHolonomicReactiveMethod::getAssociatedPTG
mrpt::nav::CParameterizedTrajectoryGenerator * getAssociatedPTG() const
Returns the pointer set by setAssociatedPTG()
Definition: CAbstractHolonomicReactiveMethod.cpp:46
nav-precomp.h
mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput
Input parameters for CAbstractHolonomicReactiveMethod::navigate()
Definition: CAbstractHolonomicReactiveMethod.h:38
mrpt::nav::CLogFileRecord_FullEval::dirs_scores
mrpt::math::CMatrixD dirs_scores
Individual scores for each direction: (i,j), i (row) are directions, j (cols) are scores.
Definition: CHolonomicFullEval.h:177
mrpt::math::TPoint2D::y
double y
Definition: lightweight_geom_data.h:49
INVALID_K
const mrpt::nav unsigned int INVALID_K
Definition: CHolonomicFullEval.cpp:30
s
GLdouble s
Definition: glext.h:3676
MRPT_SAVE_CONFIG_VAR_COMMENT
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
Definition: config/CConfigFileBase.h:459
geometry.h
ASSERT_EQUAL_
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:153
mrpt::nav::CLogFileRecord_FullEval::serializeFrom
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CHolonomicFullEval.cpp:607
mrpt::nav::CHolonomicFullEval::TOptions::HYSTERESIS_SECTOR_COUNT
double HYSTERESIS_SECTOR_COUNT
Range of "sectors" (directions) for hysteresis over successive timesteps.
Definition: CHolonomicFullEval.h:86
MRPT_LOAD_CONFIG_VAR
#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...
Definition: config/CConfigFileBase.h:282
mrpt::nav::CAbstractHolonomicReactiveMethod::NavOutput::logRecord
CHolonomicLogFileRecord::Ptr logRecord
The navigation method will create a log record and store it here via a smart pointer.
Definition: CAbstractHolonomicReactiveMethod.h:75
mrpt::nav::CHolonomicFullEval::TOptions::gap_width_ratio_threshold
double gap_width_ratio_threshold
Ratio [0,1], times path_count, gives the minimum gap width to accept a direct motion towards target.
Definition: CHolonomicFullEval.h:108
mrpt::nav::CLogFileRecord_FullEval::CLogFileRecord_FullEval
CLogFileRecord_FullEval()
Definition: CHolonomicFullEval.cpp:591
TGap::min_eval
double min_eval
Definition: CHolonomicFullEval.cpp:53
mrpt::nav::CLogFileRecord_FullEval::serializeGetVersion
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CHolonomicFullEval.cpp:596
TGap::k_from
int k_from
Definition: CHolonomicFullEval.cpp:52
mrpt::math::TSegment2D::distance
double distance(const TPoint2D &point) const
Distance to point.
Definition: lightweight_geom_data.cpp:503
c
const GLubyte * c
Definition: glext.h:6313
mmin
int mmin(const int t1, const int t2)
Definition: xmlParser.cpp:37
mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput::maxObstacleDist
double maxObstacleDist
Maximum expected value to be found in obstacles.
Definition: CAbstractHolonomicReactiveMethod.h:56
TGap::max_eval
double max_eval
Definition: CHolonomicFullEval.cpp:53
mrpt::nav::CAbstractHolonomicReactiveMethod::NavOutput::desiredDirection
double desiredDirection
The desired motion direction, in the range [-PI, PI].
Definition: CAbstractHolonomicReactiveMethod.h:68
mrpt::obs::T2DScanProperties::rightToLeft
bool rightToLeft
Angles storage order: true=counterclockwise; false=clockwise.
Definition: T2DScanProperties.h:28
mrpt::obs::T2DScanProperties
Auxiliary struct that holds all the relevant geometry information about a 2D scan.
Definition: T2DScanProperties.h:23
mrpt::nav
Definition: CAbstractHolonomicReactiveMethod.h:23
mrpt::nav::CHolonomicFullEval::serializeTo
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CHolonomicFullEval.cpp:774
mrpt::math::TPoint2D::x
double x
X,Y coordinates.
Definition: lightweight_geom_data.h:49
mrpt::nav::ClearanceDiagram::getClearance
double getClearance(uint16_t k, double TPS_query_distance, bool integrate_over_path) const
Gets the clearance for path k and distance TPS_query_distance in one of two modes:
Definition: ClearanceDiagram.cpp:132
stl_serialization.h
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
mrpt::nav::CLogFileRecord_FullEval::evaluation
double evaluation
Definition: CHolonomicFullEval.h:173
mrpt::nav::CHolonomicFullEval::TOptions::PHASE_FACTORS
std::vector< std::vector< int32_t > > PHASE_FACTORS
Factor indices [0,4] for the factors to consider in each phase 1,2,...N of the movement decision (Def...
Definition: CHolonomicFullEval.h:94
mrpt::obs::T2DScanProperties::nRays
size_t nRays
Definition: T2DScanProperties.h:25
uint8_t
unsigned char uint8_t
Definition: rptypes.h:41
mrpt::obs::CSinCosLookUpTableFor2DScans::getSinCosForScan
const TSinCosValues & getSinCosForScan(const CObservation2DRangeScan &scan) const
Return two vectors with the cos and the sin of the angles for each of the rays in a scan,...
Definition: CSinCosLookUpTableFor2DScans.cpp:23
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
mrpt::square
T square(const T x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:18
mrpt::nav::CAbstractHolonomicReactiveMethod::NavOutput::desiredSpeed
double desiredSpeed
The desired motion speed in that direction, from 0 up to NavInput::maxRobotSpeed.
Definition: CAbstractHolonomicReactiveMethod.h:71
mrpt::nav::CHolonomicFullEval::TOptions::TARGET_SLOW_APPROACHING_DISTANCE
double TARGET_SLOW_APPROACHING_DISTANCE
Start to reduce speed when closer than this to target [m].
Definition: CHolonomicFullEval.h:80
mrpt::nav::CHolonomicFullEval::serializeGetVersion
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CHolonomicFullEval.cpp:773
mrpt::nav::CHolonomicFullEval::TOptions::saveToConfigFile
void saveToConfigFile(mrpt::config::CConfigFileBase &cfg, const std::string &section) const override
This method saves the options to a ".ini"-like file or memory-stored string list.
Definition: CHolonomicFullEval.cpp:708
mrpt::nav::CParameterizedTrajectoryGenerator::index2alpha
double index2alpha(uint16_t k) const
Alpha value for the discrete corresponding value.
Definition: CParameterizedTrajectoryGenerator.cpp:201
CHolonomicFullEval.h
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
MRPT_LOAD_CONFIG_VAR_NO_DEFAULT
#define MRPT_LOAD_CONFIG_VAR_NO_DEFAULT( variableName, variableType, configFileObject, sectionNameStr)
Definition: config/CConfigFileBase.h:377
mrpt::nav::CHolonomicFullEval::TOptions::factorNormalizeOrNot
std::vector< int32_t > factorNormalizeOrNot
0/1 to normalize factors.
Definition: CHolonomicFullEval.h:90
mrpt::math::norm
CONTAINER::Scalar norm(const CONTAINER &v)
Definition: ops_containers.h:134
mrpt::nav::CParameterizedTrajectoryGenerator::alpha2index
uint16_t alpha2index(double alpha) const
Discrete index value for the corresponding alpha value.
Definition: CParameterizedTrajectoryGenerator.cpp:216
mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput::targets
std::vector< mrpt::math::TPoint2D > targets
Relative location (x,y) of target point(s).
Definition: CAbstractHolonomicReactiveMethod.h:49
mrpt::nav::CHolonomicFullEval::postProcessDirectionEvaluations
virtual void postProcessDirectionEvaluations(std::vector< double > &dir_evals, const NavInput &ni, unsigned int trg_idx)
Definition: CHolonomicFullEval.cpp:843
mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput::obstacles
std::vector< double > obstacles
Distance to obstacles in polar coordinates, relative to the robot.
Definition: CAbstractHolonomicReactiveMethod.h:46
M_PI
#define M_PI
Definition: core/include/mrpt/core/bits_math.h:38
val
int val
Definition: mrpt_jpeglib.h:955
mrpt::nav::CAbstractHolonomicReactiveMethod
A base class for holonomic reactive navigation methods.
Definition: CAbstractHolonomicReactiveMethod.h:32
TGap::contains_target_k
bool contains_target_k
Definition: CHolonomicFullEval.cpp:54
mrpt::nav::CHolonomicFullEval::TOptions::OBSTACLE_SLOW_DOWN_DISTANCE
double OBSTACLE_SLOW_DOWN_DISTANCE
Start to reduce speed when clearance is below this value ([0,1] ratio wrt obstacle reference/max dist...
Definition: CHolonomicFullEval.h:83
mrpt::nav::CHolonomicFullEval::EvalOutput::best_eval
double best_eval
Definition: CHolonomicFullEval.h:149
TGap::TGap
TGap()
Definition: CHolonomicFullEval.cpp:58
mrpt::math::wrapToPi
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:53
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23
TGap
Definition: CHolonomicFullEval.cpp:50
MRPT_START
#define MRPT_START
Definition: exceptions.h:262
mrpt::nav::CHolonomicFullEval::EvalOutput
Definition: CHolonomicFullEval.h:146
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::format
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
mrpt::nav::CLogFileRecord_FullEval::serializeTo
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CHolonomicFullEval.cpp:597
mrpt::keep_max
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.
Definition: core/include/mrpt/core/bits_math.h:131
mrpt::nav::CHolonomicFullEval::TOptions::TOptions
TOptions()
Definition: CHolonomicFullEval.cpp:649
IMPLEMENTS_SERIALIZABLE
IMPLEMENTS_SERIALIZABLE(CLogFileRecord_FullEval, CHolonomicLogFileRecord, mrpt::nav) IMPLEMENTS_SERIALIZABLE(CHolonomicFullEval
mrpt::nav::CHolonomicFullEval::EvalOutput::best_k
unsigned int best_k
Definition: CHolonomicFullEval.h:148
mrpt::nav::CAbstractHolonomicReactiveMethod::m_enableApproachTargetSlowDown
bool m_enableApproachTargetSlowDown
Whether to decrease speed when approaching target.
Definition: CAbstractHolonomicReactiveMethod.h:132
round.h
mrpt::nav::CLogFileRecord_FullEval::Ptr
std::shared_ptr< CLogFileRecord_FullEval > Ptr
Definition: CHolonomicFullEval.h:167
mrpt::nav::CHolonomicFullEval::evalSingleTarget
void evalSingleTarget(unsigned int target_idx, const NavInput &ni, EvalOutput &eo)
Evals one single target of the potentially many of them in NavInput.
Definition: CHolonomicFullEval.cpp:73
mrpt::nav::CHolonomicFullEval::TOptions::LOG_SCORE_MATRIX
bool LOG_SCORE_MATRIX
(default:false, to save space)
Definition: CHolonomicFullEval.h:100
mrpt::utils::CConfigFileBase
mrpt::config::CConfigFileBase CConfigFileBase
Definition: utils/CConfigFileBase.h:7
mrpt::nav::CHolonomicFullEval::TOptions::factorWeights
std::vector< double > factorWeights
See docs above.
Definition: CHolonomicFullEval.h:88
mrpt::math::TPoint2D
Lightweight 2D point.
Definition: lightweight_geom_data.h:42
mrpt::nav::CHolonomicFullEval::TOptions::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CHolonomicFullEval.cpp:665
mrpt::math::TSegment2D
2D segment, consisting of two points.
Definition: lightweight_geom_data.h:958
mrpt::nav::CHolonomicFullEval::m_dirs_scores
mrpt::math::CMatrixD m_dirs_scores
Individual scores for each direction: (i,j), i (row) are directions, j (cols) are scores.
Definition: CHolonomicFullEval.h:138
mrpt::nav::CHolonomicFullEval::TOptions::PHASE_THRESHOLDS
std::vector< double > PHASE_THRESHOLDS
Phase 1,2,N-1...
Definition: CHolonomicFullEval.h:97
mrpt::nav::CHolonomicFullEval::m_sincos_lut
mrpt::obs::CSinCosLookUpTableFor2DScans m_sincos_lut
Definition: CHolonomicFullEval.h:158
mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput::maxRobotSpeed
double maxRobotSpeed
Maximum robot speed, in the same units than obstacles, per second.
Definition: CAbstractHolonomicReactiveMethod.h:52
span
GLenum GLenum GLvoid GLvoid GLvoid * span
Definition: glext.h:3577
mrpt::config::MRPT_SAVE_VALUE_PADDING
int MRPT_SAVE_VALUE_PADDING()
Definition: CConfigFileBase.cpp:27
mrpt::nav::CHolonomicFullEval::direction2sector
unsigned int direction2sector(const double a, const unsigned int N)
Definition: CHolonomicFullEval.cpp:581
TGap::k_best_eval
int k_best_eval
Direction with the best evaluation inside the gap.
Definition: CHolonomicFullEval.cpp:56
mrpt::nav::CHolonomicFullEval::EvalOutput::phase_scores
std::vector< std::vector< double > > phase_scores
Definition: CHolonomicFullEval.h:150
mrpt::abs_diff
T abs_diff(const T a, const T b)
Efficient and portable evaluation of the absolute difference of two unsigned integer values (but will...
Definition: core/include/mrpt/core/bits_math.h:97
mrpt::nav::CHolonomicFullEval::options
TOptions options
Parameters of the algorithm (can be set manually or loaded from CHolonomicFullEval::initialize or opt...
Definition: CHolonomicFullEval.h:121
mrpt::nav::CHolonomicFullEval::TOptions::TOO_CLOSE_OBSTACLE
double TOO_CLOSE_OBSTACLE
Directions with collision-free distances below this threshold are not elegible.
Definition: CHolonomicFullEval.h:78
min
#define min(a, b)
Definition: rplidar_driver.cpp:42
mrpt::nav::CAbstractHolonomicReactiveMethod::NavOutput
Output for CAbstractHolonomicReactiveMethod::navigate()
Definition: CAbstractHolonomicReactiveMethod.h:65
mrpt::nav::CHolonomicLogFileRecord::dirs_eval
std::vector< std::vector< double > > dirs_eval
Final [N-1] and earlier stages [0...N-1] evaluation scores for each direction, in the same order of T...
Definition: CHolonomicLogFileRecord.h:34
MRPT_END
#define MRPT_END
Definition: exceptions.h:266
mrpt::nav::CHolonomicLogFileRecord
A base class for log records for different holonomic navigation methods.
Definition: CHolonomicLogFileRecord.h:27
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:13
TGap::k_to
int k_to
Definition: CHolonomicFullEval.cpp:52
empty
EIGEN_STRONG_INLINE bool empty() const
Definition: eigen_plugins.h:601
in
GLuint in
Definition: glext.h:7274
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::nav::CLogFileRecord_FullEval::selectedTarget
int32_t selectedTarget
Normally = 0.
Definition: CHolonomicFullEval.h:179
mrpt::nav::CHolonomicFullEval::serializeFrom
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CHolonomicFullEval.cpp:789
mrpt::math::TSegment2D::point1
TPoint2D point1
Origin point.
Definition: lightweight_geom_data.h:964
mrpt::nav::CHolonomicFullEval::m_last_selected_sector
unsigned int m_last_selected_sector
Definition: CHolonomicFullEval.h:133
CArchive.h
mrpt::math::TSegment2D::point2
TPoint2D point2
Destiny point.
Definition: lightweight_geom_data.h:968
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput::clearance
const mrpt::nav::ClearanceDiagram * clearance
The computed clearance for each direction (optional in some implementations).
Definition: CAbstractHolonomicReactiveMethod.h:59
mrpt::nav::CHolonomicFullEval::TOptions::clearance_threshold_ratio
double clearance_threshold_ratio
Ratio [0,1], times path_count, gives the minimum number of paths at each side of a target direction t...
Definition: CHolonomicFullEval.h:105
y
GLenum GLint GLint y
Definition: glext.h:3538
mrpt::obs::T2DScanProperties::aperture
double aperture
Definition: T2DScanProperties.h:26
mrpt::config::MRPT_SAVE_NAME_PADDING
int MRPT_SAVE_NAME_PADDING()
Default padding sizes for macros MRPT_SAVE_CONFIG_VAR_COMMENT(), etc.
Definition: CConfigFileBase.cpp:26
x
GLenum GLint x
Definition: glext.h:3538
mrpt::nav::CHolonomicFullEval::navigate
void navigate(const NavInput &ni, NavOutput &no) override
Invokes the holonomic navigation algorithm itself.
Definition: CHolonomicFullEval.cpp:485
mrpt::system::sprintf_container
std::string sprintf_container(const char *fmt, const T &V)
Generates a string for a container in the format [A,B,C,...], and the fmt string for each vector elem...
Definition: string_utils.h:118
a
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
mrpt::nav::CLogFileRecord_FullEval::selectedSector
int32_t selectedSector
Member data.
Definition: CHolonomicFullEval.h:172
mrpt::nav::CHolonomicFullEval
Full evaluation of all possible directions within the discrete set of input directions.
Definition: CHolonomicFullEval.h:57



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST