MRPT  2.0.4
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-2020, 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 
14 #include <mrpt/core/lock_helper.h>
16 #include <mrpt/io/CMemoryStream.h>
18 #include <mrpt/math/geometry.h>
19 #include <mrpt/math/ops_containers.h> // sum()
20 #include <mrpt/math/wrap2pi.h>
23 #include <mrpt/system/filesystem.h>
24 #include <array>
25 #include <iomanip>
26 #include <limits>
27 
28 using namespace mrpt;
29 using namespace mrpt::io;
30 using namespace mrpt::poses;
31 using namespace mrpt::math;
32 using namespace mrpt::system;
33 using namespace mrpt::nav;
34 using namespace mrpt::serialization;
35 using namespace std;
36 
37 // ------ CAbstractPTGBasedReactive::TNavigationParamsPTG -----
38 std::string CAbstractPTGBasedReactive::TNavigationParamsPTG::getAsText() const
39 {
40  std::string s =
41  CWaypointsNavigator::TNavigationParamsWaypoints::getAsText();
42  s += "restrict_PTG_indices: ";
43  s += mrpt::containers::sprintf_vector("%u ", this->restrict_PTG_indices);
44  s += "\n";
45  return s;
46 }
47 
48 bool CAbstractPTGBasedReactive::TNavigationParamsPTG::isEqual(
50 {
51  auto o =
53  &rhs);
54  return o != nullptr &&
55  CWaypointsNavigator::TNavigationParamsWaypoints::isEqual(rhs) &&
56  restrict_PTG_indices == o->restrict_PTG_indices;
57 }
58 
59 // Ctor:
60 CAbstractPTGBasedReactive::CAbstractPTGBasedReactive(
61  CRobot2NavInterface& react_iterf_impl, bool enableConsoleOutput,
62  bool enableLogFile, const std::string& sLogDir)
63  : CWaypointsNavigator(react_iterf_impl),
64  m_enableConsoleOutput(enableConsoleOutput),
65  m_navlogfiles_dir(sLogDir)
66 {
67  this->enableLogFile(enableLogFile);
68 }
69 
71 {
72  m_closing_navigator = true;
73 
74  // Wait to end of navigation (multi-thread...)
75  m_nav_cs.lock();
76  m_nav_cs.unlock();
77 
78  // Just in case.
79  try
80  {
81  // Call base class method, NOT the generic, virtual one,
82  // to avoid problems if we are in the dtor, while the vtable
83  // is being destroyed.
84  CAbstractNavigator::stop(false /*not emergency*/);
85  }
86  catch (...)
87  {
88  }
89 
90  m_logFile.reset();
91 
92  // Free holonomic method:
93  this->deleteHolonomicObjects();
94 }
95 
97 {
98  this->preDestructor(); // ensure the robot is stopped; free dynamic objects
99 }
100 
101 /** \callergraph */
103 {
104  auto lck = mrpt::lockHelper(m_nav_cs);
105 
107 
109  m_multiobjopt->clear();
110 
111  // Compute collision grids:
112  STEP1_InitPTGs();
113 }
114 
115 /*---------------------------------------------------------------
116  enableLogFile
117  ---------------------------------------------------------------*/
119 {
120  auto lck = mrpt::lockHelper(m_nav_cs);
121 
122  try
123  {
124  // Disable:
125  // -------------------------------
126  if (!enable)
127  {
128  if (m_logFile)
129  {
131  "[CAbstractPTGBasedReactive::enableLogFile] Stopping "
132  "logging.");
133  m_logFile.reset(); // Close file:
134  }
135  else
136  return; // Already disabled.
137  }
138  else
139  { // Enable
140  // -------------------------------
141  if (m_logFile) return; // Already enabled:
142 
143  // Open file, find the first free file-name.
145  "[CAbstractPTGBasedReactive::enableLogFile] Creating rnav log "
146  "directory: %s",
147  m_navlogfiles_dir.c_str());
150  {
152  "Could not create directory for navigation logs: `%s`",
153  m_navlogfiles_dir.c_str());
154  }
155 
156  std::string filToOpen;
157  for (unsigned int nFile = 0;; nFile++)
158  {
159  filToOpen = mrpt::format(
160  "%s/log_%03u.reactivenavlog", m_navlogfiles_dir.c_str(),
161  nFile);
162  if (!system::fileExists(filToOpen)) break;
163  }
164 
165  // Open log file:
166  {
167  std::unique_ptr<CFileGZOutputStream> fil(
168  new CFileGZOutputStream);
169  bool ok = fil->open(filToOpen, 1 /* compress level */);
170  if (!ok)
171  {
173  "Error opening log file: `%s`", filToOpen.c_str());
174  }
175  else
176  {
177  m_logFile = std::move(fil);
178  }
179  }
180 
182  "[CAbstractPTGBasedReactive::enableLogFile] Logging to "
183  "file `%s`",
184  filToOpen.c_str()));
185  }
186  }
187  catch (const std::exception& e)
188  {
190  "[CAbstractPTGBasedReactive::enableLogFile] Exception: %s",
191  e.what());
192  }
193 }
194 
196 {
198  o = lastLogRecord;
199 }
200 
202 {
203  m_holonomicMethod.clear();
204 }
205 
207  const std::string& method, const mrpt::config::CConfigFileBase& ini)
208 {
209  auto lck = mrpt::lockHelper(m_nav_cs);
210 
211  this->deleteHolonomicObjects();
212  const size_t nPTGs = this->getPTG_count();
213  ASSERT_(nPTGs != 0);
214  m_holonomicMethod.resize(nPTGs);
215 
216  for (size_t i = 0; i < nPTGs; i++)
217  {
218  m_holonomicMethod[i] =
220  if (!m_holonomicMethod[i])
222  "Non-registered holonomic method className=`%s`",
223  method.c_str());
224 
225  m_holonomicMethod[i]->setAssociatedPTG(this->getPTG(i));
226  m_holonomicMethod[i]->initialize(ini); // load params
227  }
228 }
229 
230 /** The main method: executes one time-iteration of the reactive navigation
231  * algorithm.
232  * \callergraph */
234 {
236  m_navProfiler, "CAbstractPTGBasedReactive::performNavigationStep()");
237 
238  // Security tests:
239  if (m_closing_navigator) return; // Are we closing in the main thread?
240  if (!m_init_done)
241  THROW_EXCEPTION("Have you called loadConfigFile() before?");
243  const size_t nPTGs = this->getPTG_count();
244 
245  // Whether to worry about log files:
246  const bool fill_log_record = (m_logFile || m_enableKeepLogRecords);
247  CLogFileRecord newLogRec;
248  newLogRec.infoPerPTG.resize(nPTGs + 1); /* +1: [N] is the "NOP cmdvel"
249  option; not to be present in all
250  log entries. */
251 
252  // At the beginning of each log file, add an introductory block explaining
253  // which PTGs are we using:
254  {
255  if (m_logFile &&
256  m_logFile.get() != m_prev_logfile) // Only the first time
257  {
258  m_prev_logfile = m_logFile.get();
259  for (size_t i = 0; i < nPTGs; i++)
260  {
261  // If we make a direct copy (=) we will store the entire, heavy,
262  // collision grid.
263  // Let's just store the parameters of each PTG by serializing
264  // it, so paths can be reconstructed
265  // by invoking initialize()
267  auto arch = archiveFrom(buf);
268  arch << *this->getPTG(i);
269  buf.Seek(0);
270  newLogRec.infoPerPTG[i].ptg = std::dynamic_pointer_cast<
272  arch.ReadObject());
273  }
274  }
275  }
276 
277  CTimeLoggerEntry tle1(m_timelogger, "navigationStep");
278 
279  try
280  {
281  totalExecutionTime.Tic(); // Start timer
282 
283  const mrpt::system::TTimeStamp tim_start_iteration =
285 
286  // Compute target location relative to current robot pose:
287  // ---------------------------------------------------------------------
288  // Detect changes in target since last iteration (for NOP):
289  const bool target_changed_since_last_iteration =
292  if (target_changed_since_last_iteration)
294 
295  // Load the list of target(s) from the navigationParam user command.
296  // Semantic is: any of the target is good. If several targets are
297  // reachable, head to latest one.
298  std::vector<CAbstractNavigator::TargetInfo> targets;
299  {
300  auto p = dynamic_cast<
302  m_navigationParams.get());
303  if (p && !p->multiple_targets.empty())
304  {
305  targets = p->multiple_targets;
306  }
307  else
308  {
309  targets.push_back(m_navigationParams->target);
310  }
311  }
312  const size_t nTargets = targets.size(); // Normally = 1, will be >1 if
313  // we want the robot local
314  // planner to be "smarter" in
315  // skipping dynamic obstacles.
316  ASSERT_(nTargets >= 1);
317 
318  STEP1_InitPTGs(); // Will only recompute if
319  // "m_PTGsMustBeReInitialized==true"
320 
321  // STEP2: Load the obstacles and sort them in height bands.
322  // -----------------------------------------------------------------------------
323  bool sense_ok;
324  {
327  "CAbstractPTGBasedReactive::performNavigationStep().STEP2_"
328  "SenseObstacles()");
329 
330  sense_ok = STEP2_SenseObstacles();
331  }
332 
333  if (!sense_ok)
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);
485  std::transform(
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  {
510  "CAbstractPTGBasedReactive::performNavigationStep().update_PTG_"
511  "dynamic_states");
512 
513  for (size_t i = 0; i < nPTGs; i++)
514  getPTG(i)->updateNavDynamicState(ptg_dynState);
515  }
516 
517  m_infoPerPTG.assign(nPTGs + 1, TInfoPerPTG()); // reset contents
518  m_infoPerPTG_timestamp = tim_start_iteration;
519  vector<TCandidateMovementPTG> candidate_movs(
520  nPTGs + 1); // the last extra one is for the evaluation of "NOP
521  // motion command" choice.
522 
523  for (size_t indexPTG = 0; indexPTG < nPTGs; indexPTG++)
524  {
527  "CAbstractPTGBasedReactive::performNavigationStep().eval_"
528  "regular_PTG");
529 
530  CParameterizedTrajectoryGenerator* ptg = getPTG(indexPTG);
531  TInfoPerPTG& ipf = m_infoPerPTG[indexPTG];
532 
533  // Ensure the method knows about its associated PTG:
534  auto holoMethod = this->getHoloMethod(indexPTG);
535  ASSERT_(holoMethod);
536  holoMethod->setAssociatedPTG(this->getPTG(indexPTG));
537 
538  // The picked movement in TP-Space (to be determined by holonomic
539  // method below)
540  TCandidateMovementPTG& cm = candidate_movs[indexPTG];
541 
544  ptg, indexPTG, relTargets, rel_pose_PTG_origin_wrt_sense, ipf,
545  cm, newLogRec, false /* this is a regular PTG reactive case */,
546  *holoMethod, tim_start_iteration, *m_navigationParams);
547  } // end for each PTG
548 
549  // check for collision, which is reflected by ALL TP-Obstacles being
550  // zero:
551  bool is_all_ptg_collision = true;
552  for (size_t indexPTG = 0; indexPTG < nPTGs; indexPTG++)
553  {
554  bool is_collision = true;
555  const auto& obs = m_infoPerPTG[indexPTG].TP_Obstacles;
556  for (const auto o : obs)
557  {
558  if (o != 0)
559  {
560  is_collision = false;
561  break;
562  }
563  }
564  if (!is_collision)
565  {
566  is_all_ptg_collision = false;
567  break;
568  }
569  }
570  if (is_all_ptg_collision)
571  {
572  m_pending_events.emplace_back(std::bind(
574  std::ref(m_robot)));
575  }
576 
577  // Round #2: Evaluate dont sending any new velocity command ("NOP"
578  // motion)
579  // =========
580  bool NOP_not_too_old = true;
581  bool NOP_not_too_close_and_have_to_slowdown = true;
582  double NOP_max_time = -1.0, NOP_At = -1.0;
583  double slowdowndist = .0;
584  CParameterizedTrajectoryGenerator* last_sent_ptg =
587  : nullptr;
588  if (last_sent_ptg)
589  {
590  // So supportSpeedAtTarget() below is evaluated in the correct
591  // context:
593  }
594 
595  // This approach is only possible if:
596  const bool can_do_nop_motion =
598  !target_changed_since_last_iteration && last_sent_ptg &&
599  last_sent_ptg->supportVelCmdNOP()) &&
600  (NOP_not_too_old =
602  m_lastSentVelCmd.tim_send_cmd_vel, tim_start_iteration)) <
603  (NOP_max_time =
604  last_sent_ptg->maxTimeInVelCmdNOP(
606  std::max(0.1, m_lastSentVelCmd.speed_scale))) &&
607  (NOP_not_too_close_and_have_to_slowdown =
608  (last_sent_ptg->supportSpeedAtTarget() ||
609  (relTargetDist >
611  ->getTargetApproachSlowDownDistance())
612  // slowdowndist is assigned here, inside the if()
613  // to be sure the index in m_lastSentVelCmd is valid!
614  )));
615 
616  if (!NOP_not_too_old)
617  {
618  newLogRec.additional_debug_msgs["PTG_cont"] = mrpt::format(
619  "PTG-continuation not allowed: previous command timed-out "
620  "(At=%.03f > Max_At=%.03f)",
621  NOP_At, NOP_max_time);
622  }
623  if (!NOP_not_too_close_and_have_to_slowdown)
624  {
625  newLogRec.additional_debug_msgs["PTG_cont_trgdst"] = mrpt::format(
626  "PTG-continuation not allowed: target too close and must start "
627  "slow-down (trgDist=%.03f < SlowDownDist=%.03f)",
628  relTargetDist, slowdowndist);
629  }
630 
631  mrpt::math::TPose2D rel_cur_pose_wrt_last_vel_cmd_NOP(0, 0, 0),
632  rel_pose_PTG_origin_wrt_sense_NOP(0, 0, 0);
633 
634  if (can_do_nop_motion)
635  {
638  "CAbstractPTGBasedReactive::performNavigationStep().eval_NOP");
639 
640  // Add the estimation of how long it takes to run the changeSpeeds()
641  // callback (usually a tiny period):
642  const mrpt::system::TTimeStamp tim_send_cmd_vel_corrected =
646 
647  // Note: we use (uncorrected) raw odometry as basis to the following
648  // calculation since it's normally
649  // smoother than particle filter-based localization data, more
650  // accurate in the middle/long term,
651  // but not in the short term:
652  mrpt::math::TPose2D robot_pose_at_send_cmd, robot_odom_at_send_cmd;
653  bool valid_odom, valid_pose;
654 
656  tim_send_cmd_vel_corrected, robot_odom_at_send_cmd, valid_odom);
658  tim_send_cmd_vel_corrected, robot_pose_at_send_cmd, valid_pose);
659 
660  if (valid_odom && valid_pose)
661  {
662  ASSERT_(last_sent_ptg != nullptr);
663 
664  std::vector<TPose2D> relTargets_NOPs;
665  std::transform(
666  targets.begin(), targets.end(), // in
667  std::back_inserter(relTargets_NOPs), // out
668  [robot_pose_at_send_cmd](
670  return e.target_coords - robot_pose_at_send_cmd;
671  });
672  ASSERT_EQUAL_(relTargets_NOPs.size(), targets.size());
673 
674  rel_pose_PTG_origin_wrt_sense_NOP =
675  robot_odom_at_send_cmd -
676  (m_curPoseVel.rawOdometry + relPoseSense);
677  rel_cur_pose_wrt_last_vel_cmd_NOP =
678  m_curPoseVel.rawOdometry - robot_odom_at_send_cmd;
679 
680  // Update PTG response to dynamic params:
681  last_sent_ptg->updateNavDynamicState(
683 
684  if (fill_log_record)
685  {
686  newLogRec.additional_debug_msgs
687  ["rel_cur_pose_wrt_last_vel_cmd_NOP(interp)"] =
688  rel_cur_pose_wrt_last_vel_cmd_NOP.asString();
689  newLogRec.additional_debug_msgs
690  ["robot_odom_at_send_cmd(interp)"] =
691  robot_odom_at_send_cmd.asString();
692  }
693 
694  // No need to call setAssociatedPTG(), already correctly
695  // associated above.
696 
699  last_sent_ptg, m_lastSentVelCmd.ptg_index, relTargets_NOPs,
700  rel_pose_PTG_origin_wrt_sense_NOP, m_infoPerPTG[nPTGs],
701  candidate_movs[nPTGs], newLogRec,
702  true /* this is the PTG continuation (NOP) choice */,
704  tim_start_iteration, *m_navigationParams,
705  rel_cur_pose_wrt_last_vel_cmd_NOP);
706 
707  } // end valid interpolated origin pose
708  else
709  {
710  // Can't interpolate pose, hence can't evaluate NOP:
711  candidate_movs[nPTGs].speed =
712  -0.01; // <0 means inviable movement
713  }
714  } // end can_do_NOP_motion
715 
716  // Evaluate all the candidates and pick the "best" one, using
717  // the user-defined multiobjective optimizer
718  // --------------------------------------------------------------
721  int best_ptg_idx = m_multiobjopt->decide(candidate_movs, mo_info);
722 
723  if (fill_log_record)
724  {
725  if (mo_info.final_evaluation.size() == newLogRec.infoPerPTG.size())
726  {
727  for (unsigned int i = 0; i < newLogRec.infoPerPTG.size(); i++)
728  {
729  newLogRec.infoPerPTG[i].evaluation =
730  mo_info.final_evaluation[i];
731  }
732  }
733  int idx = 0;
734  for (const auto& le : mo_info.log_entries)
735  {
737  "MultiObjMotOptmzr_msg%03i", idx++)] = le;
738  }
739  }
740 
741  // Pick best movement (or null if none is good)
742  const TCandidateMovementPTG* selectedHolonomicMovement = nullptr;
743  if (best_ptg_idx >= 0)
744  {
745  selectedHolonomicMovement = &candidate_movs[best_ptg_idx];
746  }
747 
748  // If the selected PTG is (N+1), it means the NOP cmd. vel is selected
749  // as the best alternative, i.e. do NOT send any new motion command.
750  const bool best_is_NOP_cmdvel = (best_ptg_idx == int(nPTGs));
751 
752  // ---------------------------------------------------------------------
753  // SEND MOVEMENT COMMAND TO THE ROBOT
754  // ---------------------------------------------------------------------
756  if (best_is_NOP_cmdvel)
757  {
758  // Notify the robot that we want it to keep executing the last
759  // cmdvel:
760  if (!this->changeSpeedsNOP())
761  {
763  "\nERROR calling changeSpeedsNOP()!! Stopping robot and "
764  "finishing navigation\n");
765  if (fill_log_record)
766  {
768  newLogRec, relTargets, best_ptg_idx,
769  m_robot.getEmergencyStopCmd(), nPTGs,
770  best_is_NOP_cmdvel, rel_cur_pose_wrt_last_vel_cmd_NOP,
771  rel_pose_PTG_origin_wrt_sense_NOP,
772  0, // executionTimeValue,
773  0, // tim_changeSpeed,
774  tim_start_iteration);
775  }
776  return;
777  }
778  }
779  else
780  {
781  // Make sure the dynamic state of a NOP cmd has not overwritten the
782  // state for a "regular" PTG choice:
783  for (size_t i = 0; i < nPTGs; i++)
784  {
785  getPTG(i)->updateNavDynamicState(ptg_dynState);
786  }
787 
788  // STEP7: Get the non-holonomic movement command.
789  // ---------------------------------------------------------------------
790  double cmd_vel_speed_ratio = 1.0;
791  if (selectedHolonomicMovement)
792  {
793  CTimeLoggerEntry tle2(
794  m_timelogger, "navigationStep.selectedHolonomicMovement");
795  cmd_vel_speed_ratio =
796  generate_vel_cmd(*selectedHolonomicMovement, new_vel_cmd);
797  ASSERT_(new_vel_cmd);
798  }
799 
800  if (!new_vel_cmd /* which means best_PTG_eval==.0*/ ||
801  new_vel_cmd->isStopCmd())
802  {
804  "Best velocity command is STOP (no way found), calling "
805  "robot.stop()");
806  this->stop(true /* emergency */); // don't call
807  // doEmergencyStop() here
808  // since that will stop
809  // navigation completely
810  new_vel_cmd = m_robot.getEmergencyStopCmd();
812  }
813  else
814  {
815  mrpt::system::TTimeStamp tim_send_cmd_vel;
816  {
818  m_timlog_delays, "changeSpeeds()");
819  tim_send_cmd_vel = mrpt::system::now();
820  newLogRec.timestamps["tim_send_cmd_vel"] = tim_send_cmd_vel;
821  if (!this->changeSpeeds(*new_vel_cmd))
822  {
824  "\nERROR calling changeSpeeds()!! Stopping robot "
825  "and finishing navigation\n");
826  if (fill_log_record)
827  {
828  new_vel_cmd = m_robot.getEmergencyStopCmd();
830  newLogRec, relTargets, best_ptg_idx,
831  new_vel_cmd, nPTGs, best_is_NOP_cmdvel,
832  rel_cur_pose_wrt_last_vel_cmd_NOP,
833  rel_pose_PTG_origin_wrt_sense_NOP,
834  0, // executionTimeValue,
835  0, // tim_changeSpeed,
836  tim_start_iteration);
837  }
838  return;
839  }
840  }
841  // Save last sent cmd:
842  m_lastSentVelCmd.speed_scale = cmd_vel_speed_ratio;
843  m_lastSentVelCmd.ptg_index = best_ptg_idx;
845  selectedHolonomicMovement
846  ? selectedHolonomicMovement->PTG->alpha2index(
847  selectedHolonomicMovement->direction)
848  : 0;
850  selectedHolonomicMovement->props["holo_stage_eval"];
851 
853  best_ptg_idx >= 0
854  ? m_infoPerPTG[best_ptg_idx]
855  .TP_Obstacles[m_lastSentVelCmd.ptg_alpha_index]
856  : .0;
858  (selectedHolonomicMovement->props["is_slowdown"] != 0.0);
859 
861  m_lastSentVelCmd.tim_send_cmd_vel = tim_send_cmd_vel;
862  m_lastSentVelCmd.ptg_dynState = ptg_dynState;
863 
864  // Update delay model:
865  const double timoff_sendVelCmd = mrpt::system::timeDifference(
866  tim_start_iteration, tim_send_cmd_vel);
867  timoff_sendVelCmd_avr.filter(timoff_sendVelCmd);
868  newLogRec.values["timoff_sendVelCmd"] = timoff_sendVelCmd;
869  newLogRec.values["timoff_sendVelCmd_avr"] =
871  }
872  }
873 
874  // ------- end of motion decision zone ---------
875 
876  // Statistics:
877  // ----------------------------------------------------
878  const double executionTimeValue = executionTime.Tac();
879  meanExecutionTime.filter(executionTimeValue);
881 
882  const double tim_changeSpeed =
883  m_timlog_delays.getLastTime("changeSpeeds()");
884  tim_changeSpeed_avr.filter(tim_changeSpeed);
885 
886  // Running period estim:
887  const double period_tim = timerForExecutionPeriod.Tac();
888  if (period_tim > 1.5 * meanExecutionPeriod.getLastOutput())
889  {
891  "Timing warning: Suspicious executionPeriod=%.03f ms is far "
892  "above the average of %.03f ms",
893  1e3 * period_tim, meanExecutionPeriod.getLastOutput() * 1e3);
894  }
895  meanExecutionPeriod.filter(period_tim);
897 
899  {
901  "CMD: %s "
902  "speedScale=%.04f "
903  "T=%.01lfms Exec:%.01lfms|%.01lfms "
904  "PTG#%i\n",
905  new_vel_cmd ? new_vel_cmd->asString().c_str() : "NOP",
906  selectedHolonomicMovement ? selectedHolonomicMovement->speed
907  : .0,
909  1000.0 * meanExecutionTime.getLastOutput(),
910  1000.0 * meanTotalExecutionTime.getLastOutput(), best_ptg_idx));
911  }
912  if (fill_log_record)
913  {
915  newLogRec, relTargets, best_ptg_idx, new_vel_cmd, nPTGs,
916  best_is_NOP_cmdvel, rel_cur_pose_wrt_last_vel_cmd_NOP,
917  rel_pose_PTG_origin_wrt_sense_NOP, executionTimeValue,
918  tim_changeSpeed, tim_start_iteration);
919  }
920  }
921  catch (const std::exception& e)
922  {
924  std::string("[CAbstractPTGBasedReactive::performNavigationStep] "
925  "Stopping robot and finishing navigation due to "
926  "exception:\n") +
927  std::string(e.what()));
928  }
929  catch (...)
930  {
932  "[CAbstractPTGBasedReactive::performNavigationStep] Stopping robot "
933  "and finishing navigation due to untyped exception.");
934  }
935 }
936 
937 /** \callergraph */
939  CLogFileRecord& newLogRec, const std::vector<TPose2D>& relTargets,
940  int nSelectedPTG, const mrpt::kinematics::CVehicleVelCmd::Ptr& new_vel_cmd,
941  const int nPTGs, const bool best_is_NOP_cmdvel,
942  const mrpt::math::TPose2D& rel_cur_pose_wrt_last_vel_cmd_NOP,
943  const mrpt::math::TPose2D& rel_pose_PTG_origin_wrt_sense_NOP,
944  const double executionTimeValue, const double tim_changeSpeed,
945  const mrpt::system::TTimeStamp& tim_start_iteration)
946 {
947  // ---------------------------------------
948  // STEP8: Generate log record
949  // ---------------------------------------
951  m_navProfiler, "CAbstractPTGBasedReactive::STEP8_GenerateLogRecord()");
952 
953  m_timelogger.enter("navigationStep.populate_log_info");
954 
955  this->loggingGetWSObstaclesAndShape(newLogRec);
956 
959  newLogRec.WS_targets_relative = relTargets;
960  newLogRec.nSelectedPTG = nSelectedPTG;
961  newLogRec.cur_vel = m_curPoseVel.velGlobal;
962  newLogRec.cur_vel_local = m_curPoseVel.velLocal;
963  newLogRec.cmd_vel = new_vel_cmd;
964  newLogRec.values["estimatedExecutionPeriod"] =
966  newLogRec.values["executionTime"] = executionTimeValue;
967  newLogRec.values["executionTime_avr"] = meanExecutionTime.getLastOutput();
968  newLogRec.values["time_changeSpeeds()"] = tim_changeSpeed;
969  newLogRec.values["time_changeSpeeds()_avr"] =
971  newLogRec.values["CWaypointsNavigator::navigationStep()"] =
972  m_timlog_delays.getLastTime("CWaypointsNavigator::navigationStep()");
973  newLogRec.values["CAbstractNavigator::navigationStep()"] =
974  m_timlog_delays.getLastTime("CAbstractNavigator::navigationStep()");
975  newLogRec.timestamps["tim_start_iteration"] = tim_start_iteration;
976  newLogRec.timestamps["curPoseAndVel"] = m_curPoseVel.timestamp;
977  newLogRec.nPTGs = nPTGs;
978 
979  // NOP mode stuff:
981  rel_cur_pose_wrt_last_vel_cmd_NOP;
983  rel_pose_PTG_origin_wrt_sense_NOP;
984  newLogRec.ptg_index_NOP =
985  best_is_NOP_cmdvel ? m_lastSentVelCmd.ptg_index : -1;
988 
989  m_timelogger.leave("navigationStep.populate_log_info");
990 
991  // Save to log file:
992  // --------------------------------------
993  {
995  m_timelogger, "navigationStep.write_log_file");
996  if (m_logFile) archiveFrom(*m_logFile) << newLogRec;
997  }
998  // Set as last log record
999  {
1000  auto lck = mrpt::lockHelper(m_critZoneLastLog);
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 
1784  const mrpt::config::CConfigFileBase& c, const std::string& s)
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 
1808  mrpt::config::CConfigFileBase& c, const std::string& s) const
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  {
1907  auto* filter = new mrpt::maps::CPointCloudFilterByDistance;
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  {
1962  mrpt::rtti::CObject::Ptr obj = cl->createObject();
1963  auto* holo =
1964  dynamic_cast<CAbstractHolonomicReactiveMethod*>(obj.get());
1965  if (holo)
1966  {
1967  holo->saveConfigFile(c);
1968  }
1969  }
1970  }
1971 
1972  // Decider method:
1973  if (m_multiobjopt)
1974  {
1975  // Save my current settings:
1976  m_multiobjopt->saveConfigFile(c);
1977  }
1978  else
1979  {
1980  // save options of ALL known methods:
1983  for (const auto& cl : lst)
1984  {
1985  mrpt::rtti::CObject::Ptr obj = cl->createObject();
1986  auto* momo =
1987  dynamic_cast<CMultiObjectiveMotionOptimizerBase*>(obj.get());
1988  if (momo)
1989  {
1990  momo->saveConfigFile(c);
1991  }
1992  }
1993  }
1994 }
1995 
1997  const double dist)
1998 {
1999  for (auto& o : m_holonomicMethod)
2000  {
2001  o->setTargetApproachSlowDownDistance(dist);
2002  }
2003 }
2004 
2006 {
2007  ASSERT_(!m_holonomicMethod.empty());
2008  return m_holonomicMethod[0]->getTargetApproachSlowDownDistance();
2009 }
2010 
2012  int idx)
2013 {
2014  return m_holonomicMethod[idx].get();
2015 }
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:87
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 < ...
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).
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...
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");
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.
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y yaw]", yaw in degrees) ...
Definition: TPose2D.cpp:24
#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
T x
X,Y coordinates.
Definition: TPoint2D.h:25
A safe way to call enter() and leave() of a mrpt::system::CTimeLogger upon construction and destructi...
std::vector< double > final_evaluation
The final evaluation score for each candidate.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
void registerUserMeasure(const std::string_view &event_name, const double value, const bool is_time=false) noexcept
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...
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.
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...
#define MRPT_LOG_WARN_FMT(_FMT_STRING,...)
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().
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:592
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.
LockHelper< T > lockHelper(T &t)
Syntactic sugar to easily create a locker to any kind of std::mutex.
Definition: lock_helper.h:50
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:102
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].
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const =0
saves all available parameters, in a forma loadable by initialize()
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.
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.
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...
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:
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.
void enter(const std::string_view &func_name) noexcept
Start of a named section.
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...
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.
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))
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
double leave(const std::string_view &func_name) noexcept
End of a named section.
#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()
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:76
mrpt::poses::CPose2DInterpolator m_latestOdomPoses
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)
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
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
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...
mrpt::math::LowPassFilter_IIR1 tim_changeSpeed_avr
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
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...
void saveToConfigFile(mrpt::config::CConfigFileBase &c, const std::string &section) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...
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:24



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 5711e29ae Wed May 27 14:29:47 2020 +0200 at miƩ may 27 14:30:10 CEST 2020