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



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018