MRPT  1.9.9
CAbstractPTGBasedReactive.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "nav-precomp.h" // Precomp header
11 
15 #include <mrpt/io/CMemoryStream.h>
17 #include <mrpt/math/geometry.h>
18 #include <mrpt/math/ops_containers.h> // sum()
19 #include <mrpt/math/wrap2pi.h>
22 #include <mrpt/system/filesystem.h>
23 #include <array>
24 #include <iomanip>
25 #include <limits>
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 // Ctor:
59 CAbstractPTGBasedReactive::CAbstractPTGBasedReactive(
60  CRobot2NavInterface& react_iterf_impl, bool enableConsoleOutput,
61  bool enableLogFile, const std::string& sLogDir)
62  : CWaypointsNavigator(react_iterf_impl),
63  m_enableConsoleOutput(enableConsoleOutput),
64  m_navlogfiles_dir(sLogDir)
65 {
66  this->enableLogFile(enableLogFile);
67 }
68 
70 {
71  m_closing_navigator = true;
72 
73  // Wait to end of navigation (multi-thread...)
74  m_nav_cs.lock();
75  m_nav_cs.unlock();
76 
77  // Just in case.
78  try
79  {
80  // Call base class method, NOT the generic, virtual one,
81  // to avoid problems if we are in the dtor, while the vtable
82  // is being destroyed.
83  CAbstractNavigator::stop(false /*not emergency*/);
84  }
85  catch (...)
86  {
87  }
88 
89  m_logFile.reset();
90 
91  // Free holonomic method:
92  this->deleteHolonomicObjects();
93 }
94 
96 {
97  this->preDestructor(); // ensure the robot is stopped; free dynamic objects
98 }
99 
100 /** \callergraph */
102 {
103  std::lock_guard<std::recursive_mutex> csl(m_nav_cs);
104 
106 
108  m_multiobjopt->clear();
109 
110  // Compute collision grids:
111  STEP1_InitPTGs();
112 }
113 
114 /*---------------------------------------------------------------
115  enableLogFile
116  ---------------------------------------------------------------*/
118 {
119  std::lock_guard<std::recursive_mutex> csl(m_nav_cs);
120 
121  try
122  {
123  // Disable:
124  // -------------------------------
125  if (!enable)
126  {
127  if (m_logFile)
128  {
130  "[CAbstractPTGBasedReactive::enableLogFile] Stopping "
131  "logging.");
132  m_logFile.reset(); // Close file:
133  }
134  else
135  return; // Already disabled.
136  }
137  else
138  { // Enable
139  // -------------------------------
140  if (m_logFile) return; // Already enabled:
141 
142  // Open file, find the first free file-name.
144  "[CAbstractPTGBasedReactive::enableLogFile] Creating rnav log "
145  "directory: %s",
146  m_navlogfiles_dir.c_str());
149  {
151  "Could not create directory for navigation logs: `%s`",
152  m_navlogfiles_dir.c_str());
153  }
154 
155  std::string filToOpen;
156  for (unsigned int nFile = 0;; nFile++)
157  {
158  filToOpen = mrpt::format(
159  "%s/log_%03u.reactivenavlog", m_navlogfiles_dir.c_str(),
160  nFile);
161  if (!system::fileExists(filToOpen)) break;
162  }
163 
164  // Open log file:
165  {
166  std::unique_ptr<CFileGZOutputStream> fil(
167  new CFileGZOutputStream);
168  bool ok = fil->open(filToOpen, 1 /* compress level */);
169  if (!ok)
170  {
172  "Error opening log file: `%s`", filToOpen.c_str());
173  }
174  else
175  {
176  m_logFile = std::move(fil);
177  }
178  }
179 
181  "[CAbstractPTGBasedReactive::enableLogFile] Logging to "
182  "file `%s`",
183  filToOpen.c_str()));
184  }
185  }
186  catch (const std::exception& e)
187  {
189  "[CAbstractPTGBasedReactive::enableLogFile] Exception: %s",
190  e.what());
191  }
192 }
193 
195 {
196  std::lock_guard<std::recursive_mutex> lock(m_critZoneLastLog);
197  o = lastLogRecord;
198 }
199 
201 {
202  m_holonomicMethod.clear();
203 }
204 
206  const std::string& method, const mrpt::config::CConfigFileBase& ini)
207 {
208  std::lock_guard<std::recursive_mutex> csl(m_nav_cs);
209 
210  this->deleteHolonomicObjects();
211  const size_t nPTGs = this->getPTG_count();
212  ASSERT_(nPTGs != 0);
213  m_holonomicMethod.resize(nPTGs);
214 
215  for (size_t i = 0; i < nPTGs; i++)
216  {
217  m_holonomicMethod[i] =
219  if (!m_holonomicMethod[i])
221  "Non-registered holonomic method className=`%s`",
222  method.c_str());
223 
224  m_holonomicMethod[i]->setAssociatedPTG(this->getPTG(i));
225  m_holonomicMethod[i]->initialize(ini); // load params
226  }
227 }
228 
229 /** The main method: executes one time-iteration of the reactive navigation
230  * algorithm.
231  * \callergraph */
233 {
235  m_navProfiler, "CAbstractPTGBasedReactive::performNavigationStep()");
236 
237  // Security tests:
238  if (m_closing_navigator) return; // Are we closing in the main thread?
239  if (!m_init_done)
240  THROW_EXCEPTION("Have you called loadConfigFile() before?");
242  const size_t nPTGs = this->getPTG_count();
243 
244  // Whether to worry about log files:
245  const bool fill_log_record = (m_logFile || m_enableKeepLogRecords);
246  CLogFileRecord newLogRec;
247  newLogRec.infoPerPTG.resize(nPTGs + 1); /* +1: [N] is the "NOP cmdvel"
248  option; not to be present in all
249  log entries. */
250 
251  // At the beginning of each log file, add an introductory block explaining
252  // which PTGs are we using:
253  {
254  if (m_logFile &&
255  m_logFile.get() != m_prev_logfile) // Only the first time
256  {
257  m_prev_logfile = m_logFile.get();
258  for (size_t i = 0; i < nPTGs; i++)
259  {
260  // If we make a direct copy (=) we will store the entire, heavy,
261  // collision grid.
262  // Let's just store the parameters of each PTG by serializing
263  // it, so paths can be reconstructed
264  // by invoking initialize()
266  auto arch = archiveFrom(buf);
267  arch << *this->getPTG(i);
268  buf.Seek(0);
269  newLogRec.infoPerPTG[i].ptg = std::dynamic_pointer_cast<
271  arch.ReadObject());
272  }
273  }
274  }
275 
276  CTimeLoggerEntry tle1(m_timelogger, "navigationStep");
277 
278  try
279  {
280  totalExecutionTime.Tic(); // Start timer
281 
282  const mrpt::system::TTimeStamp tim_start_iteration =
284 
285  // Compute target location relative to current robot pose:
286  // ---------------------------------------------------------------------
287  // Detect changes in target since last iteration (for NOP):
288  const bool target_changed_since_last_iteration =
291  if (target_changed_since_last_iteration)
293 
294  // Load the list of target(s) from the navigationParam user command.
295  // Semantic is: any of the target is good. If several targets are
296  // reachable, head to latest one.
297  std::vector<CAbstractNavigator::TargetInfo> targets;
298  {
299  auto p = dynamic_cast<
301  m_navigationParams.get());
302  if (p && !p->multiple_targets.empty())
303  {
304  targets = p->multiple_targets;
305  }
306  else
307  {
308  targets.push_back(m_navigationParams->target);
309  }
310  }
311  const size_t nTargets = targets.size(); // Normally = 1, will be >1 if
312  // we want the robot local
313  // planner to be "smarter" in
314  // skipping dynamic obstacles.
315  ASSERT_(nTargets >= 1);
316 
317  STEP1_InitPTGs(); // Will only recompute if
318  // "m_PTGsMustBeReInitialized==true"
319 
320  // STEP2: Load the obstacles and sort them in height bands.
321  // -----------------------------------------------------------------------------
322  bool sense_ok;
323  {
326  "CAbstractPTGBasedReactive::performNavigationStep().STEP2_"
327  "SenseObstacles()");
328 
329  sense_ok = STEP2_SenseObstacles();
330  }
331 
332  if (!sense_ok)
333  {
335  "Error while loading and sorting the obstacles. Robot will be "
336  "stopped.");
337  if (fill_log_record)
338  {
339  CPose2D rel_cur_pose_wrt_last_vel_cmd_NOP,
340  rel_pose_PTG_origin_wrt_sense_NOP;
342  newLogRec,
343  std::vector<mrpt::math::TPose2D>() /*no targets*/,
344  -1, // best_ptg_idx,
345  m_robot.getEmergencyStopCmd(), nPTGs,
346  false, // best_is_NOP_cmdvel,
347  rel_cur_pose_wrt_last_vel_cmd_NOP.asTPose(),
348  rel_pose_PTG_origin_wrt_sense_NOP.asTPose(),
349  0, // executionTimeValue,
350  0, // tim_changeSpeed,
351  tim_start_iteration);
352  }
353  return;
354  }
355 
356  // ------- start of motion decision zone ---------
357  executionTime.Tic();
358 
359  // Round #1: As usual, pure reactive, evaluate all PTGs and all
360  // directions from scratch.
361  // =========
362 
363  mrpt::math::TPose2D rel_pose_PTG_origin_wrt_sense(0, 0, 0),
364  relPoseSense(0, 0, 0), relPoseVelCmd(0, 0, 0);
366  {
367  /*
368  * Delays model
369  *
370  * Event: OBSTACLES_SENSED RNAV_ITERATION_STARTS
371  * GET_ROBOT_POSE_VEL VEL_CMD_SENT_TO_ROBOT
372  * Timestamp: (m_WS_Obstacles_timestamp) (tim_start_iteration)
373  * (m_curPoseVelTimestamp) ("tim_send_cmd_vel")
374  * Delay |
375  * <---+--------------->|<--------------+-------->| |
376  * estimator: | |
377  * | |
378  * timoff_obstacles <-+ |
379  * +--> timoff_curPoseVelAge |
380  * |<---------------------------------+--------------->|
381  * +-->
382  * timoff_sendVelCmd_avr (estimation)
383  *
384  * |<-------------------->|
385  * tim_changeSpeed_avr
386  * (estim)
387  *
388  * |<-----------------------------------------------|-------------------------->|
389  * Relative poses: relPoseSense
390  * relPoseVelCmd
391  * Time offsets (signed): timoff_pose2sense
392  * timoff_pose2VelCmd
393  */
394  const double timoff_obstacles = mrpt::system::timeDifference(
395  tim_start_iteration, m_WS_Obstacles_timestamp);
396  timoff_obstacles_avr.filter(timoff_obstacles);
397  newLogRec.values["timoff_obstacles"] = timoff_obstacles;
398  newLogRec.values["timoff_obstacles_avr"] =
400  newLogRec.timestamps["obstacles"] = m_WS_Obstacles_timestamp;
401 
402  const double timoff_curPoseVelAge = mrpt::system::timeDifference(
403  tim_start_iteration, m_curPoseVel.timestamp);
404  timoff_curPoseAndSpeed_avr.filter(timoff_curPoseVelAge);
405  newLogRec.values["timoff_curPoseVelAge"] = timoff_curPoseVelAge;
406  newLogRec.values["timoff_curPoseVelAge_avr"] =
408 
409  // time offset estimations:
410  const double timoff_pose2sense =
411  timoff_obstacles - timoff_curPoseVelAge;
412 
413  double timoff_pose2VelCmd;
414  timoff_pose2VelCmd = timoff_sendVelCmd_avr.getLastOutput() +
416  timoff_curPoseVelAge;
417  newLogRec.values["timoff_pose2sense"] = timoff_pose2sense;
418  newLogRec.values["timoff_pose2VelCmd"] = timoff_pose2VelCmd;
419 
420  if (std::abs(timoff_pose2sense) > 1.25)
422  "timoff_pose2sense=%e is too large! Path extrapolation may "
423  "be not accurate.",
424  timoff_pose2sense);
425  if (std::abs(timoff_pose2VelCmd) > 1.25)
427  "timoff_pose2VelCmd=%e is too large! Path extrapolation "
428  "may be not accurate.",
429  timoff_pose2VelCmd);
430 
431  // Path extrapolation: robot relative poses along current path
432  // estimation:
433  relPoseSense = m_curPoseVel.velLocal * timoff_pose2sense;
434  relPoseVelCmd = m_curPoseVel.velLocal * timoff_pose2VelCmd;
435 
436  // relative pose for PTGs:
437  rel_pose_PTG_origin_wrt_sense = relPoseVelCmd - relPoseSense;
438 
439  // logging:
440  newLogRec.relPoseSense = relPoseSense;
441  newLogRec.relPoseVelCmd = relPoseVelCmd;
442  }
443  else
444  {
445  // No delays model: poses to their default values.
446  }
447 
448  for (auto& t : targets)
449  {
450  if (t.target_frame_id != m_curPoseVel.pose_frame_id)
451  {
452  auto frame_tf = m_frame_tf.lock();
453  if (!frame_tf)
454  {
456  "Different frame_id's but no frame_tf object "
457  "attached (or it expired)!: target_frame_id=`%s` != "
458  "pose_frame_id=`%s`",
459  t.target_frame_id.c_str(),
460  m_curPoseVel.pose_frame_id.c_str());
461  }
462  mrpt::math::TPose2D robot_frame2map_frame;
463  frame_tf->lookupTransform(
464  t.target_frame_id, m_curPoseVel.pose_frame_id,
465  robot_frame2map_frame);
466 
468  "frame_tf: target_frame_id=`%s` as seen from "
469  "pose_frame_id=`%s` = %s",
470  t.target_frame_id.c_str(),
471  m_curPoseVel.pose_frame_id.c_str(),
472  robot_frame2map_frame.asString().c_str());
473 
474  t.target_coords = robot_frame2map_frame + t.target_coords;
475  t.target_frame_id =
476  m_curPoseVel.pose_frame_id; // Now the coordinates are in
477  // the same frame than robot
478  // pose
479  }
480  }
481 
482  std::vector<TPose2D> relTargets;
483  const auto curPoseExtrapol = (m_curPoseVel.pose + relPoseVelCmd);
485  targets.begin(), targets.end(), // in
486  std::back_inserter(relTargets), // out
487  [curPoseExtrapol](const CAbstractNavigator::TargetInfo& e) {
488  return e.target_coords - curPoseExtrapol;
489  });
490  ASSERT_EQUAL_(relTargets.size(), targets.size());
491 
492  // Use the distance to the first target as reference:
493  const double relTargetDist = relTargets.begin()->norm();
494 
495  // PTG dynamic state
496  /** Allow PTGs to be responsive to target location, dynamics, etc. */
498 
499  ptg_dynState.curVelLocal = m_curPoseVel.velLocal;
500  ptg_dynState.relTarget = relTargets[0];
501  ptg_dynState.targetRelSpeed =
502  m_navigationParams->target.targetDesiredRelSpeed;
503 
504  newLogRec.navDynState = ptg_dynState;
505 
506  {
509  "CAbstractPTGBasedReactive::performNavigationStep().update_PTG_"
510  "dynamic_states");
511 
512  for (size_t i = 0; i < nPTGs; i++)
513  getPTG(i)->updateNavDynamicState(ptg_dynState);
514  }
515 
516  m_infoPerPTG.assign(nPTGs + 1, TInfoPerPTG()); // reset contents
517  m_infoPerPTG_timestamp = tim_start_iteration;
518  vector<TCandidateMovementPTG> candidate_movs(
519  nPTGs + 1); // the last extra one is for the evaluation of "NOP
520  // motion command" choice.
521 
522  for (size_t indexPTG = 0; indexPTG < nPTGs; indexPTG++)
523  {
526  "CAbstractPTGBasedReactive::performNavigationStep().eval_"
527  "regular_PTG");
528 
529  CParameterizedTrajectoryGenerator* ptg = getPTG(indexPTG);
530  TInfoPerPTG& ipf = m_infoPerPTG[indexPTG];
531 
532  // Ensure the method knows about its associated PTG:
533  auto holoMethod = this->getHoloMethod(indexPTG);
534  ASSERT_(holoMethod);
535  holoMethod->setAssociatedPTG(this->getPTG(indexPTG));
536 
537  // The picked movement in TP-Space (to be determined by holonomic
538  // method below)
539  TCandidateMovementPTG& cm = candidate_movs[indexPTG];
540 
543  ptg, indexPTG, relTargets, rel_pose_PTG_origin_wrt_sense, ipf,
544  cm, newLogRec, false /* this is a regular PTG reactive case */,
545  *holoMethod, tim_start_iteration, *m_navigationParams);
546  } // end for each PTG
547 
548  // check for collision, which is reflected by ALL TP-Obstacles being
549  // zero:
550  bool is_all_ptg_collision = true;
551  for (size_t indexPTG = 0; indexPTG < nPTGs; indexPTG++)
552  {
553  bool is_collision = true;
554  const auto& obs = m_infoPerPTG[indexPTG].TP_Obstacles;
555  for (const auto o : obs)
556  {
557  if (o != 0)
558  {
559  is_collision = false;
560  break;
561  }
562  }
563  if (!is_collision)
564  {
565  is_all_ptg_collision = false;
566  break;
567  }
568  }
569  if (is_all_ptg_collision)
570  {
571  m_pending_events.emplace_back(std::bind(
573  std::ref(m_robot)));
574  }
575 
576  // Round #2: Evaluate dont sending any new velocity command ("NOP"
577  // motion)
578  // =========
579  bool NOP_not_too_old = true;
580  bool NOP_not_too_close_and_have_to_slowdown = true;
581  double NOP_max_time = -1.0, NOP_At = -1.0;
582  double slowdowndist = .0;
583  CParameterizedTrajectoryGenerator* last_sent_ptg =
586  : nullptr;
587  if (last_sent_ptg)
588  {
589  // So supportSpeedAtTarget() below is evaluated in the correct
590  // context:
592  }
593 
594  // This approach is only possible if:
595  const bool can_do_nop_motion =
597  !target_changed_since_last_iteration && last_sent_ptg &&
598  last_sent_ptg->supportVelCmdNOP()) &&
599  (NOP_not_too_old =
601  m_lastSentVelCmd.tim_send_cmd_vel, tim_start_iteration)) <
602  (NOP_max_time =
603  last_sent_ptg->maxTimeInVelCmdNOP(
605  std::max(0.1, m_lastSentVelCmd.speed_scale))) &&
606  (NOP_not_too_close_and_have_to_slowdown =
607  (last_sent_ptg->supportSpeedAtTarget() ||
608  (relTargetDist >
610  ->getTargetApproachSlowDownDistance())
611  // slowdowndist is assigned here, inside the if()
612  // to be sure the index in m_lastSentVelCmd is valid!
613  )));
614 
615  if (!NOP_not_too_old)
616  {
617  newLogRec.additional_debug_msgs["PTG_cont"] = mrpt::format(
618  "PTG-continuation not allowed: previous command timed-out "
619  "(At=%.03f > Max_At=%.03f)",
620  NOP_At, NOP_max_time);
621  }
622  if (!NOP_not_too_close_and_have_to_slowdown)
623  {
624  newLogRec.additional_debug_msgs["PTG_cont_trgdst"] = mrpt::format(
625  "PTG-continuation not allowed: target too close and must start "
626  "slow-down (trgDist=%.03f < SlowDownDist=%.03f)",
627  relTargetDist, slowdowndist);
628  }
629 
630  mrpt::math::TPose2D rel_cur_pose_wrt_last_vel_cmd_NOP(0, 0, 0),
631  rel_pose_PTG_origin_wrt_sense_NOP(0, 0, 0);
632 
633  if (can_do_nop_motion)
634  {
637  "CAbstractPTGBasedReactive::performNavigationStep().eval_NOP");
638 
639  // Add the estimation of how long it takes to run the changeSpeeds()
640  // callback (usually a tiny period):
641  const mrpt::system::TTimeStamp tim_send_cmd_vel_corrected =
645 
646  // Note: we use (uncorrected) raw odometry as basis to the following
647  // calculation since it's normally
648  // smoother than particle filter-based localization data, more
649  // accurate in the middle/long term,
650  // but not in the short term:
651  mrpt::math::TPose2D robot_pose_at_send_cmd, robot_odom_at_send_cmd;
652  bool valid_odom, valid_pose;
653 
655  tim_send_cmd_vel_corrected, robot_odom_at_send_cmd, valid_odom);
657  tim_send_cmd_vel_corrected, robot_pose_at_send_cmd, valid_pose);
658 
659  if (valid_odom && valid_pose)
660  {
661  ASSERT_(last_sent_ptg != nullptr);
662 
663  std::vector<TPose2D> relTargets_NOPs;
665  targets.begin(), targets.end(), // in
666  std::back_inserter(relTargets_NOPs), // out
667  [robot_pose_at_send_cmd](
669  return e.target_coords - robot_pose_at_send_cmd;
670  });
671  ASSERT_EQUAL_(relTargets_NOPs.size(), targets.size());
672 
673  rel_pose_PTG_origin_wrt_sense_NOP =
674  robot_odom_at_send_cmd -
675  (m_curPoseVel.rawOdometry + relPoseSense);
676  rel_cur_pose_wrt_last_vel_cmd_NOP =
677  m_curPoseVel.rawOdometry - robot_odom_at_send_cmd;
678 
679  // Update PTG response to dynamic params:
680  last_sent_ptg->updateNavDynamicState(
682 
683  if (fill_log_record)
684  {
685  newLogRec.additional_debug_msgs
686  ["rel_cur_pose_wrt_last_vel_cmd_NOP(interp)"] =
687  rel_cur_pose_wrt_last_vel_cmd_NOP.asString();
688  newLogRec.additional_debug_msgs
689  ["robot_odom_at_send_cmd(interp)"] =
690  robot_odom_at_send_cmd.asString();
691  }
692 
693  // No need to call setAssociatedPTG(), already correctly
694  // associated above.
695 
698  last_sent_ptg, m_lastSentVelCmd.ptg_index, relTargets_NOPs,
699  rel_pose_PTG_origin_wrt_sense_NOP, m_infoPerPTG[nPTGs],
700  candidate_movs[nPTGs], newLogRec,
701  true /* this is the PTG continuation (NOP) choice */,
703  tim_start_iteration, *m_navigationParams,
704  rel_cur_pose_wrt_last_vel_cmd_NOP);
705 
706  } // end valid interpolated origin pose
707  else
708  {
709  // Can't interpolate pose, hence can't evaluate NOP:
710  candidate_movs[nPTGs].speed =
711  -0.01; // <0 means inviable movement
712  }
713  } // end can_do_NOP_motion
714 
715  // Evaluate all the candidates and pick the "best" one, using
716  // the user-defined multiobjective optimizer
717  // --------------------------------------------------------------
720  int best_ptg_idx = m_multiobjopt->decide(candidate_movs, mo_info);
721 
722  if (fill_log_record)
723  {
724  if (mo_info.final_evaluation.size() == newLogRec.infoPerPTG.size())
725  {
726  for (unsigned int i = 0; i < newLogRec.infoPerPTG.size(); i++)
727  {
728  newLogRec.infoPerPTG[i].evaluation =
729  mo_info.final_evaluation[i];
730  }
731  }
732  int idx = 0;
733  for (const auto& le : mo_info.log_entries)
734  {
736  "MultiObjMotOptmzr_msg%03i", idx++)] = le;
737  }
738  }
739 
740  // Pick best movement (or null if none is good)
741  const TCandidateMovementPTG* selectedHolonomicMovement = nullptr;
742  if (best_ptg_idx >= 0)
743  {
744  selectedHolonomicMovement = &candidate_movs[best_ptg_idx];
745  }
746 
747  // If the selected PTG is (N+1), it means the NOP cmd. vel is selected
748  // as the best alternative, i.e. do NOT send any new motion command.
749  const bool best_is_NOP_cmdvel = (best_ptg_idx == int(nPTGs));
750 
751  // ---------------------------------------------------------------------
752  // SEND MOVEMENT COMMAND TO THE ROBOT
753  // ---------------------------------------------------------------------
755  if (best_is_NOP_cmdvel)
756  {
757  // Notify the robot that we want it to keep executing the last
758  // cmdvel:
759  if (!this->changeSpeedsNOP())
760  {
762  "\nERROR calling changeSpeedsNOP()!! Stopping robot and "
763  "finishing navigation\n");
764  if (fill_log_record)
765  {
767  newLogRec, relTargets, best_ptg_idx,
768  m_robot.getEmergencyStopCmd(), nPTGs,
769  best_is_NOP_cmdvel, rel_cur_pose_wrt_last_vel_cmd_NOP,
770  rel_pose_PTG_origin_wrt_sense_NOP,
771  0, // executionTimeValue,
772  0, // tim_changeSpeed,
773  tim_start_iteration);
774  }
775  return;
776  }
777  }
778  else
779  {
780  // Make sure the dynamic state of a NOP cmd has not overwritten the
781  // state for a "regular" PTG choice:
782  for (size_t i = 0; i < nPTGs; i++)
783  {
784  getPTG(i)->updateNavDynamicState(ptg_dynState);
785  }
786 
787  // STEP7: Get the non-holonomic movement command.
788  // ---------------------------------------------------------------------
789  double cmd_vel_speed_ratio = 1.0;
790  if (selectedHolonomicMovement)
791  {
792  CTimeLoggerEntry tle2(
793  m_timelogger, "navigationStep.selectedHolonomicMovement");
794  cmd_vel_speed_ratio =
795  generate_vel_cmd(*selectedHolonomicMovement, new_vel_cmd);
796  ASSERT_(new_vel_cmd);
797  }
798 
799  if (!new_vel_cmd /* which means best_PTG_eval==.0*/ ||
800  new_vel_cmd->isStopCmd())
801  {
803  "Best velocity command is STOP (no way found), calling "
804  "robot.stop()");
805  this->stop(true /* emergency */); // don't call
806  // doEmergencyStop() here
807  // since that will stop
808  // navigation completely
809  new_vel_cmd = m_robot.getEmergencyStopCmd();
811  }
812  else
813  {
814  mrpt::system::TTimeStamp tim_send_cmd_vel;
815  {
817  m_timlog_delays, "changeSpeeds()");
818  tim_send_cmd_vel = mrpt::system::now();
819  newLogRec.timestamps["tim_send_cmd_vel"] = tim_send_cmd_vel;
820  if (!this->changeSpeeds(*new_vel_cmd))
821  {
823  "\nERROR calling changeSpeeds()!! Stopping robot "
824  "and finishing navigation\n");
825  if (fill_log_record)
826  {
827  new_vel_cmd = m_robot.getEmergencyStopCmd();
829  newLogRec, relTargets, best_ptg_idx,
830  new_vel_cmd, nPTGs, best_is_NOP_cmdvel,
831  rel_cur_pose_wrt_last_vel_cmd_NOP,
832  rel_pose_PTG_origin_wrt_sense_NOP,
833  0, // executionTimeValue,
834  0, // tim_changeSpeed,
835  tim_start_iteration);
836  }
837  return;
838  }
839  }
840  // Save last sent cmd:
841  m_lastSentVelCmd.speed_scale = cmd_vel_speed_ratio;
842  m_lastSentVelCmd.ptg_index = best_ptg_idx;
844  selectedHolonomicMovement
845  ? selectedHolonomicMovement->PTG->alpha2index(
846  selectedHolonomicMovement->direction)
847  : 0;
849  selectedHolonomicMovement->props["holo_stage_eval"];
850 
852  best_ptg_idx >= 0
853  ? m_infoPerPTG[best_ptg_idx]
854  .TP_Obstacles[m_lastSentVelCmd.ptg_alpha_index]
855  : .0;
857  (selectedHolonomicMovement->props["is_slowdown"] != 0.0);
858 
860  m_lastSentVelCmd.tim_send_cmd_vel = tim_send_cmd_vel;
861  m_lastSentVelCmd.ptg_dynState = ptg_dynState;
862 
863  // Update delay model:
864  const double timoff_sendVelCmd = mrpt::system::timeDifference(
865  tim_start_iteration, tim_send_cmd_vel);
866  timoff_sendVelCmd_avr.filter(timoff_sendVelCmd);
867  newLogRec.values["timoff_sendVelCmd"] = timoff_sendVelCmd;
868  newLogRec.values["timoff_sendVelCmd_avr"] =
870  }
871  }
872 
873  // ------- end of motion decision zone ---------
874 
875  // Statistics:
876  // ----------------------------------------------------
877  const double executionTimeValue = executionTime.Tac();
878  meanExecutionTime.filter(executionTimeValue);
880 
881  const double tim_changeSpeed =
882  m_timlog_delays.getLastTime("changeSpeeds()");
883  tim_changeSpeed_avr.filter(tim_changeSpeed);
884 
885  // Running period estim:
886  const double period_tim = timerForExecutionPeriod.Tac();
887  if (period_tim > 1.5 * meanExecutionPeriod.getLastOutput())
888  {
890  "Timing warning: Suspicious executionPeriod=%.03f ms is far "
891  "above the average of %.03f ms",
892  1e3 * period_tim, meanExecutionPeriod.getLastOutput() * 1e3);
893  }
894  meanExecutionPeriod.filter(period_tim);
896 
898  {
900  "CMD: %s "
901  "speedScale=%.04f "
902  "T=%.01lfms Exec:%.01lfms|%.01lfms "
903  "PTG#%i\n",
904  new_vel_cmd ? new_vel_cmd->asString().c_str() : "NOP",
905  selectedHolonomicMovement ? selectedHolonomicMovement->speed
906  : .0,
908  1000.0 * meanExecutionTime.getLastOutput(),
909  1000.0 * meanTotalExecutionTime.getLastOutput(), best_ptg_idx));
910  }
911  if (fill_log_record)
912  {
914  newLogRec, relTargets, best_ptg_idx, new_vel_cmd, nPTGs,
915  best_is_NOP_cmdvel, rel_cur_pose_wrt_last_vel_cmd_NOP,
916  rel_pose_PTG_origin_wrt_sense_NOP, executionTimeValue,
917  tim_changeSpeed, tim_start_iteration);
918  }
919  }
920  catch (const std::exception& e)
921  {
923  std::string("[CAbstractPTGBasedReactive::performNavigationStep] "
924  "Stopping robot and finishing navigation due to "
925  "exception:\n") +
926  std::string(e.what()));
927  }
928  catch (...)
929  {
931  "[CAbstractPTGBasedReactive::performNavigationStep] Stopping robot "
932  "and finishing navigation due to untyped exception.");
933  }
934 }
935 
936 /** \callergraph */
938  CLogFileRecord& newLogRec, const std::vector<TPose2D>& relTargets,
939  int nSelectedPTG, const mrpt::kinematics::CVehicleVelCmd::Ptr& new_vel_cmd,
940  const int nPTGs, const bool best_is_NOP_cmdvel,
941  const mrpt::math::TPose2D& rel_cur_pose_wrt_last_vel_cmd_NOP,
942  const mrpt::math::TPose2D& rel_pose_PTG_origin_wrt_sense_NOP,
943  const double executionTimeValue, const double tim_changeSpeed,
944  const mrpt::system::TTimeStamp& tim_start_iteration)
945 {
946  // ---------------------------------------
947  // STEP8: Generate log record
948  // ---------------------------------------
950  m_navProfiler, "CAbstractPTGBasedReactive::STEP8_GenerateLogRecord()");
951 
952  m_timelogger.enter("navigationStep.populate_log_info");
953 
954  this->loggingGetWSObstaclesAndShape(newLogRec);
955 
958  newLogRec.WS_targets_relative = relTargets;
959  newLogRec.nSelectedPTG = nSelectedPTG;
960  newLogRec.cur_vel = m_curPoseVel.velGlobal;
961  newLogRec.cur_vel_local = m_curPoseVel.velLocal;
962  newLogRec.cmd_vel = new_vel_cmd;
963  newLogRec.values["estimatedExecutionPeriod"] =
965  newLogRec.values["executionTime"] = executionTimeValue;
966  newLogRec.values["executionTime_avr"] = meanExecutionTime.getLastOutput();
967  newLogRec.values["time_changeSpeeds()"] = tim_changeSpeed;
968  newLogRec.values["time_changeSpeeds()_avr"] =
970  newLogRec.values["CWaypointsNavigator::navigationStep()"] =
971  m_timlog_delays.getLastTime("CWaypointsNavigator::navigationStep()");
972  newLogRec.values["CAbstractNavigator::navigationStep()"] =
973  m_timlog_delays.getLastTime("CAbstractNavigator::navigationStep()");
974  newLogRec.timestamps["tim_start_iteration"] = tim_start_iteration;
975  newLogRec.timestamps["curPoseAndVel"] = m_curPoseVel.timestamp;
976  newLogRec.nPTGs = nPTGs;
977 
978  // NOP mode stuff:
980  rel_cur_pose_wrt_last_vel_cmd_NOP;
982  rel_pose_PTG_origin_wrt_sense_NOP;
983  newLogRec.ptg_index_NOP =
984  best_is_NOP_cmdvel ? m_lastSentVelCmd.ptg_index : -1;
987 
988  m_timelogger.leave("navigationStep.populate_log_info");
989 
990  // Save to log file:
991  // --------------------------------------
992  {
994  m_timelogger, "navigationStep.write_log_file");
995  if (m_logFile) archiveFrom(*m_logFile) << newLogRec;
996  }
997  // Set as last log record
998  {
999  std::lock_guard<std::recursive_mutex> lock_log(
1000  m_critZoneLastLog); // Lock
1001  lastLogRecord = newLogRec; // COPY
1002  }
1003 }
1004 
1005 /** \callergraph */
1007  TCandidateMovementPTG& cm, const std::vector<double>& in_TPObstacles,
1008  const mrpt::nav::ClearanceDiagram& in_clearance,
1009  const std::vector<mrpt::math::TPose2D>& WS_Targets,
1010  const std::vector<CAbstractPTGBasedReactive::PTGTarget>& TP_Targets,
1012  const bool this_is_PTG_continuation,
1013  const mrpt::math::TPose2D& rel_cur_pose_wrt_last_vel_cmd_NOP,
1014  const unsigned int ptg_idx4weights,
1015  const mrpt::system::TTimeStamp tim_start_iteration,
1017 {
1018  MRPT_START;
1020  m_navProfiler,
1021  "CAbstractPTGBasedReactive::calc_move_candidate_scores()");
1022 
1023  const double ref_dist = cm.PTG->getRefDistance();
1024 
1025  // Replaced by: TP_Targets[i].*
1026  // const double target_dir = (TP_Target.x!=0 || TP_Target.y!=0) ?
1027  // atan2( TP_Target.y, TP_Target.x) : 0.0;
1028  // const int target_k = static_cast<int>( cm.PTG->alpha2index(
1029  // target_dir ) );
1030  // const double target_d_norm = TP_Target.norm();
1031 
1032  // We need to evaluate the movement wrt to ONE target of the possibly many
1033  // input ones.
1034  // Policy: use the target whose TP-Space direction is closer to this
1035  // candidate direction:
1036  size_t selected_trg_idx = 0;
1037  {
1038  double best_trg_angdist = std::numeric_limits<double>::max();
1039  for (size_t i = 0; i < TP_Targets.size(); i++)
1040  {
1041  const double angdist = std::abs(mrpt::math::angDistance(
1042  TP_Targets[i].target_alpha, cm.direction));
1043  if (angdist < best_trg_angdist)
1044  {
1045  best_trg_angdist = angdist;
1046  selected_trg_idx = i;
1047  }
1048  }
1049  }
1050  ASSERT_(selected_trg_idx < WS_Targets.size());
1051  const auto WS_Target = WS_Targets[selected_trg_idx];
1052  const auto TP_Target = TP_Targets[selected_trg_idx];
1053 
1054  const double target_d_norm = TP_Target.target_dist;
1055 
1056  // Picked movement direction:
1057  const int move_k = static_cast<int>(cm.PTG->alpha2index(cm.direction));
1058  const double target_WS_d = WS_Target.norm();
1059 
1060  // Coordinates of the trajectory end for the given PTG and "alpha":
1061  const double d = std::min(in_TPObstacles[move_k], 0.99 * target_d_norm);
1062  uint32_t nStep;
1063  bool pt_in_range = cm.PTG->getPathStepForDist(move_k, d, nStep);
1064  ASSERT_(pt_in_range);
1065  mrpt::math::TPose2D pose;
1066  cm.PTG->getPathPose(move_k, nStep, pose);
1067 
1068  // Make sure that the target slow-down is honored, as seen in real-world
1069  // Euclidean space
1070  // (as opposed to TP-Space, where the holo methods are evaluated)
1071  if (m_navigationParams &&
1072  m_navigationParams->target.targetDesiredRelSpeed < 1.0 &&
1073  !m_holonomicMethod.empty() && getHoloMethod(0) != nullptr &&
1074  !cm.PTG->supportSpeedAtTarget() // If the PTG is able to handle the
1075  // slow-down on its own, dont change
1076  // speed here
1077  )
1078  {
1079  const double TARGET_SLOW_APPROACHING_DISTANCE =
1081 
1082  const double Vf =
1083  m_navigationParams->target.targetDesiredRelSpeed; // [0,1]
1084 
1085  const double f = std::min(
1086  1.0,
1087  Vf + target_WS_d * (1.0 - Vf) / TARGET_SLOW_APPROACHING_DISTANCE);
1088  if (f < cm.speed)
1089  {
1090  newLogRec.additional_debug_msgs["PTG_eval.speed"] = mrpt::format(
1091  "Relative speed reduced %.03f->%.03f based on Euclidean "
1092  "nearness to target.",
1093  cm.speed, f);
1094  cm.speed = f;
1095  }
1096  }
1097 
1098  // Start storing params in the candidate move structure:
1099  cm.props["ptg_idx"] = ptg_idx4weights;
1100  cm.props["ref_dist"] = ref_dist;
1101  cm.props["target_dir"] = TP_Target.target_alpha;
1102  cm.props["target_k"] = TP_Target.target_k;
1103  cm.props["target_d_norm"] = target_d_norm;
1104  cm.props["move_k"] = move_k;
1105  double& move_cur_d = cm.props["move_cur_d"] =
1106  0; // current robot path normalized distance over path (0 unless in a
1107  // NOP cmd)
1108  cm.props["is_PTG_cont"] = this_is_PTG_continuation ? 1 : 0;
1109  cm.props["num_paths"] = in_TPObstacles.size();
1110  cm.props["WS_target_x"] = WS_Target.x;
1111  cm.props["WS_target_y"] = WS_Target.y;
1112  cm.props["robpose_x"] = pose.x;
1113  cm.props["robpose_y"] = pose.y;
1114  cm.props["robpose_phi"] = pose.phi;
1115  cm.props["ptg_priority"] =
1116  cm.PTG->getScorePriority() *
1117  cm.PTG->evalPathRelativePriority(TP_Target.target_k, target_d_norm);
1118  const bool is_slowdown =
1119  this_is_PTG_continuation
1121  : (cm.PTG->supportSpeedAtTarget() && TP_Target.target_k == move_k &&
1122  target_d_norm < 0.99 * in_TPObstacles[move_k]);
1123  cm.props["is_slowdown"] = is_slowdown ? 1 : 0;
1124  cm.props["holo_stage_eval"] =
1125  this_is_PTG_continuation
1127  : (hlfr && !hlfr->dirs_eval.empty() &&
1128  hlfr->dirs_eval.rbegin()->size() == in_TPObstacles.size())
1129  ? hlfr->dirs_eval.rbegin()->at(move_k)
1130  : .0;
1131  // Factor 1: Free distance for the chosen PTG and "alpha" in the TP-Space:
1132  // ----------------------------------------------------------------------
1133  double& colfree = cm.props["collision_free_distance"];
1134 
1135  // distance to collision:
1136  colfree = in_TPObstacles[move_k]; // we'll next substract here the
1137  // already-traveled distance, for NOP
1138  // motion candidates.
1139 
1140  // Special case for NOP motion cmd:
1141  // consider only the empty space *after* the current robot pose, which is
1142  // not at the origin.
1143 
1144  if (this_is_PTG_continuation)
1145  {
1146  int cur_k = 0;
1147  double cur_norm_d = .0;
1148  bool is_exact, is_time_based = false;
1149  uint32_t cur_ptg_step = 0;
1150 
1151  // Use: time-based prediction for shorter distances, PTG inverse
1152  // mapping-based for longer ranges:
1153  const double maxD = params_abstract_ptg_navigator
1155  if (std::abs(rel_cur_pose_wrt_last_vel_cmd_NOP.x) > maxD ||
1156  std::abs(rel_cur_pose_wrt_last_vel_cmd_NOP.y) > maxD)
1157  {
1158  is_exact = cm.PTG->inverseMap_WS2TP(
1159  rel_cur_pose_wrt_last_vel_cmd_NOP.x,
1160  rel_cur_pose_wrt_last_vel_cmd_NOP.y, cur_k, cur_norm_d);
1161  }
1162  else
1163  {
1164  // Use time:
1165  is_time_based = true;
1166  is_exact = true; // well, sort of...
1167  const double NOP_At =
1170  m_lastSentVelCmd.tim_send_cmd_vel, tim_start_iteration);
1171  newLogRec.additional_debug_msgs["PTG_eval.NOP_At"] =
1172  mrpt::format("%.06f s", NOP_At);
1173  cur_k = move_k;
1174  cur_ptg_step = mrpt::round(NOP_At / cm.PTG->getPathStepDuration());
1175  cur_norm_d = cm.PTG->getPathDist(cur_k, cur_ptg_step) /
1176  cm.PTG->getRefDistance();
1177  {
1178  const double cur_a = cm.PTG->index2alpha(cur_k);
1179  log.TP_Robot.x = cos(cur_a) * cur_norm_d;
1180  log.TP_Robot.y = sin(cur_a) * cur_norm_d;
1181  cm.starting_robot_dir = cur_a;
1182  cm.starting_robot_dist = cur_norm_d;
1183  }
1184  }
1185 
1186  if (!is_exact)
1187  {
1188  // Don't trust this step: we are not 100% sure of the robot pose in
1189  // TP-Space for this "PTG continuation" step:
1190  cm.speed = -0.01; // this enforces a 0 global evaluation score
1191  newLogRec.additional_debug_msgs["PTG_eval"] =
1192  "PTG-continuation not allowed, cur. pose out of PTG domain.";
1193  return;
1194  }
1195  bool WS_point_is_unique = true;
1196  if (!is_time_based)
1197  {
1198  bool ok1 = cm.PTG->getPathStepForDist(
1200  cur_norm_d * cm.PTG->getRefDistance(), cur_ptg_step);
1201  if (ok1)
1202  {
1203  // Check bijective:
1204  WS_point_is_unique = cm.PTG->isBijectiveAt(cur_k, cur_ptg_step);
1205  const uint32_t predicted_step =
1208  mrpt::system::now()) /
1209  cm.PTG->getPathStepDuration();
1210  WS_point_is_unique =
1211  WS_point_is_unique &&
1212  cm.PTG->isBijectiveAt(move_k, predicted_step);
1213  newLogRec.additional_debug_msgs["PTG_eval.bijective"] =
1214  mrpt::format(
1215  "isBijectiveAt(): k=%i step=%i -> %s",
1216  static_cast<int>(cur_k), static_cast<int>(cur_ptg_step),
1217  WS_point_is_unique ? "yes" : "no");
1218 
1219  if (!WS_point_is_unique)
1220  {
1221  // Don't trust direction:
1222  cur_k = move_k;
1223  cur_ptg_step = predicted_step;
1224  cur_norm_d = cm.PTG->getPathDist(cur_k, cur_ptg_step);
1225  }
1226  {
1227  const double cur_a = cm.PTG->index2alpha(cur_k);
1228  log.TP_Robot.x = cos(cur_a) * cur_norm_d;
1229  log.TP_Robot.y = sin(cur_a) * cur_norm_d;
1230  cm.starting_robot_dir = cur_a;
1231  cm.starting_robot_dist = cur_norm_d;
1232  }
1233 
1234  mrpt::math::TPose2D predicted_rel_pose;
1235  cm.PTG->getPathPose(
1236  m_lastSentVelCmd.ptg_alpha_index, cur_ptg_step,
1237  predicted_rel_pose);
1238  const auto predicted_pose_global =
1239  m_lastSentVelCmd.poseVel.rawOdometry + predicted_rel_pose;
1240  const double predicted2real_dist = mrpt::hypot_fast(
1241  predicted_pose_global.x - m_curPoseVel.rawOdometry.x,
1242  predicted_pose_global.y - m_curPoseVel.rawOdometry.y);
1243  newLogRec.additional_debug_msgs["PTG_eval.lastCmdPose(raw)"] =
1245  newLogRec.additional_debug_msgs["PTG_eval.PTGcont"] =
1246  mrpt::format(
1247  "mismatchDistance=%.03f cm", 1e2 * predicted2real_dist);
1248 
1249  if (predicted2real_dist >
1251  .max_distance_predicted_actual_path &&
1252  (!is_slowdown ||
1253  (target_d_norm - cur_norm_d) * ref_dist > 2.0 /*meters*/))
1254  {
1255  cm.speed =
1256  -0.01; // this enforces a 0 global evaluation score
1257  newLogRec.additional_debug_msgs["PTG_eval"] =
1258  "PTG-continuation not allowed, mismatchDistance above "
1259  "threshold.";
1260  return;
1261  }
1262  }
1263  else
1264  {
1265  cm.speed = -0.01; // this enforces a 0 global evaluation score
1266  newLogRec.additional_debug_msgs["PTG_eval"] =
1267  "PTG-continuation not allowed, couldn't get PTG step for "
1268  "cur. robot pose.";
1269  return;
1270  }
1271  }
1272 
1273  // Path following isn't perfect: we can't be 100% sure of whether the
1274  // robot followed exactly
1275  // the intended path (`kDirection`), or if it's actually a bit shifted,
1276  // as reported in `cur_k`.
1277  // Take the least favorable case.
1278  // [Update] Do this only when the PTG gave us a unique-mapped WS<->TPS
1279  // point:
1280 
1281  colfree = WS_point_is_unique
1282  ? std::min(in_TPObstacles[move_k], in_TPObstacles[cur_k])
1283  : in_TPObstacles[move_k];
1284 
1285  // Only discount free space if there was a real obstacle, not the "end
1286  // of path" due to limited refDistance.
1287  if (colfree < 0.99)
1288  {
1289  colfree -= cur_norm_d;
1290  }
1291 
1292  // Save estimated robot pose over path as a parameter for scores:
1293  move_cur_d = cur_norm_d;
1294  }
1295 
1296  // Factor4: Decrease in euclidean distance between (x,y) and the target:
1297  // Moving away of the target is negatively valued.
1298  cm.props["dist_eucl_final"] =
1299  mrpt::hypot_fast(WS_Target.x - pose.x, WS_Target.y - pose.y);
1300 
1301  // dist_eucl_min: Use PTG clearance methods to evaluate the real-world
1302  // (WorkSpace) minimum distance to target:
1303  {
1304  using map_d2d_t = std::map<double, double>;
1305  map_d2d_t pathDists;
1306  const double D = cm.PTG->getRefDistance();
1307  const int num_steps = ceil(D * 2.0);
1308  for (int i = 0; i < num_steps; i++)
1309  {
1310  pathDists[i / double(num_steps)] =
1311  100.0; // default normalized distance to target (a huge value)
1312  }
1313 
1315  WS_Target.x, WS_Target.y, move_k, pathDists,
1316  false /*treat point as target, not obstacle*/);
1317 
1318  const auto it = std::min_element(
1319  pathDists.begin(), pathDists.end(),
1320  [colfree](map_d2d_t::value_type& l, map_d2d_t::value_type& r)
1321  -> bool { return (l.second < r.second) && l.first < colfree; });
1322  cm.props["dist_eucl_min"] = (it != pathDists.end())
1323  ? it->second * cm.PTG->getRefDistance()
1324  : 100.0;
1325  }
1326 
1327  // Factor5: Hysteresis:
1328  // -----------------------------------------------------
1329  double& hysteresis = cm.props["hysteresis"];
1330  hysteresis = .0;
1331 
1332  if (cm.PTG->supportVelCmdNOP())
1333  {
1334  hysteresis = this_is_PTG_continuation ? 1.0 : 0.;
1335  }
1336  else if (m_last_vel_cmd)
1337  {
1339  desired_cmd = cm.PTG->directionToMotionCommand(move_k);
1341  const mrpt::kinematics::CVehicleVelCmd* ptr2 = desired_cmd.get();
1342  if (typeid(*ptr1) == typeid(*ptr2))
1343  {
1344  ASSERT_EQUAL_(
1345  m_last_vel_cmd->getVelCmdLength(),
1346  desired_cmd->getVelCmdLength());
1347 
1348  double simil_score = 0.5;
1349  for (size_t i = 0; i < desired_cmd->getVelCmdLength(); i++)
1350  {
1351  const double scr =
1352  exp(-std::abs(
1353  desired_cmd->getVelCmdElement(i) -
1354  m_last_vel_cmd->getVelCmdElement(i)) /
1355  0.20);
1356  mrpt::keep_min(simil_score, scr);
1357  }
1358  hysteresis = simil_score;
1359  }
1360  }
1361 
1362  // Factor6: clearance
1363  // -----------------------------------------------------
1364  // clearance indicators that may be useful in deciding the best motion:
1365  double& clearance = cm.props["clearance"];
1366  clearance = in_clearance.getClearance(
1367  move_k, target_d_norm * 1.01, false /* spot, dont interpolate */);
1368  cm.props["clearance_50p"] = in_clearance.getClearance(
1369  move_k, target_d_norm * 0.5, false /* spot, dont interpolate */);
1370  cm.props["clearance_path"] = in_clearance.getClearance(
1371  move_k, target_d_norm * 0.9, true /* average */);
1372  cm.props["clearance_path_50p"] = in_clearance.getClearance(
1373  move_k, target_d_norm * 0.5, true /* average */);
1374 
1375  // Factor: ETA (Estimated Time of Arrival to target or to closest obstacle,
1376  // whatever it's first)
1377  // -----------------------------------------------------
1378  double& eta = cm.props["eta"];
1379  eta = .0;
1380  if (cm.PTG && cm.speed > .0) // for valid cases only
1381  {
1382  // OK, we have a direct path to target without collisions.
1383  const double path_len_meters = d * ref_dist;
1384 
1385  // Calculate their ETA
1386  uint32_t target_step;
1387  bool valid_step =
1388  cm.PTG->getPathStepForDist(move_k, path_len_meters, target_step);
1389  if (valid_step)
1390  {
1391  eta = cm.PTG->getPathStepDuration() *
1392  target_step /* PTG original time to get to target point */
1393  * cm.speed /* times the speed scale factor*/;
1394 
1395  double discount_time = .0;
1396  if (this_is_PTG_continuation)
1397  {
1398  // Heuristic: discount the time already executed.
1399  // Note that hm.speed above scales the overall path time using
1400  // the current speed scale, not the exact
1401  // integration over the past timesteps. It's an approximation,
1402  // probably good enough...
1403  discount_time = mrpt::system::timeDifference(
1404  m_lastSentVelCmd.tim_send_cmd_vel, tim_start_iteration);
1405  }
1406  eta -= discount_time; // This could even become negative if the
1407  // approximation is poor...
1408  }
1409  }
1410 
1411  MRPT_END;
1412 }
1413 
1415  const TCandidateMovementPTG& in_movement,
1417 {
1418  mrpt::system::CTimeLoggerEntry tle(m_timelogger, "generate_vel_cmd");
1419  double cmdvel_speed_scale = 1.0;
1420  try
1421  {
1422  if (in_movement.speed == 0)
1423  {
1424  // The robot will stop:
1425  new_vel_cmd =
1427  new_vel_cmd->setToStop();
1428  }
1429  else
1430  {
1431  // Take the normalized movement command:
1432  new_vel_cmd = in_movement.PTG->directionToMotionCommand(
1433  in_movement.PTG->alpha2index(in_movement.direction));
1434 
1435  // Scale holonomic speeds to real-world one:
1436  new_vel_cmd->cmdVel_scale(in_movement.speed);
1437  cmdvel_speed_scale *= in_movement.speed;
1438 
1439  if (!m_last_vel_cmd) // first iteration? Use default values:
1440  m_last_vel_cmd =
1442 
1443  // Honor user speed limits & "blending":
1444  const double beta = meanExecutionPeriod.getLastOutput() /
1447  cmdvel_speed_scale *= new_vel_cmd->cmdVel_limits(
1448  *m_last_vel_cmd, beta,
1450  }
1451 
1452  m_last_vel_cmd = new_vel_cmd; // Save for filtering in next step
1453  }
1454  catch (const std::exception& e)
1455  {
1457  "[CAbstractPTGBasedReactive::generate_vel_cmd] Exception: "
1458  << e.what());
1459  }
1460  return cmdvel_speed_scale;
1461 }
1462 
1463 /** \callergraph */
1465  const mrpt::math::TPoint2D& wp) const
1466 {
1467  MRPT_START;
1468 
1469  const size_t N = this->getPTG_count();
1470  if (m_infoPerPTG.size() < N ||
1474  return false; // We didn't run yet or obstacle info is old
1475 
1476  for (size_t i = 0; i < N; i++)
1477  {
1478  const CParameterizedTrajectoryGenerator* ptg = getPTG(i);
1479 
1480  const std::vector<double>& tp_obs =
1481  m_infoPerPTG[i].TP_Obstacles; // normalized distances
1482  if (tp_obs.size() != ptg->getPathCount())
1483  continue; // May be this PTG has not been used so far? (Target out
1484  // of domain,...)
1485 
1486  int wp_k;
1487  double wp_norm_d;
1488  bool is_into_domain =
1489  ptg->inverseMap_WS2TP(wp.x, wp.y, wp_k, wp_norm_d);
1490  if (!is_into_domain) continue;
1491 
1492  ASSERT_(wp_k < int(tp_obs.size()));
1493 
1494  const double collision_free_dist = tp_obs[wp_k];
1495  if (collision_free_dist > 1.01 * wp_norm_d)
1496  return true; // free path found to target
1497  }
1498 
1499  return false; // no way found
1500  MRPT_END;
1501 }
1502 
1503 /** \callergraph */
1505 {
1507 }
1508 
1509 /** \callergraph */
1511 {
1514 
1515  CWaypointsNavigator::onStartNewNavigation(); // Call base method we
1516  // override
1517 }
1518 
1521 {
1522  ptg_index = -1;
1523  ptg_alpha_index = -1;
1524  tim_send_cmd_vel = INVALID_TIMESTAMP;
1525  poseVel = TRobotPoseVel();
1526  colfreedist_move_k = .0;
1527  was_slowdown = false;
1528  speed_scale = 1.0;
1529  original_holo_eval = .0;
1531 }
1533 {
1534  return this->poseVel.timestamp != INVALID_TIMESTAMP;
1535 }
1536 
1537 /** \callergraph */
1539  CParameterizedTrajectoryGenerator* ptg, const size_t indexPTG,
1540  const std::vector<mrpt::math::TPose2D>& relTargets,
1541  const mrpt::math::TPose2D& rel_pose_PTG_origin_wrt_sense, TInfoPerPTG& ipf,
1542  TCandidateMovementPTG& cm, CLogFileRecord& newLogRec,
1543  const bool this_is_PTG_continuation,
1545  const mrpt::system::TTimeStamp tim_start_iteration,
1546  const TNavigationParams& navp,
1547  const mrpt::math::TPose2D& rel_cur_pose_wrt_last_vel_cmd_NOP)
1548 {
1550  m_navProfiler, "CAbstractPTGBasedReactive::build_movement_candidate()");
1551 
1552  ASSERT_(ptg);
1553 
1554  const size_t idx_in_log_infoPerPTGs =
1555  this_is_PTG_continuation ? getPTG_count() : indexPTG;
1556 
1558  cm.PTG = ptg;
1559 
1560  // If the user doesn't want to use this PTG, just mark it as invalid:
1561  ipf.targets.clear();
1562  bool use_this_ptg = true;
1563  {
1564  const auto* navpPTG = dynamic_cast<const TNavigationParamsPTG*>(&navp);
1565  if (navpPTG && !navpPTG->restrict_PTG_indices.empty())
1566  {
1567  use_this_ptg = false;
1568  for (size_t i = 0;
1569  i < navpPTG->restrict_PTG_indices.size() && !use_this_ptg; i++)
1570  {
1571  if (navpPTG->restrict_PTG_indices[i] == indexPTG)
1572  use_this_ptg = true;
1573  }
1574  }
1575  }
1576 
1577  double timeForTPObsTransformation = .0, timeForHolonomicMethod = .0;
1578 
1579  // Normal PTG validity filter: check if target falls into the PTG domain:
1580  bool any_TPTarget_is_valid = false;
1581  if (use_this_ptg)
1582  {
1583  for (const auto& trg : relTargets)
1584  {
1585  PTGTarget ptg_target;
1586 
1587  ptg_target.valid_TP = ptg->inverseMap_WS2TP(
1588  trg.x, trg.y, ptg_target.target_k, ptg_target.target_dist);
1589  if (!ptg_target.valid_TP) continue;
1590 
1591  any_TPTarget_is_valid = true;
1592  ptg_target.target_alpha = ptg->index2alpha(ptg_target.target_k);
1593  ptg_target.TP_Target.x =
1594  cos(ptg_target.target_alpha) * ptg_target.target_dist;
1595  ptg_target.TP_Target.y =
1596  sin(ptg_target.target_alpha) * ptg_target.target_dist;
1597 
1598  ipf.targets.emplace_back(ptg_target);
1599  }
1600  }
1601 
1602  if (!any_TPTarget_is_valid)
1603  {
1605  "mov_candidate_%u", static_cast<unsigned int>(indexPTG))] =
1606  "PTG discarded since target(s) is(are) out of domain.";
1607  }
1608  else
1609  {
1610  // STEP3(b): Build TP-Obstacles
1611  // -----------------------------------------------------------------------------
1612  {
1613  tictac.Tic();
1614 
1615  // Initialize TP-Obstacles:
1616  const size_t Ki = ptg->getAlphaValuesCount();
1617  ptg->initTPObstacles(ipf.TP_Obstacles);
1619  {
1620  ptg->initClearanceDiagram(ipf.clearance);
1621  }
1622 
1623  // Implementation-dependent conversion:
1625  indexPTG, ipf.TP_Obstacles, ipf.clearance,
1626  mrpt::math::TPose2D(0, 0, 0) - rel_pose_PTG_origin_wrt_sense,
1628 
1630  {
1632  }
1633 
1634  // Distances in TP-Space are normalized to [0,1]:
1635  const double _refD = 1.0 / ptg->getRefDistance();
1636  for (size_t i = 0; i < Ki; i++) ipf.TP_Obstacles[i] *= _refD;
1637 
1638  timeForTPObsTransformation = tictac.Tac();
1639  if (m_timelogger.isEnabled())
1641  "navigationStep.STEP3_WSpaceToTPSpace",
1642  timeForTPObsTransformation);
1643  }
1644 
1645  // STEP4: Holonomic navigation method
1646  // -----------------------------------------------------------------------------
1647  if (!this_is_PTG_continuation)
1648  {
1649  tictac.Tic();
1650 
1651  // Slow down if we are approaching the final target, etc.
1652  holoMethod.enableApproachTargetSlowDown(
1653  navp.target.targetDesiredRelSpeed < .11);
1654 
1655  // Prepare holonomic algorithm call:
1657  ni.clearance = &ipf.clearance;
1658  ni.maxObstacleDist = 1.0;
1659  ni.maxRobotSpeed = 1.0; // So, we use a normalized max speed here.
1660  ni.obstacles = ipf.TP_Obstacles; // Normalized [0,1]
1661 
1662  ni.targets.clear(); // Normalized [0,1]
1663  for (const auto& t : ipf.targets)
1664  {
1665  ni.targets.push_back(t.TP_Target);
1666  }
1667 
1669 
1670  holoMethod.navigate(ni, no);
1671 
1672  // Extract resuls:
1673  cm.direction = no.desiredDirection;
1674  cm.speed = no.desiredSpeed;
1675  HLFR = no.logRecord;
1676 
1677  // Security: Scale down the velocity when heading towards obstacles,
1678  // such that it's assured that we never go thru an obstacle!
1679  const int kDirection =
1680  static_cast<int>(cm.PTG->alpha2index(cm.direction));
1681  double obsFreeNormalizedDistance = ipf.TP_Obstacles[kDirection];
1682 
1683  // Take into account the future robot pose after NOP motion
1684  // iterations to slow down accordingly *now*
1685  if (ptg->supportVelCmdNOP())
1686  {
1687  const double v = mrpt::hypot_fast(
1689  const double d = v * ptg->maxTimeInVelCmdNOP(kDirection);
1690  obsFreeNormalizedDistance = std::min(
1691  obsFreeNormalizedDistance,
1692  std::max(0.90, obsFreeNormalizedDistance - d));
1693  }
1694 
1695  double velScale = 1.0;
1696  ASSERT_(
1699  if (obsFreeNormalizedDistance <
1701  {
1702  if (obsFreeNormalizedDistance <=
1704  velScale = 0.0; // security stop
1705  else
1706  velScale =
1707  (obsFreeNormalizedDistance -
1711  }
1712 
1713  // Scale:
1714  cm.speed *= velScale;
1715 
1716  timeForHolonomicMethod = tictac.Tac();
1717  if (m_timelogger.isEnabled())
1719  "navigationStep.STEP4_HolonomicMethod",
1720  timeForHolonomicMethod);
1721  }
1722  else
1723  {
1724  // "NOP cmdvel" case: don't need to re-run holo algorithm, just keep
1725  // the last selection:
1727  cm.speed = 1.0; // Not used.
1728  }
1729 
1730  // STEP5: Evaluate each movement to assign them a "evaluation" value.
1731  // ---------------------------------------------------------------------
1732  {
1733  CTimeLoggerEntry tle2(
1734  m_timelogger, "navigationStep.calc_move_candidate_scores");
1735 
1737  cm, ipf.TP_Obstacles, ipf.clearance, relTargets, ipf.targets,
1738  newLogRec.infoPerPTG[idx_in_log_infoPerPTGs], newLogRec,
1739  this_is_PTG_continuation, rel_cur_pose_wrt_last_vel_cmd_NOP,
1740  indexPTG, tim_start_iteration, HLFR);
1741 
1742  // Store NOP related extra vars:
1743  cm.props["original_col_free_dist"] =
1744  this_is_PTG_continuation ? m_lastSentVelCmd.colfreedist_move_k
1745  : .0;
1746 
1747  // SAVE LOG
1748  newLogRec.infoPerPTG[idx_in_log_infoPerPTGs].evalFactors = cm.props;
1749  }
1750 
1751  } // end "valid_TP"
1752 
1753  // Logging:
1754  const bool fill_log_record =
1755  (m_logFile != nullptr || m_enableKeepLogRecords);
1756  if (fill_log_record)
1757  {
1759  newLogRec.infoPerPTG[idx_in_log_infoPerPTGs];
1760  if (!this_is_PTG_continuation)
1761  ipp.PTG_desc = ptg->getDescription();
1762  else
1763  ipp.PTG_desc = mrpt::format(
1764  "NOP cmdvel (prev PTG idx=%u)",
1765  static_cast<unsigned int>(m_lastSentVelCmd.ptg_index));
1766 
1768  ipf.TP_Obstacles, ipp.TP_Obstacles);
1769  ipp.clearance = ipf.clearance;
1770  ipp.TP_Targets.clear();
1771  for (const auto& t : ipf.targets)
1772  {
1773  ipp.TP_Targets.push_back(t.TP_Target);
1774  }
1775  ipp.HLFR = HLFR;
1776  ipp.desiredDirection = cm.direction;
1777  ipp.desiredSpeed = cm.speed;
1778  ipp.timeForTPObsTransformation = timeForTPObsTransformation;
1779  ipp.timeForHolonomicMethod = timeForHolonomicMethod;
1780  }
1781 }
1782 
1785 {
1786  MRPT_START;
1787 
1788  robot_absolute_speed_limits.loadConfigFile(c, s);
1789 
1790  MRPT_LOAD_CONFIG_VAR_REQUIRED_CS(holonomic_method, string);
1791  MRPT_LOAD_CONFIG_VAR_REQUIRED_CS(motion_decider_method, string);
1792  MRPT_LOAD_CONFIG_VAR_REQUIRED_CS(ref_distance, double);
1793  MRPT_LOAD_CONFIG_VAR_CS(speedfilter_tau, double);
1794  MRPT_LOAD_CONFIG_VAR_CS(secure_distance_start, double);
1795  MRPT_LOAD_CONFIG_VAR_CS(secure_distance_end, double);
1796  MRPT_LOAD_CONFIG_VAR_CS(use_delays_model, bool);
1797  MRPT_LOAD_CONFIG_VAR_CS(max_distance_predicted_actual_path, double);
1799  min_normalized_free_space_for_ptg_continuation, double);
1800  MRPT_LOAD_CONFIG_VAR_CS(enable_obstacle_filtering, bool);
1801  MRPT_LOAD_CONFIG_VAR_CS(evaluate_clearance, bool);
1802  MRPT_LOAD_CONFIG_VAR_CS(max_dist_for_timebased_path_prediction, double);
1803 
1804  MRPT_END;
1805 }
1806 
1809 {
1810  robot_absolute_speed_limits.saveToConfigFile(c, s);
1811 
1812  // Build list of known holo methods:
1813  string lstHoloStr = "# List of known classes:\n";
1814  {
1817  for (const auto& cl : lst)
1818  lstHoloStr +=
1819  string("# - `") + string(cl->className) + string("`\n");
1820  }
1822  holonomic_method,
1823  string("C++ class name of the holonomic navigation method to run in "
1824  "the transformed TP-Space.\n") +
1825  lstHoloStr);
1826 
1827  // Build list of known decider methods:
1828  string lstDecidersStr = "# List of known classes:\n";
1829  {
1832  for (const auto& cl : lst)
1833  lstDecidersStr +=
1834  string("# - `") + string(cl->className) + string("`\n");
1835  }
1837  motion_decider_method,
1838  string("C++ class name of the motion decider method.\n") +
1839  lstDecidersStr);
1840 
1842  ref_distance,
1843  "Maximum distance up to obstacles will be considered (D_{max} in "
1844  "papers).");
1846  speedfilter_tau,
1847  "Time constant (in seconds) for the low-pass filter applied to "
1848  "kinematic velocity commands (default=0: no filtering)");
1850  secure_distance_start,
1851  "In normalized distance [0,1], start/end of a ramp function that "
1852  "scales the holonomic navigator output velocity.");
1854  secure_distance_end,
1855  "In normalized distance [0,1], start/end of a ramp function that "
1856  "scales the holonomic navigator output velocity.");
1858  use_delays_model,
1859  "Whether to use robot pose inter/extrapolation to improve accuracy "
1860  "(Default:false)");
1862  max_distance_predicted_actual_path,
1863  "Max distance [meters] to discard current PTG and issue a new vel cmd "
1864  "(default= 0.05)");
1866  min_normalized_free_space_for_ptg_continuation,
1867  "Min normalized dist [0,1] after current pose in a PTG continuation to "
1868  "allow it.");
1870  enable_obstacle_filtering,
1871  "Enabled obstacle filtering (params in its own section)");
1873  evaluate_clearance,
1874  "Enable exact computation of clearance (default=false)");
1876  max_dist_for_timebased_path_prediction,
1877  "Max dist [meters] to use time-based path prediction for NOP "
1878  "evaluation");
1879 }
1880 
1883  : holonomic_method(),
1884  ptg_cache_files_directory("."),
1885 
1886  robot_absolute_speed_limits()
1887 
1888 {
1889 }
1890 
1893 {
1894  MRPT_START;
1896 
1897  // At this point, we have been called from the derived class, who must be
1898  // already loaded all its specific params, including PTGs.
1899 
1900  // Load my params:
1902  c, "CAbstractPTGBasedReactive");
1903 
1904  // Filtering:
1906  {
1909  filter->options.loadFromConfigFile(c, "CPointCloudFilterByDistance");
1910  }
1911  else
1912  {
1913  m_WS_filter.reset();
1914  }
1915 
1916  // Movement chooser:
1919  if (!m_multiobjopt)
1921  "Non-registered CMultiObjectiveMotionOptimizerBase className=`%s`",
1923 
1924  m_multiobjopt->loadConfigFile(c);
1925 
1926  // Holo method:
1928  ASSERT_(!m_holonomicMethod.empty());
1929  CWaypointsNavigator::loadConfigFile(c); // Load parent params
1930 
1931  m_init_done =
1932  true; // If we reached this point without an exception, all is good.
1933  MRPT_END;
1934 }
1935 
1938 {
1941  c, "CAbstractPTGBasedReactive");
1942 
1943  // Filtering:
1944  {
1946  filter.options.saveToConfigFile(c, "CPointCloudFilterByDistance");
1947  }
1948 
1949  // Holo method:
1950  if (!m_holonomicMethod.empty() && m_holonomicMethod[0])
1951  {
1952  // Save my current settings:
1953  m_holonomicMethod[0]->saveConfigFile(c);
1954  }
1955  else
1956  {
1957  // save options of ALL known methods:
1960  for (const auto& cl : lst)
1961  {
1963  mrpt::rtti::CObject::Ptr(cl->createObject());
1964  auto* holo =
1965  dynamic_cast<CAbstractHolonomicReactiveMethod*>(obj.get());
1966  if (holo)
1967  {
1968  holo->saveConfigFile(c);
1969  }
1970  }
1971  }
1972 
1973  // Decider method:
1974  if (m_multiobjopt)
1975  {
1976  // Save my current settings:
1977  m_multiobjopt->saveConfigFile(c);
1978  }
1979  else
1980  {
1981  // save options of ALL known methods:
1984  for (const auto& cl : lst)
1985  {
1987  mrpt::rtti::CObject::Ptr(cl->createObject());
1988  auto* momo =
1989  dynamic_cast<CMultiObjectiveMotionOptimizerBase*>(obj.get());
1990  if (momo)
1991  {
1992  momo->saveConfigFile(c);
1993  }
1994  }
1995  }
1996 }
1997 
1999  const double dist)
2000 {
2001  for (auto& o : m_holonomicMethod)
2002  {
2003  o->setTargetApproachSlowDownDistance(dist);
2004  }
2005 }
2006 
2008 {
2009  ASSERT_(!m_holonomicMethod.empty());
2010  return m_holonomicMethod[0]->getTargetApproachSlowDownDistance();
2011 }
2012 
2014  int idx)
2015 {
2016  return m_holonomicMethod[idx].get();
2017 }
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:86
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() )
std::list< std::function< void(void)> > m_pending_events
Events generated during navigationStep(), enqueued to be called at the end of the method execution to...
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 < ...
double x
X,Y coordinates.
Definition: TPoint2D.h:23
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:161
#define MRPT_START
Definition: exceptions.h:241
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
GLdouble GLdouble t
Definition: glext.h:3695
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.
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.
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].
double x
X,Y coordinates.
Definition: TPose2D.h:30
void onStartNewNavigation() override
Called whenever a new navigation has been started.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
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.
bool open(const std::string &fileName, int compress_level=1, mrpt::optional_ref< std::string > error_msg=std::nullopt)
Open a file for write, choosing the compression level.
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:4062
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:128
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:95
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:86
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:3682
#define MRPT_LOG_WARN_FMT(_FMT_STRING,...)
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
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.
mrpt::math::TPose2D asTPose() const
Definition: CPose2D.cpp:468
virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd)
Default: forward call to m_robot.changeSpeed().
void registerUserMeasure(const std::string_view &event_name, const double value, const bool is_time=false)
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:586
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:120
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
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:89
virtual size_t getPTG_count() const =0
Returns the number of different PTGs that have been setup.
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:137
double speed
TP-Space movement speed, normalized to [0,1].
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:6406
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)
Definition: TTwist2D.h:26
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.
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.
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 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).
std::map< std::string, double > values
Known values:
GLsizei const GLchar ** string
Definition: glext.h:4116
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 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.
mrpt::system::CTimeLogger m_navProfiler
Publicly available time profiling object.
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:3684
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
pose_t & interpolate(const mrpt::Clock::time_point &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...
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...
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:3711
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()
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:39
virtual 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.
std::vector< TInfoPerPTG > infoPerPTG
The info for each applied PTG: must contain "nPTGs * nSecDistances" elements.
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))
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
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:245
mrpt::math::LowPassFilter_IIR1 meanTotalExecutionTime
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.
Definition: TPose2D.h:22
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...
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...
virtual double getTargetApproachSlowDownDistance() const =0
Returns the actual value of this parameter [m], as set via the children class options structure...
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.h:146
virtual void navigate(const NavInput &ni, NavOutput &no)=0
Invokes the holonomic navigation algorithm itself.
Input parameters for CAbstractHolonomicReactiveMethod::navigate()
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:137
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.h:123
Saves data to a file and transparently compress the data using the given compression level...
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
mrpt::poses::CPose2DInterpolator m_latestOdomPoses
GLuint GLenum GLenum transform
Definition: glext.h:7089
std::string sprintf_vector(const char *fmt, const VEC &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:24
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
unsigned __int32 uint32_t
Definition: rptypes.h:50
Lightweight 2D point.
Definition: TPoint2D.h:31
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:6398
virtual CAbstractHolonomicReactiveMethod * getHoloMethod(int idx)
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)
Definition: TPose2D.h:32
void enter(const std::string_view &func_name)
Start of a named section.
std::shared_ptr< CObject > Ptr
Definition: CObject.h:171
mrpt::system::CTimeLogger m_timelogger
A complete time logger.
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
double getLastOutput() const
Definition: filters.cpp:29
The structure used to store all relevant information about each transformation into TP-Space...
double leave(const std::string_view &func_name)
End of a named section.
mrpt::math::LowPassFilter_IIR1 tim_changeSpeed_avr
GLenum filter
Definition: glext.h:5134
mrpt::math::TPose2D rawOdometry
raw odometry (frame does not match to "pose", but is expected to be smoother in the short term)...
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...
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.
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: ce1a28c9f Fri Aug 23 08:02:09 2019 +0200 at vie ago 23 08:10:11 CEST 2019