MRPT  1.9.9
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 }
double m_badNavAlarm_minDistTarget
For sending an alarm (error event) when it seems that we are not approaching toward the target in a w...
virtual bool stop(bool isEmergencyStop=true)=0
Stop the robot right now.
std::recursive_mutex m_nav_cs
mutex for all navigation methods
TRobotPoseVel m_curPoseVel
Current robot pose (updated in CAbstractNavigator::navigationStep() )
std::list< std::function< void(void)> > m_pending_events
Events generated during navigationStep(), enqueued to be called at the end of the method execution to...
This class implements a config file-like interface over a memory-stored string list.
virtual bool startWatchdog(float T_ms)
Start the watchdog timer of the robot platform, if any, for maximum expected delay between consecutiv...
virtual void loadConfigFile(const mrpt::config::CConfigFileBase &c)
Loads all params from a file.
#define MRPT_START
Definition: exceptions.h:241
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
double distance(const TPoint2D &point) const
Distance to point.
Definition: TSegment2D.cpp:19
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
TPoint2D_< double > TPoint2D
Lightweight 2D point.
Definition: TPoint2D.h:213
virtual bool stopWatchdog()
Stop the watchdog timer.
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...
void logFmt(const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3
Alternative logging method, which mimics the printf behavior.
void rotate(const double ang)
Transform the (vx,vy) components for a counterclockwise rotation of ang radians.
Definition: Twist2D.cpp:40
A safe way to call enter() and leave() of a mrpt::system::CTimeLogger upon construction and destructi...
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::system::TTimeStamp getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.h:82
#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 ...
~CAbstractNavigator() override
dtor
Base for all high-level navigation commands.
float targetAllowedDistance
(Default=0.5 meters) Allowed distance to target in order to end the navigation.
#define MRPT_LOG_THROTTLE_DEBUG_FMT(_PERIOD_SECONDS, _FMT_STRING,...)
Usage: MRPT_LOG_THROTTLE_DEBUG_FMT(5.0, "i=%u", i);
mrpt::vision::TStereoCalibParams params
virtual void sendNavigationEndEvent()
Callback: End of navigation command (reach of single goal, or final waypoint of waypoint list) ...
STL namespace.
virtual void updateCurrentPoseAndSpeeds()
Call to the robot getCurrentPoseAndSpeeds() and updates members m_curPoseVel accordingly.
bool targetIsRelative
(Default=false) Whether the target coordinates are in global coordinates (false) or are relative to t...
virtual double getNavigationTime()
Returns the number of seconds ellapsed since the constructor of this class was invoked, or since the last call of resetNavigationTimer().
double dist_check_target_is_blocked
(Default value=0.6) When closer than this distance, check if the target is blocked to abort navigatio...
void setInterpolationMethod(TInterpolatorMethod method)
Change the method used to interpolate the robot path.
virtual void navigationStep()
This method must be called periodically in order to effectively run the navigation.
std::string pose_frame_id
map frame ID for pose
virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd)
Default: forward call to m_robot.changeSpeed().
virtual bool changeSpeedsNOP()
Default: forward call to m_robot.changeSpeedsNOP().
virtual bool isEqual(const TNavigationParamsBase &o) const =0
TState m_navigationState
Current internal state of navigator:
Virtual base for velocity commands of different kinematic models of planar mobile robot...
virtual void performNavigationStep()=0
To be implemented in derived classes.
bool operator==(const CAbstractNavigator::TNavigationParamsBase &, const CAbstractNavigator::TNavigationParamsBase &)
The struct for configuring navigation requests.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
CAbstractNavigator(CRobot2NavInterface &robot_interface_impl)
ctor
This class allows loading and storing values and vectors of different types from a configuration text...
TState
The different states for the navigation system.
LockHelper< T > lockHelper(T &t)
Syntactic sugar to easily create a locker to any kind of std::mutex.
Definition: lock_helper.h:50
2D segment, consisting of two points.
Definition: TSegment2D.h:20
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.
double dist_to_target_for_sending_event
Default value=0, means use the "targetAllowedDistance" passed by the user in the navigation request...
double targetDesiredRelSpeed
(Default=.05) Desired relative speed (wrt maximum speed), in range [0,1], of the vehicle at target...
virtual void onNavigateCommandReceived()
Called after each call to CAbstractNavigator::navigate()
virtual void sendNavigationStartEvent()
Callback: Start of navigation command.
virtual void sendNavigationEndDueToErrorEvent()
Callback: Error asking sensory data from robot or sending motor commands.
std::string target_frame_id
(Default="map") Frame ID in which target is given.
virtual void resetNavError()
Resets a NAV_ERROR state back to IDLE
std::unique_ptr< TNavigationParams > m_navigationParams
Current navigation parameters.
void internal_onStartNewNavigation()
Called before starting a new navigation.
Individual target info in CAbstractNavigator::TNavigationParamsBase and derived classes.
virtual void onStartNewNavigation()=0
Called whenever a new navigation has been started.
virtual void sendWaySeemsBlockedEvent()
Callback: No progression made towards target for a predefined period of time.
virtual void resume()
Continues with suspended navigation.
mrpt::math::TPose2D target_coords
Coordinates of desired target location.
TState m_lastNavigationState
Last internal state of navigator:
double alarm_seems_not_approaching_target_timeout
navigator timeout (seconds) [Default=30 sec]
void enable(bool enabled=true)
void setVerbosityLevel(const VerbosityLevel level)
alias of setMinLoggingLevel()
const double PREVIOUS_POSES_MAX_AGE
mrpt::system::CTimeLogger m_navProfiler
Publicly available time profiling object.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
virtual void performNavigationStepNavigating(bool call_virtual_nav_method=true)
Factorization of the part inside navigationStep(), for the case of state being NAVIGATING.
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.
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const
Saves all current options to a config file.
virtual bool changeSpeedsNOP()
Just like changeSpeeds(), but will be called when the last velocity command is still the preferred so...
std::string getAsText() const
Gets navigation params as a human-readable format.
void clear()
Clears the current sequence of poses.
virtual void doEmergencyStop(const std::string &msg)
Stops the robot and set navigation state to error.
virtual void processNavigateCommand(const TNavigationParams *params)
Does the job of navigate(), except the call to onNavigateCommandReceived()
#define MRPT_LOG_ERROR(_STRING)
TAbstractNavigatorParams params_abstract_navigator
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. ...
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
CRobot2NavInterface & m_robot
The navigator-robot interface.
COutputLogger()
Default class constructor.
#define MRPT_END
Definition: exceptions.h:245
bool isEqual(const TNavigationParamsBase &o) const override
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)...
void getContent(std::string &str) const
Return the current contents of the virtual "config file".
Lightweight 2D pose.
Definition: TPose2D.h:22
virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd)=0
Sends a velocity command to the robot.
iterator erase(iterator element_to_erase)
virtual void cancel()
Cancel current navegation.
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
std::string error_msg
Human friendly description of the error.
bool operator==(const TargetInfo &o) const
#define MRPT_LOG_WARN(_STRING)
virtual void suspend()
Suspend current navegation.
std::weak_ptr< mrpt::poses::FrameTransformer< 2 > > m_frame_tf
Optional, user-provided frame transformer.
void insert(const mrpt::Clock::time_point &t, const pose_t &p)
Inserts a new pose in the sequence.
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
int hysteresis_check_target_is_blocked
(Default=3) How many steps should the condition for dist_check_target_is_blocked be fulfilled to rais...
virtual bool checkHasReachedTarget(const double targetDist) const
Default implementation: check if target_dist is below the accepted distance.
mrpt::poses::CPose2DInterpolator m_latestOdomPoses
bool m_navigationEndEventSent
Will be false until the navigation end is sent, and it is reset with each new command.
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
mrpt::system::TTimeStamp m_badNavAlarm_lastMinDistTime
mrpt::poses::CPose2DInterpolator m_latestPoses
Latest robot poses (updated in CAbstractNavigator::navigationStep() )
double phi
Orientation (rads)
Definition: TPose2D.h:32
std::string getAsText() const override
Gets navigation params as a human-readable format.
#define MRPT_LOG_THROTTLE_WARN(_PERIOD_SECONDS, _STRING)
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
mrpt::math::TPose2D rawOdometry
raw odometry (frame does not match to "pose", but is expected to be smoother in the short term)...
virtual bool stop(bool isEmergencyStop)
Default: forward call to m_robot.stop().
#define MRPT_LOG_INFO(_STRING)
virtual void navigate(const TNavigationParams *params)
Navigation request to a single target location.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020