MRPT  2.0.3
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 }
mrpt::nav::CAbstractNavigator::changeSpeeds
virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd)
Default: forward call to m_robot.changeSpeed().
Definition: CAbstractNavigator.cpp:421
mrpt::io::CMemoryStream::Seek
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.
Definition: CMemoryStream.cpp:126
filesystem.h
ops_containers.h
mrpt::system::timeDifference
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
mrpt::nav::CAbstractPTGBasedReactive::meanExecutionPeriod
mrpt::math::LowPassFilter_IIR1 meanExecutionPeriod
Runtime estimation of execution period of the method.
Definition: CAbstractPTGBasedReactive.h:321
mrpt::nav::CLogFileRecord
A class for storing, saving and loading a reactive navigation log record for the CReactiveNavigationS...
Definition: CLogFileRecord.h:29
nav-precomp.h
mrpt::nav::CLogFileRecord::values
std::map< std::string, double > values
Known values:
Definition: CLogFileRecord.h:82
mrpt::nav::CAbstractNavigator::TRobotPoseVel::timestamp
mrpt::system::TTimeStamp timestamp
Definition: CAbstractNavigator.h:368
mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput
Input parameters for CAbstractHolonomicReactiveMethod::navigate()
Definition: CAbstractHolonomicReactiveMethod.h:35
mrpt::poses::CPose2D::asTPose
mrpt::math::TPose2D asTPose() const
Definition: CPose2D.cpp:468
CAbstractPTGBasedReactive.h
mrpt::nav::CAbstractPTGBasedReactive::m_timelogger
mrpt::system::CTimeLogger m_timelogger
A complete time logger.
Definition: CAbstractPTGBasedReactive.h:312
mrpt::nav::CMultiObjectiveMotionOptimizerBase::Factory
static CMultiObjectiveMotionOptimizerBase::Ptr Factory(const std::string &className) noexcept
Class factory from C++ class name.
Definition: CMultiObjectiveMotionOptimizerBase.cpp:223
mrpt::nav::CParameterizedTrajectoryGenerator::evalClearanceSingleObstacle
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 ...
Definition: CParameterizedTrajectoryGenerator.cpp:512
mrpt::nav::CAbstractPTGBasedReactive::~CAbstractPTGBasedReactive
~CAbstractPTGBasedReactive() override
Definition: CAbstractPTGBasedReactive.cpp:96
mrpt::rtti::CObject::Ptr
std::shared_ptr< CObject > Ptr
Definition: CObject.h:184
mrpt::nav::CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::secure_distance_start
double secure_distance_start
In normalized distances, the start and end of a ramp function that scales the velocity output from th...
Definition: CAbstractPTGBasedReactive.h:206
mrpt::nav::CAbstractPTGBasedReactive::implementSenseObstacles
virtual bool implementSenseObstacles(mrpt::system::TTimeStamp &obs_timestamp)=0
Return false on any fatal error.
mrpt::maps::CPointCloudFilterByDistance
Implementation of pointcloud filtering based on requisities for minimum neigbouring points in both,...
Definition: CPointCloudFilterByDistance.h:26
mrpt::nav::CLogFileRecord::cur_vel
mrpt::math::TTwist2D cur_vel
The actual robot velocities in global (map) coordinates, as read from sensors, in "m/sec" and "rad/se...
Definition: CLogFileRecord.h:110
mrpt::nav::CAbstractPTGBasedReactive::TNavigationParamsPTG
The struct for configuring navigation requests to CAbstractPTGBasedReactive and derived classes.
Definition: CAbstractPTGBasedReactive.h:97
mrpt::containers::copy_container_typecasting
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...
Definition: copy_container_typecasting.h:29
INVALID_TIMESTAMP
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
mrpt::io
Definition: img/CImage.h:24
mrpt::nav::TCandidateMovementPTG::PTG
CParameterizedTrajectoryGenerator * PTG
The associated PTG.
Definition: TCandidateMovementPTG.h:24
mrpt::nav::CAbstractNavigator::TargetInfo
Individual target info in CAbstractNavigator::TNavigationParamsBase and derived classes.
Definition: CAbstractNavigator.h:68
mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::isValid
bool isValid() const
Definition: CAbstractPTGBasedReactive.cpp:1532
mrpt::nav::CLogFileRecord::TInfoPerPTG::desiredDirection
double desiredDirection
The results from the holonomic method.
Definition: CLogFileRecord.h:55
mrpt::nav::CParameterizedTrajectoryGenerator::getSupportedKinematicVelocityCommand
virtual mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand() const =0
Returns an empty kinematic velocity command object of the type supported by this PTG.
mrpt::nav::CMultiObjectiveMotionOptimizerBase::TResultInfo::log_entries
std::vector< std::string > log_entries
Optionally, debug logging info will be stored here by the implementor classes.
Definition: CMultiObjectiveMotionOptimizerBase.h:44
MRPT_SAVE_CONFIG_VAR_COMMENT
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
Definition: config/CConfigFileBase.h:480
mrpt::nav::CHolonomicLogFileRecord::Ptr
std::shared_ptr< CHolonomicLogFileRecord > Ptr
Definition: CHolonomicLogFileRecord.h:26
mrpt::nav::CParameterizedTrajectoryGenerator::directionToMotionCommand
virtual mrpt::kinematics::CVehicleVelCmd::Ptr directionToMotionCommand(uint16_t k) const =0
Converts a discretized "alpha" value into a feasible motion command or action.
geometry.h
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::targetRelSpeed
double targetRelSpeed
Desired relative speed [0,1] at target.
Definition: CParameterizedTrajectoryGenerator.h:176
mrpt::nav::CAbstractPTGBasedReactive::TInfoPerPTG::clearance
ClearanceDiagram clearance
Clearance for each path.
Definition: CAbstractPTGBasedReactive.h:419
ASSERT_EQUAL_
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
mrpt::nav::TCandidateMovementPTG::starting_robot_dir
double starting_robot_dir
Default to 0, they can be used to reflect a robot starting at a position not at (0,...
Definition: TCandidateMovementPTG.h:32
mrpt::nav::CAbstractPTGBasedReactive::meanExecutionTime
mrpt::math::LowPassFilter_IIR1 meanExecutionTime
Definition: CAbstractPTGBasedReactive.h:318
mrpt::nav::CAbstractNavigator::m_last_curPoseVelUpdate_robot_time
double m_last_curPoseVelUpdate_robot_time
Definition: CAbstractNavigator.h:376
mrpt::maps::CPointCloudFilterBase::Ptr
std::shared_ptr< CPointCloudFilterBase > Ptr
Definition: CPointCloudFilterBase.h:36
mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::poseVel
TRobotPoseVel poseVel
Robot pose & velocities and timestamp of when it was queried.
Definition: CAbstractPTGBasedReactive.h:447
mrpt::nav::CAbstractHolonomicReactiveMethod::NavOutput::logRecord
CHolonomicLogFileRecord::Ptr logRecord
The navigation method will create a log record and store it here via a smart pointer.
Definition: CAbstractHolonomicReactiveMethod.h:72
MRPT_LOG_DEBUG
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
Definition: system/COutputLogger.h:427
mrpt::io::CFileGZOutputStream::open
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.
Definition: CFileGZOutputStream.cpp:44
mrpt::math::TPose2D::phi
double phi
Orientation (rads)
Definition: TPose2D.h:32
mrpt::nav::CAbstractNavigator::TRobotPoseVel::pose_frame_id
std::string pose_frame_id
map frame ID for pose
Definition: CAbstractNavigator.h:370
mrpt::nav::CAbstractPTGBasedReactive::m_infoPerPTG_timestamp
mrpt::system::TTimeStamp m_infoPerPTG_timestamp
Definition: CAbstractPTGBasedReactive.h:424
mrpt::nav::CAbstractPTGBasedReactive::m_enableKeepLogRecords
bool m_enableKeepLogRecords
See enableKeepLogRecords.
Definition: CAbstractPTGBasedReactive.h:296
mrpt::nav::CAbstractPTGBasedReactive::m_WS_Obstacles_timestamp
mrpt::system::TTimeStamp m_WS_Obstacles_timestamp
Definition: CAbstractPTGBasedReactive.h:406
mrpt::nav::CAbstractNavigator::TRobotPoseVel::velLocal
mrpt::math::TTwist2D velLocal
Definition: CAbstractNavigator.h:364
mrpt::math::TPoint2D_< double >
mrpt::system::directoryExists
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
mrpt::nav::CLogFileRecord::ptg_index_NOP
int16_t ptg_index_NOP
Negative means no NOP mode evaluation, so the rest of "NOP variables" should be ignored.
Definition: CLogFileRecord.h:125
mrpt::nav::CParameterizedTrajectoryGenerator::evalPathRelativePriority
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,...
Definition: CParameterizedTrajectoryGenerator.h:312
mrpt::nav::CAbstractPTGBasedReactive::PTGTarget::target_k
int target_k
The discrete version of target_alpha.
Definition: CAbstractPTGBasedReactive.h:364
mrpt::nav::CAbstractNavigator::m_navigationParams
std::unique_ptr< TNavigationParams > m_navigationParams
Current navigation parameters.
Definition: CAbstractNavigator.h:350
mrpt::math::angDistance
T angDistance(T from, T to)
Computes the shortest angular increment (or distance) between two planar orientations,...
Definition: wrap2pi.h:95
mrpt::nav::CLogFileRecord::ptg_last_k_NOP
uint16_t ptg_last_k_NOP
Definition: CLogFileRecord.h:126
mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput::maxObstacleDist
double maxObstacleDist
Maximum expected value to be found in obstacles.
Definition: CAbstractHolonomicReactiveMethod.h:53
mrpt::nav::CAbstractPTGBasedReactive::initialize
void initialize() override
Must be called for loading collision grids, or the first navigation command may last a long time to b...
Definition: CAbstractPTGBasedReactive.cpp:102
mrpt::nav::CLogFileRecord::infoPerPTG
std::vector< TInfoPerPTG > infoPerPTG
The info for each applied PTG: must contain "nPTGs * nSecDistances" elements.
Definition: CLogFileRecord.h:74
mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::tim_send_cmd_vel
mrpt::system::TTimeStamp tim_send_cmd_vel
Timestamp of when the cmd was sent.
Definition: CAbstractPTGBasedReactive.h:445
mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::ptg_dynState
CParameterizedTrajectoryGenerator::TNavDynamicState ptg_dynState
Definition: CAbstractPTGBasedReactive.h:455
mrpt::nav::CAbstractHolonomicReactiveMethod::NavOutput::desiredDirection
double desiredDirection
The desired motion direction, in the range [-PI, PI].
Definition: CAbstractHolonomicReactiveMethod.h:65
mrpt::nav::CAbstractNavigator::TNavigationParams::target
TargetInfo target
Navigation target.
Definition: CAbstractNavigator.h:121
CFileGZOutputStream.h
mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::original_holo_eval
double original_holo_eval
Definition: CAbstractPTGBasedReactive.h:454
mrpt::nav::CMultiObjectiveMotionOptimizerBase::saveConfigFile
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const =0
mrpt::nav::CAbstractPTGBasedReactive::m_holonomicMethod
std::vector< CAbstractHolonomicReactiveMethod::Ptr > m_holonomicMethod
The holonomic navigation algorithm (one object per PTG, so internal states are maintained)
Definition: CAbstractPTGBasedReactive.h:291
mrpt::nav::CAbstractPTGBasedReactive::enableLogFile
void enableLogFile(bool enable)
Enables/disables saving log files.
Definition: CAbstractPTGBasedReactive.cpp:118
mrpt::system::now
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
mrpt::nav::TCandidateMovementPTG::props
mrpt::system::TParameters< double > props
List of properties.
Definition: TCandidateMovementPTG.h:39
mrpt::nav::CLogFileRecord::ptg_last_navDynState
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState ptg_last_navDynState
Definition: CLogFileRecord.h:130
mrpt::nav
Definition: CAbstractHolonomicReactiveMethod.h:20
mrpt::nav::CMultiObjectiveMotionOptimizerBase::TResultInfo
Definition: CMultiObjectiveMotionOptimizerBase.h:32
CMemoryStream.h
mrpt::math::TPose2D::y
double y
Definition: TPose2D.h:30
mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::colfreedist_move_k
double colfreedist_move_k
TP-Obstacles in the move direction at the instant of picking this movement.
Definition: CAbstractPTGBasedReactive.h:450
mrpt::nav::CRobot2NavInterface::getEmergencyStopCmd
virtual mrpt::kinematics::CVehicleVelCmd::Ptr getEmergencyStopCmd()=0
Gets the emergency stop command for the current robot.
mrpt::nav::CAbstractPTGBasedReactive::TInfoPerPTG::targets
std::vector< PTGTarget > targets
Definition: CAbstractPTGBasedReactive.h:414
mrpt::nav::CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::speedfilter_tau
double speedfilter_tau
Time constant (in seconds) for the low-pass filter applied to kinematic velocity commands (default=0:...
Definition: CAbstractPTGBasedReactive.h:187
mrpt::nav::ClearanceDiagram::getClearance
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:
Definition: ClearanceDiagram.cpp:131
wrap2pi.h
mrpt::nav::CAbstractPTGBasedReactive::setHolonomicMethod
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,...
Definition: CAbstractPTGBasedReactive.cpp:206
mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::was_slowdown
bool was_slowdown
Definition: CAbstractPTGBasedReactive.h:451
mrpt::nav::CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::saveToConfigFile
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.
Definition: CAbstractPTGBasedReactive.cpp:1807
mrpt::nav::CLogFileRecord::navDynState
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState navDynState
Definition: CLogFileRecord.h:69
mrpt::math::TTwist2D::vx
double vx
Velocity components: X,Y (m/s)
Definition: TTwist2D.h:26
mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::ptg_index
int ptg_index
0-based index of used PTG
Definition: CAbstractPTGBasedReactive.h:441
mrpt::nav::CLogFileRecord::TInfoPerPTG::desiredSpeed
double desiredSpeed
Definition: CLogFileRecord.h:55
THROW_EXCEPTION_FMT
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::nav::CAbstractPTGBasedReactive::timoff_obstacles_avr
mrpt::math::LowPassFilter_IIR1 timoff_obstacles_avr
Definition: CAbstractPTGBasedReactive.h:323
mrpt::nav::CLogFileRecord::TInfoPerPTG::PTG_desc
std::string PTG_desc
A short description for the applied PTG.
Definition: CLogFileRecord.h:43
mrpt::nav::CWaypointsNavigator::TNavigationParamsWaypoints
The struct for configuring navigation requests to CWaypointsNavigator and derived classes.
Definition: CWaypointsNavigator.h:43
mrpt::nav::CRobot2NavInterface
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
Definition: CRobot2NavInterface.h:43
mrpt::poses::CPoseInterpolatorBase::interpolate
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.
Definition: CPoseInterpolatorBase.hpp:70
MRPT_LOG_WARN_FMT
#define MRPT_LOG_WARN_FMT(_FMT_STRING,...)
Definition: system/COutputLogger.h:465
mrpt::nav::CAbstractPTGBasedReactive::getHoloMethod
virtual CAbstractHolonomicReactiveMethod * getHoloMethod(int idx)
Definition: CAbstractPTGBasedReactive.cpp:2011
mrpt::hypot_fast
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.
Definition: core/include/mrpt/core/bits_math.h:31
mrpt::math::TTwist2D::vy
double vy
Definition: TTwist2D.h:26
mrpt::nav::CAbstractPTGBasedReactive::m_last_vel_cmd
mrpt::kinematics::CVehicleVelCmd::Ptr m_last_vel_cmd
Last velocity commands.
Definition: CAbstractPTGBasedReactive.h:300
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::nav::CAbstractPTGBasedReactive::m_critZoneLastLog
std::recursive_mutex m_critZoneLastLog
Critical zones.
Definition: CAbstractPTGBasedReactive.h:303
mrpt::nav::CAbstractPTGBasedReactive::m_prev_logfile
mrpt::io::CStream * m_prev_logfile
The current log file stream, or nullptr if not being used.
Definition: CAbstractPTGBasedReactive.h:294
mrpt::nav::CAbstractHolonomicReactiveMethod::NavOutput::desiredSpeed
double desiredSpeed
The desired motion speed in that direction, from 0 up to NavInput::maxRobotSpeed.
Definition: CAbstractHolonomicReactiveMethod.h:68
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::relTarget
mrpt::math::TPose2D relTarget
Current relative target location.
Definition: CParameterizedTrajectoryGenerator.h:174
mrpt::system::fileExists
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
printf_vector.h
mrpt::nav::CWaypointsNavigator
This class extends CAbstractNavigator with the capability of following a list of waypoints.
Definition: CWaypointsNavigator.h:38
mrpt::nav::CLogFileRecord::TInfoPerPTG::TP_Obstacles
mrpt::math::CVectorFloat TP_Obstacles
Distances until obstacles, in "pseudometers", first index for -PI direction, last one for PI directio...
Definition: CLogFileRecord.h:46
MRPT_LOG_DEBUG_FMT
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
Definition: system/COutputLogger.h:461
mrpt::nav::CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::evaluate_clearance
bool evaluate_clearance
Default: false.
Definition: CAbstractPTGBasedReactive.h:220
mrpt::nav::CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::holonomic_method
std::string holonomic_method
C++ class name of the holonomic navigation method to run in the transformed TP-Space.
Definition: CAbstractPTGBasedReactive.h:176
mrpt::nav::CAbstractNavigator::TRobotPoseVel::pose
mrpt::math::TPose2D pose
Definition: CAbstractNavigator.h:363
mrpt::nav::CAbstractPTGBasedReactive::TInfoPerPTG::TP_Obstacles
std::vector< double > TP_Obstacles
One distance per discretized alpha value, describing the "polar plot" of TP obstacles.
Definition: CAbstractPTGBasedReactive.h:417
mrpt::nav::CParameterizedTrajectoryGenerator::inverseMap_WS2TP
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...
mrpt::nav::CRobot2NavInterface::sendApparentCollisionEvent
virtual void sendApparentCollisionEvent()
Callback: Apparent collision event (i.e.
Definition: CRobot2NavInterface.cpp:102
mrpt::nav::CParameterizedTrajectoryGenerator::index2alpha
double index2alpha(uint16_t k) const
Alpha value for the discrete corresponding value.
Definition: CParameterizedTrajectoryGenerator.cpp:208
mrpt::nav::CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::robot_absolute_speed_limits
mrpt::kinematics::CVehicleVelCmd::TVelCmdParams robot_absolute_speed_limits
Params related to speed limits.
Definition: CAbstractPTGBasedReactive.h:217
mrpt::nav::CAbstractPTGBasedReactive::tictac
mrpt::system::CTicTac tictac
Definition: CAbstractPTGBasedReactive.h:317
mrpt::nav::CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::secure_distance_end
double secure_distance_end
Definition: CAbstractPTGBasedReactive.h:206
mrpt::nav::CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::loadFromConfigFile
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.
Definition: CAbstractPTGBasedReactive.cpp:1783
mrpt::system::CTicTac::Tac
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
mrpt::system::CTimeLogger::registerUserMeasure
void registerUserMeasure(const std::string_view &event_name, const double value, const bool is_time=false) noexcept
Definition: CTimeLogger.cpp:358
mrpt::nav::CLogFileRecord::additional_debug_msgs
std::map< std::string, std::string > additional_debug_msgs
Additional debug traces.
Definition: CLogFileRecord.h:91
mrpt::nav::CAbstractPTGBasedReactive::m_PTGsMustBeReInitialized
bool m_PTGsMustBeReInitialized
Definition: CAbstractPTGBasedReactive.h:313
MRPT_LOAD_CONFIG_VAR_REQUIRED_CS
#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 ...
Definition: config/CConfigFileBase.h:420
mrpt::nav::CAbstractPTGBasedReactive::loggingGetWSObstaclesAndShape
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...
mrpt::nav::CParameterizedTrajectoryGenerator::alpha2index
uint16_t alpha2index(double alpha) const
Discrete index value for the corresponding alpha value.
Definition: CParameterizedTrajectoryGenerator.cpp:223
mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput::targets
std::vector< mrpt::math::TPoint2D > targets
Relative location (x,y) of target point(s).
Definition: CAbstractHolonomicReactiveMethod.h:46
mrpt::nav::CAbstractPTGBasedReactive::setTargetApproachSlowDownDistance
void setTargetApproachSlowDownDistance(const double dist)
Changes this parameter in all inner holonomic navigator instances [m].
Definition: CAbstractPTGBasedReactive.cpp:1996
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::curVelLocal
mrpt::math::TTwist2D curVelLocal
Current vehicle velocity (local frame of reference)
Definition: CParameterizedTrajectoryGenerator.h:172
mrpt::nav::TCandidateMovementPTG::starting_robot_dist
double starting_robot_dist
Definition: TCandidateMovementPTG.h:32
mrpt::nav::CAbstractPTGBasedReactive::timoff_curPoseAndSpeed_avr
mrpt::math::LowPassFilter_IIR1 timoff_curPoseAndSpeed_avr
Definition: CAbstractPTGBasedReactive.h:323
mrpt::nav::CAbstractNavigator::TNavigationParams
The struct for configuring navigation requests.
Definition: CAbstractNavigator.h:118
mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput::obstacles
std::vector< double > obstacles
Distance to obstacles in polar coordinates, relative to the robot.
Definition: CAbstractHolonomicReactiveMethod.h:43
mrpt::nav::CAbstractPTGBasedReactive::PTGTarget::target_alpha
double target_alpha
TP-Target.
Definition: CAbstractPTGBasedReactive.h:362
mrpt::nav::CAbstractNavigator::TRobotPoseVel
Definition: CAbstractNavigator.h:361
mrpt::nav::CAbstractPTGBasedReactive::getPTG_count
virtual size_t getPTG_count() const =0
Returns the number of different PTGs that have been setup.
mrpt::nav::CAbstractHolonomicReactiveMethod
A base class for holonomic reactive navigation methods.
Definition: CAbstractHolonomicReactiveMethod.h:29
mrpt::nav::CAbstractPTGBasedReactive::m_multiobjopt
mrpt::nav::CMultiObjectiveMotionOptimizerBase::Ptr m_multiobjopt
Definition: CAbstractPTGBasedReactive.h:410
mrpt::nav::CLogFileRecord::TInfoPerPTG::timeForHolonomicMethod
double timeForHolonomicMethod
Definition: CLogFileRecord.h:53
mrpt::nav::CAbstractPTGBasedReactive::timoff_sendVelCmd_avr
mrpt::math::LowPassFilter_IIR1 timoff_sendVelCmd_avr
Definition: CAbstractPTGBasedReactive.h:324
mrpt::nav::CAbstractPTGBasedReactive::PTGTarget::TP_Target
mrpt::math::TPoint2D TP_Target
The Target, in TP-Space (x,y)
Definition: CAbstractPTGBasedReactive.h:360
mrpt::nav::CLogFileRecord::timestamps
std::map< std::string, mrpt::system::TTimeStamp > timestamps
Known values:
Definition: CLogFileRecord.h:89
mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::TSentVelCmd
TSentVelCmd()
Definition: CAbstractPTGBasedReactive.cpp:1519
mrpt::nav::CLogFileRecord::cmd_vel
mrpt::kinematics::CVehicleVelCmd::Ptr cmd_vel
The final motion command sent to robot, in "m/sec" and "rad/sec".
Definition: CLogFileRecord.h:104
CPointCloudFilterByDistance.h
mrpt::nav::CAbstractNavigator::TargetInfo::targetDesiredRelSpeed
double targetDesiredRelSpeed
(Default=.05) Desired relative speed (wrt maximum speed), in range [0,1], of the vehicle at target.
Definition: CAbstractNavigator.h:88
mrpt::nav::CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::enable_obstacle_filtering
bool enable_obstacle_filtering
Definition: CAbstractPTGBasedReactive.h:218
MRPT_LOAD_CONFIG_VAR_CS
#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
Definition: config/CConfigFileBase.h:315
mrpt::rtti::getAllRegisteredClassesChildrenOf
std::vector< const TRuntimeClassId * > getAllRegisteredClassesChildrenOf(const TRuntimeClassId *parent_id)
Like getAllRegisteredClasses(), but filters the list to only include children clases of a given base ...
Definition: internal_class_registry.cpp:222
mrpt::system::CTimeLogger::leave
double leave(const std::string_view &func_name) noexcept
End of a named section.
Definition: system/CTimeLogger.h:146
mrpt::nav::CAbstractPTGBasedReactive::getPTG
virtual CParameterizedTrajectoryGenerator * getPTG(size_t i)=0
Gets the i'th PTG.
mrpt::nav::CLogFileRecord::WS_targets_relative
std::vector< mrpt::math::TPose2D > WS_targets_relative
Relative poses (wrt to robotPoseLocalization) for.
Definition: CLogFileRecord.h:101
mrpt::nav::CAbstractNavigator::TRobotPoseVel::rawOdometry
mrpt::math::TPose2D rawOdometry
raw odometry (frame does not match to "pose", but is expected to be smoother in the short term).
Definition: CAbstractNavigator.h:367
mrpt::nav::CParameterizedTrajectoryGenerator::supportSpeedAtTarget
virtual bool supportSpeedAtTarget() const
Returns true if this PTG takes into account the desired velocity at target.
Definition: CParameterizedTrajectoryGenerator.h:291
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24
mrpt::nav::CAbstractPTGBasedReactive::deleteHolonomicObjects
void deleteHolonomicObjects()
Delete m_holonomicMethod.
Definition: CAbstractPTGBasedReactive.cpp:201
mrpt::nav::CAbstractPTGBasedReactive::STEP2_SenseObstacles
bool STEP2_SenseObstacles()
Definition: CAbstractPTGBasedReactive.cpp:1504
mrpt::nav::CAbstractPTGBasedReactive::build_movement_candidate
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))
Definition: CAbstractPTGBasedReactive.cpp:1538
mrpt::nav::CParameterizedTrajectoryGenerator::initTPObstacles
void initTPObstacles(std::vector< double > &TP_Obstacles) const
Resizes and populates the initial appropriate contents in a vector of tp-obstacles (collision-free ra...
Definition: CParameterizedTrajectoryGenerator.cpp:263
mrpt::nav::CAbstractHolonomicReactiveMethod::saveConfigFile
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const =0
saves all available parameters, in a forma loadable by initialize()
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::nav::CAbstractPTGBasedReactive::totalExecutionTime
mrpt::system::CTicTac totalExecutionTime
Definition: CAbstractPTGBasedReactive.h:317
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::nav::CAbstractNavigator::doEmergencyStop
virtual void doEmergencyStop(const std::string &msg)
Stops the robot and set navigation state to error.
Definition: CAbstractNavigator.cpp:257
mrpt::io::CFileGZOutputStream
Saves data to a file and transparently compress the data using the given compression level.
Definition: io/CFileGZOutputStream.h:26
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::nav::ClearanceDiagram
Clearance information for one particular PTG and one set of obstacles.
Definition: ClearanceDiagram.h:28
mrpt::nav::CLogFileRecord::rel_cur_pose_wrt_last_vel_cmd_NOP
mrpt::math::TPose2D rel_cur_pose_wrt_last_vel_cmd_NOP
Definition: CLogFileRecord.h:127
mrpt::nav::CAbstractPTGBasedReactive::loadConfigFile
void loadConfigFile(const mrpt::config::CConfigFileBase &c) override
Loads all params from a file.
Definition: CAbstractPTGBasedReactive.cpp:1891
MRPT_LOG_ERROR_FMT
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
Definition: system/COutputLogger.h:467
mrpt::nav::CLogFileRecord::TInfoPerPTG::HLFR
CHolonomicLogFileRecord::Ptr HLFR
Other useful info about holonomic method execution.
Definition: CLogFileRecord.h:61
mrpt::nav::TCandidateMovementPTG
Stores a candidate movement in TP-Space-based navigation.
Definition: TCandidateMovementPTG.h:21
mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::speed_scale
double speed_scale
[0,1] scale of the raw cmd_vel as generated by the PTG
Definition: CAbstractPTGBasedReactive.h:453
mrpt::nav::CAbstractNavigator::m_latestOdomPoses
mrpt::poses::CPose2DInterpolator m_latestOdomPoses
Definition: CAbstractNavigator.h:379
mrpt::nav::CLogFileRecord::TInfoPerPTG::TP_Robot
mrpt::math::TPoint2D TP_Robot
Robot location in TP-Space: normally (0,0), except during "NOP cmd vel" steps.
Definition: CLogFileRecord.h:51
mrpt::nav::CParameterizedTrajectoryGenerator::getDescription
virtual std::string getDescription() const =0
Gets a short textual description of the PTG and its parameters.
mrpt::nav::CAbstractNavigator::m_robot
CRobot2NavInterface & m_robot
The navigator-robot interface.
Definition: CAbstractNavigator.h:353
mrpt::nav::CAbstractHolonomicReactiveMethod::enableApproachTargetSlowDown
void enableApproachTargetSlowDown(bool enable)
Definition: CAbstractHolonomicReactiveMethod.h:119
mrpt::nav::CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::use_delays_model
bool use_delays_model
Definition: CAbstractPTGBasedReactive.h:207
mrpt::nav::CLogFileRecord::cur_vel_local
mrpt::math::TTwist2D cur_vel_local
The actual robot velocities in local (robot) coordinates, as read from sensors, in "m/sec" and "rad/s...
Definition: CLogFileRecord.h:113
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState
Dynamic state that may affect the PTG path parameterization.
Definition: CParameterizedTrajectoryGenerator.h:169
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: TPose2D.h:22
mrpt::system::CTimeLogger::enter
void enter(const std::string_view &func_name) noexcept
Start of a named section.
Definition: system/CTimeLogger.h:140
mrpt::system::TTimeStamp
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
mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep
void performNavigationStep() override
The main method for the navigator.
Definition: CAbstractPTGBasedReactive.cpp:233
mrpt::nav::CAbstractPTGBasedReactive::m_closing_navigator
bool m_closing_navigator
Signal that the destructor has been called, so no more calls are accepted from other threads.
Definition: CAbstractPTGBasedReactive.h:404
mrpt::nav::CAbstractNavigator::stop
virtual bool stop(bool isEmergencyStop)
Default: forward call to m_robot.stop().
Definition: CAbstractNavigator.cpp:429
mrpt::nav::CAbstractPTGBasedReactive::meanTotalExecutionTime
mrpt::math::LowPassFilter_IIR1 meanTotalExecutionTime
Definition: CAbstractPTGBasedReactive.h:319
mrpt::system::CTimeLogger::isEnabled
bool isEnabled() const
Definition: system/CTimeLogger.h:119
mrpt::keep_min
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value.
Definition: core/include/mrpt/core/bits_math.h:145
mrpt::nav::CMultiObjectiveMotionOptimizerBase
Virtual base class for multi-objective motion choosers, as used for reactive navigation engines.
Definition: CMultiObjectiveMotionOptimizerBase.h:24
mrpt::math::TPoint2D_data::y
T y
Definition: TPoint2D.h:25
mrpt::system::CTicTac::Tic
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
mrpt::nav::CParameterizedTrajectoryGenerator::supportVelCmdNOP
virtual bool supportVelCmdNOP() const
Returns true if it is possible to stop sending velocity commands to the robot and,...
Definition: CParameterizedTrajectoryGenerator.cpp:54
mrpt::nav::CParameterizedTrajectoryGenerator::getPathCount
uint16_t getPathCount() const
Get the number of different, discrete paths in this family.
Definition: CParameterizedTrajectoryGenerator.h:360
mrpt::nav::CAbstractHolonomicReactiveMethod::getTargetApproachSlowDownDistance
virtual double getTargetApproachSlowDownDistance() const =0
Returns the actual value of this parameter [m], as set via the children class options structure.
mrpt::nav::CAbstractHolonomicReactiveMethod::Factory
static CAbstractHolonomicReactiveMethod::Ptr Factory(const std::string &className) noexcept
Definition: CAbstractHolonomicReactiveMethod.cpp:51
mrpt::nav::CLogFileRecord::TInfoPerPTG
The structure used to store all relevant information about each transformation into TP-Space.
Definition: CLogFileRecord.h:40
mrpt::nav::CParameterizedTrajectoryGenerator::getPathStepDuration
virtual double getPathStepDuration() const =0
Returns the duration (in seconds) of each "step".
lock_helper.h
mrpt::nav::CLogFileRecord::robotPoseOdometry
mrpt::math::TPose2D robotPoseOdometry
Definition: CLogFileRecord.h:95
mrpt::io::CMemoryStream
This CStream derived class allow using a memory buffer as a CStream.
Definition: io/CMemoryStream.h:26
mrpt::nav::CAbstractPTGBasedReactive::m_enableConsoleOutput
bool m_enableConsoleOutput
Enables / disables the console debug output.
Definition: CAbstractPTGBasedReactive.h:306
mrpt::nav::CAbstractPTGBasedReactive::TInfoPerPTG
Definition: CAbstractPTGBasedReactive.h:412
mrpt::nav::CAbstractNavigator::m_curPoseVel
TRobotPoseVel m_curPoseVel
Current robot pose (updated in CAbstractNavigator::navigationStep() )
Definition: CAbstractNavigator.h:375
mrpt::serialization
Definition: aligned_serialization.h:13
mrpt::containers::sprintf_vector
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
mrpt::nav::CAbstractPTGBasedReactive::m_lastSentVelCmd
TSentVelCmd m_lastSentVelCmd
Definition: CAbstractPTGBasedReactive.h:462
mrpt::nav::CParameterizedTrajectoryGenerator::isBijectiveAt
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.
Definition: CParameterizedTrajectoryGenerator.h:154
mrpt::nav::CParameterizedTrajectoryGenerator::getPathStepForDist
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 < ...
mrpt::kinematics::CVehicleVelCmd::Ptr
std::shared_ptr< CVehicleVelCmd > Ptr
Definition: CVehicleVelCmd.h:22
mrpt::nav::CAbstractPTGBasedReactive::PTGTarget
Definition: CAbstractPTGBasedReactive.h:355
mrpt::nav::CAbstractPTGBasedReactive::m_infoPerPTG
std::vector< TInfoPerPTG > m_infoPerPTG
Temporary buffers for working with each PTG during a navigationStep()
Definition: CAbstractPTGBasedReactive.h:423
mrpt::system::CTimeLoggerEntry
A safe way to call enter() and leave() of a mrpt::system::CTimeLogger upon construction and destructi...
Definition: system/CTimeLogger.h:189
mrpt::nav::CAbstractNavigator::m_navProfiler
mrpt::system::CTimeLogger m_navProfiler
Publicly available time profiling object.
Definition: CAbstractNavigator.h:268
mrpt::math::TPose2D::asString
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
mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput::maxRobotSpeed
double maxRobotSpeed
Maximum robot speed, in the same units than obstacles, per second.
Definition: CAbstractHolonomicReactiveMethod.h:49
mrpt::nav::CAbstractNavigator::changeSpeedsNOP
virtual bool changeSpeedsNOP()
Default: forward call to m_robot.changeSpeedsNOP().
Definition: CAbstractNavigator.cpp:427
mrpt::nav::CAbstractPTGBasedReactive::STEP8_GenerateLogRecord
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)
Definition: CAbstractPTGBasedReactive.cpp:938
mrpt::nav::CParameterizedTrajectoryGenerator::getRefDistance
double getRefDistance() const
Definition: CParameterizedTrajectoryGenerator.h:370
mrpt::nav::CParameterizedTrajectoryGenerator::getPathPose
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_LOG_ERROR_STREAM
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
Definition: system/COutputLogger.h:477
mrpt::nav::CAbstractPTGBasedReactive::tim_changeSpeed_avr
mrpt::math::LowPassFilter_IIR1 tim_changeSpeed_avr
Definition: CAbstractPTGBasedReactive.h:322
mrpt::system::CTimeLogger::getLastTime
double getLastTime(const std::string &name) const
Return the last execution time of the given "section", or 0 if it hasn't ever been called "enter" wit...
Definition: CTimeLogger.cpp:405
mrpt::serialization::archiveFrom
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT's CStream,...
Definition: CArchive.h:592
CLASS_ID
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:102
mrpt::nav::CAbstractPTGBasedReactive::impl_waypoint_is_reachable
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,...
Definition: CAbstractPTGBasedReactive.cpp:1464
mrpt::nav::CAbstractPTGBasedReactive::generate_vel_cmd
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.
Definition: CAbstractPTGBasedReactive.cpp:1414
mrpt::nav::CAbstractPTGBasedReactive::m_init_done
bool m_init_done
Whether loadConfigFile() has been called or not.
Definition: CAbstractPTGBasedReactive.h:308
mrpt::math::TPoint2D_data::x
T x
X,Y coordinates.
Definition: TPoint2D.h:25
mrpt::nav::TCandidateMovementPTG::direction
double direction
TP-Space movement direction.
Definition: TCandidateMovementPTG.h:26
mrpt::nav::CAbstractPTGBasedReactive::m_navlogfiles_dir
std::string m_navlogfiles_dir
Default: "./reactivenav.logs".
Definition: CAbstractPTGBasedReactive.h:469
mrpt::nav::CParameterizedTrajectoryGenerator::getPathDist
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.
mrpt::nav::CParameterizedTrajectoryGenerator::maxTimeInVelCmdNOP
virtual double maxTimeInVelCmdNOP(int path_k) const
Only for PTGs supporting supportVelCmdNOP(): this is the maximum time (in seconds) for which the path...
Definition: CParameterizedTrajectoryGenerator.cpp:58
mrpt::lockHelper
LockHelper< T > lockHelper(T &t)
Syntactic sugar to easily create a locker to any kind of std::mutex.
Definition: lock_helper.h:50
mrpt::nav::CAbstractNavigator::m_frame_tf
std::weak_ptr< mrpt::poses::FrameTransformer< 2 > > m_frame_tf
Optional, user-provided frame transformer.
Definition: CAbstractNavigator.h:356
copy_container_typecasting.h
mrpt::nav::CWaypointsNavigator::saveConfigFile
void saveConfigFile(mrpt::config::CConfigFileBase &c) const override
Saves all current options to a config file.
Definition: CWaypointsNavigator.cpp:492
mrpt::math::LowPassFilter_IIR1::getLastOutput
double getLastOutput() const
Definition: filters.cpp:29
mrpt::system::timestampAdd
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
mrpt::nav::TCandidateMovementPTG::speed
double speed
TP-Space movement speed, normalized to [0,1].
Definition: TCandidateMovementPTG.h:29
mrpt::nav::CAbstractHolonomicReactiveMethod::NavOutput
Output for CAbstractHolonomicReactiveMethod::navigate()
Definition: CAbstractHolonomicReactiveMethod.h:62
mrpt::nav::CParameterizedTrajectoryGenerator::updateClearancePost
void updateClearancePost(ClearanceDiagram &cd, const std::vector< double > &TP_obstacles) const
Definition: CParameterizedTrajectoryGenerator.cpp:506
mrpt::nav::CAbstractPTGBasedReactive::getTargetApproachSlowDownDistance
double getTargetApproachSlowDownDistance() const
Returns this parameter for the first inner holonomic navigator instances [m] (should be the same in a...
Definition: CAbstractPTGBasedReactive.cpp:2005
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::nav::CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::max_dist_for_timebased_path_prediction
double max_dist_for_timebased_path_prediction
Max dist [meters] to use time-based path prediction for NOP evaluation.
Definition: CAbstractPTGBasedReactive.h:223
mrpt::nav::CLogFileRecord::nPTGs
uint32_t nPTGs
The number of PTGS:
Definition: CLogFileRecord.h:71
mrpt::nav::CAbstractPTGBasedReactive::preDestructor
void preDestructor()
To be called during children destructors to assure thread-safe destruction, and free of shared object...
Definition: CAbstractPTGBasedReactive.cpp:70
mrpt::maps::CPointCloudFilterByDistance::TOptions::saveToConfigFile
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.
Definition: CPointCloudFilterByDistance.cpp:219
mrpt::nav::CAbstractPTGBasedReactive::calc_move_candidate_scores
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.
Definition: CAbstractPTGBasedReactive.cpp:1006
mrpt::nav::CAbstractPTGBasedReactive::params_abstract_ptg_navigator
TAbstractPTGNavigatorParams params_abstract_ptg_navigator
Definition: CAbstractPTGBasedReactive.h:234
mrpt::nav::CLogFileRecord::relPoseVelCmd
mrpt::math::TPose2D relPoseVelCmd
Definition: CLogFileRecord.h:97
mrpt::nav::CLogFileRecord::nSelectedPTG
int32_t nSelectedPTG
The selected PTG.
Definition: CLogFileRecord.h:76
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::nav::CLogFileRecord::TInfoPerPTG::clearance
mrpt::nav::ClearanceDiagram clearance
Clearance for each path.
Definition: CLogFileRecord.h:66
mrpt::math::LowPassFilter_IIR1::filter
double filter(double x)
Processes one input sample, updates the filter state and return the filtered value.
Definition: filters.cpp:21
mrpt::nav::CAbstractNavigator::m_pending_events
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...
Definition: CAbstractNavigator.h:289
mrpt::nav::CParameterizedTrajectoryGenerator::updateNavDynamicState
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...
Definition: CParameterizedTrajectoryGenerator.cpp:360
mrpt::nav::CAbstractPTGBasedReactive::TNavigationParamsPTG::restrict_PTG_indices
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...
Definition: CAbstractPTGBasedReactive.h:103
mrpt::nav::CAbstractPTGBasedReactive::m_WS_filter
mrpt::maps::CPointCloudFilterBase::Ptr m_WS_filter
Default: none.
Definition: CAbstractPTGBasedReactive.h:408
mrpt::nav::CAbstractNavigator::m_timlog_delays
mrpt::system::CTimeLogger m_timlog_delays
Time logger to collect delay-related stats.
Definition: CAbstractNavigator.h:382
mrpt::maps::CPointCloudFilterByDistance::options
TOptions options
Definition: CPointCloudFilterByDistance.h:69
mrpt::nav::CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::motion_decider_method
std::string motion_decider_method
C++ class name of the motion chooser.
Definition: CAbstractPTGBasedReactive.h:178
mrpt::nav::CAbstractPTGBasedReactive::PTGTarget::valid_TP
bool valid_TP
For each PTG, whether target falls into the PTG domain.
Definition: CAbstractPTGBasedReactive.h:358
mrpt::nav::CAbstractPTGBasedReactive::PTGTarget::target_dist
double target_dist
Definition: CAbstractPTGBasedReactive.h:362
mrpt::nav::CMultiObjectiveMotionOptimizerBase::TResultInfo::final_evaluation
std::vector< double > final_evaluation
The final evaluation score for each candidate.
Definition: CMultiObjectiveMotionOptimizerBase.h:41
mrpt::nav::CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::TAbstractPTGNavigatorParams
TAbstractPTGNavigatorParams()
Definition: CAbstractPTGBasedReactive.cpp:1882
mrpt::nav::CLogFileRecord::robotPoseLocalization
mrpt::math::TPose2D robotPoseLocalization
The robot pose (from odometry and from the localization/SLAM system).
Definition: CLogFileRecord.h:95
mrpt::nav::CAbstractPTGBasedReactive::STEP1_InitPTGs
virtual void STEP1_InitPTGs()=0
mrpt::nav::CAbstractPTGBasedReactive::m_copy_prev_navParams
std::unique_ptr< TNavigationParams > m_copy_prev_navParams
A copy of last-iteration navparams, used to detect changes.
Definition: CAbstractPTGBasedReactive.h:473
CArchive.h
mrpt::nav::CAbstractNavigator::TRobotPoseVel::velGlobal
mrpt::math::TTwist2D velGlobal
Definition: CAbstractNavigator.h:364
mrpt::nav::CAbstractPTGBasedReactive::executionTime
mrpt::system::CTicTac executionTime
Definition: CAbstractPTGBasedReactive.h:317
ok
bool ok
Definition: chessboard_stereo_camera_calib_unittest.cpp:61
mrpt::nav::CAbstractPTGBasedReactive::STEP3_WSpaceToTPSpace
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::nav::CAbstractHolonomicReactiveMethod::NavInput::clearance
const mrpt::nav::ClearanceDiagram * clearance
The computed clearance for each direction (optional in some implementations).
Definition: CAbstractHolonomicReactiveMethod.h:56
mrpt::kinematics::CVehicleVelCmd
Virtual base for velocity commands of different kinematic models of planar mobile robot.
Definition: CVehicleVelCmd.h:20
mrpt::system::createDirectory
bool createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:161
mrpt::nav::CAbstractPTGBasedReactive::getLastLogRecord
void getLastLogRecord(CLogFileRecord &o)
Provides a copy of the last log record with information about execution.
Definition: CAbstractPTGBasedReactive.cpp:195
mrpt::nav::CLogFileRecord::relPoseSense
mrpt::math::TPose2D relPoseSense
Definition: CLogFileRecord.h:96
mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::ptg_alpha_index
int ptg_alpha_index
Path index for selected PTG.
Definition: CAbstractPTGBasedReactive.h:443
mrpt::nav::CLogFileRecord::TInfoPerPTG::TP_Targets
std::vector< mrpt::math::TPoint2D > TP_Targets
Target(s) location in TP-Space.
Definition: CLogFileRecord.h:48
mrpt::nav::CAbstractPTGBasedReactive::timerForExecutionPeriod
mrpt::system::CTicTac timerForExecutionPeriod
Definition: CAbstractPTGBasedReactive.h:309
mrpt::nav::CWaypointsNavigator::loadConfigFile
void loadConfigFile(const mrpt::config::CConfigFileBase &c) override
Loads all params from a file.
Definition: CWaypointsNavigator.cpp:482
mrpt::nav::CAbstractNavigator::m_latestPoses
mrpt::poses::CPose2DInterpolator m_latestPoses
Latest robot poses (updated in CAbstractNavigator::navigationStep() )
Definition: CAbstractNavigator.h:379
mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::reset
void reset()
Definition: CAbstractPTGBasedReactive.cpp:1520
mrpt::nav::CAbstractHolonomicReactiveMethod::navigate
virtual void navigate(const NavInput &ni, NavOutput &no)=0
Invokes the holonomic navigation algorithm itself.
mrpt::math::TPose2D::x
double x
X,Y coordinates.
Definition: TPose2D.h:30
mrpt::nav::CLogFileRecord::TInfoPerPTG::timeForTPObsTransformation
double timeForTPObsTransformation
Time, in seconds.
Definition: CLogFileRecord.h:53
mrpt::nav::CParameterizedTrajectoryGenerator
This is the base class for any user-defined PTG.
Definition: CParameterizedTrajectoryGenerator.h:78
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::nav::CAbstractPTGBasedReactive::m_logFile
std::unique_ptr< mrpt::io::CStream > m_logFile
Definition: CAbstractPTGBasedReactive.h:292
mrpt::system
Definition: backtrace.h:14
mrpt::nav::CAbstractNavigator::TNavigationParamsBase
Base for all high-level navigation commands.
Definition: CAbstractNavigator.h:104
mrpt::nav::CParameterizedTrajectoryGenerator::getAlphaValuesCount
uint16_t getAlphaValuesCount() const
Get the number of different, discrete paths in this family.
Definition: CParameterizedTrajectoryGenerator.h:358
mrpt::nav::CAbstractPTGBasedReactive::saveConfigFile
void saveConfigFile(mrpt::config::CConfigFileBase &c) const override
Saves all current options to a config file.
Definition: CAbstractPTGBasedReactive.cpp:1936
mrpt::nav::CAbstractPTGBasedReactive::onStartNewNavigation
void onStartNewNavigation() override
Called whenever a new navigation has been started.
Definition: CAbstractPTGBasedReactive.cpp:1510
mrpt::nav::CAbstractPTGBasedReactive::lastLogRecord
CLogFileRecord lastLogRecord
The last log.
Definition: CAbstractPTGBasedReactive.h:298
mrpt::nav::CLogFileRecord::rel_pose_PTG_origin_wrt_sense_NOP
mrpt::math::TPose2D rel_pose_PTG_origin_wrt_sense_NOP
Definition: CLogFileRecord.h:128
mrpt::nav::CAbstractNavigator::m_nav_cs
std::recursive_mutex m_nav_cs
mutex for all navigation methods
Definition: CAbstractNavigator.h:359
mrpt::nav::CParameterizedTrajectoryGenerator::getScorePriority
double getScorePriority() const
When used in path planning, a multiplying factor (default=1.0) for the scores for this PTG.
Definition: CParameterizedTrajectoryGenerator.h:379
mrpt::nav::CWaypointsNavigator::onStartNewNavigation
void onStartNewNavigation() override
Called whenever a new navigation has been started.
Definition: CWaypointsNavigator.cpp:474
mrpt::nav::CParameterizedTrajectoryGenerator::initClearanceDiagram
void initClearanceDiagram(ClearanceDiagram &cd) const
Must be called to resize a CD to its correct size, before calling updateClearance()
Definition: CParameterizedTrajectoryGenerator.cpp:465



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 15:49:54 UTC 2020