Main MRPT website > C++ reference for MRPT 1.9.9
CHolonomicND.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>
17 
18 using namespace mrpt;
19 using namespace mrpt::math;
20 using namespace mrpt::nav;
21 using namespace std;
22 
26 
27 /** Initialize the parameters of the navigator, from some
28 * configuration file, or default values if filename is set to NULL.
29 */
30 CHolonomicND::CHolonomicND(const mrpt::config::CConfigFileBase* INI_FILE)
32  m_last_selected_sector(std::numeric_limits<unsigned int>::max())
33 {
34  if (INI_FILE != nullptr) initialize(*INI_FILE);
35 }
36 
37 void CHolonomicND::initialize(const mrpt::config::CConfigFileBase& INI_FILE)
38 {
39  options.loadFromConfigFile(INI_FILE, getConfigFileSectionName());
40 }
41 void CHolonomicND::saveConfigFile(mrpt::config::CConfigFileBase& c) const
42 {
43  options.saveToConfigFile(c, getConfigFileSectionName());
44 }
45 
46 /*---------------------------------------------------------------
47  Navigate
48  ---------------------------------------------------------------*/
49 void CHolonomicND::navigate(const NavInput& ni, NavOutput& no)
50 {
51  const auto ptg = getAssociatedPTG();
52  const double ptg_ref_dist = ptg ? ptg->getRefDistance() : 1.0;
53 
54  TGapArray gaps;
55  TSituations situation;
56  unsigned int selectedSector;
57  double riskEvaluation;
58  double evaluation;
59 
60  // Create a log record for returning data.
61  CLogFileRecord_ND::Ptr log = mrpt::make_aligned_shared<CLogFileRecord_ND>();
62  no.logRecord = log;
63 
64  // Search gaps:
65  gaps.clear();
66  ASSERT_(!ni.targets.empty());
67  const auto trg = *ni.targets.rbegin();
68 
69  gapsEstimator(ni.obstacles, trg, gaps);
70 
71  // Select best gap:
72  searchBestGap(
73  ni.obstacles, 1.0 /* max obs range*/, gaps, trg, selectedSector,
74  evaluation, situation, riskEvaluation, *log);
75 
76  if (situation == SITUATION_NO_WAY_FOUND)
77  {
78  // No way found!
79  no.desiredDirection = 0;
80  no.desiredSpeed = 0;
81  }
82  else
83  {
84  // A valid movement:
85  no.desiredDirection = CParameterizedTrajectoryGenerator::index2alpha(
86  selectedSector, ni.obstacles.size());
87 
88  // Speed control: Reduction factors
89  // ---------------------------------------------
90  const double targetNearnessFactor =
91  m_enableApproachTargetSlowDown
92  ? std::min(
93  1.0,
94  trg.norm() / (options.TARGET_SLOW_APPROACHING_DISTANCE /
95  ptg_ref_dist))
96  : 1.0;
97 
98  const double riskFactor =
99  std::min(1.0, riskEvaluation / options.RISK_EVALUATION_DISTANCE);
100  no.desiredSpeed =
101  ni.maxRobotSpeed * std::min(riskFactor, targetNearnessFactor);
102  }
103 
104  m_last_selected_sector = selectedSector;
105 
106  // LOG --------------------------
107  if (log)
108  {
109  // gaps:
110  {
111  int i, n = gaps.size();
112  log->gaps_ini.resize(n);
113  log->gaps_end.resize(n);
114  for (i = 0; i < n; i++)
115  {
116  log->gaps_ini[i] = gaps[i].ini;
117  log->gaps_end[i] = gaps[i].end;
118  }
119  }
120  // Selection:
121  log->selectedSector = selectedSector;
122  log->evaluation = evaluation;
123  log->situation = situation;
124  log->riskEvaluation = riskEvaluation;
125  }
126 }
127 
128 /*---------------------------------------------------------------
129  Find gaps in the obtacles (Beta version)
130  ---------------------------------------------------------------*/
131 void CHolonomicND::gapsEstimator(
132  const std::vector<double>& obstacles, const mrpt::math::TPoint2D& target,
133  TGapArray& gaps_out)
134 {
135  const size_t n = obstacles.size();
136  ASSERT_(n > 2);
137 
138  // ================ Parameters ================
139  const int GAPS_MIN_WIDTH = ceil(n * 0.01); // was: 3
140  const double GAPS_MIN_DEPTH_CONSIDERED = 0.6;
141  const double GAPS_MAX_RELATIVE_DEPTH = 0.5;
142  // ============================================
143 
144  // Find the maximum distances to obstacles:
145  // ----------------------------------------------------------
146  float overall_max_dist = std::numeric_limits<float>::min(),
147  overall_min_dist = std::numeric_limits<float>::max();
148  for (size_t i = 1; i < (n - 1); i++)
149  {
150  mrpt::keep_max(overall_max_dist, obstacles[i]);
151  mrpt::keep_min(overall_min_dist, obstacles[i]);
152  }
153  double max_depth = overall_max_dist - overall_min_dist;
154 
155  // Build list of "GAPS":
156  // --------------------------------------------------------
157  TGapArray gaps_temp;
158  gaps_temp.reserve(150);
159 
160  for (double threshold_ratio = 0.95; threshold_ratio >= 0.05;
161  threshold_ratio -= 0.05)
162  {
163  const double dist_threshold =
164  threshold_ratio * overall_max_dist +
165  (1.0f - threshold_ratio) *
166  min(target.norm(), GAPS_MIN_DEPTH_CONSIDERED);
167 
168  bool is_inside = false;
169  size_t sec_ini = 0, sec_end = 0;
170  double maxDist = 0.;
171 
172  for (size_t i = 0; i < n; i++)
173  {
174  if (!is_inside && (obstacles[i] >= dist_threshold)) // A gap begins
175  {
176  sec_ini = i;
177  maxDist = obstacles[i];
178  is_inside = true;
179  }
180  else if (
181  is_inside &&
182  (i == (n - 1) || obstacles[i] < dist_threshold)) // A gap ends
183  {
184  if (obstacles[i] < dist_threshold)
185  sec_end = i - 1;
186  else
187  sec_end = i;
188 
189  is_inside = false;
190 
191  if ((sec_end - sec_ini) >= (size_t)GAPS_MIN_WIDTH)
192  {
193  // Add new gap:
194  gaps_temp.resize(gaps_temp.size() + 1);
195  TGap& newGap = *gaps_temp.rbegin();
196 
197  newGap.ini = sec_ini;
198  newGap.end = sec_end;
199  newGap.minDistance =
200  min(obstacles[sec_ini], obstacles[sec_end]);
201  newGap.maxDistance = maxDist;
202  }
203  }
204 
205  if (is_inside) maxDist = std::max(maxDist, obstacles[i]);
206  }
207  }
208 
209  // Start to filter the gap list
210  //--------------------------------------------------------------
211 
212  const size_t nTempGaps = gaps_temp.size();
213 
214  std::vector<bool> delete_gaps;
215  delete_gaps.assign(nTempGaps, false);
216 
217  // First, remove redundant gaps
218  for (size_t i = 0; i < nTempGaps; i++)
219  {
220  if (delete_gaps[i] == 1) continue;
221 
222  for (size_t j = i + 1; j < nTempGaps; j++)
223  {
224  if (gaps_temp[i].ini == gaps_temp[j].ini ||
225  gaps_temp[i].end == gaps_temp[j].end)
226  delete_gaps[j] = 1;
227  }
228  }
229 
230  // Remove gaps with a big depth
231  for (size_t i = 0; i < nTempGaps; i++)
232  {
233  if (delete_gaps[i] == 1) continue;
234 
235  if ((gaps_temp[i].maxDistance - gaps_temp[i].minDistance) >
236  max_depth * GAPS_MAX_RELATIVE_DEPTH)
237  delete_gaps[i] = 1;
238  }
239 
240  // Delete gaps which contain more than one other gaps
241  for (size_t i = 0; i < nTempGaps; i++)
242  {
243  if (delete_gaps[i]) continue;
244 
245  unsigned int inner_gap_count = 0;
246 
247  for (unsigned int j = 0; j < nTempGaps; j++)
248  {
249  if (i == j || delete_gaps[j]) continue;
250 
251  // j is inside of i?
252  if (gaps_temp[j].ini >= gaps_temp[i].ini &&
253  gaps_temp[j].end <= gaps_temp[i].end)
254  if (++inner_gap_count > 1)
255  {
256  delete_gaps[i] = 1;
257  break;
258  }
259  }
260  }
261 
262  // Delete gaps included in other gaps
263  for (size_t i = 0; i < nTempGaps; i++)
264  {
265  if (delete_gaps[i]) continue;
266 
267  for (unsigned int j = 0; j < nTempGaps; j++)
268  {
269  if (i == j || delete_gaps[j]) continue;
270  if (gaps_temp[i].ini <= gaps_temp[j].ini &&
271  gaps_temp[i].end >= gaps_temp[j].end)
272  delete_gaps[j] = 1;
273  }
274  }
275 
276  // Copy as result only those gaps not marked for deletion:
277  // --------------------------------------------------------
278  gaps_out.clear();
279  gaps_out.reserve(nTempGaps / 2);
280  for (size_t i = 0; i < nTempGaps; i++)
281  {
282  if (delete_gaps[i]) continue;
283 
284  // Compute the representative direction ("sector") for this gap:
285  calcRepresentativeSectorForGap(gaps_temp[i], target, obstacles);
286 
287  gaps_out.push_back(gaps_temp[i]);
288  }
289 }
290 
291 /*---------------------------------------------------------------
292  Search the best gap.
293  ---------------------------------------------------------------*/
294 void CHolonomicND::searchBestGap(
295  const std::vector<double>& obstacles, const double maxObsRange,
296  const TGapArray& in_gaps, const mrpt::math::TPoint2D& target,
297  unsigned int& out_selDirection, double& out_selEvaluation,
298  TSituations& out_situation, double& out_riskEvaluation,
299  CLogFileRecord_ND& log)
300 {
301  // For evaluating the "risk":
302  unsigned int min_risk_eval_sector = 0;
303  unsigned int max_risk_eval_sector = obstacles.size() - 1;
304  const unsigned int target_sector =
305  direction2sector(atan2(target.y, target.x), obstacles.size());
306  const double target_dist = std::max(0.01, target.norm());
307  // (Risk is evaluated at the end, for all the situations)
308 
309  // D1 : Straight path?
310  // --------------------------------------------------------
311  const int freeSectorsNearTarget = ceil(0.02 * obstacles.size());
312  bool theyAreFree = true, caseD1 = false;
313  if (target_sector > static_cast<unsigned int>(freeSectorsNearTarget) &&
314  target_sector <
315  static_cast<unsigned int>(obstacles.size() - freeSectorsNearTarget))
316  {
317  const double min_free_dist =
318  std::min(1.05 * target_dist, 0.95 * maxObsRange);
319  for (int j = -freeSectorsNearTarget;
320  theyAreFree && j <= freeSectorsNearTarget; j++)
321  if (obstacles[(int(target_sector) + j) % obstacles.size()] <
322  min_free_dist)
323  theyAreFree = false;
324  caseD1 = theyAreFree;
325  }
326 
327  if (caseD1)
328  {
329  // S1: Move straight towards target:
330  out_selDirection = target_sector;
331 
332  // In case of several paths, the shortest:
333  out_selEvaluation =
334  1.0 + std::max(0.0, (maxObsRange - target_dist) / maxObsRange);
335  out_situation = SITUATION_TARGET_DIRECTLY;
336  }
337  else
338  {
339  // Evaluate all gaps (if any):
340  std::vector<double> gaps_evaluation;
341  int selected_gap = -1;
342  double selected_gap_eval = -100;
343 
344  evaluateGaps(
345  obstacles, maxObsRange, in_gaps, target_sector, target_dist,
346  gaps_evaluation);
347 
348  log.gaps_eval = gaps_evaluation;
349 
350  // D2: is there any gap "beyond" the target (and not too far away)?
351  // (Not used)
352  // ----------------------------------------------------------------
353 
354  // unsigned int dist;
355  // for ( unsigned int i=0;i<in_gaps.size();i++ )
356  //{
357  // dist = mrpt::abs_diff(target_sector,
358  // in_gaps[i].representative_sector );
359  // if (dist > 0.5*obstacles.size())
360  // dist = obstacles.size() - dist;
361  //
362  // if ( in_gaps[i].maxDistance >= target_dist && dist <=
363  //(int)floor(options.MAX_SECTOR_DIST_FOR_D2_PERCENT * obstacles.size())
364  //)
365  //
366  // if ( gaps_evaluation[i]>selected_gap_eval )
367  // {
368  // selected_gap_eval = gaps_evaluation[i];
369  // selected_gap = i;
370  // }
371  //}
372 
373  // Keep the best gaps (if none was picked up to this point)
374  if (selected_gap == -1)
375  for (unsigned int i = 0; i < in_gaps.size(); i++)
376  if (gaps_evaluation[i] > selected_gap_eval)
377  {
378  selected_gap_eval = gaps_evaluation[i];
379  selected_gap = i;
380  }
381 
382  // D3: Wasn't a good enough gap (or there were none)?
383  // ----------------------------------------------------------
384  if (selected_gap_eval <= 0)
385  {
386  // S2: No way found
387  // ------------------------------------------------------
388  out_selDirection = 0;
389  out_selEvaluation = 0.0; // Worst case
390  out_situation = SITUATION_NO_WAY_FOUND;
391  }
392  else
393  {
394  // The selected gap:
395  const TGap& gap = in_gaps[selected_gap];
396 
397  const unsigned int sectors_to_be_wide =
398  round(options.WIDE_GAP_SIZE_PERCENT * obstacles.size());
399 
400  out_selDirection = in_gaps[selected_gap].representative_sector;
401  out_selEvaluation = selected_gap_eval;
402 
403  // D4: Is it a WIDE gap?
404  // -----------------------------------------------------
405  if ((gap.end - gap.ini) < sectors_to_be_wide)
406  {
407  // S3: Narrow gap
408  // -------------------------------------------
409  out_situation = SITUATION_SMALL_GAP;
410  }
411  else
412  {
413  // S4: Wide gap
414  // -------------------------------------------
415  out_situation = SITUATION_WIDE_GAP;
416  }
417 
418  // Evaluate the risk only within the gap:
419  min_risk_eval_sector = gap.ini;
420  max_risk_eval_sector = gap.end;
421  }
422  }
423 
424  // Evaluate short-term minimum distance to obstacles, in a small interval
425  // around the selected direction:
426  const unsigned int risk_eval_nsectors =
427  round(options.RISK_EVALUATION_SECTORS_PERCENT * obstacles.size());
428  const unsigned int sec_ini = std::max(
429  min_risk_eval_sector, risk_eval_nsectors < out_selDirection
430  ? out_selDirection - risk_eval_nsectors
431  : 0);
432  const unsigned int sec_fin =
433  std::min(max_risk_eval_sector, out_selDirection + risk_eval_nsectors);
434 
435  out_riskEvaluation = 0.0;
436  for (unsigned int i = sec_ini; i <= sec_fin; i++)
437  out_riskEvaluation += obstacles[i];
438  out_riskEvaluation /= (sec_fin - sec_ini + 1);
439 }
440 
441 /*---------------------------------------------------------------
442  Fills in the representative sector
443  field in the gap structure:
444  ---------------------------------------------------------------*/
445 void CHolonomicND::calcRepresentativeSectorForGap(
446  TGap& gap, const mrpt::math::TPoint2D& target,
447  const std::vector<double>& obstacles)
448 {
449  int sector;
450  const unsigned int sectors_to_be_wide =
451  round(options.WIDE_GAP_SIZE_PERCENT * obstacles.size());
452  const unsigned int target_sector =
453  direction2sector(atan2(target.y, target.x), obstacles.size());
454 
455  if ((gap.end - gap.ini) <
456  sectors_to_be_wide) // Select the intermediate sector
457  {
458 #if 1
459  sector = round(0.5f * gap.ini + 0.5f * gap.end);
460 #else
461  float min_dist_obs_near_ini = 1, min_dist_obs_near_end = 1;
462  int i;
463  for (i = gap.ini; i >= max(0, gap.ini - 2); i--)
464  min_dist_obs_near_ini = min(min_dist_obs_near_ini, obstacles[i]);
465  for (i = gap.end; i <= min((int)obstacles.size() - 1, gap.end + 2); i++)
466  min_dist_obs_near_end = min(min_dist_obs_near_end, obstacles[i]);
467  sector = round(
468  (min_dist_obs_near_ini * gap.ini +
469  min_dist_obs_near_end * gap.end) /
470  (min_dist_obs_near_ini + min_dist_obs_near_end));
471 #endif
472  }
473  else // Select a sector close to the target but spaced
474  // "sectors_to_be_wide/2" from it
475  {
476  unsigned int dist_ini = mrpt::abs_diff(target_sector, gap.ini);
477  unsigned int dist_end = mrpt::abs_diff(target_sector, gap.end);
478 
479  if (dist_ini > 0.5 * obstacles.size())
480  dist_ini = obstacles.size() - dist_ini;
481  if (dist_end > 0.5 * obstacles.size())
482  dist_end = obstacles.size() - dist_end;
483 
484  int dir;
485  if (dist_ini < dist_end)
486  {
487  sector = gap.ini;
488  dir = +1;
489  }
490  else
491  {
492  sector = gap.end;
493  dir = -1;
494  }
495 
496  sector = sector + dir * static_cast<int>(sectors_to_be_wide) / 2;
497  }
498 
499  keep_max(sector, 0);
500  keep_min(sector, static_cast<int>(obstacles.size()) - 1);
501 
502  gap.representative_sector = sector;
503 }
504 
505 /*---------------------------------------------------------------
506  Evaluate each gap
507  ---------------------------------------------------------------*/
508 void CHolonomicND::evaluateGaps(
509  const std::vector<double>& obstacles, const double maxObsRange,
510  const TGapArray& gaps, const unsigned int target_sector,
511  const float target_dist, std::vector<double>& out_gaps_evaluation)
512 {
513  out_gaps_evaluation.resize(gaps.size());
514 
515  const double targetAng = CParameterizedTrajectoryGenerator::index2alpha(
516  target_sector, obstacles.size());
517  const double target_x = target_dist * cos(targetAng);
518  const double target_y = target_dist * sin(targetAng);
519 
520  for (unsigned int i = 0; i < gaps.size(); i++)
521  {
522  // Short cut:
523  const TGap* gap = &gaps[i];
524 
525  const float d = min3(
526  obstacles[gap->representative_sector], maxObsRange,
527  0.95 * target_dist);
528 
529  // The TP-Space representative coordinates for this gap:
530  const double phi = CParameterizedTrajectoryGenerator::index2alpha(
531  gap->representative_sector, obstacles.size());
532  const double x = d * cos(phi);
533  const double y = d * sin(phi);
534 
535  // Factor #1: Maximum reachable distance with this PTG:
536  // -----------------------------------------------------
537  // It computes the average free distance of the gap:
538  float meanDist = 0.f;
539  for (unsigned int j = gap->ini; j <= gap->end; j++)
540  meanDist += obstacles[j];
541  meanDist /= (gap->end - gap->ini + 1);
542 
543  double factor1;
544  if (mrpt::abs_diff(gap->representative_sector, target_sector) <= 1 &&
545  target_dist < 1)
546  factor1 = std::min(target_dist, meanDist) / target_dist;
547  else
548  factor1 = meanDist;
549 
550  // Factor #2: Distance to target in "sectors"
551  // -------------------------------------------
552  unsigned int dif =
553  mrpt::abs_diff(target_sector, gap->representative_sector);
554 
555  // Handle the -PI,PI circular topology:
556  if (dif > 0.5 * obstacles.size()) dif = obstacles.size() - dif;
557 
558  const double factor2 = exp(-square(dif / (obstacles.size() * 0.25)));
559 
560  // Factor #3: Punish paths that take us far away wrt the target: **** I
561  // don't understand it *********
562  // -----------------------------------------------------
563  double closestX, closestY;
564  double dist_eucl = math::minimumDistanceFromPointToSegment(
565  target_x, target_y, // Point
566  0, 0, x, y, // Segment
567  closestX, closestY // Out
568  );
569 
570  const float factor3 =
571  (maxObsRange - std::min(maxObsRange, dist_eucl)) / maxObsRange;
572 
573  // Factor #4: Stabilizing factor (hysteresis) to avoid quick switch
574  // among very similar paths:
575  // ------------------------------------------------------------------------------------------
576  double factor_AntiCab;
577 
578  if (m_last_selected_sector != std::numeric_limits<unsigned int>::max())
579  {
580  unsigned int dist = mrpt::abs_diff(
581  m_last_selected_sector, gap->representative_sector);
582 
583  if (dist > unsigned(0.1 * obstacles.size()))
584  factor_AntiCab = 0.0;
585  else
586  factor_AntiCab = 1.0;
587  }
588  else
589  {
590  factor_AntiCab = 0;
591  }
592 
593  ASSERT_(options.factorWeights.size() == 4);
594 
595  if (obstacles[gap->representative_sector] <
596  options.TOO_CLOSE_OBSTACLE) // Too close to obstacles
597  out_gaps_evaluation[i] = 0;
598  else
599  out_gaps_evaluation[i] =
600  (options.factorWeights[0] * factor1 +
601  options.factorWeights[1] * factor2 +
602  options.factorWeights[2] * factor3 +
603  options.factorWeights[3] * factor_AntiCab) /
604  (math::sum(options.factorWeights));
605  } // for each gap
606 }
607 
608 unsigned int CHolonomicND::direction2sector(
609  const double a, const unsigned int N)
610 {
611  const int idx = round(0.5 * (N * (1 + mrpt::math::wrapToPi(a) / M_PI) - 1));
612  if (idx < 0)
613  return 0;
614  else
615  return static_cast<unsigned int>(idx);
616 }
617 
618 uint8_t CLogFileRecord_ND::serializeGetVersion() const { return 1; }
619 void CLogFileRecord_ND::serializeTo(mrpt::serialization::CArchive& out) const
620 {
621  out << gaps_ini << gaps_end << gaps_eval;
622  out << selectedSector << evaluation << riskEvaluation
623  << (uint32_t)situation;
624 }
625 
626 void CLogFileRecord_ND::serializeFrom(
628 {
629  switch (version)
630  {
631  case 0:
632  {
633  int32_t n;
634 
635  in >> n;
636  gaps_ini.resize(n);
637  gaps_end.resize(n);
638  in.ReadBuffer(&(*gaps_ini.begin()), sizeof(gaps_ini[0]) * n);
639  in.ReadBuffer(&(*gaps_end.begin()), sizeof(gaps_end[0]) * n);
640 
641  in >> n;
642  gaps_eval.resize(n);
643  in.ReadBuffer(&(*gaps_eval.begin()), sizeof(gaps_eval[0]) * n);
644 
645  in >> selectedSector >> evaluation >> riskEvaluation >> n;
646 
647  situation = (CHolonomicND::TSituations)n;
648  }
649  break;
650  case 1:
651  {
652  uint32_t n;
653  in >> gaps_ini >> gaps_end >> gaps_eval;
654  in >> selectedSector >> evaluation >> riskEvaluation >> n;
655  situation = (CHolonomicND::TSituations)n;
656  }
657  break;
658  default:
660  };
661 }
662 
663 /*---------------------------------------------------------------
664  TOptions
665  ---------------------------------------------------------------*/
666 CHolonomicND::TOptions::TOptions()
667  : // Default values:
668  TOO_CLOSE_OBSTACLE(0.15),
669  WIDE_GAP_SIZE_PERCENT(0.25),
670  RISK_EVALUATION_SECTORS_PERCENT(0.10),
671  RISK_EVALUATION_DISTANCE(0.4),
672  MAX_SECTOR_DIST_FOR_D2_PERCENT(0.25),
673  TARGET_SLOW_APPROACHING_DISTANCE(0.60),
674  factorWeights{1.0, 0.5, 2.0, 0.4}
675 {
676 }
677 
679  const mrpt::config::CConfigFileBase& source, const std::string& section)
680 {
681  MRPT_START
682 
683  // Load from config text:
684  MRPT_LOAD_CONFIG_VAR(WIDE_GAP_SIZE_PERCENT, double, source, section);
686  MAX_SECTOR_DIST_FOR_D2_PERCENT, double, source, section);
688  RISK_EVALUATION_SECTORS_PERCENT, double, source, section);
689  MRPT_LOAD_CONFIG_VAR(RISK_EVALUATION_DISTANCE, double, source, section);
690  MRPT_LOAD_CONFIG_VAR(TOO_CLOSE_OBSTACLE, double, source, section);
692  TARGET_SLOW_APPROACHING_DISTANCE, double, source, section);
693 
694  source.read_vector(
695  section, "factorWeights", std::vector<double>(), factorWeights, true);
696  ASSERT_(factorWeights.size() == 4);
697 
698  MRPT_END
699 }
700 
703 {
704  MRPT_START;
705  const int WN = mrpt::config::MRPT_SAVE_NAME_PADDING(),
707 
708  MRPT_SAVE_CONFIG_VAR_COMMENT(WIDE_GAP_SIZE_PERCENT, "");
709  MRPT_SAVE_CONFIG_VAR_COMMENT(MAX_SECTOR_DIST_FOR_D2_PERCENT, "");
710  MRPT_SAVE_CONFIG_VAR_COMMENT(RISK_EVALUATION_SECTORS_PERCENT, "");
712  RISK_EVALUATION_DISTANCE, "In normalized ps-meters [0,1]");
713  MRPT_SAVE_CONFIG_VAR_COMMENT(TOO_CLOSE_OBSTACLE, "For stopping gradually");
715  TARGET_SLOW_APPROACHING_DISTANCE, "In normalized ps-meters");
716 
717  ASSERT_EQUAL_(factorWeights.size(), 4);
718  c.write(
719  s, "factorWeights",
720  mrpt::format(
721  "%.2f %.2f %.2f %.2f", factorWeights[0], factorWeights[1],
722  factorWeights[2], factorWeights[3]),
723  WN, WV,
724  "[0]=Free space, [1]=Dist. in sectors, [2]=Closer to target "
725  "(Euclidean), [3]=Hysteresis");
726 
727  MRPT_END
728 }
729 
732 {
733  // Params:
739  // State:
740  out << m_last_selected_sector;
741 }
744 {
745  switch (version)
746  {
747  case 0:
748  {
749  // Params:
750  in >> options.factorWeights >>
756  // State:
758  }
759  break;
760  default:
762  };
763 }
n
GLenum GLsizei n
Definition: glext.h:5074
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::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:211
mrpt::nav::CHolonomicND::TGap::ini
unsigned int ini
Definition: CHolonomicND.h:69
ops_containers.h
nav-precomp.h
mrpt::nav::CHolonomicND::TGap::end
unsigned int end
Definition: CHolonomicND.h:70
mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput
Input parameters for CAbstractHolonomicReactiveMethod::navigate()
Definition: CAbstractHolonomicReactiveMethod.h:38
mrpt::nav::CHolonomicND::TOptions::TARGET_SLOW_APPROACHING_DISTANCE
double TARGET_SLOW_APPROACHING_DISTANCE
Definition: CHolonomicND.h:99
mrpt::nav::CHolonomicND::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: CHolonomicND.cpp:701
mrpt::math::TPoint2D::y
double y
Definition: lightweight_geom_data.h:49
mrpt::nav::CHolonomicND::TOptions::factorWeights
std::vector< double > factorWeights
Vector of 4 weights: [0]=Free space, [1]=Dist.
Definition: CHolonomicND.h:102
mrpt::nav::CLogFileRecord_ND
A class for storing extra information about the execution of CHolonomicND navigation.
Definition: CHolonomicND.h:165
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
IMPLEMENTS_SERIALIZABLE
IMPLEMENTS_SERIALIZABLE(CHolonomicND, CAbstractHolonomicReactiveMethod, mrpt::nav) CHolonomicND
Initialize the parameters of the navigator, from some configuration file, or default values if filena...
Definition: CHolonomicND.cpp:24
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
c
const GLubyte * c
Definition: glext.h:6313
end
GLuint GLuint end
Definition: glext.h:3528
mrpt::nav::CHolonomicND::TOptions::RISK_EVALUATION_SECTORS_PERCENT
double RISK_EVALUATION_SECTORS_PERCENT
Definition: CHolonomicND.h:97
mrpt::nav::CAbstractHolonomicReactiveMethod::NavOutput::desiredDirection
double desiredDirection
The desired motion direction, in the range [-PI, PI].
Definition: CAbstractHolonomicReactiveMethod.h:68
mrpt::nav::CHolonomicND
An implementation of the holonomic reactive navigation method "Nearness-Diagram".
Definition: CHolonomicND.h:55
mrpt::nav::CHolonomicND::TOptions::MAX_SECTOR_DIST_FOR_D2_PERCENT
double MAX_SECTOR_DIST_FOR_D2_PERCENT
Definition: CHolonomicND.h:98
mrpt::nav
Definition: CAbstractHolonomicReactiveMethod.h:23
mrpt::math::TPoint2D::x
double x
X,Y coordinates.
Definition: lightweight_geom_data.h:49
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
uint8_t
unsigned char uint8_t
Definition: rptypes.h:41
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::CHolonomicND::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: CHolonomicND.cpp:678
mrpt::nav::CHolonomicND::serializeFrom
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CHolonomicND.cpp:742
mrpt::min3
const T min3(const T &A, const T &B, const T &C)
Definition: core/include/mrpt/core/bits_math.h:103
mrpt::nav::CLogFileRecord_ND::gaps_eval
std::vector< double > gaps_eval
Definition: CHolonomicND.h:173
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
source
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
mrpt::nav::CHolonomicND::TGap::maxDistance
double maxDistance
Definition: CHolonomicND.h:71
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::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
mrpt::nav::CHolonomicND::TOptions::RISK_EVALUATION_DISTANCE
double RISK_EVALUATION_DISTANCE
Definition: CHolonomicND.h:98
mrpt::nav::CAbstractHolonomicReactiveMethod
A base class for holonomic reactive navigation methods.
Definition: CAbstractHolonomicReactiveMethod.h:32
mrpt::nav::CHolonomicND::serializeGetVersion
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CHolonomicND.cpp:730
mrpt::math::wrapToPi
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:53
CHolonomicND.h
mrpt::nav::CHolonomicND::m_last_selected_sector
unsigned int m_last_selected_sector
Definition: CHolonomicND.h:127
mrpt::nav::CHolonomicND::TGap::minDistance
double minDistance
Definition: CHolonomicND.h:72
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23
MRPT_START
#define MRPT_START
Definition: exceptions.h:262
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::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::math::minimumDistanceFromPointToSegment
double minimumDistanceFromPointToSegment(const double Px, const double Py, const double x1, const double y1, const double x2, const double y2, T &out_x, T &out_y)
Computes the closest point from a given point to a segment, and returns that minimum distance.
Definition: geometry.h:989
round.h
mrpt::nav::CHolonomicND::options
TOptions options
Parameters of the algorithm (can be set manually or loaded from CHolonomicND::initialize or options....
Definition: CHolonomicND.h:115
mrpt::nav::CLogFileRecord_ND::Ptr
std::shared_ptr< CLogFileRecord_ND > Ptr
Definition: CHolonomicND.h:167
mrpt::utils::CConfigFileBase
mrpt::config::CConfigFileBase CConfigFileBase
Definition: utils/CConfigFileBase.h:7
mrpt::math::TPoint2D
Lightweight 2D point.
Definition: lightweight_geom_data.h:42
mrpt::nav::CHolonomicND::TGap
The structure used to store a detected gap in obstacles.
Definition: CHolonomicND.h:67
int32_t
__int32 int32_t
Definition: rptypes.h:46
mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput::maxRobotSpeed
double maxRobotSpeed
Maximum robot speed, in the same units than obstacles, per second.
Definition: CAbstractHolonomicReactiveMethod.h:52
mrpt::config::MRPT_SAVE_VALUE_PADDING
int MRPT_SAVE_VALUE_PADDING()
Definition: CConfigFileBase.cpp:27
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::CHolonomicND::serializeTo
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CHolonomicND.cpp:731
min
#define min(a, b)
Definition: rplidar_driver.cpp:42
mrpt::nav::CHolonomicND::TGapArray
std::vector< TGap > TGapArray
Definition: CHolonomicND.h:76
mrpt::nav::CAbstractHolonomicReactiveMethod::NavOutput
Output for CAbstractHolonomicReactiveMethod::navigate()
Definition: CAbstractHolonomicReactiveMethod.h:65
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
in
GLuint in
Definition: glext.h:7274
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::math::TPoint2D::norm
double norm() const
Point norm.
Definition: lightweight_geom_data.h:179
CArchive.h
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
mrpt::nav::CHolonomicND::TOptions::TOO_CLOSE_OBSTACLE
double TOO_CLOSE_OBSTACLE
Definition: CHolonomicND.h:96
mrpt::nav::CHolonomicND::TOptions::WIDE_GAP_SIZE_PERCENT
double WIDE_GAP_SIZE_PERCENT
Definition: CHolonomicND.h:96
mrpt::nav::CHolonomicND::TSituations
TSituations
The set of posible situations for each trajectory.
Definition: CHolonomicND.h:80
mrpt::nav::CHolonomicND::TGap::representative_sector
unsigned int representative_sector
Definition: CHolonomicND.h:73
y
GLenum GLint GLint y
Definition: glext.h:3538
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
uint32_t
unsigned __int32 uint32_t
Definition: rptypes.h:47
x
GLenum GLint x
Definition: glext.h:3538
a
GLubyte GLubyte GLubyte a
Definition: glext.h:6279



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