MRPT  2.0.3
CAbstractNavigator.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 
13 #include <mrpt/core/lock_helper.h>
14 #include <mrpt/math/TSegment2D.h>
16 #include <limits>
17 #include <typeinfo>
18 
19 using namespace mrpt::nav;
20 using namespace std;
21 
22 const double PREVIOUS_POSES_MAX_AGE = 20; // seconds
23 
24 // Ctor: CAbstractNavigator::TargetInfo
26  : target_coords(0, 0, 0), target_frame_id("map")
27 
28 {
29 }
30 
31 // Gets navigation params as a human-readable format:
33 {
34  string s;
35  s += mrpt::format(
36  "target_coords = (%.03f,%.03f,%.03f deg)\n", target_coords.x,
37  target_coords.y, target_coords.phi);
38  s += mrpt::format("target_frame_id = \"%s\"\n", target_frame_id.c_str());
39  s += mrpt::format("targetAllowedDistance = %.03f\n", targetAllowedDistance);
40  s += mrpt::format(
41  "targetIsRelative = %s\n", targetIsRelative ? "YES" : "NO");
42  s += mrpt::format(
43  "targetIsIntermediaryWaypoint = %s\n",
44  targetIsIntermediaryWaypoint ? "YES" : "NO");
45  s += mrpt::format("targetDesiredRelSpeed = %.02f\n", targetDesiredRelSpeed);
46  return s;
47 }
48 
50  const TargetInfo& o) const
51 {
52  return target_coords == o.target_coords &&
53  target_frame_id == o.target_frame_id &&
54  targetAllowedDistance == o.targetAllowedDistance &&
55  targetIsRelative == o.targetIsRelative &&
56  targetDesiredRelSpeed == o.targetDesiredRelSpeed &&
57  targetIsIntermediaryWaypoint == o.targetIsIntermediaryWaypoint;
58 }
59 
60 // Gets navigation params as a human-readable format:
62 {
63  string s;
64  s += "navparams. Single target:\n";
65  s += target.getAsText();
66  return s;
67 }
68 
71 {
72  auto* rhs = dynamic_cast<const CAbstractNavigator::TNavigationParams*>(&o);
73  return (rhs != nullptr) && (this->target == rhs->target);
74 }
75 
77  : pose(0, 0, 0),
78  velGlobal(0, 0, 0),
79  velLocal(0, 0, 0),
80  rawOdometry(0, 0, 0),
81  timestamp(INVALID_TIMESTAMP),
82  pose_frame_id()
83 {
84 }
85 
86 /*---------------------------------------------------------------
87  Constructor
88  ---------------------------------------------------------------*/
90  : mrpt::system::COutputLogger("MRPT_navigator"), m_robot(react_iterf_impl)
91 {
95 }
96 
97 // Dtor:
99 /** \callergraph */
101 {
102  auto lck = mrpt::lockHelper(m_nav_cs);
103 
104  MRPT_LOG_DEBUG("CAbstractNavigator::cancel() called.");
106  this->stop(false /*not emergency*/);
107 }
108 
109 /** \callergraph */
111 {
112  auto lck = mrpt::lockHelper(m_nav_cs);
113 
114  MRPT_LOG_DEBUG("[CAbstractNavigator::resume() called.");
116 }
117 
118 /** \callergraph */
120 {
121  auto lck = mrpt::lockHelper(m_nav_cs);
122 
123  // Issue an "stop" if we are moving:
124  // Update: do it *always*, even if the current velocity is zero, since
125  // we may be in the middle of a multi-part motion command. It's safer.
126  this->stop(false /*not an emergency stop*/);
127 
128  MRPT_LOG_DEBUG("CAbstractNavigator::suspend() called.");
130 }
131 
132 /** \callergraph */
134 {
135  auto lck = mrpt::lockHelper(m_nav_cs);
136 
137  MRPT_LOG_DEBUG("CAbstractNavigator::resetNavError() called.");
139  {
142  }
143 }
144 
146  const std::weak_ptr<mrpt::poses::FrameTransformer<2>>& frame_tf)
147 {
148  m_frame_tf = frame_tf;
149 }
150 
152 {
153  MRPT_START
154 
155  params_abstract_navigator.loadFromConfigFile(c, "CAbstractNavigator");
156 
158  "CAbstractNavigator", "enable_time_profiler",
160 
161  // At this point, all derived classes have already loaded their parameters.
162  // Dump them to debug output:
163  {
165  this->saveConfigFile(cfg_mem);
166  MRPT_LOG_INFO(cfg_mem.getContent());
167  }
168 
169  MRPT_END
170 }
171 
173 {
174  params_abstract_navigator.saveToConfigFile(c, "CAbstractNavigator");
175 }
176 
177 /** \callergraph */
179 {
180  auto lck = mrpt::lockHelper(m_nav_cs);
181 
183  m_navProfiler, "CAbstractNavigator::navigationStep()");
184 
185  const TState prevState = m_navigationState;
186  switch (m_navigationState)
187  {
188  case IDLE:
189  case SUSPENDED:
190  try
191  {
192  // If we just arrived at this state, stop robot:
194  {
196  "[CAbstractNavigator::navigationStep()] Navigation "
197  "stopped.");
198  // this->stop(); stop() is called by the method switching
199  // the "state", so we have more flexibility
201  }
202  }
203  catch (...)
204  {
205  }
206  break;
207 
208  case NAV_ERROR:
209  try
210  {
211  // Send end-of-navigation event:
214  {
215  m_pending_events.emplace_back(std::bind(
217  std::ref(m_robot)));
218  }
219 
220  // If we just arrived at this state, stop the robot:
222  {
224  "[CAbstractNavigator::navigationStep()] Stopping "
225  "Navigation due to a NAV_ERROR state!");
226  this->stop(false /*not emergency*/);
228  }
229  }
230  catch (...)
231  {
232  }
233  break;
234 
235  case NAVIGATING:
237  true /* do call virtual method nav implementation*/);
238  break; // End case NAVIGATING
239  };
240  m_lastNavigationState = prevState;
241 
243 }
244 
245 /** \callergraph */
247 {
248  // Invoke pending events:
249  for (auto& ev : m_pending_events)
250  {
251  ev();
252  }
253  m_pending_events.clear();
254 }
255 
256 /** \callergraph */
257 void CAbstractNavigator::doEmergencyStop(const std::string& msg)
258 {
259  try
260  {
261  this->stop(true /* emergency*/);
262  }
263  catch (...)
264  {
265  }
267  // don't overwrite an error msg from a caller:
269  {
272  std::string("doEmergencyStop called for: ") + msg;
273  }
274  MRPT_LOG_ERROR(msg);
275 }
276 
278 {
279  auto lck = mrpt::lockHelper(m_nav_cs);
280 
281  m_navigationEndEventSent = false;
282  m_navigationParams.reset();
283 }
284 
286 {
287  MRPT_START
288  auto lck = mrpt::lockHelper(m_nav_cs);
289 
290  ASSERT_(params != nullptr);
291  ASSERT_(
292  params->target.targetDesiredRelSpeed >= .0 &&
293  params->target.targetDesiredRelSpeed <= 1.0);
294 
295  // Copy data:
296  m_navigationParams = params->clone();
297 
298  // Transform: relative -> absolute, if needed.
299  if (m_navigationParams->target.targetIsRelative)
300  {
302  m_navigationParams->target.target_coords =
303  m_curPoseVel.pose + m_navigationParams->target.target_coords;
304  m_navigationParams->target.targetIsRelative =
305  false; // Now it's not relative
306  }
307 
308  // new state:
311 
312  // Reset the bad navigation alarm:
313  m_badNavAlarm_minDistTarget = std::numeric_limits<double>::max();
315 
316  MRPT_END
317 }
318 
321 {
322  MRPT_START
324  this->processNavigateCommand(params);
325  MRPT_END
326 }
327 
329 {
330  // Ignore calls too-close in time, e.g. from the navigationStep() methods of
331  // AbstractNavigator and a derived, overriding class.
332  const double robot_time_secs =
333  m_robot.getNavigationTime(); // this is clockwall time for real robots,
334  // simulated time in simulators.
335 
336  const double MIN_TIME_BETWEEN_POSE_UPDATES = 20e-3;
338  {
339  const double last_call_age =
340  robot_time_secs - m_last_curPoseVelUpdate_robot_time;
341  if (last_call_age < MIN_TIME_BETWEEN_POSE_UPDATES)
342  {
344  5.0,
345  "updateCurrentPoseAndSpeeds: ignoring call, since last call "
346  "was only %f ms ago.",
347  last_call_age * 1e3);
348  return; // previous data is still valid: don't query the robot
349  // again
350  }
351  }
352 
353  {
355  m_navProfiler, "getCurrentPoseAndSpeeds()");
356  m_curPoseVel.pose_frame_id = std::string("map"); // default
361  {
364  m_navErrorReason.error_msg = std::string(
365  "ERROR calling m_robot.getCurrentPoseAndSpeeds, stopping robot "
366  "and finishing navigation");
367  try
368  {
369  this->stop(true /*emergency*/);
370  }
371  catch (...)
372  {
373  }
375  "ERROR calling m_robot.getCurrentPoseAndSpeeds, stopping robot "
376  "and finishing navigation");
377  throw std::runtime_error(
378  "ERROR calling m_robot.getCurrentPoseAndSpeeds, stopping robot "
379  "and finishing navigation");
380  }
381  }
384 
385  m_last_curPoseVelUpdate_robot_time = robot_time_secs;
386  const bool changed_frame_id =
389 
390  if (changed_frame_id)
391  {
392  // If frame changed, clear past poses. This could be improved by
393  // requesting
394  // the transf between the two frames, but it's probably not worth.
397  }
398 
399  // Append to list of past poses:
402 
403  // Purge old ones:
404  while (m_latestPoses.size() > 1 &&
406  m_latestPoses.begin()->first, m_latestPoses.rbegin()->first) >
408  {
410  }
411  while (m_latestOdomPoses.size() > 1 &&
413  m_latestOdomPoses.begin()->first,
415  {
417  }
418 }
419 
420 /** \callergraph */
422  const mrpt::kinematics::CVehicleVelCmd& vel_cmd)
423 {
424  return m_robot.changeSpeeds(vel_cmd);
425 }
426 /** \callergraph */
428 /** \callergraph */
429 bool CAbstractNavigator::stop(bool isEmergencyStop)
430 {
431  return m_robot.stop(isEmergencyStop);
432 }
433 
435 
436  = default;
438  const mrpt::config::CConfigFileBase& c, const std::string& s)
439 {
444 }
446  mrpt::config::CConfigFileBase& c, const std::string& s) const
447 {
449  dist_to_target_for_sending_event,
450  "Default value=0, means use the `targetAllowedDistance` passed by the "
451  "user in the navigation request.");
453  alarm_seems_not_approaching_target_timeout,
454  "navigator timeout (seconds) [Default=30 sec]");
456  dist_check_target_is_blocked,
457  "When closer than this distance, check if the target is blocked to "
458  "abort navigation with an error. [Default=0.6 m]");
460  dist_to_target_for_sending_event,
461  "Default value=0, means use the `targetAllowedDistance` passed by the"
462  " user in the navigation request.");
464  alarm_seems_not_approaching_target_timeout,
465  "navigator timeout (seconds) [Default=30 sec]");
467  dist_check_target_is_blocked,
468  "When closer than this distance, check if the target is blocked to "
469  "abort navigation with an error. [Default=0.6 m]");
471  hysteresis_check_target_is_blocked,
472  "How many steps should the condition for dist_check_target_is_blocked "
473  "be fulfilled to raise an event");
474 }
475 
479 {
480  return typeid(a) == typeid(b) && a.isEqual(b);
481 }
482 
483 /** \callergraph */
484 bool CAbstractNavigator::checkHasReachedTarget(const double targetDist) const
485 {
486  return (targetDist < m_navigationParams->target.targetAllowedDistance);
487 }
488 
489 /** \callergraph */
491 {
492  m_robot.startWatchdog(1000); // Watchdog = 1 seg
493  m_latestPoses.clear(); // Clear cache of last poses.
496 }
497 
498 /** \callergraph */
500  bool call_virtual_nav_method)
501 {
502  const TState prevState = m_navigationState;
503  try
504  {
506  {
508  "[CAbstractNavigator::navigationStep()] Starting Navigation. "
509  "Watchdog initiated...\n");
510  if (m_navigationParams)
512  "[CAbstractNavigator::navigationStep()] Navigation "
513  "Params:\n%s\n",
514  m_navigationParams->getAsText().c_str()));
515 
517  }
518 
519  // Have we just started the navigation?
521  {
522  m_pending_events.emplace_back(std::bind(
524  std::ref(m_robot)));
525  }
526 
527  /* ----------------------------------------------------------------
528  Get current robot dyn state:
529  ---------------------------------------------------------------- */
531 
532  /* ----------------------------------------------------------------
533  Have we reached the target location?
534  ---------------------------------------------------------------- */
535  // Build a 2D segment from the current robot pose to the previous one:
537  const mrpt::math::TSegment2D seg_robot_mov = mrpt::math::TSegment2D(
539  m_latestPoses.size() > 1
540  ? mrpt::math::TPoint2D((++m_latestPoses.rbegin())->second)
541  : mrpt::math::TPoint2D((m_latestPoses.rbegin())->second));
542 
543  if (m_navigationParams)
544  {
545  const double targetDist = seg_robot_mov.distance(
546  mrpt::math::TPoint2D(m_navigationParams->target.target_coords));
547 
548  // Should "End of navigation" event be sent??
549  if (!m_navigationParams->target.targetIsIntermediaryWaypoint &&
551  targetDist <
553  {
555  m_pending_events.emplace_back(std::bind(
557  std::ref(m_robot)));
558  }
559 
560  // Have we really reached the target?
561  if (checkHasReachedTarget(targetDist))
562  {
564  logFmt(
566  "Navigation target (%.03f,%.03f) was reached\n",
567  m_navigationParams->target.target_coords.x,
568  m_navigationParams->target.target_coords.y);
569 
570  if (!m_navigationParams->target.targetIsIntermediaryWaypoint)
571  {
572  this->stop(false /*not emergency*/);
574  {
576  m_pending_events.emplace_back(std::bind(
578  std::ref(m_robot)));
579  }
580  }
581  return;
582  }
583 
584  // Check the "no approaching the target"-alarm:
585  // -----------------------------------------------------------
586  if (targetDist < m_badNavAlarm_minDistTarget)
587  {
588  m_badNavAlarm_minDistTarget = targetDist;
590  }
591  else
592  {
593  // Too much time have passed?
598  .alarm_seems_not_approaching_target_timeout)
599  {
601  "Timeout approaching the target. Aborting navigation.");
602 
605  m_navErrorReason.error_msg = std::string(
606  "Timeout approaching the target. Aborting navigation.");
607 
608  m_pending_events.emplace_back(std::bind(
610  std::ref(m_robot)));
611  return;
612  }
613  }
614 
615  // Check if the target seems to be at reach, but it's clearly
616  // occupied by obstacles:
617  if (targetDist <
619  {
620  const auto rel_trg = m_navigationParams->target.target_coords -
622  const bool is_col = checkCollisionWithLatestObstacles(rel_trg);
623  if (is_col)
624  {
625  const bool send_event =
629 
630  if (send_event)
631  {
633  5.0,
634  "Target seems to be blocked by obstacles. Invoking"
635  " sendCannotGetCloserToBlockedTargetEvent().");
636 
637  m_pending_events.emplace_back(std::bind(
639  sendCannotGetCloserToBlockedTargetEvent,
640  std::ref(m_robot)));
641 
643  }
644  }
645  else
646  {
648  }
649  }
650  }
651 
652  // The normal execution of the navigation: Execute one step:
653  if (call_virtual_nav_method)
654  {
656  }
657  }
658  catch (const std::exception& e)
659  {
662  {
665  std::string("Exception: ") + std::string(e.what());
666  }
667 
669  "[CAbstractNavigator::navigationStep] Exception:\n %s", e.what());
670  if (m_rethrow_exceptions) throw;
671  }
672  catch (...)
673  {
676  {
678  m_navErrorReason.error_msg = "Untyped exception";
679  }
680 
682  "[CAbstractNavigator::navigationStep] Untyped exception!");
683  if (m_rethrow_exceptions) throw;
684  }
685  m_navigationState = prevState;
686 }
687 
689  const mrpt::math::TPose2D& relative_robot_pose) const
690 {
691  // Default impl:
692  return false;
693 }
mrpt::nav::CAbstractNavigator::m_navigationState
TState m_navigationState
Current internal state of navigator:
Definition: CAbstractNavigator.h:348
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::nav::CAbstractNavigator::ERR_NONE
@ ERR_NONE
Definition: CAbstractNavigator.h:186
mrpt::nav::CRobot2NavInterface::changeSpeeds
virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd)=0
Sends a velocity command to the robot.
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::CAbstractNavigator::onNavigateCommandReceived
virtual void onNavigateCommandReceived()
Called after each call to CAbstractNavigator::navigate()
Definition: CAbstractNavigator.cpp:277
nav-precomp.h
mrpt::poses::CPoseInterpolatorBase::size
size_t size() const
Definition: CPoseInterpolatorBase.h:107
mrpt::nav::CAbstractNavigator::TargetInfo::TargetInfo
TargetInfo()
Definition: CAbstractNavigator.cpp:25
mrpt::nav::CAbstractNavigator::TRobotPoseVel::timestamp
mrpt::system::TTimeStamp timestamp
Definition: CAbstractNavigator.h:368
mrpt::nav::CRobot2NavInterface::sendNavigationEndEvent
virtual void sendNavigationEndEvent()
Callback: End of navigation command (reach of single goal, or final waypoint of waypoint list)
Definition: CRobot2NavInterface.cpp:63
mrpt::nav::CAbstractNavigator::ERR_OTHER
@ ERR_OTHER
Definition: CAbstractNavigator.h:189
mrpt::nav::CAbstractNavigator::performNavigationStep
virtual void performNavigationStep()=0
To be implemented in derived classes.
mrpt::nav::CAbstractNavigator::TAbstractNavigatorParams::dist_to_target_for_sending_event
double dist_to_target_for_sending_event
Default value=0, means use the "targetAllowedDistance" passed by the user in the navigation request.
Definition: CAbstractNavigator.h:239
INVALID_TIMESTAMP
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
CConfigFileMemory.h
mrpt::nav::CAbstractNavigator::TargetInfo
Individual target info in CAbstractNavigator::TNavigationParamsBase and derived classes.
Definition: CAbstractNavigator.h:68
mrpt::nav::CAbstractNavigator::NAV_ERROR
@ NAV_ERROR
Definition: CAbstractNavigator.h:177
MRPT_LOG_INFO
#define MRPT_LOG_INFO(_STRING)
Definition: system/COutputLogger.h:429
mrpt::nav::CAbstractNavigator::dispatchPendingNavEvents
void dispatchPendingNavEvents()
Definition: CAbstractNavigator.cpp:246
MRPT_SAVE_CONFIG_VAR_COMMENT
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
Definition: config/CConfigFileBase.h:480
mrpt::nav::CAbstractNavigator::checkHasReachedTarget
virtual bool checkHasReachedTarget(const double targetDist) const
Default implementation: check if target_dist is below the accepted distance.
Definition: CAbstractNavigator.cpp:484
mrpt::nav::CAbstractNavigator::TState
TState
The different states for the navigation system.
Definition: CAbstractNavigator.h:172
mrpt::system::COutputLogger::setVerbosityLevel
void setVerbosityLevel(const VerbosityLevel level)
alias of setMinLoggingLevel()
Definition: COutputLogger.cpp:149
mrpt::nav::CAbstractNavigator::m_last_curPoseVelUpdate_robot_time
double m_last_curPoseVelUpdate_robot_time
Definition: CAbstractNavigator.h:376
MRPT_LOG_DEBUG
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
Definition: system/COutputLogger.h:427
mrpt::nav::CAbstractNavigator::m_badNavAlarm_minDistTarget
double m_badNavAlarm_minDistTarget
For sending an alarm (error event) when it seems that we are not approaching toward the target in a w...
Definition: CAbstractNavigator.h:387
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::CAbstractNavigator::m_navErrorReason
TErrorReason m_navErrorReason
Definition: CAbstractNavigator.h:274
mrpt::nav::CAbstractNavigator::TRobotPoseVel::velLocal
mrpt::math::TTwist2D velLocal
Definition: CAbstractNavigator.h:364
mrpt::math::TPoint2D_< double >
mrpt::math::TTwist2D::rotate
void rotate(const double ang)
Transform the (vx,vy) components for a counterclockwise rotation of ang radians.
Definition: Twist2D.cpp:40
mrpt::nav::CAbstractNavigator::internal_onStartNewNavigation
void internal_onStartNewNavigation()
Called before starting a new navigation.
Definition: CAbstractNavigator.cpp:490
mrpt::nav::CAbstractNavigator::navigationStep
virtual void navigationStep()
This method must be called periodically in order to effectively run the navigation.
Definition: CAbstractNavigator.cpp:178
mrpt::nav::CAbstractNavigator::setFrameTF
void setFrameTF(const std::weak_ptr< mrpt::poses::FrameTransformer< 2 >> &frame_tf)
Sets a user-provided frame transformer object; used only if providing targets in a frame ID different...
Definition: CAbstractNavigator.cpp:145
mrpt::nav::CAbstractNavigator::m_rethrow_exceptions
bool m_rethrow_exceptions
Definition: CAbstractNavigator.h:279
mrpt::nav::CAbstractNavigator::m_navigationParams
std::unique_ptr< TNavigationParams > m_navigationParams
Current navigation parameters.
Definition: CAbstractNavigator.h:350
mrpt::nav::CRobot2NavInterface::startWatchdog
virtual bool startWatchdog(float T_ms)
Start the watchdog timer of the robot platform, if any, for maximum expected delay between consecutiv...
Definition: CRobot2NavInterface.cpp:37
mrpt::nav::CAbstractNavigator::TErrorReason
Definition: CAbstractNavigator.h:191
mrpt::nav::CAbstractNavigator::updateCurrentPoseAndSpeeds
virtual void updateCurrentPoseAndSpeeds()
Call to the robot getCurrentPoseAndSpeeds() and updates members m_curPoseVel accordingly.
Definition: CAbstractNavigator.cpp:328
mrpt::nav::CAbstractNavigator::processNavigateCommand
virtual void processNavigateCommand(const TNavigationParams *params)
Does the job of navigate(), except the call to onNavigateCommandReceived()
Definition: CAbstractNavigator.cpp:285
mrpt::nav::CRobot2NavInterface::changeSpeedsNOP
virtual bool changeSpeedsNOP()
Just like changeSpeeds(), but will be called when the last velocity command is still the preferred so...
Definition: CRobot2NavInterface.cpp:21
mrpt::nav::CAbstractNavigator::ERR_EMERGENCY_STOP
@ ERR_EMERGENCY_STOP
Definition: CAbstractNavigator.h:187
mrpt::nav::CRobot2NavInterface::getNavigationTime
virtual double getNavigationTime()
Returns the number of seconds ellapsed since the constructor of this class was invoked,...
Definition: CRobot2NavInterface.cpp:119
mrpt::nav::CAbstractNavigator::TAbstractNavigatorParams::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: CAbstractNavigator.cpp:445
mrpt::nav
Definition: CAbstractHolonomicReactiveMethod.h:20
mrpt::nav::CAbstractNavigator::TargetInfo::targetAllowedDistance
float targetAllowedDistance
(Default=0.5 meters) Allowed distance to target in order to end the navigation.
Definition: CAbstractNavigator.h:78
mrpt::nav::CAbstractNavigator::TRobotPoseVel::TRobotPoseVel
TRobotPoseVel()
Definition: CAbstractNavigator.cpp:76
mrpt::config::CConfigFileBase::read_bool
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:155
MRPT_LOG_THROTTLE_WARN
#define MRPT_LOG_THROTTLE_WARN(_PERIOD_SECONDS, _STRING)
Definition: system/COutputLogger.h:453
mrpt::poses::CPoseInterpolatorBase::erase
iterator erase(iterator element_to_erase)
Definition: CPoseInterpolatorBase.h:101
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::nav::CAbstractNavigator::m_navigationEndEventSent
bool m_navigationEndEventSent
Will be false until the navigation end is sent, and it is reset with each new command.
Definition: CAbstractNavigator.h:277
mrpt::nav::CRobot2NavInterface
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
Definition: CRobot2NavInterface.h:43
mrpt::nav::CAbstractNavigator::TargetInfo::targetIsRelative
bool targetIsRelative
(Default=false) Whether the target coordinates are in global coordinates (false) or are relative to t...
Definition: CAbstractNavigator.h:82
mrpt::nav::CAbstractNavigator::ERR_CANNOT_REACH_TARGET
@ ERR_CANNOT_REACH_TARGET
Definition: CAbstractNavigator.h:188
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::nav::CAbstractNavigator::TRobotPoseVel::pose
mrpt::math::TPose2D pose
Definition: CAbstractNavigator.h:363
mrpt::nav::CAbstractNavigator::TAbstractNavigatorParams::TAbstractNavigatorParams
TAbstractNavigatorParams()
mrpt::poses::CPoseInterpolatorBase::setInterpolationMethod
void setInterpolationMethod(TInterpolatorMethod method)
Change the method used to interpolate the robot path.
Definition: CPoseInterpolatorBase.hpp:361
mrpt::nav::CAbstractNavigator::TNavigationParams::isEqual
bool isEqual(const TNavigationParamsBase &o) const override
Definition: CAbstractNavigator.cpp:69
mrpt::poses::CPoseInterpolatorBase::rbegin
reverse_iterator rbegin()
Definition: CPoseInterpolatorBase.h:79
mrpt::nav::CAbstractNavigator::params_abstract_navigator
TAbstractNavigatorParams params_abstract_navigator
Definition: CAbstractNavigator.h:258
mrpt::poses::CPoseInterpolatorBase::insert
void insert(const mrpt::Clock::time_point &t, const pose_t &p)
Inserts a new pose in the sequence.
Definition: CPoseInterpolatorBase.hpp:47
mrpt::nav::CAbstractNavigator::onStartNewNavigation
virtual void onStartNewNavigation()=0
Called whenever a new navigation has been started.
mrpt::nav::CAbstractNavigator::loadConfigFile
virtual void loadConfigFile(const mrpt::config::CConfigFileBase &c)
Loads all params from a file.
Definition: CAbstractNavigator.cpp:151
mrpt::nav::CRobot2NavInterface::stopWatchdog
virtual bool stopWatchdog()
Stop the watchdog timer.
Definition: CRobot2NavInterface.cpp:47
mrpt::nav::CAbstractNavigator::TNavigationParams
The struct for configuring navigation requests.
Definition: CAbstractNavigator.h:118
mrpt::system::COutputLogger::logFmt
void logFmt(const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3
Alternative logging method, which mimics the printf behavior.
Definition: COutputLogger.cpp:91
mrpt::nav::operator==
bool operator==(const CAbstractNavigator::TNavigationParamsBase &, const CAbstractNavigator::TNavigationParamsBase &)
Definition: CAbstractNavigator.cpp:476
mrpt::nav::CAbstractNavigator::m_lastNavigationState
TState m_lastNavigationState
Last internal state of navigator:
Definition: CAbstractNavigator.h:273
mrpt::nav::CAbstractNavigator::performNavigationStepNavigating
virtual void performNavigationStepNavigating(bool call_virtual_nav_method=true)
Factorization of the part inside navigationStep(), for the case of state being NAVIGATING.
Definition: CAbstractNavigator.cpp:499
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::CAbstractNavigator::TargetInfo::target_frame_id
std::string target_frame_id
(Default="map") Frame ID in which target is given.
Definition: CAbstractNavigator.h:75
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::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_START
#define MRPT_START
Definition: exceptions.h:241
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::nav::CRobot2NavInterface::sendWaySeemsBlockedEvent
virtual void sendWaySeemsBlockedEvent()
Callback: No progression made towards target for a predefined period of time.
Definition: CRobot2NavInterface.cpp:94
MRPT_LOG_ERROR_FMT
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
Definition: system/COutputLogger.h:467
MRPT_LOG_ERROR
#define MRPT_LOG_ERROR(_STRING)
Definition: system/COutputLogger.h:433
mrpt::poses::imLinear2Neig
@ imLinear2Neig
Definition: CPoseInterpolatorBase.h:38
mrpt::nav::CAbstractNavigator::TargetInfo::target_coords
mrpt::math::TPose2D target_coords
Coordinates of desired target location.
Definition: CAbstractNavigator.h:72
mrpt::nav::CAbstractNavigator::m_latestOdomPoses
mrpt::poses::CPose2DInterpolator m_latestOdomPoses
Definition: CAbstractNavigator.h:379
mrpt::nav::CAbstractNavigator::m_robot
CRobot2NavInterface & m_robot
The navigator-robot interface.
Definition: CAbstractNavigator.h:353
TSegment2D.h
mrpt::poses::CPoseInterpolatorBase::clear
void clear()
Clears the current sequence of poses.
Definition: CPoseInterpolatorBase.hpp:35
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: TPose2D.h:22
mrpt::nav::CAbstractNavigator::TNavigationParams::getAsText
std::string getAsText() const override
Gets navigation params as a human-readable format.
Definition: CAbstractNavigator.cpp:61
mrpt::system::LVL_WARN
@ LVL_WARN
Definition: system/COutputLogger.h:32
mrpt::nav::CAbstractNavigator::TargetInfo::getAsText
std::string getAsText() const
Gets navigation params as a human-readable format.
Definition: CAbstractNavigator.cpp:32
mrpt::nav::CAbstractNavigator::stop
virtual bool stop(bool isEmergencyStop)
Default: forward call to m_robot.stop().
Definition: CAbstractNavigator.cpp:429
mrpt::system::CTimeLogger::isEnabled
bool isEnabled() const
Definition: system/CTimeLogger.h:119
CAbstractNavigator.h
mrpt::config::CConfigFileMemory::getContent
void getContent(std::string &str) const
Return the current contents of the virtual "config file".
Definition: CConfigFileMemory.cpp:65
mrpt::math::TSegment2D
2D segment, consisting of two points.
Definition: TSegment2D.h:20
lock_helper.h
mrpt::poses::FrameTransformer< 2 >
params
mrpt::vision::TStereoCalibParams params
Definition: chessboard_stereo_camera_calib_unittest.cpp:24
mrpt::nav::CAbstractNavigator::m_curPoseVel
TRobotPoseVel m_curPoseVel
Current robot pose (updated in CAbstractNavigator::navigationStep() )
Definition: CAbstractNavigator.h:375
mrpt::nav::CRobot2NavInterface::getCurrentPoseAndSpeeds
virtual bool getCurrentPoseAndSpeeds(mrpt::math::TPose2D &curPose, mrpt::math::TTwist2D &curVelGlobal, mrpt::system::TTimeStamp &timestamp, mrpt::math::TPose2D &curOdometry, std::string &frame_id)=0
Get the current pose and velocity of the robot.
mrpt::nav::CAbstractNavigator::NAVIGATING
@ NAVIGATING
Definition: CAbstractNavigator.h:175
mrpt::nav::CAbstractNavigator::~CAbstractNavigator
~CAbstractNavigator() override
dtor
MRPT_LOG_WARN
#define MRPT_LOG_WARN(_STRING)
Definition: system/COutputLogger.h:431
mrpt::nav::CAbstractNavigator::suspend
virtual void suspend()
Suspend current navegation.
Definition: CAbstractNavigator.cpp:119
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::nav::CAbstractNavigator::resetNavError
virtual void resetNavError()
Resets a NAV_ERROR state back to IDLE
Definition: CAbstractNavigator.cpp:133
mrpt::math::TSegment2D::distance
double distance(const TPoint2D &point) const
Distance to point.
Definition: TSegment2D.cpp:19
mrpt::nav::CAbstractNavigator::changeSpeedsNOP
virtual bool changeSpeedsNOP()
Default: forward call to m_robot.changeSpeedsNOP().
Definition: CAbstractNavigator.cpp:427
mrpt::system::LVL_DEBUG
@ LVL_DEBUG
Definition: system/COutputLogger.h:30
mrpt::nav::CRobot2NavInterface::stop
virtual bool stop(bool isEmergencyStop=true)=0
Stop the robot right now.
mrpt::system::getCurrentTime
mrpt::system::TTimeStamp getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.h:82
mrpt::poses::CPoseInterpolatorBase::begin
iterator begin()
Definition: CPoseInterpolatorBase.h:73
mrpt::nav::CAbstractNavigator::TAbstractNavigatorParams::hysteresis_check_target_is_blocked
int hysteresis_check_target_is_blocked
(Default=3) How many steps should the condition for dist_check_target_is_blocked be fulfilled to rais...
Definition: CAbstractNavigator.h:247
mrpt::nav::CAbstractNavigator::SUSPENDED
@ SUSPENDED
Definition: CAbstractNavigator.h:176
mrpt::nav::CAbstractNavigator::checkCollisionWithLatestObstacles
virtual bool checkCollisionWithLatestObstacles(const mrpt::math::TPose2D &relative_robot_pose) const
Checks whether the robot shape, when placed at the given pose (relative to the current pose),...
Definition: CAbstractNavigator.cpp:688
mrpt::system::CTimeLogger::enable
void enable(bool enabled=true)
Definition: system/CTimeLogger.h:117
mrpt::poses::CPoseInterpolatorBase::empty
bool empty() const
Definition: CPoseInterpolatorBase.h:108
mrpt::nav::CAbstractNavigator::TNavigationParamsBase::isEqual
virtual bool isEqual(const TNavigationParamsBase &o) const =0
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
MRPT_LOG_THROTTLE_DEBUG_FMT
#define MRPT_LOG_THROTTLE_DEBUG_FMT(_PERIOD_SECONDS, _FMT_STRING,...)
Usage: MRPT_LOG_THROTTLE_DEBUG_FMT(5.0, "i=%u", i);
Definition: system/COutputLogger.h:496
mrpt::nav::CAbstractNavigator::m_last_curPoseVelUpdate_pose_frame_id
std::string m_last_curPoseVelUpdate_pose_frame_id
Definition: CAbstractNavigator.h:377
mrpt::nav::CAbstractNavigator::cancel
virtual void cancel()
Cancel current navegation.
Definition: CAbstractNavigator.cpp:100
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::nav::CRobot2NavInterface::sendNavigationStartEvent
virtual void sendNavigationStartEvent()
Callback: Start of navigation command.
Definition: CRobot2NavInterface.cpp:56
mrpt::nav::CAbstractNavigator::CAbstractNavigator
CAbstractNavigator(CRobot2NavInterface &robot_interface_impl)
ctor
Definition: CAbstractNavigator.cpp:89
mrpt::nav::CAbstractNavigator::navigate
virtual void navigate(const TNavigationParams *params)
Navigation request to a single target location.
Definition: CAbstractNavigator.cpp:319
mrpt::nav::CAbstractNavigator::IDLE
@ IDLE
Definition: CAbstractNavigator.h:174
mrpt::nav::CAbstractNavigator::m_counter_check_target_is_blocked
int m_counter_check_target_is_blocked
Definition: CAbstractNavigator.h:278
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::CAbstractNavigator::TErrorReason::error_msg
std::string error_msg
Human friendly description of the error.
Definition: CAbstractNavigator.h:197
mrpt::nav::CAbstractNavigator::TargetInfo::operator==
bool operator==(const TargetInfo &o) const
Definition: CAbstractNavigator.cpp:49
mrpt::nav::CAbstractNavigator::TAbstractNavigatorParams::alarm_seems_not_approaching_target_timeout
double alarm_seems_not_approaching_target_timeout
navigator timeout (seconds) [Default=30 sec]
Definition: CAbstractNavigator.h:241
mrpt::config::CConfigFileMemory
This class implements a config file-like interface over a memory-stored string list.
Definition: config/CConfigFileMemory.h:36
mrpt::nav::CAbstractNavigator::TRobotPoseVel::velGlobal
mrpt::math::TTwist2D velGlobal
Definition: CAbstractNavigator.h:364
mrpt::nav::CRobot2NavInterface::sendNavigationEndDueToErrorEvent
virtual void sendNavigationEndDueToErrorEvent()
Callback: Error asking sensory data from robot or sending motor commands.
Definition: CRobot2NavInterface.cpp:86
mrpt::kinematics::CVehicleVelCmd
Virtual base for velocity commands of different kinematic models of planar mobile robot.
Definition: CVehicleVelCmd.h:20
mrpt::nav::CAbstractNavigator::TargetInfo::targetIsIntermediaryWaypoint
bool targetIsIntermediaryWaypoint
Definition: CAbstractNavigator.h:89
mrpt::nav::CAbstractNavigator::TAbstractNavigatorParams::dist_check_target_is_blocked
double dist_check_target_is_blocked
(Default value=0.6) When closer than this distance, check if the target is blocked to abort navigatio...
Definition: CAbstractNavigator.h:244
mrpt::nav::CAbstractNavigator::TErrorReason::error_code
TErrorCode error_code
Definition: CAbstractNavigator.h:195
mrpt::nav::CAbstractNavigator::saveConfigFile
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const
Saves all current options to a config file.
Definition: CAbstractNavigator.cpp:172
mrpt::nav::CAbstractNavigator::m_latestPoses
mrpt::poses::CPose2DInterpolator m_latestPoses
Latest robot poses (updated in CAbstractNavigator::navigationStep() )
Definition: CAbstractNavigator.h:379
mrpt::nav::CAbstractNavigator::TAbstractNavigatorParams::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: CAbstractNavigator.cpp:437
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::system::COutputLogger::COutputLogger
COutputLogger()
Default class constructor.
Definition: COutputLogger.cpp:70
mrpt::nav::CAbstractNavigator::m_badNavAlarm_lastMinDistTime
mrpt::system::TTimeStamp m_badNavAlarm_lastMinDistTime
Definition: CAbstractNavigator.h:388
mrpt::nav::CAbstractNavigator::TNavigationParamsBase
Base for all high-level navigation commands.
Definition: CAbstractNavigator.h:104
mrpt::nav::CAbstractNavigator::m_nav_cs
std::recursive_mutex m_nav_cs
mutex for all navigation methods
Definition: CAbstractNavigator.h:359
mrpt::nav::CAbstractNavigator::resume
virtual void resume()
Continues with suspended navigation.
Definition: CAbstractNavigator.cpp:110
PREVIOUS_POSES_MAX_AGE
const double PREVIOUS_POSES_MAX_AGE
Definition: CAbstractNavigator.cpp:22



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 29 13:06:46 UTC 2020