MRPT  1.9.9
CHolonomicFullEval.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 
12 #include <mrpt/core/round.h>
13 #include <mrpt/math/geometry.h>
18 #include <Eigen/Dense> // col(),...
19 #include <cmath>
20 
21 using namespace mrpt;
22 using namespace mrpt::math;
23 using namespace mrpt::nav;
24 using namespace std;
25 
30 
31 const unsigned int INVALID_K = std::numeric_limits<unsigned int>::max();
32 
33 const unsigned NUM_FACTORS = 7U;
34 
36  const mrpt::config::CConfigFileBase* INI_FILE)
38  m_last_selected_sector(std::numeric_limits<unsigned int>::max())
39 {
40  if (INI_FILE != nullptr) initialize(*INI_FILE);
41 }
42 
43 void CHolonomicFullEval::saveConfigFile(mrpt::config::CConfigFileBase& c) const
44 {
45  options.saveToConfigFile(c, getConfigFileSectionName());
46 }
47 
49 {
50  options.loadFromConfigFile(c, getConfigFileSectionName());
51 }
52 
53 struct TGap
54 {
55  int k_from{-1}, k_to{-1};
56  double min_eval, max_eval;
57  bool contains_target_k{false};
58  /** Direction with the best evaluation inside the gap. */
59  int k_best_eval{-1};
60 
61  TGap()
62  : min_eval(std::numeric_limits<double>::max()),
63  max_eval(-std::numeric_limits<double>::max())
64 
65  {
66  }
67 };
68 
69 void CHolonomicFullEval::evalSingleTarget(
70  unsigned int target_idx, const NavInput& ni, EvalOutput& eo)
71 {
72  ASSERT_(target_idx < ni.targets.size());
73  const auto target = ni.targets[target_idx];
74 
75  using mrpt::square;
76 
77  eo = EvalOutput();
78 
79  const auto ptg = getAssociatedPTG();
80  const size_t nDirs = ni.obstacles.size();
81 
82  const double target_dir = ::atan2(target.y, target.x);
83  const unsigned int target_k =
84  CParameterizedTrajectoryGenerator::alpha2index(target_dir, nDirs);
85  const double target_dist = target.norm();
86 
87  m_dirs_scores.resize(nDirs, options.factorWeights.size() + 2);
88 
89  // TP-Obstacles in 2D:
90  std::vector<mrpt::math::TPoint2D> obstacles_2d(nDirs);
91 
93  sp.aperture = 2.0 * M_PI;
94  sp.nRays = nDirs;
95  sp.rightToLeft = true;
96  const auto& sc_lut = m_sincos_lut.getSinCosForScan(sp);
97 
98  for (unsigned int i = 0; i < nDirs; i++)
99  {
100  obstacles_2d[i].x = ni.obstacles[i] * sc_lut.ccos[i];
101  obstacles_2d[i].y = ni.obstacles[i] * sc_lut.csin[i];
102  }
103 
104  // Sanity checks:
105  ASSERT_EQUAL_(options.factorWeights.size(), NUM_FACTORS);
106  ASSERT_ABOVE_(nDirs, 3);
107 
108  for (unsigned int i = 0; i < nDirs; i++)
109  {
110  double scores[NUM_FACTORS]; // scores for each criterion
111 
112  // Too close to obstacles? (unless target is in between obstacles and
113  // the robot)
114  if (ni.obstacles[i] < options.TOO_CLOSE_OBSTACLE &&
115  !(i == target_k && ni.obstacles[i] > 1.02 * target_dist))
116  {
117  for (size_t l = 0; l < NUM_FACTORS; l++) m_dirs_scores(i, l) = .0;
118  continue;
119  }
120 
121  const double d = std::min(ni.obstacles[i], 0.95 * target_dist);
122 
123  // The TP-Space representative coordinates for this direction:
124  const double x = d * sc_lut.ccos[i];
125  const double y = d * sc_lut.csin[i];
126 
127  // Factor [0]: collision-free distance
128  // -----------------------------------------------------
129  if (mrpt::abs_diff(i, target_k) <= 1 &&
130  target_dist < 1.0 - options.TOO_CLOSE_OBSTACLE &&
131  ni.obstacles[i] > 1.05 * target_dist)
132  {
133  // Don't count obstacles ahead of the target.
134  scores[0] =
135  std::max(target_dist, ni.obstacles[i]) / (target_dist * 1.05);
136  }
137  else
138  {
139  scores[0] =
140  std::max(0.0, ni.obstacles[i] - options.TOO_CLOSE_OBSTACLE);
141  }
142 
143  // Discount "circular loop aparent free distance" here, but don't count
144  // it for clearance, since those are not real obstacle points.
145  if (ptg != nullptr)
146  {
147  const double max_real_freespace =
148  ptg->getActualUnloopedPathLength(i);
149  const double max_real_freespace_norm =
150  max_real_freespace / ptg->getRefDistance();
151 
152  mrpt::keep_min(scores[0], max_real_freespace_norm);
153  }
154 
155  // Factor [1]: Closest approach to target along straight line
156  // (Euclidean)
157  // -------------------------------------------
159  sg.point1.x = 0;
160  sg.point1.y = 0;
161  sg.point2.x = x;
162  sg.point2.y = y;
163 
164  // Range of attainable values: 0=passes thru target. 2=opposite
165  // direction
166  double min_dist_target_along_path = sg.distance(target);
167 
168  // Idea: if this segment is taking us *away* from target, don't make
169  // the segment to start at (0,0), since all paths "running away"
170  // will then have identical minimum distances to target. Use the
171  // middle of the segment instead:
172  const double endpt_dist_to_target = (target - TPoint2D(x, y)).norm();
173  const double endpt_dist_to_target_norm =
174  std::min(1.0, endpt_dist_to_target);
175 
176  if ((endpt_dist_to_target_norm > target_dist &&
177  endpt_dist_to_target_norm >= 0.95 * target_dist) &&
178  /* the path does not get any closer to trg */
179  min_dist_target_along_path >
180  1.05 * std::min(target_dist, endpt_dist_to_target_norm))
181  {
182  // path takes us away or way blocked:
183  sg.point1.x = x * 0.5;
184  sg.point1.y = y * 0.5;
185  min_dist_target_along_path = sg.distance(target);
186  }
187 
188  scores[1] = 1.0 / (1.0 + square(min_dist_target_along_path));
189 
190  // Factor [2]: Distance of end collision-free point to target
191  // (Euclidean)
192  // Factor [5]: idem (except: no decimation afterwards)
193  // -----------------------------------------------------
194  scores[2] = std::sqrt(1.01 - endpt_dist_to_target_norm);
195  scores[5] = scores[2];
196  // the 1.01 instead of 1.0 is to be 100% sure we don't get a domain
197  // error in sqrt()
198 
199  // Factor [3]: Stabilizing factor (hysteresis) to avoid quick switch
200  // among very similar paths:
201  // ------------------------------------------------------------------------------------------
202  if (m_last_selected_sector != std::numeric_limits<unsigned int>::max())
203  {
204  // It's fine here to consider that -PI is far from +PI.
205  const unsigned int hist_dist =
206  mrpt::abs_diff(m_last_selected_sector, i);
207 
208  if (hist_dist >= options.HYSTERESIS_SECTOR_COUNT)
209  scores[3] = square(
210  1.0 - (hist_dist - options.HYSTERESIS_SECTOR_COUNT) /
211  double(nDirs));
212  else
213  scores[3] = 1.0;
214  }
215  else
216  {
217  scores[3] = 1.0;
218  }
219 
220  // Factor [4]: clearance to nearest obstacle along path
221  // Use TP-obstacles instead of real obstacles in Workspace since
222  // it's way faster, despite being an approximation:
223  // -------------------------------------------------------------------
224  {
225  sg.point1.x = 0;
226  sg.point1.y = 0;
227  sg.point2.x = x;
228  sg.point2.y = y;
229 
230  double& closest_obs = scores[4];
231  closest_obs = 1.0;
232 
233  // eval obstacles within a certain region of this "i" direction only
234  const int W = std::max(1, round(nDirs * 0.1));
235  const int i_min = std::max(0, static_cast<int>(i) - W);
236  const int i_max =
237  std::min(static_cast<int>(nDirs) - 1, static_cast<int>(i) + W);
238  for (int oi = i_min; oi <= i_max; oi++)
239  {
240  // "no obstacle" (norm_dist=1.0) doesn't count as a real obs:
241  if (ni.obstacles[oi] >= 0.99) continue;
242  mrpt::keep_min(closest_obs, sg.distance(obstacles_2d[oi]));
243  }
244  }
245 
246  // Factor [6]: Direct distance in "sectors":
247  // -------------------------------------------------------------------
248  scores[6] =
249  1.0 /
250  (1.0 + mrpt::square((4.0 / nDirs) * mrpt::abs_diff(i, target_k)));
251 
252  // If target is not directly reachable for this i-th direction, decimate
253  // its scorings:
254  if (target_dist < 1.0 - options.TOO_CLOSE_OBSTACLE &&
255  ni.obstacles[i] < 1.01 * target_dist)
256  {
257  // this direction cannot reach target, so assign a low score:
258  scores[1] *= 0.1;
259  scores[2] *= 0.1;
260  scores[6] *= 0.1;
261  }
262 
263  // Save stats for debugging:
264  for (size_t l = 0; l < NUM_FACTORS; l++)
265  m_dirs_scores(i, l) = scores[l];
266 
267  } // end for each direction "i"
268 
269  // Normalize factors?
270  ASSERT_(options.factorNormalizeOrNot.size() == NUM_FACTORS);
271  for (size_t l = 0; l < NUM_FACTORS; l++)
272  {
273  if (!options.factorNormalizeOrNot[l]) continue;
274 
275  const double mmax = m_dirs_scores.col(l).maxCoeff();
276  const double mmin = m_dirs_scores.col(l).minCoeff();
277  const double span = mmax - mmin;
278  if (span <= .0) continue;
279 
280  m_dirs_scores.col(l).array() -= mmin;
281  m_dirs_scores.col(l).array() /= span;
282  }
283 
284  // Phase 1: average of PHASE1_FACTORS and thresholding:
285  // ----------------------------------------------------------------------
286  const unsigned int NUM_PHASES = options.PHASE_FACTORS.size();
287  ASSERT_(NUM_PHASES >= 1);
288 
289  std::vector<double> weights_sum_phase(NUM_PHASES, .0),
290  weights_sum_phase_inv(NUM_PHASES);
291  for (unsigned int i = 0; i < NUM_PHASES; i++)
292  {
293  for (unsigned int l : options.PHASE_FACTORS[i])
294  weights_sum_phase[i] += options.factorWeights.at(l);
295  ASSERT_(weights_sum_phase[i] > .0);
296  weights_sum_phase_inv[i] = 1.0 / weights_sum_phase[i];
297  }
298 
299  eo.phase_scores = std::vector<std::vector<double>>(
300  NUM_PHASES, std::vector<double>(nDirs, .0));
301  auto& phase_scores = eo.phase_scores; // shortcut
302  double last_phase_threshold = -1.0; // don't threshold for the first phase
303 
304  for (unsigned int phase_idx = 0; phase_idx < NUM_PHASES; phase_idx++)
305  {
306  double phase_min = std::numeric_limits<double>::max(), phase_max = .0;
307 
308  for (unsigned int i = 0; i < nDirs; i++)
309  {
310  double this_dir_eval = 0;
311 
312  if (ni.obstacles[i] <
313  options.TOO_CLOSE_OBSTACLE || // Too close to obstacles ?
314  (phase_idx > 0 &&
315  phase_scores[phase_idx - 1][i] <
316  last_phase_threshold) // thresholding of the previous
317  // phase
318  )
319  {
320  this_dir_eval = .0;
321  }
322  else
323  {
324  // Weighted avrg of factors:
325  for (unsigned int l : options.PHASE_FACTORS[phase_idx])
326  this_dir_eval +=
327  options.factorWeights.at(l) *
328  std::log(std::max(1e-6, m_dirs_scores(i, l)));
329 
330  this_dir_eval *= weights_sum_phase_inv[phase_idx];
331  this_dir_eval = std::exp(this_dir_eval);
332  }
333  phase_scores[phase_idx][i] = this_dir_eval;
334 
335  mrpt::keep_max(phase_max, phase_scores[phase_idx][i]);
336  mrpt::keep_min(phase_min, phase_scores[phase_idx][i]);
337 
338  } // for each direction
339 
340  ASSERT_(options.PHASE_THRESHOLDS.size() == NUM_PHASES);
341  ASSERT_(
342  options.PHASE_THRESHOLDS[phase_idx] > .0 &&
343  options.PHASE_THRESHOLDS[phase_idx] < 1.0);
344 
345  last_phase_threshold =
346  options.PHASE_THRESHOLDS[phase_idx] * phase_max +
347  (1.0 - options.PHASE_THRESHOLDS[phase_idx]) * phase_min;
348  } // end for each phase
349 
350  // Give a chance for a derived class to manipulate the final evaluations:
351  auto& dirs_eval = phase_scores.back();
352 
353  postProcessDirectionEvaluations(dirs_eval, ni, target_idx);
354 
355  // Recalculate the threshold just in case the postProcess function above
356  // changed things:
357  {
358  double phase_min = std::numeric_limits<double>::max(), phase_max = .0;
359  for (unsigned int i = 0; i < nDirs; i++)
360  {
361  mrpt::keep_max(phase_max, dirs_eval[i]);
362  mrpt::keep_min(phase_min, dirs_eval[i]);
363  }
364  last_phase_threshold =
365  options.PHASE_THRESHOLDS.back() * phase_max +
366  (1.0 - options.PHASE_THRESHOLDS.back()) * phase_min;
367  }
368 
369  // Thresholding:
370  for (unsigned int i = 0; i < nDirs; i++)
371  {
372  double& val = dirs_eval[i];
373  if (val < last_phase_threshold) val = .0;
374  }
375 }
376 
377 void CHolonomicFullEval::navigate(const NavInput& ni, NavOutput& no)
378 {
379  using mrpt::square;
380 
381  ASSERT_(ni.clearance != nullptr);
382  ASSERT_(!ni.targets.empty());
383 
384  // Create a log record for returning data.
385  auto log = std::make_shared<CLogFileRecord_FullEval>();
386  no.logRecord = log;
387 
388  // Evaluate for each target:
389  const size_t numTrgs = ni.targets.size();
390  std::vector<EvalOutput> evals(numTrgs);
391  for (unsigned int trg_idx = 0; trg_idx < numTrgs; trg_idx++)
392  {
393  evalSingleTarget(trg_idx, ni, evals[trg_idx]);
394  }
395 
396  ASSERT_(!evals.empty());
397  const auto nDirs = evals.front().phase_scores.back().size();
398  ASSERT_EQUAL_(nDirs, ni.obstacles.size());
399 
400  // Now, sum all weights for the last stage for each target into an "overall"
401  // score vector, one score per direction of motion:
402  std::vector<double> overall_scores;
403  overall_scores.assign(nDirs, .0);
404  for (const auto& e : evals)
405  {
406  for (unsigned int i = 0; i < nDirs; i++)
407  overall_scores[i] += e.phase_scores.back()[i];
408  }
409  // Normalize:
410  for (unsigned int i = 0; i < nDirs; i++)
411  overall_scores[i] *= (1.0 / numTrgs);
412 
413  // Search for best direction in the "overall score" vector:
414 
415  // Keep the GAP with the largest maximum value within;
416  // then pick the MIDDLE point as the final selection.
417  std::vector<TGap> gaps;
418  std::size_t best_gap_idx = std::string::npos;
419  {
420  bool inside_gap = false;
421  for (unsigned int i = 0; i < nDirs; i++)
422  {
423  const double val = overall_scores[i];
424  if (val < 0.01)
425  {
426  // This direction didn't pass the cut threshold for the "last
427  // phase":
428  if (inside_gap)
429  {
430  // We just ended a gap:
431  auto& active_gap = *gaps.rbegin();
432  active_gap.k_to = i - 1;
433  inside_gap = false;
434  }
435  }
436  else
437  {
438  // higher or EQUAL to the treshold (equal is important just in
439  // case we have a "flat" diagram...)
440  if (!inside_gap)
441  {
442  // We just started a gap:
443  TGap new_gap;
444  new_gap.k_from = i;
445  gaps.emplace_back(new_gap);
446  inside_gap = true;
447  }
448  }
449 
450  if (inside_gap)
451  {
452  auto& active_gap = *gaps.rbegin();
453  if (val >= active_gap.max_eval)
454  {
455  active_gap.k_best_eval = i;
456  }
457  mrpt::keep_max(active_gap.max_eval, val);
458  mrpt::keep_min(active_gap.min_eval, val);
459 
460  if (best_gap_idx == std::string::npos ||
461  val > gaps[best_gap_idx].max_eval)
462  {
463  best_gap_idx = gaps.size() - 1;
464  }
465  }
466  } // end for i
467 
468  // Handle the case where we end with an open, active gap:
469  if (inside_gap)
470  {
471  auto& active_gap = *gaps.rbegin();
472  active_gap.k_to = nDirs - 1;
473  }
474  }
475 
476  // Not heading to target: go thru the "middle" of the gap to maximize
477  // clearance
478  int best_dir_k = -1;
479  double best_dir_eval = 0;
480 
481  // We may have no gaps if all paths are blocked by obstacles, for example:
482  if (!gaps.empty())
483  {
484  ASSERT_(best_gap_idx < gaps.size());
485  const TGap& best_gap = gaps[best_gap_idx];
486  best_dir_k = best_gap.k_best_eval;
487  best_dir_eval = overall_scores.at(best_dir_k);
488  }
489 
490  // Prepare NavigationOutput data:
491  if (best_dir_eval == .0)
492  {
493  // No way found!
494  no.desiredDirection = 0;
495  no.desiredSpeed = 0;
496  }
497  else
498  {
499  // A valid movement:
500  const auto ptg = getAssociatedPTG();
501  const double ptg_ref_dist = ptg ? ptg->getRefDistance() : 1.0;
502 
503  no.desiredDirection =
504  CParameterizedTrajectoryGenerator::index2alpha(best_dir_k, nDirs);
505 
506  // Speed control: Reduction factors
507  // ---------------------------------------------
508  const double targetNearnessFactor =
509  m_enableApproachTargetSlowDown
510  ? std::min(
511  1.0, ni.targets.front().norm() /
512  (options.TARGET_SLOW_APPROACHING_DISTANCE /
513  ptg_ref_dist))
514  : 1.0;
515 
516  const double obs_dist = ni.obstacles[best_dir_k];
517  // Was: min with obs_clearance too.
518  const double obs_dist_th = std::max(
519  options.TOO_CLOSE_OBSTACLE,
520  options.OBSTACLE_SLOW_DOWN_DISTANCE * ni.maxObstacleDist);
521  double riskFactor = 1.0;
522  if (obs_dist <= options.TOO_CLOSE_OBSTACLE)
523  {
524  riskFactor = 0.0;
525  }
526  else if (
527  obs_dist < obs_dist_th && obs_dist_th > options.TOO_CLOSE_OBSTACLE)
528  {
529  riskFactor = (obs_dist - options.TOO_CLOSE_OBSTACLE) /
530  (obs_dist_th - options.TOO_CLOSE_OBSTACLE);
531  }
532  no.desiredSpeed =
533  ni.maxRobotSpeed * std::min(riskFactor, targetNearnessFactor);
534  }
535 
536  m_last_selected_sector = best_dir_k;
537 
538  // LOG --------------------------
539  if (log)
540  {
541  log->selectedTarget = 0; // was: best_trg_idx
542  log->selectedSector = best_dir_k;
543  log->evaluation = best_dir_eval;
544  // Copy the evaluation of first phases for (arbitrarily) the first
545  // target, then overwrite the scores of its last phase with the OVERALL
546  // phase scores:
547  log->dirs_eval = evals.front().phase_scores;
548  log->dirs_eval.back() = overall_scores;
549 
550  if (options.LOG_SCORE_MATRIX)
551  {
552  log->dirs_scores = m_dirs_scores;
553  }
554  }
555 }
556 
557 unsigned int CHolonomicFullEval::direction2sector(
558  const double a, const unsigned int N)
559 {
560  const int idx = round(0.5 * (N * (1 + mrpt::math::wrapToPi(a) / M_PI) - 1));
561  if (idx < 0)
562  return 0;
563  else
564  return static_cast<unsigned int>(idx);
565 }
566 
567 CLogFileRecord_FullEval::CLogFileRecord_FullEval() : dirs_scores() {}
568 uint8_t CLogFileRecord_FullEval::serializeGetVersion() const { return 3; }
571 {
573  << evaluation << selectedTarget /*v3*/;
574 }
575 
576 /*---------------------------------------------------------------
577  readFromStream
578  ---------------------------------------------------------------*/
580  mrpt::serialization::CArchive& in, uint8_t version)
581 {
582  switch (version)
583  {
584  case 0:
585  case 1:
586  case 2:
587  case 3:
588  {
589  if (version >= 2)
590  {
592  }
593  else
594  {
597  if (version >= 1)
598  {
600  }
601  }
603  if (version >= 3)
604  {
605  in >> selectedTarget;
606  }
607  else
608  {
609  selectedTarget = 0;
610  }
611  }
612  break;
613  default:
615  };
616 }
617 
618 /*---------------------------------------------------------------
619  TOptions
620  ---------------------------------------------------------------*/
622  : factorWeights{0.1, 0.5, 0.5, 0.01, 1, 1, 1},
623  factorNormalizeOrNot{0, 0, 0, 0, 1, 0, 0},
624  PHASE_FACTORS{{1, 2}, {4}, {0, 2}},
625  PHASE_THRESHOLDS{0.5, 0.6, 0.7}
626 
627 {
628 }
629 
631  const mrpt::config::CConfigFileBase& c, const std::string& s)
632 {
633  MRPT_START
634 
635  // Load from config text:
636  MRPT_LOAD_CONFIG_VAR(TOO_CLOSE_OBSTACLE, double, c, s);
637  MRPT_LOAD_CONFIG_VAR(TARGET_SLOW_APPROACHING_DISTANCE, double, c, s);
638  MRPT_LOAD_CONFIG_VAR(OBSTACLE_SLOW_DOWN_DISTANCE, double, c, s);
639  MRPT_LOAD_CONFIG_VAR(HYSTERESIS_SECTOR_COUNT, double, c, s);
640  MRPT_LOAD_CONFIG_VAR(LOG_SCORE_MATRIX, bool, c, s);
641  MRPT_LOAD_CONFIG_VAR(clearance_threshold_ratio, double, c, s);
642  MRPT_LOAD_CONFIG_VAR(gap_width_ratio_threshold, double, c, s);
643 
644  c.read_vector(
645  s, "factorWeights", std::vector<double>(), factorWeights, true);
646  ASSERT_EQUAL_(factorWeights.size(), NUM_FACTORS);
647 
648  c.read_vector(
649  s, "factorNormalizeOrNot", factorNormalizeOrNot, factorNormalizeOrNot);
650  ASSERT_EQUAL_(factorNormalizeOrNot.size(), factorWeights.size());
651 
652  // Phases:
653  int PHASE_COUNT = 0;
654  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(PHASE_COUNT, int, c, s);
655 
656  PHASE_FACTORS.resize(PHASE_COUNT);
657  PHASE_THRESHOLDS.resize(PHASE_COUNT);
658  for (int i = 0; i < PHASE_COUNT; i++)
659  {
660  c.read_vector(
661  s, mrpt::format("PHASE%i_FACTORS", i + 1), PHASE_FACTORS[i],
662  PHASE_FACTORS[i], true);
663  ASSERT_(!PHASE_FACTORS[i].empty());
664 
665  PHASE_THRESHOLDS[i] = c.read_double(
666  s, mrpt::format("PHASE%i_THRESHOLD", i + 1), .0, true);
667  ASSERT_(PHASE_THRESHOLDS[i] >= .0 && PHASE_THRESHOLDS[i] <= 1.0);
668  }
669 
670  MRPT_END
671 }
672 
674  mrpt::config::CConfigFileBase& c, const std::string& s) const
675 {
676  MRPT_START;
677 
678  const int WN = mrpt::config::MRPT_SAVE_NAME_PADDING(),
680 
682  TOO_CLOSE_OBSTACLE,
683  "Directions with collision-free distances below this threshold are not "
684  "elegible.");
686  TARGET_SLOW_APPROACHING_DISTANCE,
687  "Start to reduce speed when closer than this to target.");
689  OBSTACLE_SLOW_DOWN_DISTANCE,
690  "Start to reduce speed when clearance is below this value ([0,1] ratio "
691  "wrt obstacle reference/max distance)");
693  HYSTERESIS_SECTOR_COUNT,
694  "Range of `sectors` (directions) for hysteresis over successive "
695  "timesteps");
697  LOG_SCORE_MATRIX, "Save the entire score matrix in log files");
699  clearance_threshold_ratio,
700  "Ratio [0,1], times path_count, gives the minimum number of paths at "
701  "each side of a target direction to be accepted as desired direction");
703  gap_width_ratio_threshold,
704  "Ratio [0,1], times path_count, gives the minimum gap width to accept "
705  "a direct motion towards target.");
706 
707  ASSERT_EQUAL_(factorWeights.size(), NUM_FACTORS);
708  c.write(
709  s, "factorWeights",
710  mrpt::system::sprintf_container("%.2f ", factorWeights), WN, WV,
711  "[0]=Free space, [1]=Dist. in sectors, [2]=Closer to target "
712  "(Euclidean), [3]=Hysteresis, [4]=clearance along path, [5]=Like [2] "
713  "without decimation if path obstructed");
714  c.write(
715  s, "factorNormalizeOrNot",
716  mrpt::system::sprintf_container("%u ", factorNormalizeOrNot), WN, WV,
717  "Normalize factors or not (1/0)");
718 
719  c.write(
720  s, "PHASE_COUNT", PHASE_FACTORS.size(), WN, WV,
721  "Number of evaluation phases to run (params for each phase below)");
722 
723  for (unsigned int i = 0; i < PHASE_FACTORS.size(); i++)
724  {
725  c.write(
726  s, mrpt::format("PHASE%u_THRESHOLD", i + 1), PHASE_THRESHOLDS[i],
727  WN, WV,
728  "Phase scores must be above this relative range threshold [0,1] to "
729  "be considered in next phase (Default:`0.75`)");
730  c.write(
731  s, mrpt::format("PHASE%u_FACTORS", i + 1),
732  mrpt::system::sprintf_container("%d ", PHASE_FACTORS[i]), WN, WV,
733  "Indices of the factors above to be considered in this phase");
734  }
735 
736  MRPT_END;
737 }
738 
739 uint8_t CHolonomicFullEval::serializeGetVersion() const { return 4; }
741 {
742  // Params:
744  << options.PHASE_FACTORS << // v3
746  << options.PHASE_THRESHOLDS // v3
751  ;
752  // State:
754 }
756  mrpt::serialization::CArchive& in, uint8_t version)
757 {
758  switch (version)
759  {
760  case 0:
761  case 1:
762  case 2:
763  case 3:
764  case 4:
765  {
766  // Params:
768 
769  if (version >= 3)
770  {
771  in >> options.PHASE_FACTORS;
772  }
773  else
774  {
775  options.PHASE_THRESHOLDS.resize(2);
777  }
780 
781  if (version >= 3)
782  {
784  }
785  else
786  {
787  options.PHASE_THRESHOLDS.resize(1);
788  in >> options.PHASE_THRESHOLDS[0];
789  }
790 
791  if (version >= 1) in >> options.OBSTACLE_SLOW_DOWN_DISTANCE;
792  if (version >= 2) in >> options.factorNormalizeOrNot;
793 
794  if (version >= 4)
795  {
798  }
799 
800  // State:
802  }
803  break;
804  default:
806  };
807 }
808 
810  std::vector<double>& dir_evals, const NavInput& ni, unsigned int trg_idx)
811 {
812  // Default: do nothing
813 }
double gap_width_ratio_threshold
Ratio [0,1], times path_count, gives the minimum gap width to accept a direct motion towards target...
virtual void postProcessDirectionEvaluations(std::vector< double > &dir_evals, const NavInput &ni, unsigned int trg_idx)
If desired, override in a derived class to manipulate the final evaluations of each directions...
double x
X,Y coordinates.
Definition: TPoint2D.h:23
double min_eval
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...
#define MRPT_START
Definition: exceptions.h:241
A class for storing extra information about the execution of CHolonomicFullEval navigation.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
app initialize(argc, argv)
double TOO_CLOSE_OBSTACLE
Directions with collision-free distances below this threshold are not elegible.
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. ...
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
int MRPT_SAVE_NAME_PADDING()
Default padding sizes for macros MRPT_SAVE_CONFIG_VAR_COMMENT(), etc.
std::vector< double > obstacles
Distance to obstacles in polar coordinates, relative to the robot.
double distance(const TPoint2D &point) const
Distance to point.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
This file implements several operations that operate element-wise on individual or pairs of container...
A base class for holonomic reactive navigation methods.
double TARGET_SLOW_APPROACHING_DISTANCE
Start to reduce speed when closer than this to target [m].
STL namespace.
mrpt::nav const unsigned int INVALID_K
std::vector< int32_t > factorNormalizeOrNot
0/1 to normalize factors.
IMPLEMENTS_SERIALIZABLE(CLogFileRecord_FullEval, CHolonomicLogFileRecord, mrpt::nav) IMPLEMENTS_SERIALIZABLE(CHolonomicFullEval
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
double clearance_threshold_ratio
Ratio [0,1], times path_count, gives the minimum number of paths at each side of a target direction t...
std::vector< double > PHASE_THRESHOLDS
Phase 1,2,N-1...
std::vector< mrpt::math::TPoint2D > targets
Relative location (x,y) of target point(s).
TOptions options
Parameters of the algorithm (can be set manually or loaded from CHolonomicFullEval::initialize or opt...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
double OBSTACLE_SLOW_DOWN_DISTANCE
Start to reduce speed when clearance is below this value ([0,1] ratio wrt obstacle reference/max dist...
const unsigned NUM_FACTORS
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
double maxRobotSpeed
Maximum robot speed, in the same units than obstacles, per second.
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
2D segment, consisting of two points.
Definition: TSegment2D.h:20
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
Auxiliary struct that holds all the relevant geometry information about a 2D scan.
T square(const T x)
Inline function for the square of a number.
A base class for log records for different holonomic navigation methods.
double desiredDirection
The desired motion direction, in the range [-PI, PI].
mrpt::config::CConfigFileBase CConfigFileBase
std::vector< double > factorWeights
See docs above.
int val
Definition: mrpt_jpeglib.h:957
int MRPT_SAVE_VALUE_PADDING()
#define MRPT_LOAD_CONFIG_VAR_NO_DEFAULT( variableName, variableType, configFileObject, sectionNameStr)
CHolonomicLogFileRecord::Ptr logRecord
The navigation method will create a log record and store it here via a smart pointer.
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
TPoint2D point2
Destiny point.
Definition: TSegment2D.h:30
double maxObstacleDist
Maximum expected value to be found in obstacles.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
Full evaluation of all possible directions within the discrete set of input directions.
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())
bool empty() const
Definition: ts_hash_map.h:191
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...
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...
TPoint2D point1
Origin point.
Definition: TSegment2D.h:26
double desiredSpeed
The desired motion speed in that direction, from 0 up to NavInput::maxRobotSpeed. ...
#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...
const mrpt::nav::ClearanceDiagram * clearance
The computed clearance for each direction (optional in some implementations).
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
mrpt::vision::TStereoCalibResults out
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
#define MRPT_END
Definition: exceptions.h:245
double HYSTERESIS_SECTOR_COUNT
Range of "sectors" (directions) for hysteresis over successive timesteps.
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
Input parameters for CAbstractHolonomicReactiveMethod::navigate()
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
bool rightToLeft
Angles storage order: true=counterclockwise; false=clockwise.
Lightweight 2D point.
Definition: TPoint2D.h:31
Output for CAbstractHolonomicReactiveMethod::navigate()
mrpt::math::CMatrixD dirs_scores
Individual scores for each direction: (i,j), i (row) are directions, j (cols) are scores...
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:125
std::vector< std::vector< double > > phase_scores
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...
T abs_diff(const T a, const T b)
Efficient and portable evaluation of the absolute difference of two unsigned integer values (but will...
void read_vector(const std::string &section, const std::string &name, const VECTOR_TYPE &defaultValue, VECTOR_TYPE &outValues, bool failIfNotFound=false) const
Reads a configuration parameter of type vector, stored in the file as a string: "[v1 v2 v3 ...
CONTAINER::Scalar norm(const CONTAINER &v)
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.
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7e629e01a Sat Dec 14 00:05:55 2019 +0100 at sáb dic 14 00:15:10 CET 2019