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



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019