Main MRPT website > C++ reference for MRPT 1.9.9
CAbstractNavigator.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-2018, 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 
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),
26  target_frame_id("map"),
27  targetAllowedDistance(0.5),
28  targetIsRelative(false),
29  targetDesiredRelSpeed(.05),
30  targetIsIntermediaryWaypoint(false)
31 {
32 }
33 
34 // Gets navigation params as a human-readable format:
36 {
37  string s;
38  s += mrpt::format(
39  "target_coords = (%.03f,%.03f,%.03f deg)\n", target_coords.x,
40  target_coords.y, target_coords.phi);
41  s += mrpt::format("target_frame_id = \"%s\"\n", target_frame_id.c_str());
42  s += mrpt::format("targetAllowedDistance = %.03f\n", targetAllowedDistance);
43  s += mrpt::format(
44  "targetIsRelative = %s\n", targetIsRelative ? "YES" : "NO");
45  s += mrpt::format(
46  "targetIsIntermediaryWaypoint = %s\n",
47  targetIsIntermediaryWaypoint ? "YES" : "NO");
48  s += mrpt::format("targetDesiredRelSpeed = %.02f\n", targetDesiredRelSpeed);
49  return s;
50 }
51 
53  const TargetInfo& o) const
54 {
55  return target_coords == o.target_coords &&
56  target_frame_id == o.target_frame_id &&
57  targetAllowedDistance == o.targetAllowedDistance &&
58  targetIsRelative == o.targetIsRelative &&
59  targetDesiredRelSpeed == o.targetDesiredRelSpeed &&
60  targetIsIntermediaryWaypoint == o.targetIsIntermediaryWaypoint;
61 }
62 
63 // Gets navigation params as a human-readable format:
65 {
66  string s;
67  s += "navparams. Single target:\n";
68  s += target.getAsText();
69  return s;
70 }
71 
74 {
75  auto* rhs = dynamic_cast<const CAbstractNavigator::TNavigationParams*>(&o);
76  return (rhs != nullptr) && (this->target == rhs->target);
77 }
78 
80  : pose(0, 0, 0),
81  velGlobal(0, 0, 0),
82  velLocal(0, 0, 0),
83  rawOdometry(0, 0, 0),
84  timestamp(INVALID_TIMESTAMP),
85  pose_frame_id()
86 {
87 }
88 
89 /*---------------------------------------------------------------
90  Constructor
91  ---------------------------------------------------------------*/
93  : mrpt::system::COutputLogger("MRPT_navigator"),
98  m_robot(react_iterf_impl),
99  m_curPoseVel(),
101  m_latestPoses(),
103  m_timlog_delays(true, "CAbstractNavigator::m_timlog_delays")
104 {
108 }
109 
110 // Dtor:
112 /** \callergraph */
114 {
115  std::lock_guard<std::recursive_mutex> csl(m_nav_cs);
116  MRPT_LOG_DEBUG("CAbstractNavigator::cancel() called.");
118  this->stop(false /*not emergency*/);
119 }
120 
121 /** \callergraph */
123 {
124  std::lock_guard<std::recursive_mutex> csl(m_nav_cs);
125 
126  MRPT_LOG_DEBUG("[CAbstractNavigator::resume() called.");
128 }
129 
130 /** \callergraph */
132 {
133  std::lock_guard<std::recursive_mutex> csl(m_nav_cs);
134 
135  // Issue an "stop" if we are moving:
136  // Update: do it *always*, even if the current velocity is zero, since
137  // we may be in the middle of a multi-part motion command. It's safer.
138  this->stop(false /*not an emergency stop*/);
139 
140  MRPT_LOG_DEBUG("CAbstractNavigator::suspend() called.");
142 }
143 
144 /** \callergraph */
146 {
147  std::lock_guard<std::recursive_mutex> csl(m_nav_cs);
148 
149  MRPT_LOG_DEBUG("CAbstractNavigator::resetNavError() called.");
151 }
152 
154  const std::weak_ptr<mrpt::poses::FrameTransformer<2>>& frame_tf)
155 {
156  m_frame_tf = frame_tf;
157 }
158 
160 {
161  MRPT_START
162 
163  params_abstract_navigator.loadFromConfigFile(c, "CAbstractNavigator");
164 
165  // At this point, all derived classes have already loaded their parameters.
166  // Dump them to debug output:
167  {
169  this->saveConfigFile(cfg_mem);
170  MRPT_LOG_INFO(cfg_mem.getContent());
171  }
172 
173  MRPT_END
174 }
175 
177 {
178  params_abstract_navigator.saveToConfigFile(c, "CAbstractNavigator");
179 }
180 
181 /** \callergraph */
183 {
184  std::lock_guard<std::recursive_mutex> csl(m_nav_cs);
186  m_timlog_delays, "CAbstractNavigator::navigationStep()");
187 
188  const TState prevState = m_navigationState;
189  switch (m_navigationState)
190  {
191  case IDLE:
192  case SUSPENDED:
193  try
194  {
195  // If we just arrived at this state, stop robot:
197  {
199  "[CAbstractNavigator::navigationStep()] Navigation "
200  "stopped.");
201  // this->stop(); stop() is called by the method switching
202  // the "state", so we have more flexibility
204  }
205  }
206  catch (...)
207  {
208  }
209  break;
210 
211  case NAV_ERROR:
212  try
213  {
214  // Send end-of-navigation event:
217  {
218  m_pending_events.push_back(std::bind(
220  std::ref(m_robot)));
221  }
222 
223  // If we just arrived at this state, stop the robot:
225  {
227  "[CAbstractNavigator::navigationStep()] Stopping "
228  "Navigation due to a NAV_ERROR state!");
229  this->stop(false /*not emergency*/);
231  }
232  }
233  catch (...)
234  {
235  }
236  break;
237 
238  case NAVIGATING:
240  true /* do call virtual method nav implementation*/);
241  break; // End case NAVIGATING
242  };
243  m_lastNavigationState = prevState;
244 
246 }
247 
248 /** \callergraph */
250 {
251  // Invoke pending events:
252  for (auto& ev : m_pending_events)
253  {
254  ev();
255  }
256  m_pending_events.clear();
257 }
258 
259 /** \callergraph */
261 {
262  try
263  {
264  this->stop(true /* emergency*/);
265  }
266  catch (...)
267  {
268  }
270  MRPT_LOG_ERROR(msg);
271 }
272 
274 {
275  std::lock_guard<std::recursive_mutex> csl(m_nav_cs);
276 
277  m_navigationEndEventSent = false;
278  m_navigationParams.reset();
279 }
280 
282 {
283  MRPT_START;
284  std::lock_guard<std::recursive_mutex> csl(m_nav_cs);
285 
286  ASSERT_(params != nullptr);
287  ASSERT_(
288  params->target.targetDesiredRelSpeed >= .0 &&
289  params->target.targetDesiredRelSpeed <= 1.0);
290 
291  // Copy data:
292  m_navigationParams = params->clone();
293 
294  // Transform: relative -> absolute, if needed.
295  if (m_navigationParams->target.targetIsRelative)
296  {
298  m_navigationParams->target.target_coords =
299  m_curPoseVel.pose + m_navigationParams->target.target_coords;
300  m_navigationParams->target.targetIsRelative =
301  false; // Now it's not relative
302  }
303 
304  // new state:
306 
307  // Reset the bad navigation alarm:
308  m_badNavAlarm_minDistTarget = std::numeric_limits<double>::max();
310 
311  MRPT_END;
312 }
313 
316 {
317  MRPT_START;
319  this->processNavigateCommand(params);
320  MRPT_END;
321 }
322 
324 {
325  // Ignore calls too-close in time, e.g. from the navigationStep() methods of
326  // AbstractNavigator and a derived, overriding class.
327  const double robot_time_secs =
328  m_robot.getNavigationTime(); // this is clockwall time for real robots,
329  // simulated time in simulators.
330 
331  const double MIN_TIME_BETWEEN_POSE_UPDATES = 20e-3;
333  {
334  const double last_call_age =
335  robot_time_secs - m_last_curPoseVelUpdate_robot_time;
336  if (last_call_age < MIN_TIME_BETWEEN_POSE_UPDATES)
337  {
339  5.0,
340  "updateCurrentPoseAndSpeeds: ignoring call, since last call "
341  "was only %f ms ago.",
342  last_call_age * 1e3);
343  return; // previous data is still valid: don't query the robot
344  // again
345  }
346  }
347 
348  {
350  m_timlog_delays, "getCurrentPoseAndSpeeds()");
351  m_curPoseVel.pose_frame_id = std::string("map"); // default
356  {
358  try
359  {
360  this->stop(true /*emergency*/);
361  }
362  catch (...)
363  {
364  }
366  "ERROR calling m_robot.getCurrentPoseAndSpeeds, stopping robot "
367  "and finishing navigation");
368  throw std::runtime_error(
369  "ERROR calling m_robot.getCurrentPoseAndSpeeds, stopping robot "
370  "and finishing navigation");
371  }
372  }
375 
376  m_last_curPoseVelUpdate_robot_time = robot_time_secs;
377  const bool changed_frame_id =
380 
381  if (changed_frame_id)
382  {
383  // If frame changed, clear past poses. This could be improved by
384  // requesting
385  // the transf between the two frames, but it's probably not worth.
388  }
389 
390  // Append to list of past poses:
393 
394  // Purge old ones:
395  while (m_latestPoses.size() > 1 &&
397  m_latestPoses.begin()->first, m_latestPoses.rbegin()->first) >
399  {
401  }
402  while (m_latestOdomPoses.size() > 1 &&
404  m_latestOdomPoses.begin()->first,
406  {
408  }
409 }
410 
411 /** \callergraph */
413  const mrpt::kinematics::CVehicleVelCmd& vel_cmd)
414 {
415  return m_robot.changeSpeeds(vel_cmd);
416 }
417 /** \callergraph */
419 /** \callergraph */
420 bool CAbstractNavigator::stop(bool isEmergencyStop)
421 {
422  return m_robot.stop(isEmergencyStop);
423 }
424 
426  : dist_to_target_for_sending_event(0),
427  alarm_seems_not_approaching_target_timeout(30),
428  dist_check_target_is_blocked(0.6),
429  hysteresis_check_target_is_blocked(3)
430 {
431 }
434 {
435  MRPT_LOAD_CONFIG_VAR_CS(dist_to_target_for_sending_event, double);
436  MRPT_LOAD_CONFIG_VAR_CS(alarm_seems_not_approaching_target_timeout, double);
437  MRPT_LOAD_CONFIG_VAR_CS(dist_check_target_is_blocked, double);
438  MRPT_LOAD_CONFIG_VAR_CS(hysteresis_check_target_is_blocked, int);
439 }
442 {
444  dist_to_target_for_sending_event,
445  "Default value=0, means use the `targetAllowedDistance` passed by the "
446  "user in the navigation request.");
448  alarm_seems_not_approaching_target_timeout,
449  "navigator timeout (seconds) [Default=30 sec]");
451  dist_check_target_is_blocked,
452  "When closer than this distance, check if the target is blocked to "
453  "abort navigation with an error. [Default=0.6 m]");
455  dist_to_target_for_sending_event,
456  "Default value=0, means use the `targetAllowedDistance` passed by the"
457  " user in the navigation request.");
459  alarm_seems_not_approaching_target_timeout,
460  "navigator timeout (seconds) [Default=30 sec]");
462  dist_check_target_is_blocked,
463  "When closer than this distance, check if the target is blocked to "
464  "abort navigation with an error. [Default=0.6 m]");
466  hysteresis_check_target_is_blocked,
467  "How many steps should the condition for dist_check_target_is_blocked "
468  "be fulfilled to raise an event");
469 }
470 
474 {
475  return typeid(a) == typeid(b) && a.isEqual(b);
476 }
477 
478 /** \callergraph */
479 bool CAbstractNavigator::checkHasReachedTarget(const double targetDist) const
480 {
481  return (targetDist < m_navigationParams->target.targetAllowedDistance);
482 }
483 
484 /** \callergraph */
486 {
487  m_robot.startWatchdog(1000); // Watchdog = 1 seg
488  m_latestPoses.clear(); // Clear cache of last poses.
491 }
492 
493 /** \callergraph */
495  bool call_virtual_nav_method)
496 {
497  const TState prevState = m_navigationState;
498  try
499  {
501  {
503  "[CAbstractNavigator::navigationStep()] Starting Navigation. "
504  "Watchdog initiated...\n");
505  if (m_navigationParams)
507  "[CAbstractNavigator::navigationStep()] Navigation "
508  "Params:\n%s\n",
509  m_navigationParams->getAsText().c_str()));
510 
512  }
513 
514  // Have we just started the navigation?
516  {
517  m_pending_events.push_back(std::bind(
519  std::ref(m_robot)));
520  }
521 
522  /* ----------------------------------------------------------------
523  Get current robot dyn state:
524  ---------------------------------------------------------------- */
526 
527  /* ----------------------------------------------------------------
528  Have we reached the target location?
529  ---------------------------------------------------------------- */
530  // Build a 2D segment from the current robot pose to the previous one:
532  const mrpt::math::TSegment2D seg_robot_mov = mrpt::math::TSegment2D(
534  m_latestPoses.size() > 1
535  ? mrpt::math::TPoint2D((++m_latestPoses.rbegin())->second)
536  : mrpt::math::TPoint2D((m_latestPoses.rbegin())->second));
537 
538  if (m_navigationParams)
539  {
540  const double targetDist = seg_robot_mov.distance(
541  mrpt::math::TPoint2D(m_navigationParams->target.target_coords));
542 
543  // Should "End of navigation" event be sent??
544  if (!m_navigationParams->target.targetIsIntermediaryWaypoint &&
546  targetDist <
548  {
550  m_pending_events.push_back(std::bind(
552  std::ref(m_robot)));
553  }
554 
555  // Have we really reached the target?
556  if (checkHasReachedTarget(targetDist))
557  {
559  logFmt(
561  "Navigation target (%.03f,%.03f) was reached\n",
562  m_navigationParams->target.target_coords.x,
563  m_navigationParams->target.target_coords.y);
564 
565  if (!m_navigationParams->target.targetIsIntermediaryWaypoint)
566  {
567  this->stop(false /*not emergency*/);
569  {
571  m_pending_events.push_back(std::bind(
573  std::ref(m_robot)));
574  }
575  }
576  return;
577  }
578 
579  // Check the "no approaching the target"-alarm:
580  // -----------------------------------------------------------
581  if (targetDist < m_badNavAlarm_minDistTarget)
582  {
583  m_badNavAlarm_minDistTarget = targetDist;
585  }
586  else
587  {
588  // Too much time have passed?
593  .alarm_seems_not_approaching_target_timeout)
594  {
596  "Timeout approaching the target. Aborting navigation.");
597 
599 
600  m_pending_events.push_back(std::bind(
602  std::ref(m_robot)));
603  return;
604  }
605  }
606 
607  // Check if the target seems to be at reach, but it's clearly
608  // occupied by obstacles:
609  if (targetDist <
611  {
612  const auto rel_trg = m_navigationParams->target.target_coords -
614  const bool is_col = checkCollisionWithLatestObstacles(rel_trg);
615  if (is_col)
616  {
617  const bool send_event =
621 
622  if (send_event)
623  {
625  5.0,
626  "Target seems to be blocked by obstacles. Invoking"
627  " sendCannotGetCloserToBlockedTargetEvent().");
628 
629  m_pending_events.push_back(std::bind(
631  sendCannotGetCloserToBlockedTargetEvent,
632  std::ref(m_robot)));
633 
635  }
636  }
637  else
638  {
640  }
641  }
642  }
643 
644  // The normal execution of the navigation: Execute one step:
645  if (call_virtual_nav_method)
646  {
648  }
649  }
650  catch (std::exception& e)
651  {
653  "[CAbstractNavigator::navigationStep] Exception:\n %s", e.what());
654  }
655  catch (...)
656  {
658  "[CAbstractNavigator::navigationStep] Untyped exception!");
659  }
660  m_navigationState = prevState;
661 }
662 
664  const mrpt::math::TPose2D& relative_robot_pose) const
665 {
666  // Default impl:
667  return false;
668 }
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() )
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:262
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
mrpt::system::CTimeLogger m_timlog_delays
Time logger to collect delay-related stats.
#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...
mrpt::system::TTimeStamp getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.cpp:74
GLenum GLint ref
Definition: glext.h:4050
#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 ...
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.
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:3676
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:113
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.
void insert(mrpt::system::TTimeStamp t, const pose_t &p)
Inserts a new pose in the sequence.
virtual 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:6313
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.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
GLubyte GLubyte b
Definition: glext.h:6279
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:4101
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:
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:15
std::vector< std::function< void(void)> > m_pending_events
Events generated during navigationStep(), enqueued to be called at the end of the method execution to...
void setVerbosityLevel(const VerbosityLevel level)
alias of setMinLoggingLevel()
const double PREVIOUS_POSES_MAX_AGE
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.
void doEmergencyStop(const std::string &msg)
Stops the robot and set navigation state to error.
void processNavigateCommand(const TNavigationParams *params)
Does the job of navigate(), except the call to onNavigateCommandReceived()
#define MRPT_LOG_ERROR(_STRING)
TAbstractNavigatorParams params_abstract_navigator
virtual 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:266
virtual 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.
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)
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.
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.cpp:209
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.
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
GLenum const GLfloat * params
Definition: glext.h:3534
mrpt::poses::CPose2DInterpolator m_latestPoses
Latest robot poses (updated in CAbstractNavigator::navigationStep() )
double phi
Orientation (rads)
virtual std::string getAsText() const override
Gets navigation params as a human-readable format.
#define MRPT_LOG_THROTTLE_WARN(_PERIOD_SECONDS, _STRING)
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: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019