Main MRPT website > C++ reference for MRPT 1.5.5
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-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "nav-precomp.h" // Precomp header
11 
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("target_coords = (%.03f,%.03f,%.03f deg)\n", target_coords.x, target_coords.y, target_coords.phi);
39  s += mrpt::format("target_frame_id = \"%s\"\n", target_frame_id.c_str());
40  s += mrpt::format("targetAllowedDistance = %.03f\n", targetAllowedDistance);
41  s += mrpt::format("targetIsRelative = %s\n", targetIsRelative ? "YES" : "NO");
42  s += mrpt::format("targetIsIntermediaryWaypoint = %s\n", targetIsIntermediaryWaypoint ? "YES" : "NO");
43  s += mrpt::format("targetDesiredRelSpeed = %.02f\n", targetDesiredRelSpeed);
44  return s;
45 }
46 
48 {
49  return target_coords == o.target_coords &&
50  target_frame_id == o.target_frame_id &&
51  targetAllowedDistance == o.targetAllowedDistance &&
52  targetIsRelative == o.targetIsRelative &&
53  targetDesiredRelSpeed == o.targetDesiredRelSpeed &&
54  targetIsIntermediaryWaypoint == o.targetIsIntermediaryWaypoint
55  ;
56 }
57 
58 // Gets navigation params as a human-readable format:
60 {
61  string s;
62  s+= "navparams. Single target:\n";
63  s+= target.getAsText();
64  return s;
65 }
66 
68 {
69  auto * rhs = dynamic_cast<const CAbstractNavigator::TNavigationParams *>(&o);
70  return (rhs != nullptr) && (this->target == rhs->target);
71 }
72 
74  pose(0,0,0),
75  velGlobal(0,0,0),
76  velLocal(0,0,0),
77  rawOdometry(0,0,0),
78  timestamp(INVALID_TIMESTAMP),
79  pose_frame_id()
80 {
81 }
82 
83 /*---------------------------------------------------------------
84  Constructor
85  ---------------------------------------------------------------*/
87  mrpt::utils::COutputLogger("MRPT_navigator"),
92  m_navigationParams ( nullptr ),
93  m_robot ( react_iterf_impl ),
94  m_frame_tf (nullptr),
95  m_curPoseVel (),
97  m_latestPoses (),
99  m_timlog_delays (true, "CAbstractNavigator::m_timlog_delays")
100 {
103  this->setVerbosityLevel(mrpt::utils::LVL_DEBUG);
104 }
105 
106 // Dtor:
108 {
110 }
111 
112 /*---------------------------------------------------------------
113  cancel
114  ---------------------------------------------------------------*/
116 {
118  MRPT_LOG_DEBUG("CAbstractNavigator::cancel() called.");
120  this->stop(false /*not emergency*/);
121 }
122 
123 
124 /*---------------------------------------------------------------
125  resume
126  ---------------------------------------------------------------*/
128 {
130 
131  MRPT_LOG_DEBUG("[CAbstractNavigator::resume() called.");
132  if ( m_navigationState == SUSPENDED )
134 }
135 
136 
137 /*---------------------------------------------------------------
138  suspend
139  ---------------------------------------------------------------*/
141 {
143 
144  // Issue an "stop" if we are moving:
145  // Update: do it *always*, even if the current velocity is zero, since
146  // we may be in the middle of a multi-part motion command. It's safer.
147  this->stop(false /*not an emergency stop*/);
148 
149  MRPT_LOG_DEBUG("CAbstractNavigator::suspend() called.");
150  if ( m_navigationState == NAVIGATING )
152 }
153 
155 {
157 
158  MRPT_LOG_DEBUG("CAbstractNavigator::resetNavError() called.");
159  if ( m_navigationState == NAV_ERROR )
161 }
162 
164 {
165  m_frame_tf = frame_tf;
166 }
167 
169 {
170  MRPT_START
171 
172  params_abstract_navigator.loadFromConfigFile(c, "CAbstractNavigator");
173 
174  // At this point, all derived classes have already loaded their parameters.
175  // Dump them to debug output:
176  {
178  this->saveConfigFile(cfg_mem);
179  MRPT_LOG_INFO(cfg_mem.getContent());
180  }
181 
182  MRPT_END
183 }
184 
186 {
187  params_abstract_navigator.saveToConfigFile(c, "CAbstractNavigator");
188 }
189 
190 /*---------------------------------------------------------------
191  navigationStep
192  ---------------------------------------------------------------*/
194 {
196  mrpt::utils::CTimeLoggerEntry tle(m_timlog_delays, "CAbstractNavigator::navigationStep()");
197 
198  const TState prevState = m_navigationState;
199  switch ( m_navigationState )
200  {
201  case IDLE:
202  case SUSPENDED:
203  try
204  {
205  // If we just arrived at this state, stop robot:
207  {
208  MRPT_LOG_INFO("[CAbstractNavigator::navigationStep()] Navigation stopped.");
209  // this->stop(); stop() is called by the method switching the "state", so we have more flexibility
211  }
212  } catch (...) { }
213  break;
214 
215  case NAV_ERROR:
216  try
217  {
218  // Send end-of-navigation event:
220  {
221  TPendingEvent ev;
223  m_pending_events.push_back(ev);
224  }
225 
226  // If we just arrived at this state, stop the robot:
228  {
229  MRPT_LOG_ERROR("[CAbstractNavigator::navigationStep()] Stoping Navigation due to a NAV_ERROR state!");
230  this->stop(false /*not emergency*/);
232  }
233  } catch (...) { }
234  break;
235 
236  case NAVIGATING:
237  this->performNavigationStepNavigating(true /* do call virtual method nav implementation*/);
238  break; // End case NAVIGATING
239  };
240  m_lastNavigationState = prevState;
241 
243 }
244 
246 {
247  // Invoke pending events:
248  for (auto &ev : m_pending_events)
249  {
250  if (ev.event_noargs != nullptr) {
251  (m_robot.*(ev.event_noargs))();
252  }
253  else if (ev.event_wp_reached) {
254  m_robot.sendWaypointReachedEvent(ev.event_wp_reached_index, ev.event_wp_reached_reached);
255  }
256  else if (ev.event_new_wp) {
257  m_robot.sendNewWaypointTargetEvent(ev.event_new_wp_index);
258  }
259  else if (ev.event_cannot_get_closer_target) {
260  bool do_abort_nav;
262 
263  if (do_abort_nav)
264  {
265  MRPT_LOG_WARN("sendCannotGetCloserToBlockedTargetEvent() told me to abort navigation.");
267 
268  TPendingEvent ev;
270  m_pending_events.push_back(ev);
271  }
272  }
273  } // end for each pending event
274  m_pending_events.clear();
275 }
276 
278 {
279  try {
280  this->stop(true /* emergency*/);
281  }
282  catch (...) { }
284  MRPT_LOG_ERROR(msg);
285 }
286 
287 
289 {
291 
292  m_navigationEndEventSent = false;
294 }
295 
297 {
298  MRPT_START;
300 
301  ASSERT_(params != nullptr);
302  ASSERT_(params->target.targetDesiredRelSpeed >= .0 && params->target.targetDesiredRelSpeed <= 1.0);
303 
304  // Copy data:
307  ASSERT_(m_navigationParams != nullptr);
308 
309  // Transform: relative -> absolute, if needed.
311  {
314  m_navigationParams->target.targetIsRelative = false; // Now it's not relative
315  }
316 
317  // new state:
319 
320  // Reset the bad navigation alarm:
321  m_badNavAlarm_minDistTarget = std::numeric_limits<double>::max();
323 
324  MRPT_END;
325 }
326 
328 {
329  MRPT_START;
331  this->processNavigateCommand(params);
332  MRPT_END;
333 }
334 
336 {
337  // Ignore calls too-close in time, e.g. from the navigationStep() methods of
338  // AbstractNavigator and a derived, overriding class.
339  const double robot_time_secs = m_robot.getNavigationTime(); // this is clockwall time for real robots, simulated time in simulators.
340 
341  const double MIN_TIME_BETWEEN_POSE_UPDATES = 20e-3;
343  {
344  const double last_call_age = robot_time_secs - m_last_curPoseVelUpdate_robot_time;
345  if (last_call_age < MIN_TIME_BETWEEN_POSE_UPDATES)
346  {
347  MRPT_LOG_THROTTLE_DEBUG_FMT(5.0,"updateCurrentPoseAndSpeeds: ignoring call, since last call was only %f ms ago.", last_call_age*1e3);
348  return; // previous data is still valid: don't query the robot again
349  }
350  }
351 
352  {
353  mrpt::utils::CTimeLoggerEntry tle(m_timlog_delays, "getCurrentPoseAndSpeeds()");
354  m_curPoseVel.pose_frame_id = std::string("map"); // default
356  {
358  try {
359  this->stop(true /*emergency*/);
360  }
361  catch (...) {}
362  MRPT_LOG_ERROR("ERROR calling m_robot.getCurrentPoseAndSpeeds, stopping robot and finishing navigation");
363  throw std::runtime_error("ERROR calling m_robot.getCurrentPoseAndSpeeds, stopping robot and finishing navigation");
364  }
365  }
368 
369  m_last_curPoseVelUpdate_robot_time = robot_time_secs;
370  const bool changed_frame_id = (m_last_curPoseVelUpdate_pose_frame_id != m_curPoseVel.pose_frame_id);
372 
373  if (changed_frame_id)
374  {
375  // If frame changed, clear past poses. This could be improved by requesting
376  // the transf between the two frames, but it's probably not worth.
379  }
380 
381  // Append to list of past poses:
384 
385  // Purge old ones:
386  while (m_latestPoses.size() > 1 &&
388  {
390  }
391  while (m_latestOdomPoses.size() > 1 &&
393  {
395  }
396 }
397 
399 {
400  return m_robot.changeSpeeds(vel_cmd);
401 }
403 {
404  return m_robot.changeSpeedsNOP();
405 }
406 bool CAbstractNavigator::stop(bool isEmergencyStop)
407 {
408  return m_robot.stop(isEmergencyStop);
409 }
410 
412  dist_to_target_for_sending_event(0),
413  alarm_seems_not_approaching_target_timeout(30),
414  dist_check_target_is_blocked(0.6),
415  hysteresis_check_target_is_blocked(3)
416 {
417 }
419 {
420  MRPT_LOAD_CONFIG_VAR_CS(dist_to_target_for_sending_event, double);
421  MRPT_LOAD_CONFIG_VAR_CS(alarm_seems_not_approaching_target_timeout, double);
422  MRPT_LOAD_CONFIG_VAR_CS(dist_check_target_is_blocked, double);
423  MRPT_LOAD_CONFIG_VAR_CS(hysteresis_check_target_is_blocked, int);
424 }
426 {
427  MRPT_SAVE_CONFIG_VAR_COMMENT(dist_to_target_for_sending_event, "Default value=0, means use the `targetAllowedDistance` passed by the user in the navigation request.");
428  MRPT_SAVE_CONFIG_VAR_COMMENT(alarm_seems_not_approaching_target_timeout, "navigator timeout (seconds) [Default=30 sec]");
429  MRPT_SAVE_CONFIG_VAR_COMMENT(dist_check_target_is_blocked, "When closer than this distance, check if the target is blocked to abort navigation with an error. [Default=0.6 m]");
430  MRPT_SAVE_CONFIG_VAR_COMMENT(hysteresis_check_target_is_blocked, "How many steps should the condition for dist_check_target_is_blocked be fulfilled to raise an event");
431 }
432 
434 {
435  return typeid(a) == typeid(b) && a.isEqual(b);
436 }
437 
438 bool CAbstractNavigator::checkHasReachedTarget(const double targetDist) const
439 {
440  return (targetDist < m_navigationParams->target.targetAllowedDistance);
441 }
442 
444 {
445  m_robot.startWatchdog(1000); // Watchdog = 1 seg
446  m_latestPoses.clear(); // Clear cache of last poses.
449 }
450 
451 void CAbstractNavigator::performNavigationStepNavigating(bool call_virtual_nav_method)
452 {
453  const TState prevState = m_navigationState;
454  try
455  {
457  {
458  MRPT_LOG_INFO("[CAbstractNavigator::navigationStep()] Starting Navigation. Watchdog initiated...\n");
459  if (m_navigationParams)
460  MRPT_LOG_DEBUG(mrpt::format("[CAbstractNavigator::navigationStep()] Navigation Params:\n%s\n", m_navigationParams->getAsText().c_str()));
461 
463  }
464 
465  // Have we just started the navigation?
467  {
468  TPendingEvent ev;
470  m_pending_events.push_back(ev);
471  }
472 
473  /* ----------------------------------------------------------------
474  Get current robot dyn state:
475  ---------------------------------------------------------------- */
477 
478  /* ----------------------------------------------------------------
479  Have we reached the target location?
480  ---------------------------------------------------------------- */
481  // Build a 2D segment from the current robot pose to the previous one:
483  const mrpt::math::TSegment2D seg_robot_mov = mrpt::math::TSegment2D(
485  m_latestPoses.size() > 1 ?
487  :
489  );
490 
491  if (m_navigationParams)
492  {
493  const double targetDist = seg_robot_mov.distance(mrpt::math::TPoint2D(m_navigationParams->target.target_coords));
494 
495  // Should "End of navigation" event be sent??
497  {
499  TPendingEvent ev;
501  m_pending_events.push_back(ev);
502  }
503 
504  // Have we really reached the target?
505  if (checkHasReachedTarget(targetDist))
506  {
508  logFmt(mrpt::utils::LVL_WARN, "Navigation target (%.03f,%.03f) was reached\n", m_navigationParams->target.target_coords.x, m_navigationParams->target.target_coords.y);
509 
511  {
512  this->stop(false /*not emergency*/);
514  {
516  TPendingEvent ev;
518  m_pending_events.push_back(ev);
519  }
520  }
521  return;
522  }
523 
524  // Check the "no approaching the target"-alarm:
525  // -----------------------------------------------------------
526  if (targetDist < m_badNavAlarm_minDistTarget)
527  {
528  m_badNavAlarm_minDistTarget = targetDist;
530  }
531  else
532  {
533  // Too much time have passed?
535  {
536  MRPT_LOG_WARN("Timeout approaching the target. Aborting navigation.");
537 
539 
540  TPendingEvent ev;
542  m_pending_events.push_back(ev);
543  return;
544  }
545  }
546 
547  // Check if the target seems to be at reach, but it's clearly occupied by obstacles:
549  {
550  const auto rel_trg = m_navigationParams->target.target_coords - m_curPoseVel.pose;
551  const bool is_col = checkCollisionWithLatestObstacles(rel_trg);
552  if (is_col)
553  {
555 
556  if (send_event)
557  {
558  MRPT_LOG_THROTTLE_WARN(5.0,"Target seems to be blocked by obstacles. Invoking sendCannotGetCloserToBlockedTargetEvent().");
559 
560  TPendingEvent ev;
562  m_pending_events.push_back(ev);
563 
565  }
566  }
567  else
568  {
570  }
571  }
572  }
573 
574  // The normal execution of the navigation: Execute one step:
575  if (call_virtual_nav_method) {
577  }
578  }
579  catch (std::exception &e)
580  {
581  MRPT_LOG_ERROR_FMT("[CAbstractNavigator::navigationStep] Exception:\n %s",e.what());
582  }
583  catch (...)
584  {
585  MRPT_LOG_ERROR("[CAbstractNavigator::navigationStep] Untyped exception!");
586  }
587  m_navigationState = prevState;
588 }
589 
591 {
592  // Default impl:
593  return false;
594 }
595 
597  event_noargs(nullptr),
598  event_wp_reached(false),
599  event_new_wp(false),
600  event_cannot_get_closer_target(false)
601 {
602 }
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.
TRobotPoseVel m_curPoseVel
Current robot pose (updated in CAbstractNavigator::navigationStep() )
virtual bool startWatchdog(float T_ms)
Start the watchdog timer of the robot platform, if any, for maximum expected delay between consecutiv...
virtual void sendNewWaypointTargetEvent(int waypoint_index)
Callback: Heading towards a new intermediary/final waypoint in waypoint list navigation.
A class acquiring a CCriticalSection at its constructor, and releasing it at destructor.
virtual void loadConfigFile(const mrpt::utils::CConfigFileBase &c)
Loads all params from a file.
void rotate(const double ang)
Transform the (vx,vy) components for a counterclockwise rotation of ang radians.
virtual void saveConfigFile(mrpt::utils::CConfigFileBase &c) const
Saves all current options to a config file.
virtual bool stopWatchdog()
Stop the watchdog timer.
mrpt::poses::FrameTransformer< 2 > * m_frame_tf
Optional, user-provided frame transformer.
#define MRPT_LOG_THROTTLE_DEBUG_FMT(_PERIOD_SECONDS, _FMT_STRING,...)
mrpt::system::TTimeStamp BASE_IMPEXP getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.cpp:71
TNavigationParams * m_navigationParams
Current navigation parameters.
void setFrameTF(mrpt::poses::FrameTransformer< 2 > *frame_tf)
Sets a user-provided frame transformer object; used only if providing targets in a frame ID different...
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_LOAD_CONFIG_VAR_CS(variableName, variableType)
Shortcut for MRPT_LOAD_CONFIG_VAR() for config file object named c and section string named s ...
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.
functor_event_void_t event_noargs
event w/o arguments
bool targetIsRelative
(Default=false) Whether the target coordinates are in global coordinates (false) or are relative to t...
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
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:3602
void setInterpolationMethod(TInterpolatorMethod method)
Change the method used to interpolate the robot path. The default method at construction is "imSpline...
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
mrpt::utils::CTimeLogger m_timlog_delays
Time logger to collect delay-related stats.
virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd)
Default: forward call to m_robot.changeSpeed(). Can be overriden.
virtual bool changeSpeedsNOP()
Default: forward call to m_robot.changeSpeedsNOP(). Can be overriden.
This class allows loading and storing values and vectors of different types from a configuration text...
TState m_navigationState
Current internal state of navigator:
Virtual base for velocity commands of different kinematic models of planar mobile robot...
This class implements a config file-like interface over a memory-stored string list.
virtual void performNavigationStep()=0
To be implemented in derived classes.
void getContent(std::string &str) const
Return the current contents of the virtual "config file".
bool NAV_IMPEXP operator==(const CAbstractNavigator::TNavigationParamsBase &, const CAbstractNavigator::TNavigationParamsBase &)
The struct for configuring navigation requests.
CAbstractNavigator(CRobot2NavInterface &robot_interface_impl)
ctor
virtual void sendCannotGetCloserToBlockedTargetEvent(bool &do_abort_nav)
Callback: Target seems to be blocked by an obstacle.
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.
double dist_to_target_for_sending_event
Default value=0, means use the "targetAllowedDistance" passed by the user in the navigation request...
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
double targetDesiredRelSpeed
(Default=.05) Desired relative speed (wrt maximum speed), in range [0,1], of the vehicle at target...
#define MRPT_END
virtual void onNavigateCommandReceived()
Called after each call to CAbstractNavigator::navigate()
virtual void sendNavigationStartEvent()
Callback: Start of navigation command.
#define MRPT_LOG_WARN(_STRING)
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. Optional, use only for submapping applications...
const GLubyte * c
Definition: glext.h:5590
#define MRPT_LOG_INFO(_STRING)
virtual void resetNavError()
Resets a NAV_ERROR state back to IDLE
#define MRPT_LOG_THROTTLE_WARN(_PERIOD_SECONDS, _STRING)
void internal_onStartNewNavigation()
Called before starting a new navigation.
GLubyte GLubyte b
Definition: glext.h:5575
Individual target info in CAbstractNavigator::TNavigationParamsBase and derived classes.
#define MRPT_LOG_DEBUG(_STRING)
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
virtual void onStartNewNavigation()=0
Called whenever a new navigation has been started.
GLsizei const GLchar ** string
Definition: glext.h:3919
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. Heading may be ignored by some reactive implementations.
TState m_lastNavigationState
Last internal state of navigator:
virtual std::string getAsText() const MRPT_OVERRIDE
Gets navigation params as a human-readable format.
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:17
virtual void sendWaypointReachedEvent(int waypoint_index, bool reached_nSkipped)
Callback: Reached an intermediary waypoint in waypoint list navigation.
double alarm_seems_not_approaching_target_timeout
navigator timeout (seconds) [Default=30 sec]
double y
X,Y coordinates.
const double PREVIOUS_POSES_MAX_AGE
#define MRPT_START
virtual bool isEqual(const TNavigationParamsBase &o) const MRPT_OVERRIDE
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.
void delete_safe(T *&ptr)
Calls "delete" to free an object only if the pointer is not NULL, then set the pointer to NULL...
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 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.
A safe way to call enter() and leave() of a mrpt::utils::CTimeLogger upon construction and destructio...
Definition: CTimeLogger.h:127
void clear()
Clears the current sequence of poses.
void doEmergencyStop(const std::string &msg)
Stops the robot and set navigation state to error.
virtual void loadFromConfigFile(const mrpt::utils::CConfigFileBase &c, const std::string &s) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
void processNavigateCommand(const TNavigationParams *params)
Does the job of navigate(), except the call to onNavigateCommandReceived()
TAbstractNavigatorParams params_abstract_navigator
virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &c, const std::string &s) const MRPT_OVERRIDE
This method saves the options to a ".ini"-like file or memory-stored string list. ...
CRobot2NavInterface & m_robot
The navigator-robot interface.
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)...
Lightweight 2D pose.
virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd)=0
Sends a velocity command to the robot.
#define ASSERT_(f)
iterator erase(iterator element_to_erase)
virtual void cancel()
Cancel current navegation.
bool operator==(const TargetInfo &o) const
virtual void suspend()
Suspend current navegation.
virtual TNavigationParams * clone() const
#define MRPT_LOG_ERROR(_STRING)
double BASE_IMPEXP timeDifference(const mrpt::system::TTimeStamp t_first, const mrpt::system::TTimeStamp t_later)
Returns the time difference from t1 to t2 (positive if t2 is posterior to t1), in seconds...
Definition: datetime.cpp:205
virtual bool checkHasReachedTarget(const double targetDist) const
Default implementation: check if target_dist is below the accepted distance.
mrpt::synch::CCriticalSectionRecursive m_nav_cs
mutex for all navigation methods
mrpt::poses::CPose2DInterpolator m_latestOdomPoses
Latest robot poses (updated in CAbstractNavigator::navigationStep() )
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:5575
GLenum const GLfloat * params
Definition: glext.h:3514
mrpt::poses::CPose2DInterpolator m_latestPoses
double phi
Orientation (rads)
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(). Can be overriden.
std::vector< TPendingEvent > m_pending_events
Events generated during navigationStep(), enqueued to be called at the end of the method execution to...
virtual void navigate(const TNavigationParams *params)
Navigation request to a single target location.



Page generated by Doxygen 1.8.14 for MRPT 1.5.5 Git: e06b63dbf Fri Dec 1 14:41:11 2017 +0100 at lun oct 28 01:31:35 CET 2019