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-2019, 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/math/TSegment2D.h>
15 #include <limits>
16 #include <typeinfo>
17 
18 using namespace mrpt::nav;
19 using namespace std;
20 
21 const double PREVIOUS_POSES_MAX_AGE = 20; // seconds
22 
23 // Ctor: CAbstractNavigator::TargetInfo
25  : target_coords(0, 0, 0), target_frame_id("map")
26 
27 {
28 }
29 
30 // Gets navigation params as a human-readable format:
32 {
33  string s;
34  s += mrpt::format(
35  "target_coords = (%.03f,%.03f,%.03f deg)\n", target_coords.x,
36  target_coords.y, target_coords.phi);
37  s += mrpt::format("target_frame_id = \"%s\"\n", target_frame_id.c_str());
38  s += mrpt::format("targetAllowedDistance = %.03f\n", targetAllowedDistance);
39  s += mrpt::format(
40  "targetIsRelative = %s\n", targetIsRelative ? "YES" : "NO");
41  s += mrpt::format(
42  "targetIsIntermediaryWaypoint = %s\n",
43  targetIsIntermediaryWaypoint ? "YES" : "NO");
44  s += mrpt::format("targetDesiredRelSpeed = %.02f\n", targetDesiredRelSpeed);
45  return s;
46 }
47 
49  const TargetInfo& o) const
50 {
51  return target_coords == o.target_coords &&
52  target_frame_id == o.target_frame_id &&
53  targetAllowedDistance == o.targetAllowedDistance &&
54  targetIsRelative == o.targetIsRelative &&
55  targetDesiredRelSpeed == o.targetDesiredRelSpeed &&
56  targetIsIntermediaryWaypoint == o.targetIsIntermediaryWaypoint;
57 }
58 
59 // Gets navigation params as a human-readable format:
61 {
62  string s;
63  s += "navparams. Single target:\n";
64  s += target.getAsText();
65  return s;
66 }
67 
70 {
71  auto* rhs = dynamic_cast<const CAbstractNavigator::TNavigationParams*>(&o);
72  return (rhs != nullptr) && (this->target == rhs->target);
73 }
74 
76  : pose(0, 0, 0),
77  velGlobal(0, 0, 0),
78  velLocal(0, 0, 0),
79  rawOdometry(0, 0, 0),
80  timestamp(INVALID_TIMESTAMP),
81  pose_frame_id()
82 {
83 }
84 
85 /*---------------------------------------------------------------
86  Constructor
87  ---------------------------------------------------------------*/
89  : mrpt::system::COutputLogger("MRPT_navigator"), m_robot(react_iterf_impl)
90 {
94 }
95 
96 // Dtor:
98 /** \callergraph */
100 {
101  std::lock_guard<std::recursive_mutex> csl(m_nav_cs);
102  MRPT_LOG_DEBUG("CAbstractNavigator::cancel() called.");
104  this->stop(false /*not emergency*/);
105 }
106 
107 /** \callergraph */
109 {
110  std::lock_guard<std::recursive_mutex> csl(m_nav_cs);
111 
112  MRPT_LOG_DEBUG("[CAbstractNavigator::resume() called.");
114 }
115 
116 /** \callergraph */
118 {
119  std::lock_guard<std::recursive_mutex> csl(m_nav_cs);
120 
121  // Issue an "stop" if we are moving:
122  // Update: do it *always*, even if the current velocity is zero, since
123  // we may be in the middle of a multi-part motion command. It's safer.
124  this->stop(false /*not an emergency stop*/);
125 
126  MRPT_LOG_DEBUG("CAbstractNavigator::suspend() called.");
128 }
129 
130 /** \callergraph */
132 {
133  std::lock_guard<std::recursive_mutex> csl(m_nav_cs);
134 
135  MRPT_LOG_DEBUG("CAbstractNavigator::resetNavError() called.");
137  {
140  }
141 }
142 
144  const std::weak_ptr<mrpt::poses::FrameTransformer<2>>& frame_tf)
145 {
146  m_frame_tf = frame_tf;
147 }
148 
150 {
151  MRPT_START
152 
153  params_abstract_navigator.loadFromConfigFile(c, "CAbstractNavigator");
154 
155  m_navProfiler.enable(c.read_bool(
156  "CAbstractNavigator", "enable_time_profiler",
158 
159  // At this point, all derived classes have already loaded their parameters.
160  // Dump them to debug output:
161  {
163  this->saveConfigFile(cfg_mem);
164  MRPT_LOG_INFO(cfg_mem.getContent());
165  }
166 
167  MRPT_END
168 }
169 
171 {
172  params_abstract_navigator.saveToConfigFile(c, "CAbstractNavigator");
173 }
174 
175 /** \callergraph */
177 {
178  std::lock_guard<std::recursive_mutex> csl(m_nav_cs);
180  m_navProfiler, "CAbstractNavigator::navigationStep()");
181 
182  const TState prevState = m_navigationState;
183  switch (m_navigationState)
184  {
185  case IDLE:
186  case SUSPENDED:
187  try
188  {
189  // If we just arrived at this state, stop robot:
191  {
193  "[CAbstractNavigator::navigationStep()] Navigation "
194  "stopped.");
195  // this->stop(); stop() is called by the method switching
196  // the "state", so we have more flexibility
198  }
199  }
200  catch (...)
201  {
202  }
203  break;
204 
205  case NAV_ERROR:
206  try
207  {
208  // Send end-of-navigation event:
211  {
212  m_pending_events.emplace_back(std::bind(
214  std::ref(m_robot)));
215  }
216 
217  // If we just arrived at this state, stop the robot:
219  {
221  "[CAbstractNavigator::navigationStep()] Stopping "
222  "Navigation due to a NAV_ERROR state!");
223  this->stop(false /*not emergency*/);
225  }
226  }
227  catch (...)
228  {
229  }
230  break;
231 
232  case NAVIGATING:
234  true /* do call virtual method nav implementation*/);
235  break; // End case NAVIGATING
236  };
237  m_lastNavigationState = prevState;
238 
240 }
241 
242 /** \callergraph */
244 {
245  // Invoke pending events:
246  for (auto& ev : m_pending_events)
247  {
248  ev();
249  }
250  m_pending_events.clear();
251 }
252 
253 /** \callergraph */
255 {
256  try
257  {
258  this->stop(true /* emergency*/);
259  }
260  catch (...)
261  {
262  }
264  // don't overwrite an error msg from a caller:
266  {
269  std::string("doEmergencyStop called for: ") + msg;
270  }
271  MRPT_LOG_ERROR(msg);
272 }
273 
275 {
276  std::lock_guard<std::recursive_mutex> csl(m_nav_cs);
277 
278  m_navigationEndEventSent = false;
279  m_navigationParams.reset();
280 }
281 
283 {
284  MRPT_START;
285  std::lock_guard<std::recursive_mutex> csl(m_nav_cs);
286 
287  ASSERT_(params != nullptr);
288  ASSERT_(
289  params->target.targetDesiredRelSpeed >= .0 &&
290  params->target.targetDesiredRelSpeed <= 1.0);
291 
292  // Copy data:
293  m_navigationParams = params->clone();
294 
295  // Transform: relative -> absolute, if needed.
296  if (m_navigationParams->target.targetIsRelative)
297  {
299  m_navigationParams->target.target_coords =
300  m_curPoseVel.pose + m_navigationParams->target.target_coords;
301  m_navigationParams->target.targetIsRelative =
302  false; // Now it's not relative
303  }
304 
305  // new state:
308 
309  // Reset the bad navigation alarm:
310  m_badNavAlarm_minDistTarget = std::numeric_limits<double>::max();
312 
313  MRPT_END;
314 }
315 
318 {
319  MRPT_START;
321  this->processNavigateCommand(params);
322  MRPT_END;
323 }
324 
326 {
327  // Ignore calls too-close in time, e.g. from the navigationStep() methods of
328  // AbstractNavigator and a derived, overriding class.
329  const double robot_time_secs =
330  m_robot.getNavigationTime(); // this is clockwall time for real robots,
331  // simulated time in simulators.
332 
333  const double MIN_TIME_BETWEEN_POSE_UPDATES = 20e-3;
335  {
336  const double last_call_age =
337  robot_time_secs - m_last_curPoseVelUpdate_robot_time;
338  if (last_call_age < MIN_TIME_BETWEEN_POSE_UPDATES)
339  {
341  5.0,
342  "updateCurrentPoseAndSpeeds: ignoring call, since last call "
343  "was only %f ms ago.",
344  last_call_age * 1e3);
345  return; // previous data is still valid: don't query the robot
346  // again
347  }
348  }
349 
350  {
352  m_navProfiler, "getCurrentPoseAndSpeeds()");
353  m_curPoseVel.pose_frame_id = std::string("map"); // default
358  {
362  "ERROR calling m_robot.getCurrentPoseAndSpeeds, stopping robot "
363  "and finishing navigation");
364  try
365  {
366  this->stop(true /*emergency*/);
367  }
368  catch (...)
369  {
370  }
372  "ERROR calling m_robot.getCurrentPoseAndSpeeds, stopping robot "
373  "and finishing navigation");
374  throw std::runtime_error(
375  "ERROR calling m_robot.getCurrentPoseAndSpeeds, stopping robot "
376  "and finishing navigation");
377  }
378  }
381 
382  m_last_curPoseVelUpdate_robot_time = robot_time_secs;
383  const bool changed_frame_id =
386 
387  if (changed_frame_id)
388  {
389  // If frame changed, clear past poses. This could be improved by
390  // requesting
391  // the transf between the two frames, but it's probably not worth.
394  }
395 
396  // Append to list of past poses:
399 
400  // Purge old ones:
401  while (m_latestPoses.size() > 1 &&
403  m_latestPoses.begin()->first, m_latestPoses.rbegin()->first) >
405  {
407  }
408  while (m_latestOdomPoses.size() > 1 &&
410  m_latestOdomPoses.begin()->first,
412  {
414  }
415 }
416 
417 /** \callergraph */
419  const mrpt::kinematics::CVehicleVelCmd& vel_cmd)
420 {
421  return m_robot.changeSpeeds(vel_cmd);
422 }
423 /** \callergraph */
425 /** \callergraph */
426 bool CAbstractNavigator::stop(bool isEmergencyStop)
427 {
428  return m_robot.stop(isEmergencyStop);
429 }
430 
432 
433  = default;
436 {
441 }
444 {
446  dist_to_target_for_sending_event,
447  "Default value=0, means use the `targetAllowedDistance` passed by the "
448  "user in the navigation request.");
450  alarm_seems_not_approaching_target_timeout,
451  "navigator timeout (seconds) [Default=30 sec]");
453  dist_check_target_is_blocked,
454  "When closer than this distance, check if the target is blocked to "
455  "abort navigation with an error. [Default=0.6 m]");
457  dist_to_target_for_sending_event,
458  "Default value=0, means use the `targetAllowedDistance` passed by the"
459  " user in the navigation request.");
461  alarm_seems_not_approaching_target_timeout,
462  "navigator timeout (seconds) [Default=30 sec]");
464  dist_check_target_is_blocked,
465  "When closer than this distance, check if the target is blocked to "
466  "abort navigation with an error. [Default=0.6 m]");
468  hysteresis_check_target_is_blocked,
469  "How many steps should the condition for dist_check_target_is_blocked "
470  "be fulfilled to raise an event");
471 }
472 
476 {
477  return typeid(a) == typeid(b) && a.isEqual(b);
478 }
479 
480 /** \callergraph */
481 bool CAbstractNavigator::checkHasReachedTarget(const double targetDist) const
482 {
483  return (targetDist < m_navigationParams->target.targetAllowedDistance);
484 }
485 
486 /** \callergraph */
488 {
489  m_robot.startWatchdog(1000); // Watchdog = 1 seg
490  m_latestPoses.clear(); // Clear cache of last poses.
493 }
494 
495 /** \callergraph */
497  bool call_virtual_nav_method)
498 {
499  const TState prevState = m_navigationState;
500  try
501  {
503  {
505  "[CAbstractNavigator::navigationStep()] Starting Navigation. "
506  "Watchdog initiated...\n");
507  if (m_navigationParams)
509  "[CAbstractNavigator::navigationStep()] Navigation "
510  "Params:\n%s\n",
511  m_navigationParams->getAsText().c_str()));
512 
514  }
515 
516  // Have we just started the navigation?
518  {
519  m_pending_events.emplace_back(std::bind(
521  std::ref(m_robot)));
522  }
523 
524  /* ----------------------------------------------------------------
525  Get current robot dyn state:
526  ---------------------------------------------------------------- */
528 
529  /* ----------------------------------------------------------------
530  Have we reached the target location?
531  ---------------------------------------------------------------- */
532  // Build a 2D segment from the current robot pose to the previous one:
534  const mrpt::math::TSegment2D seg_robot_mov = mrpt::math::TSegment2D(
536  m_latestPoses.size() > 1
537  ? mrpt::math::TPoint2D((++m_latestPoses.rbegin())->second)
538  : mrpt::math::TPoint2D((m_latestPoses.rbegin())->second));
539 
540  if (m_navigationParams)
541  {
542  const double targetDist = seg_robot_mov.distance(
543  mrpt::math::TPoint2D(m_navigationParams->target.target_coords));
544 
545  // Should "End of navigation" event be sent??
546  if (!m_navigationParams->target.targetIsIntermediaryWaypoint &&
548  targetDist <
550  {
552  m_pending_events.emplace_back(std::bind(
554  std::ref(m_robot)));
555  }
556 
557  // Have we really reached the target?
558  if (checkHasReachedTarget(targetDist))
559  {
561  logFmt(
563  "Navigation target (%.03f,%.03f) was reached\n",
564  m_navigationParams->target.target_coords.x,
565  m_navigationParams->target.target_coords.y);
566 
567  if (!m_navigationParams->target.targetIsIntermediaryWaypoint)
568  {
569  this->stop(false /*not emergency*/);
571  {
573  m_pending_events.emplace_back(std::bind(
575  std::ref(m_robot)));
576  }
577  }
578  return;
579  }
580 
581  // Check the "no approaching the target"-alarm:
582  // -----------------------------------------------------------
583  if (targetDist < m_badNavAlarm_minDistTarget)
584  {
585  m_badNavAlarm_minDistTarget = targetDist;
587  }
588  else
589  {
590  // Too much time have passed?
595  .alarm_seems_not_approaching_target_timeout)
596  {
598  "Timeout approaching the target. Aborting navigation.");
599 
603  "Timeout approaching the target. Aborting navigation.");
604 
605  m_pending_events.emplace_back(std::bind(
607  std::ref(m_robot)));
608  return;
609  }
610  }
611 
612  // Check if the target seems to be at reach, but it's clearly
613  // occupied by obstacles:
614  if (targetDist <
616  {
617  const auto rel_trg = m_navigationParams->target.target_coords -
619  const bool is_col = checkCollisionWithLatestObstacles(rel_trg);
620  if (is_col)
621  {
622  const bool send_event =
626 
627  if (send_event)
628  {
630  5.0,
631  "Target seems to be blocked by obstacles. Invoking"
632  " sendCannotGetCloserToBlockedTargetEvent().");
633 
634  m_pending_events.emplace_back(std::bind(
636  sendCannotGetCloserToBlockedTargetEvent,
637  std::ref(m_robot)));
638 
640  }
641  }
642  else
643  {
645  }
646  }
647  }
648 
649  // The normal execution of the navigation: Execute one step:
650  if (call_virtual_nav_method)
651  {
653  }
654  }
655  catch (const std::exception& e)
656  {
659  {
662  std::string("Exception: ") + std::string(e.what());
663  }
664 
666  "[CAbstractNavigator::navigationStep] Exception:\n %s", e.what());
667  if (m_rethrow_exceptions) throw;
668  }
669  catch (...)
670  {
673  {
675  m_navErrorReason.error_msg = "Untyped exception";
676  }
677 
679  "[CAbstractNavigator::navigationStep] Untyped exception!");
680  if (m_rethrow_exceptions) throw;
681  }
682  m_navigationState = prevState;
683 }
684 
686  const mrpt::math::TPose2D& relative_robot_pose) const
687 {
688  // Default impl:
689  return false;
690 }
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...
void rotate(const double ang)
Transform the (vx,vy) components for a counterclockwise rotation of ang radians.
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");
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
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.
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
GLenum GLint ref
Definition: glext.h:4062
#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
double distance(const TPoint2D &point) const
Distance to point.
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);
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...
GLdouble s
Definition: glext.h:3682
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().
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.
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.
const GLubyte * c
Definition: glext.h:6406
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.
GLubyte GLubyte b
Definition: glext.h:6372
Individual target info in CAbstractNavigator::TNavigationParamsBase and derived classes.
virtual void onStartNewNavigation()=0
Called whenever a new navigation has been started.
GLsizei const GLchar ** string
Definition: glext.h:4116
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. ...
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
Lightweight 2D point.
Definition: TPoint2D.h:31
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
GLenum const GLfloat * params
Definition: glext.h:3538
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: 836b840ab Mon Nov 18 00:58:29 2019 +0100 at lun nov 18 01:00:14 CET 2019