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



Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019