Main MRPT website > C++ reference for MRPT 1.9.9
CAbstractPTGBasedReactive.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 
13 #include <mrpt/system/filesystem.h>
14 #include <mrpt/math/wrap2pi.h>
15 #include <mrpt/math/geometry.h>
16 #include <mrpt/math/ops_containers.h> // sum()
20 #include <mrpt/io/CMemoryStream.h>
23 #include <limits>
24 #include <iomanip>
25 #include <array>
26 
27 using namespace mrpt;
28 using namespace mrpt::io;
29 using namespace mrpt::poses;
30 using namespace mrpt::math;
31 using namespace mrpt::system;
32 using namespace mrpt::nav;
33 using namespace mrpt::serialization;
34 using namespace std;
35 
36 // ------ CAbstractPTGBasedReactive::TNavigationParamsPTG -----
37 std::string CAbstractPTGBasedReactive::TNavigationParamsPTG::getAsText() const
38 {
39  std::string s =
40  CWaypointsNavigator::TNavigationParamsWaypoints::getAsText();
41  s += "restrict_PTG_indices: ";
42  s += mrpt::containers::sprintf_vector("%u ", this->restrict_PTG_indices);
43  s += "\n";
44  return s;
45 }
46 
47 bool CAbstractPTGBasedReactive::TNavigationParamsPTG::isEqual(
49 {
50  auto o =
52  &rhs);
53  return o != nullptr &&
54  CWaypointsNavigator::TNavigationParamsWaypoints::isEqual(rhs) &&
55  restrict_PTG_indices == o->restrict_PTG_indices;
56 }
57 
58 const double ESTIM_LOWPASSFILTER_ALPHA = 0.7;
59 
60 // Ctor:
61 CAbstractPTGBasedReactive::CAbstractPTGBasedReactive(
62  CRobot2NavInterface& react_iterf_impl, bool enableConsoleOutput,
63  bool enableLogFile, const std::string& sLogDir)
64  : CWaypointsNavigator(react_iterf_impl),
65  m_holonomicMethod(),
66  m_prev_logfile(nullptr),
67  m_enableKeepLogRecords(false),
68  m_enableConsoleOutput(enableConsoleOutput),
69  m_init_done(false),
70  m_timelogger(false), // default: disabled
71  m_PTGsMustBeReInitialized(true),
72  meanExecutionTime(ESTIM_LOWPASSFILTER_ALPHA, 0.1),
73  meanTotalExecutionTime(ESTIM_LOWPASSFILTER_ALPHA, 0.1),
74  meanExecutionPeriod(ESTIM_LOWPASSFILTER_ALPHA, 0.1),
75  tim_changeSpeed_avr(ESTIM_LOWPASSFILTER_ALPHA),
76  timoff_obstacles_avr(ESTIM_LOWPASSFILTER_ALPHA),
77  timoff_curPoseAndSpeed_avr(ESTIM_LOWPASSFILTER_ALPHA),
78  timoff_sendVelCmd_avr(ESTIM_LOWPASSFILTER_ALPHA),
79  m_closing_navigator(false),
80  m_WS_Obstacles_timestamp(INVALID_TIMESTAMP),
81  m_infoPerPTG_timestamp(INVALID_TIMESTAMP),
82  m_navlogfiles_dir(sLogDir)
83 {
84  this->enableLogFile(enableLogFile);
85 }
86 
88 {
89  m_closing_navigator = true;
90 
91  // Wait to end of navigation (multi-thread...)
92  m_nav_cs.lock();
93  m_nav_cs.unlock();
94 
95  // Just in case.
96  try
97  {
98  this->stop(false /*not emergency*/);
99  }
100  catch (...)
101  {
102  }
103 
104  m_logFile.reset();
105 
106  // Free holonomic method:
107  this->deleteHolonomicObjects();
108 }
109 
111 {
112  this->preDestructor(); // ensure the robot is stopped; free dynamic objects
113 }
114 
115 /** \callergraph */
117 {
118  std::lock_guard<std::recursive_mutex> csl(m_nav_cs);
119 
121 
123  m_multiobjopt->clear();
124 
125  // Compute collision grids:
126  STEP1_InitPTGs();
127 }
128 
129 /*---------------------------------------------------------------
130  enableLogFile
131  ---------------------------------------------------------------*/
133 {
134  std::lock_guard<std::recursive_mutex> csl(m_nav_cs);
135 
136  try
137  {
138  // Disable:
139  // -------------------------------
140  if (!enable)
141  {
142  if (m_logFile)
143  {
145  "[CAbstractPTGBasedReactive::enableLogFile] Stopping "
146  "logging.");
147  m_logFile.reset(); // Close file:
148  }
149  else
150  return; // Already disabled.
151  }
152  else
153  { // Enable
154  // -------------------------------
155  if (m_logFile) return; // Already enabled:
156 
157  // Open file, find the first free file-name.
158  char aux[300];
159  unsigned int nFile = 0;
160  bool free_name = false;
161 
164  {
166  "Could not create directory for navigation logs: `%s`",
167  m_navlogfiles_dir.c_str());
168  }
169 
170  while (!free_name)
171  {
172  nFile++;
173  sprintf(
174  aux, "%s/log_%03u.reactivenavlog",
175  m_navlogfiles_dir.c_str(), nFile);
176  free_name = !system::fileExists(aux);
177  }
178 
179  // Open log file:
180  {
181  std::unique_ptr<CFileGZOutputStream> fil(
182  new CFileGZOutputStream);
183  bool ok = fil->open(aux, 1 /* compress level */);
184  if (!ok)
185  {
186  THROW_EXCEPTION_FMT("Error opening log file: `%s`", aux);
187  }
188  else
189  {
190  m_logFile.reset(fil.release());
191  }
192  }
193 
195  "[CAbstractPTGBasedReactive::enableLogFile] Logging to "
196  "file `%s`\n",
197  aux));
198  }
199  }
200  catch (std::exception& e)
201  {
203  "[CAbstractPTGBasedReactive::enableLogFile] Exception: %s",
204  e.what());
205  }
206 }
207 
209 {
210  std::lock_guard<std::recursive_mutex> lock(m_critZoneLastLog);
211  o = lastLogRecord;
212 }
213 
215 {
216  m_holonomicMethod.clear();
217 }
218 
220  const std::string& method, const mrpt::config::CConfigFileBase& ini)
221 {
222  std::lock_guard<std::recursive_mutex> csl(m_nav_cs);
223 
224  this->deleteHolonomicObjects();
225  const size_t nPTGs = this->getPTG_count();
226  ASSERT_(nPTGs != 0);
227  m_holonomicMethod.resize(nPTGs);
228 
229  for (size_t i = 0; i < nPTGs; i++)
230  {
231  m_holonomicMethod[i] =
233  if (!m_holonomicMethod[i])
235  "Non-registered holonomic method className=`%s`",
236  method.c_str());
237 
238  m_holonomicMethod[i]->setAssociatedPTG(this->getPTG(i));
239  m_holonomicMethod[i]->initialize(ini); // load params
240  }
241 }
242 
243 /** The main method: executes one time-iteration of the reactive navigation
244  * algorithm.
245  * \callergraph */
247 {
248  // Security tests:
249  if (m_closing_navigator) return; // Are we closing in the main thread?
250  if (!m_init_done)
251  THROW_EXCEPTION("Have you called loadConfigFile() before?");
253  const size_t nPTGs = this->getPTG_count();
254 
255  // Whether to worry about log files:
256  const bool fill_log_record = (m_logFile || m_enableKeepLogRecords);
257  CLogFileRecord newLogRec;
258  newLogRec.infoPerPTG.resize(nPTGs + 1); /* +1: [N] is the "NOP cmdvel"
259  option; not to be present in all
260  log entries. */
261 
262  // At the beginning of each log file, add an introductory block explaining
263  // which PTGs are we using:
264  {
265  if (m_logFile &&
266  m_logFile.get() != m_prev_logfile) // Only the first time
267  {
268  m_prev_logfile = m_logFile.get();
269  for (size_t i = 0; i < nPTGs; i++)
270  {
271  // If we make a direct copy (=) we will store the entire, heavy,
272  // collision grid.
273  // Let's just store the parameters of each PTG by serializing
274  // it, so paths can be reconstructed
275  // by invoking initialize()
277  auto arch = archiveFrom(buf);
278  arch << *this->getPTG(i);
279  buf.Seek(0);
280  newLogRec.infoPerPTG[i].ptg = std::dynamic_pointer_cast<
282  arch.ReadObject());
283  }
284  }
285  }
286 
287  CTimeLoggerEntry tle1(m_timelogger, "navigationStep");
288 
289  try
290  {
291  totalExecutionTime.Tic(); // Start timer
292 
293  const mrpt::system::TTimeStamp tim_start_iteration =
295 
296  // Compute target location relative to current robot pose:
297  // ---------------------------------------------------------------------
298  // Detect changes in target since last iteration (for NOP):
299  const bool target_changed_since_last_iteration =
302  if (target_changed_since_last_iteration)
304 
305  // Load the list of target(s) from the navigationParam user command.
306  // Semantic is: any of the target is good. If several targets are
307  // reachable, head to latest one.
308  std::vector<CAbstractNavigator::TargetInfo> targets;
309  {
310  auto p = dynamic_cast<
312  m_navigationParams.get());
313  if (p && !p->multiple_targets.empty())
314  {
315  targets = p->multiple_targets;
316  }
317  else
318  {
319  targets.push_back(m_navigationParams->target);
320  }
321  }
322  const size_t nTargets = targets.size(); // Normally = 1, will be >1 if
323  // we want the robot local
324  // planner to be "smarter" in
325  // skipping dynamic obstacles.
326  ASSERT_(nTargets >= 1);
327 
328  STEP1_InitPTGs(); // Will only recompute if
329  // "m_PTGsMustBeReInitialized==true"
330 
331  // STEP2: Load the obstacles and sort them in height bands.
332  // -----------------------------------------------------------------------------
333  if (!STEP2_SenseObstacles())
334  {
336  "Error while loading and sorting the obstacles. Robot will be "
337  "stopped.");
338  if (fill_log_record)
339  {
340  CPose2D rel_cur_pose_wrt_last_vel_cmd_NOP,
341  rel_pose_PTG_origin_wrt_sense_NOP;
343  newLogRec,
344  std::vector<mrpt::math::TPose2D>() /*no targets*/,
345  -1, // best_ptg_idx,
346  m_robot.getEmergencyStopCmd(), nPTGs,
347  false, // best_is_NOP_cmdvel,
348  rel_cur_pose_wrt_last_vel_cmd_NOP.asTPose(),
349  rel_pose_PTG_origin_wrt_sense_NOP.asTPose(),
350  0, // executionTimeValue,
351  0, // tim_changeSpeed,
352  tim_start_iteration);
353  }
354  return;
355  }
356 
357  // ------- start of motion decision zone ---------
358  executionTime.Tic();
359 
360  // Round #1: As usual, pure reactive, evaluate all PTGs and all
361  // directions from scratch.
362  // =========
363 
364  mrpt::math::TPose2D rel_pose_PTG_origin_wrt_sense(0, 0, 0),
365  relPoseSense(0, 0, 0), relPoseVelCmd(0, 0, 0);
367  {
368  /*
369  * Delays model
370  *
371  * Event: OBSTACLES_SENSED RNAV_ITERATION_STARTS
372  * GET_ROBOT_POSE_VEL VEL_CMD_SENT_TO_ROBOT
373  * Timestamp: (m_WS_Obstacles_timestamp) (tim_start_iteration)
374  * (m_curPoseVelTimestamp) ("tim_send_cmd_vel")
375  * Delay |
376  * <---+--------------->|<--------------+-------->| |
377  * estimator: | |
378  * | |
379  * timoff_obstacles <-+ |
380  * +--> timoff_curPoseVelAge |
381  * |<---------------------------------+--------------->|
382  * +-->
383  * timoff_sendVelCmd_avr (estimation)
384  *
385  * |<-------------------->|
386  * tim_changeSpeed_avr
387  * (estim)
388  *
389  * |<-----------------------------------------------|-------------------------->|
390  * Relative poses: relPoseSense
391  * relPoseVelCmd
392  * Time offsets (signed): timoff_pose2sense
393  * timoff_pose2VelCmd
394  */
395  const double timoff_obstacles = mrpt::system::timeDifference(
396  tim_start_iteration, m_WS_Obstacles_timestamp);
397  timoff_obstacles_avr.filter(timoff_obstacles);
398  newLogRec.values["timoff_obstacles"] = timoff_obstacles;
399  newLogRec.values["timoff_obstacles_avr"] =
401  newLogRec.timestamps["obstacles"] = m_WS_Obstacles_timestamp;
402 
403  const double timoff_curPoseVelAge = mrpt::system::timeDifference(
404  tim_start_iteration, m_curPoseVel.timestamp);
405  timoff_curPoseAndSpeed_avr.filter(timoff_curPoseVelAge);
406  newLogRec.values["timoff_curPoseVelAge"] = timoff_curPoseVelAge;
407  newLogRec.values["timoff_curPoseVelAge_avr"] =
409 
410  // time offset estimations:
411  const double timoff_pose2sense =
412  timoff_obstacles - timoff_curPoseVelAge;
413 
414  double timoff_pose2VelCmd;
415  timoff_pose2VelCmd = timoff_sendVelCmd_avr.getLastOutput() +
417  timoff_curPoseVelAge;
418  newLogRec.values["timoff_pose2sense"] = timoff_pose2sense;
419  newLogRec.values["timoff_pose2VelCmd"] = timoff_pose2VelCmd;
420 
421  if (std::abs(timoff_pose2sense) > 1.25)
423  "timoff_pose2sense=%e is too large! Path extrapolation may "
424  "be not accurate.",
425  timoff_pose2sense);
426  if (std::abs(timoff_pose2VelCmd) > 1.25)
428  "timoff_pose2VelCmd=%e is too large! Path extrapolation "
429  "may be not accurate.",
430  timoff_pose2VelCmd);
431 
432  // Path extrapolation: robot relative poses along current path
433  // estimation:
434  relPoseSense = m_curPoseVel.velLocal * timoff_pose2sense;
435  relPoseVelCmd = m_curPoseVel.velLocal * timoff_pose2VelCmd;
436 
437  // relative pose for PTGs:
438  rel_pose_PTG_origin_wrt_sense = relPoseVelCmd - relPoseSense;
439 
440  // logging:
441  newLogRec.relPoseSense = relPoseSense;
442  newLogRec.relPoseVelCmd = relPoseVelCmd;
443  }
444  else
445  {
446  // No delays model: poses to their default values.
447  }
448 
449  for (auto& t : targets)
450  {
451  if (t.target_frame_id != m_curPoseVel.pose_frame_id)
452  {
453  auto frame_tf = m_frame_tf.lock();
454  if (!frame_tf)
455  {
457  "Different frame_id's but no frame_tf object "
458  "attached (or it expired)!: target_frame_id=`%s` != "
459  "pose_frame_id=`%s`",
460  t.target_frame_id.c_str(),
461  m_curPoseVel.pose_frame_id.c_str());
462  }
463  mrpt::math::TPose2D robot_frame2map_frame;
464  frame_tf->lookupTransform(
465  t.target_frame_id, m_curPoseVel.pose_frame_id,
466  robot_frame2map_frame);
467 
469  "frame_tf: target_frame_id=`%s` as seen from "
470  "pose_frame_id=`%s` = %s",
471  t.target_frame_id.c_str(),
472  m_curPoseVel.pose_frame_id.c_str(),
473  robot_frame2map_frame.asString().c_str());
474 
475  t.target_coords = robot_frame2map_frame + t.target_coords;
476  t.target_frame_id =
477  m_curPoseVel.pose_frame_id; // Now the coordinates are in
478  // the same frame than robot
479  // pose
480  }
481  }
482 
483  std::vector<TPose2D> relTargets;
484  const auto curPoseExtrapol = (m_curPoseVel.pose + relPoseVelCmd);
486  targets.begin(), targets.end(), // in
487  std::back_inserter(relTargets), // out
488  [curPoseExtrapol](const CAbstractNavigator::TargetInfo& e) {
489  return e.target_coords - curPoseExtrapol;
490  });
491  ASSERT_EQUAL_(relTargets.size(), targets.size());
492 
493  // Use the distance to the first target as reference:
494  const double relTargetDist = relTargets.begin()->norm();
495 
496  // PTG dynamic state
497  /** Allow PTGs to be responsive to target location, dynamics, etc. */
499 
500  ptg_dynState.curVelLocal = m_curPoseVel.velLocal;
501  ptg_dynState.relTarget = relTargets[0];
502  ptg_dynState.targetRelSpeed =
503  m_navigationParams->target.targetDesiredRelSpeed;
504 
505  newLogRec.navDynState = ptg_dynState;
506 
507  for (size_t i = 0; i < nPTGs; i++)
508  {
509  getPTG(i)->updateNavDynamicState(ptg_dynState);
510  }
511 
512  m_infoPerPTG.assign(nPTGs + 1, TInfoPerPTG()); // reset contents
513  m_infoPerPTG_timestamp = tim_start_iteration;
514  vector<TCandidateMovementPTG> candidate_movs(
515  nPTGs + 1); // the last extra one is for the evaluation of "NOP
516  // motion command" choice.
517 
518  for (size_t indexPTG = 0; indexPTG < nPTGs; indexPTG++)
519  {
520  CParameterizedTrajectoryGenerator* ptg = getPTG(indexPTG);
521  TInfoPerPTG& ipf = m_infoPerPTG[indexPTG];
522 
523  // Ensure the method knows about its associated PTG:
524  m_holonomicMethod[indexPTG]->setAssociatedPTG(
525  this->getPTG(indexPTG));
526 
527  // The picked movement in TP-Space (to be determined by holonomic
528  // method below)
529  TCandidateMovementPTG& cm = candidate_movs[indexPTG];
530 
533  ptg, indexPTG, relTargets, rel_pose_PTG_origin_wrt_sense, ipf,
534  cm, newLogRec, false /* this is a regular PTG reactive case */,
535  *m_holonomicMethod[indexPTG], tim_start_iteration,
537  } // end for each PTG
538 
539  // check for collision, which is reflected by ALL TP-Obstacles being
540  // zero:
541  bool is_all_ptg_collision = true;
542  for (size_t indexPTG = 0; indexPTG < nPTGs; indexPTG++)
543  {
544  bool is_collision = true;
545  const auto& obs = m_infoPerPTG[indexPTG].TP_Obstacles;
546  for (const auto o : obs)
547  {
548  if (o != 0)
549  {
550  is_collision = false;
551  break;
552  }
553  }
554  if (!is_collision)
555  {
556  is_all_ptg_collision = false;
557  break;
558  }
559  }
560  if (is_all_ptg_collision)
561  {
562  m_pending_events.push_back(std::bind(
564  std::ref(m_robot)));
565  }
566 
567  // Round #2: Evaluate dont sending any new velocity command ("NOP"
568  // motion)
569  // =========
570  bool NOP_not_too_old = true;
571  bool NOP_not_too_close_and_have_to_slowdown = true;
572  double NOP_max_time = -1.0, NOP_At = -1.0;
573  double slowdowndist = .0;
574  CParameterizedTrajectoryGenerator* last_sent_ptg =
576  : nullptr;
577  if (last_sent_ptg)
578  {
579  // So supportSpeedAtTarget() below is evaluated in the correct
580  // context:
582  }
583 
584  // This approach is only possible if:
585  const bool can_do_nop_motion =
587  !target_changed_since_last_iteration && last_sent_ptg &&
588  last_sent_ptg->supportVelCmdNOP()) &&
589  (NOP_not_too_old =
591  m_lastSentVelCmd.tim_send_cmd_vel, tim_start_iteration)) <
592  (NOP_max_time =
593  last_sent_ptg->maxTimeInVelCmdNOP(
595  std::max(0.1, m_lastSentVelCmd.speed_scale))) &&
596  (NOP_not_too_close_and_have_to_slowdown =
597  (last_sent_ptg->supportSpeedAtTarget() ||
598  (relTargetDist >
600  ->getTargetApproachSlowDownDistance())
601  // slowdowndist is assigned here, inside the if()
602  // to be sure the index in m_lastSentVelCmd is valid!
603  )));
604 
605  if (!NOP_not_too_old)
606  {
607  newLogRec.additional_debug_msgs["PTG_cont"] = mrpt::format(
608  "PTG-continuation not allowed: previous command timed-out "
609  "(At=%.03f > Max_At=%.03f)",
610  NOP_At, NOP_max_time);
611  }
612  if (!NOP_not_too_close_and_have_to_slowdown)
613  {
614  newLogRec.additional_debug_msgs["PTG_cont_trgdst"] = mrpt::format(
615  "PTG-continuation not allowed: target too close and must start "
616  "slow-down (trgDist=%.03f < SlowDownDist=%.03f)",
617  relTargetDist, slowdowndist);
618  }
619 
620  mrpt::math::TPose2D rel_cur_pose_wrt_last_vel_cmd_NOP(0, 0, 0),
621  rel_pose_PTG_origin_wrt_sense_NOP(0, 0, 0);
622 
623  if (can_do_nop_motion)
624  {
625  // Add the estimation of how long it takes to run the changeSpeeds()
626  // callback (usually a tiny period):
627  const mrpt::system::TTimeStamp tim_send_cmd_vel_corrected =
631 
632  // Note: we use (uncorrected) raw odometry as basis to the following
633  // calculation since it's normally
634  // smoother than particle filter-based localization data, more
635  // accurate in the middle/long term,
636  // but not in the short term:
637  mrpt::math::TPose2D robot_pose_at_send_cmd, robot_odom_at_send_cmd;
638  bool valid_odom, valid_pose;
639 
641  tim_send_cmd_vel_corrected, robot_odom_at_send_cmd, valid_odom);
643  tim_send_cmd_vel_corrected, robot_pose_at_send_cmd, valid_pose);
644 
645  if (valid_odom && valid_pose)
646  {
647  ASSERT_(last_sent_ptg != nullptr);
648 
649  std::vector<TPose2D> relTargets_NOPs;
651  targets.begin(), targets.end(), // in
652  std::back_inserter(relTargets_NOPs), // out
653  [robot_pose_at_send_cmd](
655  return e.target_coords - robot_pose_at_send_cmd;
656  });
657  ASSERT_EQUAL_(relTargets_NOPs.size(), targets.size());
658 
659  rel_pose_PTG_origin_wrt_sense_NOP =
660  robot_odom_at_send_cmd -
661  (m_curPoseVel.rawOdometry + relPoseSense);
662  rel_cur_pose_wrt_last_vel_cmd_NOP =
663  m_curPoseVel.rawOdometry - robot_odom_at_send_cmd;
664 
665  // Update PTG response to dynamic params:
666  last_sent_ptg->updateNavDynamicState(
668 
669  if (fill_log_record)
670  {
671  newLogRec.additional_debug_msgs
672  ["rel_cur_pose_wrt_last_vel_cmd_NOP(interp)"] =
673  rel_cur_pose_wrt_last_vel_cmd_NOP.asString();
674  newLogRec.additional_debug_msgs
675  ["robot_odom_at_send_cmd(interp)"] =
676  robot_odom_at_send_cmd.asString();
677  }
678 
679  // No need to call setAssociatedPTG(), already correctly
680  // associated above.
681 
684  last_sent_ptg, m_lastSentVelCmd.ptg_index, relTargets_NOPs,
685  rel_pose_PTG_origin_wrt_sense_NOP, m_infoPerPTG[nPTGs],
686  candidate_movs[nPTGs], newLogRec,
687  true /* this is the PTG continuation (NOP) choice */,
689  tim_start_iteration, *m_navigationParams,
690  rel_cur_pose_wrt_last_vel_cmd_NOP);
691 
692  } // end valid interpolated origin pose
693  else
694  {
695  // Can't interpolate pose, hence can't evaluate NOP:
696  candidate_movs[nPTGs].speed =
697  -0.01; // <0 means inviable movement
698  }
699  } // end can_do_NOP_motion
700 
701  // Evaluate all the candidates and pick the "best" one, using
702  // the user-defined multiobjective optimizer
703  // --------------------------------------------------------------
706  int best_ptg_idx = m_multiobjopt->decide(candidate_movs, mo_info);
707 
708  if (fill_log_record)
709  {
710  if (mo_info.final_evaluation.size() == newLogRec.infoPerPTG.size())
711  {
712  for (unsigned int i = 0; i < newLogRec.infoPerPTG.size(); i++)
713  {
714  newLogRec.infoPerPTG[i].evaluation =
715  mo_info.final_evaluation[i];
716  }
717  }
718  int idx = 0;
719  for (const auto& le : mo_info.log_entries)
720  {
722  "MultiObjMotOptmzr_msg%03i", idx++)] = le;
723  }
724  }
725 
726  // Pick best movement (or null if none is good)
727  const TCandidateMovementPTG* selectedHolonomicMovement = nullptr;
728  if (best_ptg_idx >= 0)
729  {
730  selectedHolonomicMovement = &candidate_movs[best_ptg_idx];
731  }
732 
733  // If the selected PTG is (N+1), it means the NOP cmd. vel is selected
734  // as the best alternative, i.e. do NOT send any new motion command.
735  const bool best_is_NOP_cmdvel = (best_ptg_idx == int(nPTGs));
736 
737  // ---------------------------------------------------------------------
738  // SEND MOVEMENT COMMAND TO THE ROBOT
739  // ---------------------------------------------------------------------
741  if (best_is_NOP_cmdvel)
742  {
743  // Notify the robot that we want it to keep executing the last
744  // cmdvel:
745  if (!this->changeSpeedsNOP())
746  {
748  "\nERROR calling changeSpeedsNOP()!! Stopping robot and "
749  "finishing navigation\n");
750  if (fill_log_record)
751  {
753  newLogRec, relTargets, best_ptg_idx,
754  m_robot.getEmergencyStopCmd(), nPTGs,
755  best_is_NOP_cmdvel, rel_cur_pose_wrt_last_vel_cmd_NOP,
756  rel_pose_PTG_origin_wrt_sense_NOP,
757  0, // executionTimeValue,
758  0, // tim_changeSpeed,
759  tim_start_iteration);
760  }
761  return;
762  }
763  }
764  else
765  {
766  // Make sure the dynamic state of a NOP cmd has not overwritten the
767  // state for a "regular" PTG choice:
768  for (size_t i = 0; i < nPTGs; i++)
769  {
770  getPTG(i)->updateNavDynamicState(ptg_dynState);
771  }
772 
773  // STEP7: Get the non-holonomic movement command.
774  // ---------------------------------------------------------------------
775  double cmd_vel_speed_ratio = 1.0;
776  if (selectedHolonomicMovement)
777  {
778  CTimeLoggerEntry tle(
779  m_timelogger, "navigationStep.selectedHolonomicMovement");
780  cmd_vel_speed_ratio =
781  generate_vel_cmd(*selectedHolonomicMovement, new_vel_cmd);
782  ASSERT_(new_vel_cmd);
783  }
784 
785  if (!new_vel_cmd /* which means best_PTG_eval==.0*/ ||
786  new_vel_cmd->isStopCmd())
787  {
789  "Best velocity command is STOP (no way found), calling "
790  "robot.stop()");
791  this->stop(true /* emergency */); // don't call
792  // doEmergencyStop() here
793  // since that will stop
794  // navigation completely
795  new_vel_cmd = m_robot.getEmergencyStopCmd();
797  }
798  else
799  {
800  mrpt::system::TTimeStamp tim_send_cmd_vel;
801  {
803  m_timlog_delays, "changeSpeeds()");
804  tim_send_cmd_vel = mrpt::system::now();
805  newLogRec.timestamps["tim_send_cmd_vel"] = tim_send_cmd_vel;
806  if (!this->changeSpeeds(*new_vel_cmd))
807  {
809  "\nERROR calling changeSpeeds()!! Stopping robot "
810  "and finishing navigation\n");
811  if (fill_log_record)
812  {
813  new_vel_cmd = m_robot.getEmergencyStopCmd();
815  newLogRec, relTargets, best_ptg_idx,
816  new_vel_cmd, nPTGs, best_is_NOP_cmdvel,
817  rel_cur_pose_wrt_last_vel_cmd_NOP,
818  rel_pose_PTG_origin_wrt_sense_NOP,
819  0, // executionTimeValue,
820  0, // tim_changeSpeed,
821  tim_start_iteration);
822  }
823  return;
824  }
825  }
826  // Save last sent cmd:
827  m_lastSentVelCmd.speed_scale = cmd_vel_speed_ratio;
828  m_lastSentVelCmd.ptg_index = best_ptg_idx;
830  selectedHolonomicMovement
831  ? selectedHolonomicMovement->PTG->alpha2index(
832  selectedHolonomicMovement->direction)
833  : 0;
835  selectedHolonomicMovement->props["holo_stage_eval"];
836 
838  best_ptg_idx >= 0
839  ? m_infoPerPTG[best_ptg_idx]
840  .TP_Obstacles[m_lastSentVelCmd.ptg_alpha_index]
841  : .0;
843  (selectedHolonomicMovement->props["is_slowdown"] != 0.0);
844 
846  m_lastSentVelCmd.tim_send_cmd_vel = tim_send_cmd_vel;
847  m_lastSentVelCmd.ptg_dynState = ptg_dynState;
848 
849  // Update delay model:
850  const double timoff_sendVelCmd = mrpt::system::timeDifference(
851  tim_start_iteration, tim_send_cmd_vel);
852  timoff_sendVelCmd_avr.filter(timoff_sendVelCmd);
853  newLogRec.values["timoff_sendVelCmd"] = timoff_sendVelCmd;
854  newLogRec.values["timoff_sendVelCmd_avr"] =
856  }
857  }
858 
859  // ------- end of motion decision zone ---------
860 
861  // Statistics:
862  // ----------------------------------------------------
863  const double executionTimeValue = executionTime.Tac();
864  meanExecutionTime.filter(executionTimeValue);
866 
867  const double tim_changeSpeed =
868  m_timlog_delays.getLastTime("changeSpeeds()");
869  tim_changeSpeed_avr.filter(tim_changeSpeed);
870 
871  // Running period estim:
872  const double period_tim = timerForExecutionPeriod.Tac();
873  if (period_tim > 1.5 * meanExecutionPeriod.getLastOutput())
874  {
876  "Timing warning: Suspicious executionPeriod=%.03f ms is far "
877  "above the average of %.03f ms",
878  1e3 * period_tim, meanExecutionPeriod.getLastOutput() * 1e3);
879  }
880  meanExecutionPeriod.filter(period_tim);
882 
884  {
886  "CMD: %s "
887  "speedScale=%.04f "
888  "T=%.01lfms Exec:%.01lfms|%.01lfms "
889  "PTG#%i\n",
890  new_vel_cmd ? new_vel_cmd->asString().c_str() : "NOP",
891  selectedHolonomicMovement ? selectedHolonomicMovement->speed
892  : .0,
894  1000.0 * meanExecutionTime.getLastOutput(),
895  1000.0 * meanTotalExecutionTime.getLastOutput(), best_ptg_idx));
896  }
897  if (fill_log_record)
898  {
900  newLogRec, relTargets, best_ptg_idx, new_vel_cmd, nPTGs,
901  best_is_NOP_cmdvel, rel_cur_pose_wrt_last_vel_cmd_NOP,
902  rel_pose_PTG_origin_wrt_sense_NOP, executionTimeValue,
903  tim_changeSpeed, tim_start_iteration);
904  }
905  }
906  catch (std::exception& e)
907  {
909  std::string("[CAbstractPTGBasedReactive::performNavigationStep] "
910  "Stopping robot and finishing navigation due to "
911  "exception:\n") +
912  std::string(e.what()));
913  }
914  catch (...)
915  {
917  "[CAbstractPTGBasedReactive::performNavigationStep] Stopping robot "
918  "and finishing navigation due to untyped exception.");
919  }
920 }
921 
922 /** \callergraph */
924  CLogFileRecord& newLogRec, const std::vector<TPose2D>& relTargets,
925  int nSelectedPTG, const mrpt::kinematics::CVehicleVelCmd::Ptr& new_vel_cmd,
926  const int nPTGs, const bool best_is_NOP_cmdvel,
927  const mrpt::math::TPose2D& rel_cur_pose_wrt_last_vel_cmd_NOP,
928  const mrpt::math::TPose2D& rel_pose_PTG_origin_wrt_sense_NOP,
929  const double executionTimeValue, const double tim_changeSpeed,
930  const mrpt::system::TTimeStamp& tim_start_iteration)
931 {
932  // ---------------------------------------
933  // STEP8: Generate log record
934  // ---------------------------------------
935  m_timelogger.enter("navigationStep.populate_log_info");
936 
937  this->loggingGetWSObstaclesAndShape(newLogRec);
938 
941  newLogRec.WS_targets_relative = relTargets;
942  newLogRec.nSelectedPTG = nSelectedPTG;
943  newLogRec.cur_vel = m_curPoseVel.velGlobal;
944  newLogRec.cur_vel_local = m_curPoseVel.velLocal;
945  newLogRec.cmd_vel = new_vel_cmd;
946  newLogRec.values["estimatedExecutionPeriod"] =
948  newLogRec.values["executionTime"] = executionTimeValue;
949  newLogRec.values["executionTime_avr"] = meanExecutionTime.getLastOutput();
950  newLogRec.values["time_changeSpeeds()"] = tim_changeSpeed;
951  newLogRec.values["time_changeSpeeds()_avr"] =
953  newLogRec.values["CWaypointsNavigator::navigationStep()"] =
954  m_timlog_delays.getLastTime("CWaypointsNavigator::navigationStep()");
955  newLogRec.values["CAbstractNavigator::navigationStep()"] =
956  m_timlog_delays.getLastTime("CAbstractNavigator::navigationStep()");
957  newLogRec.timestamps["tim_start_iteration"] = tim_start_iteration;
958  newLogRec.timestamps["curPoseAndVel"] = m_curPoseVel.timestamp;
959  newLogRec.nPTGs = nPTGs;
960 
961  // NOP mode stuff:
963  rel_cur_pose_wrt_last_vel_cmd_NOP;
965  rel_pose_PTG_origin_wrt_sense_NOP;
966  newLogRec.ptg_index_NOP =
967  best_is_NOP_cmdvel ? m_lastSentVelCmd.ptg_index : -1;
970 
971  m_timelogger.leave("navigationStep.populate_log_info");
972 
973  // Save to log file:
974  // --------------------------------------
975  {
977  m_timelogger, "navigationStep.write_log_file");
978  if (m_logFile) archiveFrom(*m_logFile) << newLogRec;
979  }
980  // Set as last log record
981  {
982  std::lock_guard<std::recursive_mutex> lock_log(
983  m_critZoneLastLog); // Lock
984  lastLogRecord = newLogRec; // COPY
985  }
986 }
987 
988 /** \callergraph */
990  TCandidateMovementPTG& cm, const std::vector<double>& in_TPObstacles,
991  const mrpt::nav::ClearanceDiagram& in_clearance,
992  const std::vector<mrpt::math::TPose2D>& WS_Targets,
993  const std::vector<CAbstractPTGBasedReactive::PTGTarget>& TP_Targets,
995  const bool this_is_PTG_continuation,
996  const mrpt::math::TPose2D& rel_cur_pose_wrt_last_vel_cmd_NOP,
997  const unsigned int ptg_idx4weights,
998  const mrpt::system::TTimeStamp tim_start_iteration,
1000 {
1001  MRPT_START;
1002 
1003  const double ref_dist = cm.PTG->getRefDistance();
1004 
1005  // Replaced by: TP_Targets[i].*
1006  // const double target_dir = (TP_Target.x!=0 || TP_Target.y!=0) ?
1007  // atan2( TP_Target.y, TP_Target.x) : 0.0;
1008  // const int target_k = static_cast<int>( cm.PTG->alpha2index(
1009  // target_dir ) );
1010  // const double target_d_norm = TP_Target.norm();
1011 
1012  // We need to evaluate the movement wrt to ONE target of the possibly many
1013  // input ones.
1014  // Policy: use the target whose TP-Space direction is closer to this
1015  // candidate direction:
1016  size_t selected_trg_idx = 0;
1017  {
1018  double best_trg_angdist = std::numeric_limits<double>::max();
1019  for (size_t i = 0; i < TP_Targets.size(); i++)
1020  {
1021  const double angdist = std::abs(mrpt::math::angDistance(
1022  TP_Targets[i].target_alpha, cm.direction));
1023  if (angdist < best_trg_angdist)
1024  {
1025  best_trg_angdist = angdist;
1026  selected_trg_idx = i;
1027  }
1028  }
1029  }
1030  ASSERT_(selected_trg_idx < WS_Targets.size());
1031  const auto WS_Target = WS_Targets[selected_trg_idx];
1032  const auto TP_Target = TP_Targets[selected_trg_idx];
1033 
1034  const double target_d_norm = TP_Target.target_dist;
1035 
1036  // Picked movement direction:
1037  const int move_k = static_cast<int>(cm.PTG->alpha2index(cm.direction));
1038  const double target_WS_d = WS_Target.norm();
1039 
1040  // Coordinates of the trajectory end for the given PTG and "alpha":
1041  const double d = std::min(in_TPObstacles[move_k], 0.99 * target_d_norm);
1042  uint32_t nStep;
1043  bool pt_in_range = cm.PTG->getPathStepForDist(move_k, d, nStep);
1044  ASSERT_(pt_in_range);
1045  mrpt::math::TPose2D pose;
1046  cm.PTG->getPathPose(move_k, nStep, pose);
1047 
1048  // Make sure that the target slow-down is honored, as seen in real-world
1049  // Euclidean space
1050  // (as opposed to TP-Space, where the holo methods are evaluated)
1051  if (m_navigationParams &&
1052  m_navigationParams->target.targetDesiredRelSpeed < 1.0 &&
1053  !m_holonomicMethod.empty() && m_holonomicMethod[0] != nullptr &&
1054  !cm.PTG->supportSpeedAtTarget() // If the PTG is able to handle the
1055  // slow-down on its own, dont change
1056  // speed here
1057  )
1058  {
1059  const double TARGET_SLOW_APPROACHING_DISTANCE =
1060  m_holonomicMethod[0]->getTargetApproachSlowDownDistance();
1061 
1062  const double Vf =
1063  m_navigationParams->target.targetDesiredRelSpeed; // [0,1]
1064 
1065  const double f = std::min(
1066  1.0,
1067  Vf + target_WS_d * (1.0 - Vf) / TARGET_SLOW_APPROACHING_DISTANCE);
1068  if (f < cm.speed)
1069  {
1070  newLogRec.additional_debug_msgs["PTG_eval.speed"] = mrpt::format(
1071  "Relative speed reduced %.03f->%.03f based on Euclidean "
1072  "nearness to target.",
1073  cm.speed, f);
1074  cm.speed = f;
1075  }
1076  }
1077 
1078  // Start storing params in the candidate move structure:
1079  cm.props["ptg_idx"] = ptg_idx4weights;
1080  cm.props["ref_dist"] = ref_dist;
1081  cm.props["target_dir"] = TP_Target.target_alpha;
1082  cm.props["target_k"] = TP_Target.target_k;
1083  cm.props["target_d_norm"] = target_d_norm;
1084  cm.props["move_k"] = move_k;
1085  double& move_cur_d = cm.props["move_cur_d"] =
1086  0; // current robot path normalized distance over path (0 unless in a
1087  // NOP cmd)
1088  cm.props["is_PTG_cont"] = this_is_PTG_continuation ? 1 : 0;
1089  cm.props["num_paths"] = in_TPObstacles.size();
1090  cm.props["WS_target_x"] = WS_Target.x;
1091  cm.props["WS_target_y"] = WS_Target.y;
1092  cm.props["robpose_x"] = pose.x;
1093  cm.props["robpose_y"] = pose.y;
1094  cm.props["robpose_phi"] = pose.phi;
1095  cm.props["ptg_priority"] =
1096  cm.PTG->getScorePriority() *
1097  cm.PTG->evalPathRelativePriority(TP_Target.target_k, target_d_norm);
1098  const bool is_slowdown =
1099  this_is_PTG_continuation
1101  : (cm.PTG->supportSpeedAtTarget() && TP_Target.target_k == move_k);
1102  cm.props["is_slowdown"] = is_slowdown ? 1 : 0;
1103  cm.props["holo_stage_eval"] =
1104  this_is_PTG_continuation
1106  : (hlfr && !hlfr->dirs_eval.empty() &&
1107  hlfr->dirs_eval.rbegin()->size() == in_TPObstacles.size())
1108  ? hlfr->dirs_eval.rbegin()->at(move_k)
1109  : .0;
1110  // Factor 1: Free distance for the chosen PTG and "alpha" in the TP-Space:
1111  // ----------------------------------------------------------------------
1112  double& colfree = cm.props["collision_free_distance"];
1113 
1114  // distance to collision:
1115  colfree = in_TPObstacles[move_k]; // we'll next substract here the
1116  // already-traveled distance, for NOP
1117  // motion candidates.
1118 
1119  // Special case for NOP motion cmd:
1120  // consider only the empty space *after* the current robot pose, which is
1121  // not at the origin.
1122 
1123  if (this_is_PTG_continuation)
1124  {
1125  int cur_k = 0;
1126  double cur_norm_d = .0;
1127  bool is_exact, is_time_based = false;
1128  uint32_t cur_ptg_step = 0;
1129 
1130  // Use: time-based prediction for shorter distances, PTG inverse
1131  // mapping-based for longer ranges:
1132  const double maxD = params_abstract_ptg_navigator
1134  if (std::abs(rel_cur_pose_wrt_last_vel_cmd_NOP.x) > maxD ||
1135  std::abs(rel_cur_pose_wrt_last_vel_cmd_NOP.y) > maxD)
1136  {
1137  is_exact = cm.PTG->inverseMap_WS2TP(
1138  rel_cur_pose_wrt_last_vel_cmd_NOP.x,
1139  rel_cur_pose_wrt_last_vel_cmd_NOP.y, cur_k, cur_norm_d);
1140  }
1141  else
1142  {
1143  // Use time:
1144  is_time_based = true;
1145  is_exact = true; // well, sort of...
1146  const double NOP_At =
1149  m_lastSentVelCmd.tim_send_cmd_vel, tim_start_iteration);
1150  newLogRec.additional_debug_msgs["PTG_eval.NOP_At"] =
1151  mrpt::format("%.06f s", NOP_At);
1152  cur_k = move_k;
1153  cur_ptg_step = mrpt::round(NOP_At / cm.PTG->getPathStepDuration());
1154  cur_norm_d = cm.PTG->getPathDist(cur_k, cur_ptg_step) /
1155  cm.PTG->getRefDistance();
1156  {
1157  const double cur_a = cm.PTG->index2alpha(cur_k);
1158  log.TP_Robot.x = cos(cur_a) * cur_norm_d;
1159  log.TP_Robot.y = sin(cur_a) * cur_norm_d;
1160  cm.starting_robot_dir = cur_a;
1161  cm.starting_robot_dist = cur_norm_d;
1162  }
1163  }
1164 
1165  if (!is_exact)
1166  {
1167  // Don't trust this step: we are not 100% sure of the robot pose in
1168  // TP-Space for this "PTG continuation" step:
1169  cm.speed = -0.01; // this enforces a 0 global evaluation score
1170  newLogRec.additional_debug_msgs["PTG_eval"] =
1171  "PTG-continuation not allowed, cur. pose out of PTG domain.";
1172  return;
1173  }
1174  bool WS_point_is_unique = true;
1175  if (!is_time_based)
1176  {
1177  bool ok1 = cm.PTG->getPathStepForDist(
1179  cur_norm_d * cm.PTG->getRefDistance(), cur_ptg_step);
1180  if (ok1)
1181  {
1182  // Check bijective:
1183  WS_point_is_unique = cm.PTG->isBijectiveAt(cur_k, cur_ptg_step);
1184  const uint32_t predicted_step =
1187  mrpt::system::now()) /
1188  cm.PTG->getPathStepDuration();
1189  WS_point_is_unique =
1190  WS_point_is_unique &&
1191  cm.PTG->isBijectiveAt(move_k, predicted_step);
1192  newLogRec.additional_debug_msgs["PTG_eval.bijective"] =
1193  mrpt::format(
1194  "isBijectiveAt(): k=%i step=%i -> %s", (int)cur_k,
1195  (int)cur_ptg_step, WS_point_is_unique ? "yes" : "no");
1196 
1197  if (!WS_point_is_unique)
1198  {
1199  // Don't trust direction:
1200  cur_k = move_k;
1201  cur_ptg_step = predicted_step;
1202  cur_norm_d = cm.PTG->getPathDist(cur_k, cur_ptg_step);
1203  }
1204  {
1205  const double cur_a = cm.PTG->index2alpha(cur_k);
1206  log.TP_Robot.x = cos(cur_a) * cur_norm_d;
1207  log.TP_Robot.y = sin(cur_a) * cur_norm_d;
1208  cm.starting_robot_dir = cur_a;
1209  cm.starting_robot_dist = cur_norm_d;
1210  }
1211 
1212  mrpt::math::TPose2D predicted_rel_pose;
1213  cm.PTG->getPathPose(
1214  m_lastSentVelCmd.ptg_alpha_index, cur_ptg_step,
1215  predicted_rel_pose);
1216  const auto predicted_pose_global =
1217  m_lastSentVelCmd.poseVel.rawOdometry + predicted_rel_pose;
1218  const double predicted2real_dist = mrpt::hypot_fast(
1219  predicted_pose_global.x - m_curPoseVel.rawOdometry.x,
1220  predicted_pose_global.y - m_curPoseVel.rawOdometry.y);
1221  newLogRec.additional_debug_msgs["PTG_eval.lastCmdPose(raw)"] =
1223  newLogRec.additional_debug_msgs["PTG_eval.PTGcont"] =
1224  mrpt::format(
1225  "mismatchDistance=%.03f cm", 1e2 * predicted2real_dist);
1226 
1227  if (predicted2real_dist >
1229  .max_distance_predicted_actual_path &&
1230  (!is_slowdown ||
1231  (target_d_norm - cur_norm_d) * ref_dist > 2.0 /*meters*/))
1232  {
1233  cm.speed =
1234  -0.01; // this enforces a 0 global evaluation score
1235  newLogRec.additional_debug_msgs["PTG_eval"] =
1236  "PTG-continuation not allowed, mismatchDistance above "
1237  "threshold.";
1238  return;
1239  }
1240  }
1241  else
1242  {
1243  cm.speed = -0.01; // this enforces a 0 global evaluation score
1244  newLogRec.additional_debug_msgs["PTG_eval"] =
1245  "PTG-continuation not allowed, couldn't get PTG step for "
1246  "cur. robot pose.";
1247  return;
1248  }
1249  }
1250 
1251  // Path following isn't perfect: we can't be 100% sure of whether the
1252  // robot followed exactly
1253  // the intended path (`kDirection`), or if it's actually a bit shifted,
1254  // as reported in `cur_k`.
1255  // Take the least favorable case.
1256  // [Update] Do this only when the PTG gave us a unique-mapped WS<->TPS
1257  // point:
1258 
1259  colfree = WS_point_is_unique
1260  ? std::min(in_TPObstacles[move_k], in_TPObstacles[cur_k])
1261  : in_TPObstacles[move_k];
1262 
1263  // Only discount free space if there was a real obstacle, not the "end
1264  // of path" due to limited refDistance.
1265  if (colfree < 0.99)
1266  {
1267  colfree -= cur_norm_d;
1268  }
1269 
1270  // Save estimated robot pose over path as a parameter for scores:
1271  move_cur_d = cur_norm_d;
1272  }
1273 
1274  // Factor4: Decrease in euclidean distance between (x,y) and the target:
1275  // Moving away of the target is negatively valued.
1276  cm.props["dist_eucl_final"] =
1277  mrpt::hypot_fast(WS_Target.x - pose.x, WS_Target.y - pose.y);
1278 
1279  // dist_eucl_min: Use PTG clearance methods to evaluate the real-world
1280  // (WorkSpace) minimum distance to target:
1281  {
1282  using map_d2d_t = std::map<double, double>;
1283  map_d2d_t pathDists;
1284  const double D = cm.PTG->getRefDistance();
1285  const int num_steps = ceil(D * 2.0);
1286  for (int i = 0; i < num_steps; i++)
1287  {
1288  pathDists[i / double(num_steps)] =
1289  100.0; // default normalized distance to target (a huge value)
1290  }
1291 
1293  WS_Target.x, WS_Target.y, move_k, pathDists,
1294  false /*treat point as target, not obstacle*/);
1295 
1296  const auto it = std::min_element(
1297  pathDists.begin(), pathDists.end(),
1298  [colfree](map_d2d_t::value_type& l, map_d2d_t::value_type& r)
1299  -> bool { return (l.second < r.second) && l.first < colfree; });
1300  cm.props["dist_eucl_min"] = (it != pathDists.end())
1301  ? it->second * cm.PTG->getRefDistance()
1302  : 100.0;
1303  }
1304 
1305  // Factor5: Hysteresis:
1306  // -----------------------------------------------------
1307  double& hysteresis = cm.props["hysteresis"];
1308  hysteresis = .0;
1309 
1310  if (cm.PTG->supportVelCmdNOP())
1311  {
1312  hysteresis = this_is_PTG_continuation ? 1.0 : 0.;
1313  }
1314  else if (m_last_vel_cmd)
1315  {
1317  desired_cmd = cm.PTG->directionToMotionCommand(move_k);
1319  const mrpt::kinematics::CVehicleVelCmd* ptr2 = desired_cmd.get();
1320  if (typeid(*ptr1) == typeid(*ptr2))
1321  {
1322  ASSERT_EQUAL_(
1323  m_last_vel_cmd->getVelCmdLength(),
1324  desired_cmd->getVelCmdLength());
1325 
1326  double simil_score = 0.5;
1327  for (size_t i = 0; i < desired_cmd->getVelCmdLength(); i++)
1328  {
1329  const double scr =
1330  exp(-std::abs(
1331  desired_cmd->getVelCmdElement(i) -
1332  m_last_vel_cmd->getVelCmdElement(i)) /
1333  0.20);
1334  mrpt::keep_min(simil_score, scr);
1335  }
1336  hysteresis = simil_score;
1337  }
1338  }
1339 
1340  // Factor6: clearance
1341  // -----------------------------------------------------
1342  // clearance indicators that may be useful in deciding the best motion:
1343  double& clearance = cm.props["clearance"];
1344  clearance = in_clearance.getClearance(
1345  move_k, target_d_norm * 1.01, false /* spot, dont interpolate */);
1346  cm.props["clearance_50p"] = in_clearance.getClearance(
1347  move_k, target_d_norm * 0.5, false /* spot, dont interpolate */);
1348  cm.props["clearance_path"] = in_clearance.getClearance(
1349  move_k, target_d_norm * 0.9, true /* average */);
1350  cm.props["clearance_path_50p"] = in_clearance.getClearance(
1351  move_k, target_d_norm * 0.5, true /* average */);
1352 
1353  // Factor: ETA (Estimated Time of Arrival to target or to closest obstacle,
1354  // whatever it's first)
1355  // -----------------------------------------------------
1356  double& eta = cm.props["eta"];
1357  eta = .0;
1358  if (cm.PTG && cm.speed > .0) // for valid cases only
1359  {
1360  // OK, we have a direct path to target without collisions.
1361  const double path_len_meters = d * ref_dist;
1362 
1363  // Calculate their ETA
1364  uint32_t target_step;
1365  bool valid_step =
1366  cm.PTG->getPathStepForDist(move_k, path_len_meters, target_step);
1367  if (valid_step)
1368  {
1369  eta = cm.PTG->getPathStepDuration() *
1370  target_step /* PTG original time to get to target point */
1371  * cm.speed /* times the speed scale factor*/;
1372 
1373  double discount_time = .0;
1374  if (this_is_PTG_continuation)
1375  {
1376  // Heuristic: discount the time already executed.
1377  // Note that hm.speed above scales the overall path time using
1378  // the current speed scale, not the exact
1379  // integration over the past timesteps. It's an approximation,
1380  // probably good enough...
1381  discount_time = mrpt::system::timeDifference(
1382  m_lastSentVelCmd.tim_send_cmd_vel, tim_start_iteration);
1383  }
1384  eta -= discount_time; // This could even become negative if the
1385  // approximation is poor...
1386  }
1387  }
1388 
1389  MRPT_END;
1390 }
1391 
1393  const TCandidateMovementPTG& in_movement,
1395 {
1396  mrpt::system::CTimeLoggerEntry tle(m_timelogger, "generate_vel_cmd");
1397  double cmdvel_speed_scale = 1.0;
1398  try
1399  {
1400  if (in_movement.speed == 0)
1401  {
1402  // The robot will stop:
1403  new_vel_cmd =
1405  new_vel_cmd->setToStop();
1406  }
1407  else
1408  {
1409  // Take the normalized movement command:
1410  new_vel_cmd = in_movement.PTG->directionToMotionCommand(
1411  in_movement.PTG->alpha2index(in_movement.direction));
1412 
1413  // Scale holonomic speeds to real-world one:
1414  new_vel_cmd->cmdVel_scale(in_movement.speed);
1415  cmdvel_speed_scale *= in_movement.speed;
1416 
1417  if (!m_last_vel_cmd) // first iteration? Use default values:
1418  m_last_vel_cmd =
1420 
1421  // Honor user speed limits & "blending":
1422  const double beta = meanExecutionPeriod.getLastOutput() /
1425  cmdvel_speed_scale *= new_vel_cmd->cmdVel_limits(
1426  *m_last_vel_cmd, beta,
1428  }
1429 
1430  m_last_vel_cmd = new_vel_cmd; // Save for filtering in next step
1431  }
1432  catch (std::exception& e)
1433  {
1435  "[CAbstractPTGBasedReactive::generate_vel_cmd] Exception: "
1436  << e.what());
1437  }
1438  return cmdvel_speed_scale;
1439 }
1440 
1441 /** \callergraph */
1443  const mrpt::math::TPoint2D& wp) const
1444 {
1445  MRPT_START;
1446 
1447  const size_t N = this->getPTG_count();
1448  if (m_infoPerPTG.size() < N ||
1452  return false; // We didn't run yet or obstacle info is old
1453 
1454  for (size_t i = 0; i < N; i++)
1455  {
1456  const CParameterizedTrajectoryGenerator* ptg = getPTG(i);
1457 
1458  const std::vector<double>& tp_obs =
1459  m_infoPerPTG[i].TP_Obstacles; // normalized distances
1460  if (tp_obs.size() != ptg->getPathCount())
1461  continue; // May be this PTG has not been used so far? (Target out
1462  // of domain,...)
1463 
1464  int wp_k;
1465  double wp_norm_d;
1466  bool is_into_domain =
1467  ptg->inverseMap_WS2TP(wp.x, wp.y, wp_k, wp_norm_d);
1468  if (!is_into_domain) continue;
1469 
1470  ASSERT_(wp_k < int(tp_obs.size()));
1471 
1472  const double collision_free_dist = tp_obs[wp_k];
1473  if (collision_free_dist > 1.01 * wp_norm_d)
1474  return true; // free path found to target
1475  }
1476 
1477  return false; // no way found
1478  MRPT_END;
1479 }
1480 
1481 /** \callergraph */
1483 {
1485 }
1486 
1487 /** \callergraph */
1489 {
1492 
1493  CWaypointsNavigator::onStartNewNavigation(); // Call base method we
1494  // override
1495 }
1496 
1499 {
1500  ptg_index = -1;
1501  ptg_alpha_index = -1;
1502  tim_send_cmd_vel = INVALID_TIMESTAMP;
1503  poseVel = TRobotPoseVel();
1504  colfreedist_move_k = .0;
1505  was_slowdown = false;
1506  speed_scale = 1.0;
1507  original_holo_eval = .0;
1509 }
1511 {
1512  return this->poseVel.timestamp != INVALID_TIMESTAMP;
1513 }
1514 
1515 /** \callergraph */
1517  CParameterizedTrajectoryGenerator* ptg, const size_t indexPTG,
1518  const std::vector<mrpt::math::TPose2D>& relTargets,
1519  const mrpt::math::TPose2D& rel_pose_PTG_origin_wrt_sense, TInfoPerPTG& ipf,
1520  TCandidateMovementPTG& cm, CLogFileRecord& newLogRec,
1521  const bool this_is_PTG_continuation,
1523  const mrpt::system::TTimeStamp tim_start_iteration,
1524  const TNavigationParams& navp,
1525  const mrpt::math::TPose2D& rel_cur_pose_wrt_last_vel_cmd_NOP)
1526 {
1527  ASSERT_(ptg);
1528 
1529  const size_t idx_in_log_infoPerPTGs =
1530  this_is_PTG_continuation ? getPTG_count() : indexPTG;
1531 
1533  cm.PTG = ptg;
1534 
1535  // If the user doesn't want to use this PTG, just mark it as invalid:
1536  ipf.targets.clear();
1537  bool use_this_ptg = true;
1538  {
1539  const TNavigationParamsPTG* navpPTG =
1540  dynamic_cast<const TNavigationParamsPTG*>(&navp);
1541  if (navpPTG && !navpPTG->restrict_PTG_indices.empty())
1542  {
1543  use_this_ptg = false;
1544  for (size_t i = 0;
1545  i < navpPTG->restrict_PTG_indices.size() && !use_this_ptg; i++)
1546  {
1547  if (navpPTG->restrict_PTG_indices[i] == indexPTG)
1548  use_this_ptg = true;
1549  }
1550  }
1551  }
1552 
1553  double timeForTPObsTransformation = .0, timeForHolonomicMethod = .0;
1554 
1555  // Normal PTG validity filter: check if target falls into the PTG domain:
1556  bool any_TPTarget_is_valid = false;
1557  if (use_this_ptg)
1558  {
1559  for (size_t i = 0; i < relTargets.size(); i++)
1560  {
1561  const auto& trg = relTargets[i];
1562  PTGTarget ptg_target;
1563 
1564  ptg_target.valid_TP = ptg->inverseMap_WS2TP(
1565  trg.x, trg.y, ptg_target.target_k, ptg_target.target_dist);
1566  if (!ptg_target.valid_TP) continue;
1567 
1568  any_TPTarget_is_valid = true;
1569  ptg_target.target_alpha = ptg->index2alpha(ptg_target.target_k);
1570  ptg_target.TP_Target.x =
1571  cos(ptg_target.target_alpha) * ptg_target.target_dist;
1572  ptg_target.TP_Target.y =
1573  sin(ptg_target.target_alpha) * ptg_target.target_dist;
1574 
1575  ipf.targets.emplace_back(ptg_target);
1576  }
1577  }
1578 
1579  if (!any_TPTarget_is_valid)
1580  {
1582  "mov_candidate_%u", static_cast<unsigned int>(indexPTG))] =
1583  "PTG discarded since target(s) is(are) out of domain.";
1584  }
1585  else
1586  {
1587  // STEP3(b): Build TP-Obstacles
1588  // -----------------------------------------------------------------------------
1589  {
1590  tictac.Tic();
1591 
1592  // Initialize TP-Obstacles:
1593  const size_t Ki = ptg->getAlphaValuesCount();
1594  ptg->initTPObstacles(ipf.TP_Obstacles);
1596  {
1597  ptg->initClearanceDiagram(ipf.clearance);
1598  }
1599 
1600  // Implementation-dependent conversion:
1602  indexPTG, ipf.TP_Obstacles, ipf.clearance,
1603  mrpt::math::TPose2D(0, 0, 0) - rel_pose_PTG_origin_wrt_sense,
1605 
1607  {
1609  }
1610 
1611  // Distances in TP-Space are normalized to [0,1]:
1612  const double _refD = 1.0 / ptg->getRefDistance();
1613  for (size_t i = 0; i < Ki; i++) ipf.TP_Obstacles[i] *= _refD;
1614 
1615  timeForTPObsTransformation = tictac.Tac();
1616  if (m_timelogger.isEnabled())
1618  "navigationStep.STEP3_WSpaceToTPSpace",
1619  timeForTPObsTransformation);
1620  }
1621 
1622  // STEP4: Holonomic navigation method
1623  // -----------------------------------------------------------------------------
1624  if (!this_is_PTG_continuation)
1625  {
1626  tictac.Tic();
1627 
1628  // Slow down if we are approaching the final target, etc.
1629  holoMethod.enableApproachTargetSlowDown(
1630  navp.target.targetDesiredRelSpeed < .11);
1631 
1632  // Prepare holonomic algorithm call:
1634  ni.clearance = &ipf.clearance;
1635  ni.maxObstacleDist = 1.0;
1636  ni.maxRobotSpeed = 1.0; // So, we use a normalized max speed here.
1637  ni.obstacles = ipf.TP_Obstacles; // Normalized [0,1]
1638 
1639  ni.targets.clear(); // Normalized [0,1]
1640  for (const auto& t : ipf.targets)
1641  {
1642  ni.targets.push_back(t.TP_Target);
1643  }
1644 
1646 
1647  holoMethod.navigate(ni, no);
1648 
1649  // Extract resuls:
1650  cm.direction = no.desiredDirection;
1651  cm.speed = no.desiredSpeed;
1652  HLFR = no.logRecord;
1653 
1654  // Security: Scale down the velocity when heading towards obstacles,
1655  // such that it's assured that we never go thru an obstacle!
1656  const int kDirection =
1657  static_cast<int>(cm.PTG->alpha2index(cm.direction));
1658  double obsFreeNormalizedDistance = ipf.TP_Obstacles[kDirection];
1659 
1660  // Take into account the future robot pose after NOP motion
1661  // iterations to slow down accordingly *now*
1662  if (ptg->supportVelCmdNOP())
1663  {
1664  const double v = mrpt::hypot_fast(
1666  const double d = v * ptg->maxTimeInVelCmdNOP(kDirection);
1667  obsFreeNormalizedDistance = std::min(
1668  obsFreeNormalizedDistance,
1669  std::max(0.90, obsFreeNormalizedDistance - d));
1670  }
1671 
1672  double velScale = 1.0;
1673  ASSERT_(
1676  if (obsFreeNormalizedDistance <
1678  {
1679  if (obsFreeNormalizedDistance <=
1681  velScale = 0.0; // security stop
1682  else
1683  velScale =
1684  (obsFreeNormalizedDistance -
1688  }
1689 
1690  // Scale:
1691  cm.speed *= velScale;
1692 
1693  timeForHolonomicMethod = tictac.Tac();
1694  if (m_timelogger.isEnabled())
1696  "navigationStep.STEP4_HolonomicMethod",
1697  timeForHolonomicMethod);
1698  }
1699  else
1700  {
1701  // "NOP cmdvel" case: don't need to re-run holo algorithm, just keep
1702  // the last selection:
1704  cm.speed = 1.0; // Not used.
1705  }
1706 
1707  // STEP5: Evaluate each movement to assign them a "evaluation" value.
1708  // ---------------------------------------------------------------------
1709  {
1710  CTimeLoggerEntry tle(
1711  m_timelogger, "navigationStep.calc_move_candidate_scores");
1712 
1714  cm, ipf.TP_Obstacles, ipf.clearance, relTargets, ipf.targets,
1715  newLogRec.infoPerPTG[idx_in_log_infoPerPTGs], newLogRec,
1716  this_is_PTG_continuation, rel_cur_pose_wrt_last_vel_cmd_NOP,
1717  indexPTG, tim_start_iteration, HLFR);
1718 
1719  // Store NOP related extra vars:
1720  cm.props["original_col_free_dist"] =
1721  this_is_PTG_continuation ? m_lastSentVelCmd.colfreedist_move_k
1722  : .0;
1723 
1724  // SAVE LOG
1725  newLogRec.infoPerPTG[idx_in_log_infoPerPTGs].evalFactors = cm.props;
1726  }
1727 
1728  } // end "valid_TP"
1729 
1730  // Logging:
1731  const bool fill_log_record =
1732  (m_logFile != nullptr || m_enableKeepLogRecords);
1733  if (fill_log_record)
1734  {
1736  newLogRec.infoPerPTG[idx_in_log_infoPerPTGs];
1737  if (!this_is_PTG_continuation)
1738  ipp.PTG_desc = ptg->getDescription();
1739  else
1740  ipp.PTG_desc = mrpt::format(
1741  "NOP cmdvel (prev PTG idx=%u)",
1742  static_cast<unsigned int>(m_lastSentVelCmd.ptg_index));
1743 
1745  ipf.TP_Obstacles, ipp.TP_Obstacles);
1746  ipp.clearance = ipf.clearance;
1747  ipp.TP_Targets.clear();
1748  for (const auto& t : ipf.targets)
1749  {
1750  ipp.TP_Targets.push_back(t.TP_Target);
1751  }
1752  ipp.HLFR = HLFR;
1753  ipp.desiredDirection = cm.direction;
1754  ipp.desiredSpeed = cm.speed;
1755  ipp.timeForTPObsTransformation = timeForTPObsTransformation;
1756  ipp.timeForHolonomicMethod = timeForHolonomicMethod;
1757  }
1758 }
1759 
1762 {
1763  MRPT_START;
1764 
1765  robot_absolute_speed_limits.loadConfigFile(c, s);
1766 
1767  MRPT_LOAD_CONFIG_VAR_REQUIRED_CS(holonomic_method, string);
1768  MRPT_LOAD_CONFIG_VAR_REQUIRED_CS(motion_decider_method, string);
1769  MRPT_LOAD_CONFIG_VAR_REQUIRED_CS(ref_distance, double);
1770  MRPT_LOAD_CONFIG_VAR_CS(speedfilter_tau, double);
1771  MRPT_LOAD_CONFIG_VAR_CS(secure_distance_start, double);
1772  MRPT_LOAD_CONFIG_VAR_CS(secure_distance_end, double);
1773  MRPT_LOAD_CONFIG_VAR_CS(use_delays_model, bool);
1774  MRPT_LOAD_CONFIG_VAR_CS(max_distance_predicted_actual_path, double);
1776  min_normalized_free_space_for_ptg_continuation, double);
1777  MRPT_LOAD_CONFIG_VAR_CS(enable_obstacle_filtering, bool);
1778  MRPT_LOAD_CONFIG_VAR_CS(evaluate_clearance, bool);
1779  MRPT_LOAD_CONFIG_VAR_CS(max_dist_for_timebased_path_prediction, double);
1780 
1781  MRPT_END;
1782 }
1783 
1786 {
1787  robot_absolute_speed_limits.saveToConfigFile(c, s);
1788 
1789  // Build list of known holo methods:
1790  string lstHoloStr = "# List of known classes:\n";
1791  {
1794  for (const auto& cl : lst)
1795  lstHoloStr +=
1796  string("# - `") + string(cl->className) + string("`\n");
1797  }
1799  holonomic_method,
1800  string("C++ class name of the holonomic navigation method to run in "
1801  "the transformed TP-Space.\n") +
1802  lstHoloStr);
1803 
1804  // Build list of known decider methods:
1805  string lstDecidersStr = "# List of known classes:\n";
1806  {
1809  for (const auto& cl : lst)
1810  lstDecidersStr +=
1811  string("# - `") + string(cl->className) + string("`\n");
1812  }
1814  motion_decider_method,
1815  string("C++ class name of the motion decider method.\n") +
1816  lstDecidersStr);
1817 
1819  ref_distance,
1820  "Maximum distance up to obstacles will be considered (D_{max} in "
1821  "papers).");
1823  speedfilter_tau,
1824  "Time constant (in seconds) for the low-pass filter applied to "
1825  "kinematic velocity commands (default=0: no filtering)");
1827  secure_distance_start,
1828  "In normalized distance [0,1], start/end of a ramp function that "
1829  "scales the holonomic navigator output velocity.");
1831  secure_distance_end,
1832  "In normalized distance [0,1], start/end of a ramp function that "
1833  "scales the holonomic navigator output velocity.");
1835  use_delays_model,
1836  "Whether to use robot pose inter/extrapolation to improve accuracy "
1837  "(Default:false)");
1839  max_distance_predicted_actual_path,
1840  "Max distance [meters] to discard current PTG and issue a new vel cmd "
1841  "(default= 0.05)");
1843  min_normalized_free_space_for_ptg_continuation,
1844  "Min normalized dist [0,1] after current pose in a PTG continuation to "
1845  "allow it.");
1847  enable_obstacle_filtering,
1848  "Enabled obstacle filtering (params in its own section)");
1850  evaluate_clearance,
1851  "Enable exact computation of clearance (default=false)");
1853  max_dist_for_timebased_path_prediction,
1854  "Max dist [meters] to use time-based path prediction for NOP "
1855  "evaluation");
1856 }
1857 
1860  : holonomic_method(),
1861  ptg_cache_files_directory("."),
1862  ref_distance(4.0),
1863  speedfilter_tau(0.0),
1864  secure_distance_start(0.05),
1865  secure_distance_end(0.20),
1866  use_delays_model(false),
1867  max_distance_predicted_actual_path(0.15),
1868  min_normalized_free_space_for_ptg_continuation(0.2),
1869  robot_absolute_speed_limits(),
1870  enable_obstacle_filtering(true),
1871  evaluate_clearance(false),
1872  max_dist_for_timebased_path_prediction(2.0)
1873 {
1874 }
1875 
1878 {
1879  MRPT_START;
1881 
1882  // At this point, we have been called from the derived class, who must be
1883  // already loaded all its specific params, including PTGs.
1884 
1885  // Load my params:
1887  c, "CAbstractPTGBasedReactive");
1888 
1889  // Filtering:
1891  {
1895  filter->options.loadFromConfigFile(c, "CPointCloudFilterByDistance");
1896  }
1897  else
1898  {
1899  m_WS_filter.reset();
1900  }
1901 
1902  // Movement chooser:
1905  if (!m_multiobjopt)
1907  "Non-registered CMultiObjectiveMotionOptimizerBase className=`%s`",
1909 
1910  m_multiobjopt->loadConfigFile(c);
1911 
1912  // Holo method:
1914  ASSERT_(!m_holonomicMethod.empty());
1915  CWaypointsNavigator::loadConfigFile(c); // Load parent params
1916 
1917  m_init_done =
1918  true; // If we reached this point without an exception, all is good.
1919  MRPT_END;
1920 }
1921 
1924 {
1927  c, "CAbstractPTGBasedReactive");
1928 
1929  // Filtering:
1930  {
1932  filter.options.saveToConfigFile(c, "CPointCloudFilterByDistance");
1933  }
1934 
1935  // Holo method:
1936  if (!m_holonomicMethod.empty() && m_holonomicMethod[0])
1937  {
1938  // Save my current settings:
1939  m_holonomicMethod[0]->saveConfigFile(c);
1940  }
1941  else
1942  {
1943  // save options of ALL known methods:
1946  for (const auto& cl : lst)
1947  {
1949  mrpt::rtti::CObject::Ptr(cl->createObject());
1951  dynamic_cast<CAbstractHolonomicReactiveMethod*>(obj.get());
1952  if (holo)
1953  {
1954  holo->saveConfigFile(c);
1955  }
1956  }
1957  }
1958 
1959  // Decider method:
1960  if (m_multiobjopt)
1961  {
1962  // Save my current settings:
1963  m_multiobjopt->saveConfigFile(c);
1964  }
1965  else
1966  {
1967  // save options of ALL known methods:
1970  for (const auto& cl : lst)
1971  {
1973  mrpt::rtti::CObject::Ptr(cl->createObject());
1975  dynamic_cast<CMultiObjectiveMotionOptimizerBase*>(obj.get());
1976  if (momo)
1977  {
1978  momo->saveConfigFile(c);
1979  }
1980  }
1981  }
1982 }
1983 
1985  const double dist)
1986 {
1987  for (auto& o : m_holonomicMethod)
1988  {
1989  o->setTargetApproachSlowDownDistance(dist);
1990  }
1991 }
1992 
1994 {
1995  ASSERT_(!m_holonomicMethod.empty());
1996  return m_holonomicMethod[0]->getTargetApproachSlowDownDistance();
1997 }
double index2alpha(uint16_t k) const
Alpha value for the discrete corresponding value.
virtual bool supportSpeedAtTarget() const
Returns true if this PTG takes into account the desired velocity at target.
std::recursive_mutex m_nav_cs
mutex for all navigation methods
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:90
CHolonomicLogFileRecord::Ptr HLFR
Other useful info about holonomic method execution.
mrpt::math::TPose2D rel_cur_pose_wrt_last_vel_cmd_NOP
TRobotPoseVel m_curPoseVel
Current robot pose (updated in CAbstractNavigator::navigationStep() )
virtual bool getPathStepForDist(uint16_t k, double dist, uint32_t &out_step) const =0
Access path k ([0,N-1]=>[-pi,pi] in alpha): largest step count for which the traversed distance is < ...
virtual bool supportVelCmdNOP() const
Returns true if it is possible to stop sending velocity commands to the robot and, still, the robot controller will be able to keep following the last sent trajectory ("NOP" velocity commands).
bool createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:158
#define MRPT_START
Definition: exceptions.h:262
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
GLdouble GLdouble t
Definition: glext.h:3689
mrpt::system::CTimeLogger m_timlog_delays
Time logger to collect delay-related stats.
std::vector< std::string > log_entries
Optionally, debug logging info will be stored here by the implementor classes.
int32_t nSelectedPTG
The selected PTG.
#define min(a, b)
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
std::vector< mrpt::math::TPose2D > WS_targets_relative
Relative poses (wrt to robotPoseLocalization) for.
virtual void saveToConfigFile(mrpt::config::CConfigFileBase &c, const std::string &s) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...
mrpt::system::TParameters< double > props
List of properties.
double x
X,Y coordinates.
std::unique_ptr< mrpt::io::CStream > m_logFile
virtual double getPathStepDuration() const =0
Returns the duration (in seconds) of each "step".
mrpt::math::TTwist2D cur_vel_local
The actual robot velocities in local (robot) coordinates, as read from sensors, in "m/sec" and "rad/s...
void setTargetApproachSlowDownDistance(const double dist)
Changes this parameter in all inner holonomic navigator instances [m].
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
double x
X,Y coordinates.
virtual void onStartNewNavigation() override
Called whenever a new navigation has been started.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
A safe way to call enter() and leave() of a mrpt::system::CTimeLogger upon construction and destructi...
T hypot_fast(const T x, const T y)
Faster version of std::hypot(), to use when overflow is not an issue and we prefer fast code...
std::vector< double > final_evaluation
The final evaluation score for each candidate.
std::map< std::string, mrpt::system::TTimeStamp > timestamps
Known values:
std::recursive_mutex m_critZoneLastLog
Critical zones.
std::vector< const TRuntimeClassId * > getAllRegisteredClassesChildrenOf(const TRuntimeClassId *parent_id)
Like getAllRegisteredClasses(), but filters the list to only include children clases of a given base ...
mrpt::nav::ClearanceDiagram clearance
Clearance for each path.
void copy_container_typecasting(const src_container &src, dst_container &trg)
Copy all the elements in a container (vector, deque, list) into a different one performing the approp...
GLenum GLint ref
Definition: glext.h:4050
virtual bool isBijectiveAt(uint16_t k, uint32_t step) const
Returns true if a given TP-Space point maps to a unique point in Workspace, and viceversa.
std::vector< double > obstacles
Distance to obstacles in polar coordinates, relative to the robot.
mrpt::math::TPose2D rel_pose_PTG_origin_wrt_sense_NOP
CParameterizedTrajectoryGenerator * PTG
The associated PTG.
#define MRPT_LOAD_CONFIG_VAR_CS(variableName, variableType)
Shortcut for MRPT_LOAD_CONFIG_VAR() for config file object named c and section string named s ...
mrpt::math::TPose2D robotPoseOdometry
void getLastLogRecord(CLogFileRecord &o)
Provides a copy of the last log record with information about execution.
TRobotPoseVel poseVel
Robot pose & velocities and timestamp of when it was queried.
std::shared_ptr< CPointCloudFilterBase > Ptr
void initialize() override
Must be called for loading collision grids, or the first navigation command may last a long time to b...
This file implements several operations that operate element-wise on individual or pairs of container...
Base for all high-level navigation commands.
void initTPObstacles(std::vector< double > &TP_Obstacles) const
Resizes and populates the initial appropriate contents in a vector of tp-obstacles (collision-free ra...
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:127
A base class for holonomic reactive navigation methods.
T angDistance(T from, T to)
Computes the shortest angular increment (or distance) between two planar orientations, such that it is constrained to [-pi,pi] and is correct for any combination of angles (e.g.
Definition: wrap2pi.h:98
mrpt::math::TPose2D relPoseSense
void enableLogFile(bool enable)
Enables/disables saving log files.
virtual double maxTimeInVelCmdNOP(int path_k) const
Only for PTGs supporting supportVelCmdNOP(): this is the maximum time (in seconds) for which the path...
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:75
std::vector< mrpt::math::TPoint2D > TP_Targets
Target(s) location in TP-Space.
std::vector< CAbstractHolonomicReactiveMethod::Ptr > m_holonomicMethod
The holonomic navigation algorithm (one object per PTG, so internal states are maintained) ...
bool m_enableConsoleOutput
Enables / disables the console debug output.
std::string holonomic_method
C++ class name of the holonomic navigation method to run in the transformed TP-Space.
void updateClearancePost(ClearanceDiagram &cd, const std::vector< double > &TP_obstacles) const
void updateNavDynamicState(const TNavDynamicState &newState, const bool force_update=false)
To be invoked by the navigator before each navigation step, to let the PTG to react to changing dynam...
uint32_t nPTGs
The number of PTGS:
Clearance information for one particular PTG and one set of obstacles.
STL namespace.
uint16_t getPathCount() const
Get the number of different, discrete paths in this family.
std::vector< double > TP_Obstacles
One distance per discretized alpha value, describing the "polar plot" of TP obstacles.
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...
int target_k
The discrete version of target_alpha.
mrpt::math::LowPassFilter_IIR1 meanExecutionPeriod
Runtime estimation of execution period of the method.
double getLastTime(const std::string &name) const
Return the last execution time of the given "section", or 0 if it hasn&#39;t ever been called "enter" wit...
This class extends CAbstractNavigator with the capability of following a list of waypoints.
A class for storing, saving and loading a reactive navigation log record for the CReactiveNavigationS...
void setHolonomicMethod(const std::string &method, const mrpt::config::CConfigFileBase &cfgBase)
Selects which one from the set of available holonomic methods will be used into transformed TP-Space...
GLdouble s
Definition: glext.h:3676
#define MRPT_LOG_WARN_FMT(_FMT_STRING,...)
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
virtual double getPathDist(uint16_t k, uint32_t step) const =0
Access path k ([0,N-1]=>[-pi,pi] in alpha): traversed distance at discrete step step.
std::string pose_frame_id
map frame ID for pose
virtual void getPathPose(uint16_t k, uint32_t step, mrpt::math::TPose2D &p) const =0
Access path k ([0,N-1]=>[-pi,pi] in alpha): pose of the vehicle at discrete step step.
mrpt::kinematics::CVehicleVelCmd::Ptr m_last_vel_cmd
Last velocity commands.
pose_t & interpolate(mrpt::system::TTimeStamp t, pose_t &out_interp, bool &out_valid_interp) const
Returns the pose at a given time, or interpolates using splines if there is not an exact match...
mrpt::math::TPose2D asTPose() const
Definition: CPose2D.cpp:441
virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd)
Default: forward call to m_robot.changeSpeed().
std::vector< mrpt::math::TPoint2D > targets
Relative location (x,y) of target point(s).
The struct for configuring navigation requests to CWaypointsNavigator and derived classes...
virtual bool changeSpeedsNOP()
Default: forward call to m_robot.changeSpeedsNOP().
mrpt::math::TPoint2D TP_Target
The Target, in TP-Space (x,y)
Virtual base for velocity commands of different kinematic models of planar mobile robot...
void STEP8_GenerateLogRecord(CLogFileRecord &newLogRec, const std::vector< mrpt::math::TPose2D > &relTargets, int nSelectedPTG, const mrpt::kinematics::CVehicleVelCmd::Ptr &new_vel_cmd, int nPTGs, const bool best_is_NOP_cmdvel, const math::TPose2D &rel_cur_pose_wrt_last_vel_cmd_NOP, const math::TPose2D &rel_pose_PTG_origin_wrt_sense_NOP, const double executionTimeValue, const double tim_changeSpeed, const mrpt::system::TTimeStamp &tim_start_iteration)
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT&#39;s CStream, std::istream, std::ostream, std::stringstream
Definition: CArchive.h:561
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: ...
The struct for configuring navigation requests.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
double maxRobotSpeed
Maximum robot speed, in the same units than obstacles, per second.
virtual mrpt::kinematics::CVehicleVelCmd::Ptr directionToMotionCommand(uint16_t k) const =0
Converts a discretized "alpha" value into a feasible motion command or action.
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState ptg_last_navDynState
This is the base class for any user-defined PTG.
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
double speed_scale
[0,1] scale of the raw cmd_vel as generated by the PTG
double desiredDirection
The results from the holonomic method.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:85
virtual size_t getPTG_count() const =0
Returns the number of different PTGs that have been setup.
virtual void loadConfigFile(const mrpt::config::CConfigFileBase &c) override
Loads all params from a file.
static CMultiObjectiveMotionOptimizerBase::Ptr Factory(const std::string &className) noexcept
Class factory from C++ class name.
double targetDesiredRelSpeed
(Default=.05) Desired relative speed (wrt maximum speed), in range [0,1], of the vehicle at target...
mrpt::math::TPose2D robotPoseLocalization
The robot pose (from odometry and from the localization/SLAM system).
mrpt::kinematics::CVehicleVelCmd::Ptr cmd_vel
The final motion command sent to robot, in "m/sec" and "rad/sec".
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:153
double speed
TP-Space movement speed, normalized to [0,1].
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const =0
saves all available parameters, in a forma loadable by initialize()
virtual void performNavigationStep() override
The main method for the navigator.
mrpt::math::TPoint2D TP_Robot
Robot location in TP-Space: normally (0,0), except during "NOP cmd vel" steps.
const GLubyte * c
Definition: glext.h:6313
virtual double generate_vel_cmd(const TCandidateMovementPTG &in_movement, mrpt::kinematics::CVehicleVelCmd::Ptr &new_vel_cmd)
Return the [0,1] velocity scale of raw PTG cmd_vel.
double vx
Velocity components: X,Y (m/s)
This CStream derived class allow using a memory buffer as a CStream.
virtual double evalPathRelativePriority(uint16_t k, double target_distance) const
Query the PTG for the relative priority factor (0,1) of this PTG, in comparison to others...
mrpt::math::LowPassFilter_IIR1 timoff_sendVelCmd_avr
double desiredDirection
The desired motion direction, in the range [-PI, PI].
double timeForTPObsTransformation
Time, in seconds.
std::string motion_decider_method
C++ class name of the motion chooser.
mrpt::nav::CMultiObjectiveMotionOptimizerBase::Ptr m_multiobjopt
mrpt::math::TTwist2D curVelLocal
Current vehicle velocity (local frame of reference)
double max_dist_for_timebased_path_prediction
Max dist [meters] to use time-based path prediction for NOP evaluation.
std::unique_ptr< TNavigationParams > m_navigationParams
Current navigation parameters.
virtual void onStartNewNavigation() override
Called whenever a new navigation has been started.
uint64_t Seek(int64_t Offset, CStream::TSeekOrigin Origin=sFromBeginning) override
Introduces a pure virtual method for moving to a specified position in the streamed resource...
int16_t ptg_index_NOP
Negative means no NOP mode evaluation, so the rest of "NOP variables" should be ignored.
mrpt::aligned_std_vector< TInfoPerPTG > infoPerPTG
The info for each applied PTG: must contain "nPTGs * nSecDistances" elements.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
void initClearanceDiagram(ClearanceDiagram &cd) const
Must be called to resize a CD to its correct size, before calling updateClearance() ...
mrpt::math::TPose2D relTarget
Current relative target location.
Individual target info in CAbstractNavigator::TNavigationParamsBase and derived classes.
virtual void sendApparentCollisionEvent()
Callback: Apparent collision event (i.e.
std::unique_ptr< TNavigationParams > m_copy_prev_navParams
A copy of last-iteration navparams, used to detect changes.
virtual mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand() const =0
Returns an empty kinematic velocity command object of the type supported by this PTG.
CHolonomicLogFileRecord::Ptr logRecord
The navigation method will create a log record and store it here via a smart pointer.
double direction
TP-Space movement direction.
double leave(const char *func_name)
End of a named section.
double maxObstacleDist
Maximum expected value to be found in obstacles.
double starting_robot_dir
Default to 0, they can be used to reflect a robot starting at a position not at (0,0).
const double ESTIM_LOWPASSFILTER_ALPHA
std::map< std::string, double > values
Known values:
GLsizei const GLchar ** string
Definition: glext.h:4101
virtual mrpt::kinematics::CVehicleVelCmd::Ptr getEmergencyStopCmd()=0
Gets the emergency stop command for the current robot.
CParameterizedTrajectoryGenerator::TNavDynamicState ptg_dynState
bool m_closing_navigator
Signal that the destructor has been called, so no more calls are accepted from other threads...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
virtual void STEP3_WSpaceToTPSpace(const size_t ptg_idx, std::vector< double > &out_TPObstacles, mrpt::nav::ClearanceDiagram &out_clearance, const mrpt::math::TPose2D &rel_pose_PTG_origin_wrt_sense, const bool eval_clearance)=0
Builds TP-Obstacles from Workspace obstacles for the given PTG.
mrpt::math::LowPassFilter_IIR1 timoff_obstacles_avr
double secure_distance_start
In normalized distances, the start and end of a ramp function that scales the velocity output from th...
virtual bool implementSenseObstacles(mrpt::system::TTimeStamp &obs_timestamp)=0
Return false on any fatal error.
std::string sprintf_vector(const char *fmt, const std::vector< T > &V)
Generates a string for a vector in the format [A,B,C,...] to std::cout, and the fmt string for each v...
Definition: printf_vector.h:26
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:15
std::vector< std::function< void(void)> > m_pending_events
Events generated during navigationStep(), enqueued to be called at the end of the method execution to...
std::string PTG_desc
A short description for the applied PTG.
double speedfilter_tau
Time constant (in seconds) for the low-pass filter applied to kinematic velocity commands (default=0:...
uint16_t getAlphaValuesCount() const
Get the number of different, discrete paths in this family.
double filter(double x)
Processes one input sample, updates the filter state and return the filtered value.
Definition: filters.cpp:21
double desiredSpeed
The desired motion speed in that direction, from 0 up to NavInput::maxRobotSpeed. ...
mrpt::kinematics::CVehicleVelCmd::TVelCmdParams robot_absolute_speed_limits
Params related to speed limits.
ClearanceDiagram clearance
Clearance for each path.
const mrpt::nav::ClearanceDiagram * clearance
The computed clearance for each direction (optional in some implementations).
virtual void loggingGetWSObstaclesAndShape(CLogFileRecord &out_log)=0
Generates a pointcloud of obstacles, and the robot shape, to be saved in the logging record for the c...
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double getScorePriority() const
When used in path planning, a multiplying factor (default=1.0) for the scores for this PTG...
Virtual base class for multi-objective motion choosers, as used for reactive navigation engines...
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const override
Saves all current options to a config file.
mrpt::math::CVectorFloat TP_Obstacles
Distances until obstacles, in "pseudometers", first index for -PI direction, last one for PI directio...
mrpt::io::CStream * m_prev_logfile
The current log file stream, or nullptr if not being used.
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState navDynState
double colfreedist_move_k
TP-Obstacles in the move direction at the instant of picking this movement.
std::vector< TInfoPerPTG > m_infoPerPTG
Temporary buffers for working with each PTG during a navigationStep()
virtual void loadConfigFile(const mrpt::config::CConfigFileBase &c) override
Loads all params from a file.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
void doEmergencyStop(const std::string &msg)
Stops the robot and set navigation state to error.
TAbstractPTGNavigatorParams params_abstract_ptg_navigator
void deleteHolonomicObjects()
Delete m_holonomicMethod.
void build_movement_candidate(CParameterizedTrajectoryGenerator *ptg, const size_t indexPTG, const std::vector< mrpt::math::TPose2D > &relTargets, const mrpt::math::TPose2D &rel_pose_PTG_origin_wrt_sense, TInfoPerPTG &ipf, TCandidateMovementPTG &holonomicMovement, CLogFileRecord &newLogRec, const bool this_is_PTG_continuation, mrpt::nav::CAbstractHolonomicReactiveMethod &holoMethod, const mrpt::system::TTimeStamp tim_start_iteration, const TNavigationParams &navp=TNavigationParams(), const mrpt::math::TPose2D &relPoseVelCmd_NOP=mrpt::math::TPose2D(0, 0, 0))
virtual void evalClearanceSingleObstacle(const double ox, const double oy, const uint16_t k, ClearanceDiagram::dist2clearance_t &inout_realdist2clearance, bool treat_as_obstacle=true) const
Evals the robot clearance for each robot pose along path k, for the real distances in the key of the ...
Implementation of pointcloud filtering based on requisities for minimum neigbouring points in both...
CRobot2NavInterface & m_robot
The navigator-robot interface.
mrpt::math::TTwist2D cur_vel
The actual robot velocities in global (map) coordinates, as read from sensors, in "m/sec" and "rad/se...
bool m_init_done
Whether loadConfigFile() has been called or not.
#define MRPT_END
Definition: exceptions.h:266
mrpt::math::LowPassFilter_IIR1 meanTotalExecutionTime
virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &c, const std::string &s) override
This method load the options from a ".ini"-like file or memory-stored string list.
static CAbstractHolonomicReactiveMethod::Ptr Factory(const std::string &className) noexcept
#define MRPT_LOAD_CONFIG_VAR_REQUIRED_CS(variableName, variableType)
Shortcut for MRPT_LOAD_CONFIG_VAR_NO_DEFAULT() for REQUIRED variables config file object named c and ...
Lightweight 2D pose.
virtual bool impl_waypoint_is_reachable(const mrpt::math::TPoint2D &wp_local_wrt_robot) const override
Implements the way to waypoint is free function in children classes: true must be returned if...
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:31
std::string m_navlogfiles_dir
Default: "./reactivenav.logs".
virtual CParameterizedTrajectoryGenerator * getPTG(size_t i)=0
Gets the i&#39;th PTG.
mrpt::math::LowPassFilter_IIR1 meanExecutionTime
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
bool valid_TP
For each PTG, whether target falls into the PTG domain.
The struct for configuring navigation requests to CAbstractPTGBasedReactive and derived classes...
std::weak_ptr< mrpt::poses::FrameTransformer< 2 > > m_frame_tf
Optional, user-provided frame transformer.
mrpt::system::TTimeStamp timestampAdd(const mrpt::system::TTimeStamp tim, const double num_seconds)
Shifts a timestamp the given amount of seconds (>0: forwards in time, <0: backwards) ...
Definition: datetime.cpp:199
virtual void navigate(const NavInput &ni, NavOutput &no)=0
Invokes the holonomic navigation algorithm itself.
Input parameters for CAbstractHolonomicReactiveMethod::navigate()
bool open(const std::string &fileName, int compress_level=1)
Open a file for write, choosing the compression level.
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y yaw]", yaw in degrees) ...
bool directoryExists(const std::string &fileName)
Test if a given directory exists (it fails if the given path refers to an existing file)...
Definition: filesystem.cpp:136
double timeDifference(const mrpt::system::TTimeStamp t_first, const mrpt::system::TTimeStamp t_later)
Returns the time difference from t1 to t2 (positive if t2 is posterior to t1), in seconds...
Definition: datetime.cpp:209
Saves data to a file and transparently compress the data using the given compression level...
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:79
mrpt::poses::CPose2DInterpolator m_latestOdomPoses
GLuint GLenum GLenum transform
Definition: glext.h:6975
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
unsigned __int32 uint32_t
Definition: rptypes.h:47
Lightweight 2D point.
mrpt::system::TTimeStamp tim_send_cmd_vel
Timestamp of when the cmd was sent.
Dynamic state that may affect the PTG path parameterization.
Output for CAbstractHolonomicReactiveMethod::navigate()
mrpt::math::TPose2D relPoseVelCmd
GLfloat GLfloat p
Definition: glext.h:6305
void calc_move_candidate_scores(TCandidateMovementPTG &holonomicMovement, const std::vector< double > &in_TPObstacles, const mrpt::nav::ClearanceDiagram &in_clearance, const std::vector< mrpt::math::TPose2D > &WS_Targets, const std::vector< PTGTarget > &TP_Targets, CLogFileRecord::TInfoPerPTG &log, CLogFileRecord &newLogRec, const bool this_is_PTG_continuation, const mrpt::math::TPose2D &relPoseVelCmd_NOP, const unsigned int ptg_idx4weights, const mrpt::system::TTimeStamp tim_start_iteration, const mrpt::nav::CHolonomicLogFileRecord::Ptr &hlfr)
Scores holonomicMovement.
mrpt::poses::CPose2DInterpolator m_latestPoses
Latest robot poses (updated in CAbstractNavigator::navigationStep() )
double phi
Orientation (rads)
std::shared_ptr< CObject > Ptr
Definition: CObject.h:149
mrpt::system::CTimeLogger m_timelogger
A complete time logger.
double getLastOutput() const
Definition: filters.cpp:29
The structure used to store all relevant information about each transformation into TP-Space...
mrpt::math::LowPassFilter_IIR1 tim_changeSpeed_avr
GLenum filter
Definition: glext.h:5072
mrpt::math::TPose2D rawOdometry
raw odometry (frame does not match to "pose", but is expected to be smoother in the short term)...
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const =0
void registerUserMeasure(const char *event_name, const double value)
int sprintf(char *buf, size_t bufSize, const char *format,...) noexcept MRPT_printf_format_check(3
An OS-independent version of sprintf (Notice the bufSize param, which may be ignored in some compiler...
Definition: os.cpp:189
virtual bool inverseMap_WS2TP(double x, double y, int &out_k, double &out_normalized_d, double tolerance_dist=0.10) const =0
Computes the closest (alpha,d) TP coordinates of the trajectory point closest to the Workspace (WS) C...
std::vector< size_t > restrict_PTG_indices
(Default=empty) Optionally, a list of PTG indices can be sent such that the navigator will restrict i...
void enter(const char *func_name)
Start of a named section.
bool m_enableKeepLogRecords
See enableKeepLogRecords.
virtual bool stop(bool isEmergencyStop)
Default: forward call to m_robot.stop().
uint16_t alpha2index(double alpha) const
Discrete index value for the corresponding alpha value.
mrpt::math::LowPassFilter_IIR1 timoff_curPoseAndSpeed_avr
double getTargetApproachSlowDownDistance() const
Returns this parameter for the first inner holonomic navigator instances [m] (should be the same in a...
mrpt::maps::CPointCloudFilterBase::Ptr m_WS_filter
Default: none.
void preDestructor()
To be called during children destructors to assure thread-safe destruction, and free of shared object...
Stores a candidate movement in TP-Space-based navigation.
virtual std::string getDescription() const =0
Gets a short textual description of the PTG and its parameters.
std::map< std::string, std::string > additional_debug_msgs
Additional debug traces.
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const override
Saves all current options to a config file.
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019