MRPT  1.9.9
CWaypointsNavigator.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 
12 #include <mrpt/math/TSegment2D.h>
13 #include <mrpt/math/wrap2pi.h>
15 #include <mrpt/poses/CPose2D.h>
16 
17 using namespace mrpt::nav;
18 using namespace std;
19 
21 {
22  std::string s = TNavigationParams::getAsText();
23  if (!multiple_targets.empty())
24  {
25  s += "multiple_targets:\n";
26  int i = 0;
27  for (const auto& e : multiple_targets)
28  {
29  s += mrpt::format("target[%i]:\n", i++);
30  s += e.getAsText();
31  }
32  }
33  return s;
34 }
35 
38 {
39  auto o =
41  &rhs);
42  return o != nullptr &&
44  multiple_targets == o->multiple_targets;
45 }
46 
48  : CAbstractNavigator(robot_if),
49  m_was_aligning(false),
50  m_is_aligning(false),
51  m_last_alignment_cmd(mrpt::system::now())
52 {
53 }
54 
57 {
59 
60  std::lock_guard<std::recursive_mutex> csl(m_nav_waypoints_cs);
61 
62  m_was_aligning = false;
65  m_waypoint_nav_status.waypoint_index_current_goal = -1; // Not started yet.
66 }
67 
69  const TWaypointSequence& nav_request)
70 {
72 
74 
75  std::lock_guard<std::recursive_mutex> csl(m_nav_waypoints_cs);
76 
77  const size_t N = nav_request.waypoints.size();
78  ASSERTMSG_(N > 0, "List of waypoints is empty!");
79 
81  // Copy waypoints fields data, leave status fields to defaults:
82  for (size_t i = 0; i < N; i++)
83  {
84  ASSERT_(nav_request.waypoints[i].isValid());
85  m_waypoint_nav_status.waypoints[i] = nav_request.waypoints[i];
86  }
88 
89  m_waypoint_nav_status.waypoint_index_current_goal = -1; // Not started yet.
90 
91  // The main loop navigationStep() will iterate over waypoints and send them
92  // to navigate()
93  MRPT_END
94 }
95 
97  TWaypointStatusSequence& out_nav_status) const
98 {
99  // No need to lock mutex...
100  out_nav_status = m_waypoint_nav_status;
101 }
102 
103 /** \callergraph */
105 {
106  {
107  std::lock_guard<std::recursive_mutex> csl(m_nav_waypoints_cs);
109  }
111 }
112 
113 /** \callergraph */
115 {
116  MRPT_START
117 
118  using mrpt::square;
119 
120  // --------------------------------------
121  // Waypoint navigation algorithm
122  // --------------------------------------
123  m_is_aligning =
124  false; // the robot is aligning into a waypoint with a desired heading
125 
126  {
128  m_timlog_delays, "CWaypointsNavigator::navigationStep()");
129  std::lock_guard<std::recursive_mutex> csl(m_nav_waypoints_cs);
130 
132  m_waypoint_nav_status; // shortcut to save typing
133 
134  if (wps.waypoints.empty() || wps.final_goal_reached)
135  {
136  // No nav request is pending or it was canceled
137  }
138  else
139  {
140  // 0) Get current robot pose:
142 
143  // 1) default policy: go thru WPs one by one
144  const int prev_wp_index = wps.waypoint_index_current_goal;
145 
146  mrpt::math::TSegment2D robot_move_seg;
147  robot_move_seg.point1.x = m_curPoseVel.pose.x;
148  robot_move_seg.point1.y = m_curPoseVel.pose.y;
150  {
151  robot_move_seg.point2 = robot_move_seg.point1;
152  }
153  else
154  {
155  robot_move_seg.point2.x = wps.last_robot_pose.x;
156  robot_move_seg.point2.y = wps.last_robot_pose.y;
157  }
158  wps.last_robot_pose = m_curPoseVel.pose; // save for next iters
159 
160  decltype(m_pending_events) new_events;
161 
162  if (wps.waypoint_index_current_goal >= 0)
163  {
164  auto& wp = wps.waypoints[wps.waypoint_index_current_goal];
165  const double dist2target = robot_move_seg.distance(wp.target);
166  if (dist2target < wp.allowed_distance ||
167  m_was_aligning /* we were already aligning at a WP */)
168  {
169  bool consider_wp_reached = false;
170  if (wp.target_heading == TWaypoint::INVALID_NUM)
171  {
172  consider_wp_reached = true;
173  }
174  else
175  {
176  // Handle pure-rotation robot interface to honor
177  // target_heading
178  const double ang_err = mrpt::math::angDistance(
179  m_curPoseVel.pose.phi, wp.target_heading);
180  const double tim_since_last_align =
183  const double ALIGN_WAIT_TIME = 1.5; // seconds
184 
185  if (std::abs(ang_err) <=
187  .waypoint_angle_tolerance &&
188  /* give some time for the alignment (if supported in
189  this robot) to finish)*/
190  tim_since_last_align > ALIGN_WAIT_TIME)
191  {
192  consider_wp_reached = true;
193  }
194  else
195  {
196  m_is_aligning = true;
197 
198  if (!m_was_aligning)
199  {
200  // 1st time we are aligning:
201  // Send vel_cmd to the robot:
203  align_cmd = m_robot.getAlignCmd(ang_err);
204 
206  "[CWaypointsNavigator::navigationStep] "
207  "Trying to align to heading: %.02f deg. "
208  "Relative heading: %.02f deg. "
209  "With motion cmd: %s",
210  mrpt::RAD2DEG(wp.target_heading),
211  mrpt::RAD2DEG(ang_err),
212  align_cmd ? align_cmd->asString().c_str()
213  : "nullptr (operation not "
214  "supported by this robot)");
215 
216  this->stop(false /*not emergency*/); // In any
217  // case,
218  // do a
219  // "stop"
220  if (align_cmd)
221  {
222  this->changeSpeeds(*align_cmd);
223  }
224  else
225  {
226  consider_wp_reached =
227  true; // this robot does not support
228  // "in place" alignment
229  }
230  }
231  else
232  {
234  0.5,
235  "[CWaypointsNavigator::navigationStep] "
236  "Waiting for the robot to get aligned: "
237  "current_heading=%.02f deg "
238  "target_heading=%.02f deg",
240  mrpt::RAD2DEG(wp.target_heading));
241  }
242  }
243  }
244 
245  if (consider_wp_reached)
246  {
248  "[CWaypointsNavigator::navigationStep] Waypoint "
249  << (wps.waypoint_index_current_goal + 1) << "/"
250  << wps.waypoints.size()
251  << " reached."
252  " segment-to-target dist: "
253  << dist2target
254  << ", allowed_dist: " << wp.allowed_distance);
255 
256  wp.reached = true;
257  wp.skipped = false;
258  wp.timestamp_reach = mrpt::system::now();
259 
260  new_events.emplace_back(std::bind(
263  true /*reason: really reached*/));
264 
265  // Was this the final goal??
267  int(wps.waypoints.size() - 1))
268  {
270  }
271  else
272  {
273  wps.final_goal_reached = true;
274  // Make sure the end-navigation event is issued,
275  // navigation state switches to IDLE, etc:
276  this->performNavigationStepNavigating(false);
277  }
278  }
279  }
280  }
281 
282  // 2) More advanced policy: if available, use children class methods
283  // to decide
284  // which is the best candidate for the next waypoint, if we can
285  // skip current one:
286  if (!wps.final_goal_reached &&
287  wps.waypoint_index_current_goal >= 0 &&
288  wps.waypoints[wps.waypoint_index_current_goal].allow_skip)
289  {
290  const mrpt::poses::CPose2D robot_pose(m_curPoseVel.pose);
291  int most_advanced_wp = wps.waypoint_index_current_goal;
292  const int most_advanced_wp_at_begin = most_advanced_wp;
293 
294  for (int idx = wps.waypoint_index_current_goal;
295  idx < (int)wps.waypoints.size(); idx++)
296  {
297  if (idx < 0) continue;
298  if (wps.waypoints[idx].reached) continue;
299 
300  // Is it reachable?
301  mrpt::math::TPoint2D wp_local_wrt_robot;
302  robot_pose.inverseComposePoint(
303  wps.waypoints[idx].target, wp_local_wrt_robot);
304 
306  .max_distance_to_allow_skip_waypoint > 0 &&
307  wp_local_wrt_robot.norm() >
310  continue; // Skip this one, it is too far away
311 
312  const bool is_reachable =
313  this->impl_waypoint_is_reachable(wp_local_wrt_robot);
314 
315  if (is_reachable)
316  {
317  // Robustness filter: only skip to a future waypoint if
318  // it is seen as "reachable" during
319  // a given number of timesteps:
320  if (++wps.waypoints[idx].counter_seen_reachable >
323  {
324  most_advanced_wp = idx;
325  }
326  }
327 
328  // Is allowed to skip it?
329  if (!wps.waypoints[idx].allow_skip)
330  break; // Do not keep trying, since we are now allowed
331  // to skip this one.
332  }
333 
334  if (most_advanced_wp >= 0)
335  {
336  wps.waypoint_index_current_goal = most_advanced_wp;
337  for (int k = most_advanced_wp_at_begin;
338  k < most_advanced_wp; k++)
339  {
340  auto& wp = wps.waypoints[k];
341  wp.reached = true;
342  wp.skipped = true;
343  wp.timestamp_reach = mrpt::system::now();
344 
345  new_events.emplace_back(std::bind(
347  std::ref(m_robot), k, false /*reason: skipped*/));
348  }
349  }
350  }
351 
352  // Insert at the beginning, for these events to be dispatched
353  // *before* any "end of nav" event:
354  m_pending_events.insert(
355  m_pending_events.begin(), new_events.begin(), new_events.end());
356 
357  // Still not started and no better guess? Start with the first
358  // waypoint:
359  if (wps.waypoint_index_current_goal < 0)
361 
362  // 3) Should I request a new (single target) navigation command?
363  // Only if the temporary goal changed:
364  if (wps.waypoint_index_current_goal >= 0 &&
365  prev_wp_index != wps.waypoint_index_current_goal)
366  {
367  ASSERT_(
369  int(wps.waypoints.size()));
371 
372  // Notify we have a new "current waypoint"
373  // Push back so it's dispatched *after* the wp reached events:
374  m_pending_events.emplace_back(std::bind(
377 
378  // Send the current targets + "multitarget_look_ahead"
379  // additional ones to help the local planner.
381 
382  // Check skippable flag while traversing from current wp forward
383  // "multitarget_look_ahead" steps:
384  int wp_last_idx = wps.waypoint_index_current_goal;
385  for (int nstep = 0;
386  wp_last_idx < int(wps.waypoints.size()) - 1 &&
388  ++nstep)
389  {
390  if (!m_waypoint_nav_status.waypoints[wp_last_idx]
391  .allow_skip)
392  break;
393  wp_last_idx++;
394  }
395 
396  for (int wp_idx = wps.waypoint_index_current_goal;
397  wp_idx <= wp_last_idx; wp_idx++)
398  {
399  TWaypointStatus& wp = wps.waypoints[wp_idx];
400  const bool is_final_wp =
401  ((wp_idx + 1) == int(wps.waypoints.size()));
402 
404 
405  ti.target_coords.x = wp.target.x;
406  ti.target_coords.y = wp.target.y;
407  ti.target_coords.phi =
409  ? wp.target_heading
410  : .0);
413  ti.targetIsRelative = false;
414  ti.targetIsIntermediaryWaypoint = !is_final_wp;
416 
417  // For backwards compat. with single-target code, write
418  // single target info too for the first, next, waypoint:
419  if (wp_idx == wps.waypoint_index_current_goal)
420  {
421  nav_cmd.target = ti;
422  }
423  // Append to list of targets:
424  nav_cmd.multiple_targets.emplace_back(ti);
425  }
426  this->processNavigateCommand(&nav_cmd);
427 
429  "[CWaypointsNavigator::navigationStep] Active waypoint "
430  "changed. Current status:\n"
431  << this->getWaypointNavStatus().getAsText());
432  }
433  }
434  }
435 
436  // Note: navigationStep() called *after* waypoints part to get
437  // end-of-navigation events *after*
438  // waypoints-related events:
439 
440  m_was_aligning = m_is_aligning; // Let the next timestep know about this
441 
442  MRPT_END
443 }
444 
446 {
447  MRPT_START
448  m_is_aligning =
449  false; // the robot is aligning into a waypoint with a desired heading
450 
452  {
454  }
455 
456  // Call base navigation step to execute one-single waypoint navigation, as
457  // usual:
458  if (!m_is_aligning)
459  {
460  CAbstractNavigator::navigationStep(); // This internally locks
461  // "m_nav_cs"
462  }
463  else
464  {
465  // otherwise, at least, process pending events:
467  }
468 
469  MRPT_END
470 }
471 
472 /** \callergraph */
474 /** \callergraph */
476  const mrpt::math::TPoint2D& wp_local_wrt_robot) const
477 {
478  return impl_waypoint_is_reachable(wp_local_wrt_robot);
479 }
480 
482 {
483  MRPT_START
484 
485  params_waypoints_navigator.loadFromConfigFile(c, "CWaypointsNavigator");
487 
488  MRPT_END
489 }
490 
492 {
494  params_waypoints_navigator.saveToConfigFile(c, "CWaypointsNavigator");
495 }
496 
500 {
501  MRPT_LOAD_CONFIG_VAR(max_distance_to_allow_skip_waypoint, double, c, s);
502  MRPT_LOAD_CONFIG_VAR(min_timesteps_confirm_skip_waypoints, int, c, s);
503  MRPT_LOAD_CONFIG_VAR_DEGREES(waypoint_angle_tolerance, c, s);
504  MRPT_LOAD_CONFIG_VAR(multitarget_look_ahead, int, c, s);
505 }
506 
510 {
512  max_distance_to_allow_skip_waypoint,
513  "Max distance to `foresee` waypoints [meters]. (<0: unlimited)");
515  min_timesteps_confirm_skip_waypoints,
516  "Min timesteps a `future` waypoint must be seen as reachable to become "
517  "the active one.");
519  "waypoint_angle_tolerance", waypoint_angle_tolerance,
520  "Angular error tolerance for waypoints with an assigned heading [deg] "
521  "(Default: 5 deg)");
523  multitarget_look_ahead,
524  ">=0 number of waypoints to forward to the underlying navigation "
525  "engine, to ease obstacles avoidance when a waypoint is blocked "
526  "(Default=0 : none)");
527 }
528 
530  : waypoint_angle_tolerance(mrpt::DEG2RAD(5.0))
531 
532 {
533 }
534 
535 /** \callergraph */
536 bool CWaypointsNavigator::checkHasReachedTarget(const double targetDist) const
537 {
538  bool ret;
539  const TWaypointStatus* wp = nullptr;
540  const auto& wps = m_waypoint_nav_status;
541  if (m_navigationParams->target.targetIsIntermediaryWaypoint)
542  {
543  ret = false;
544  }
545  else if (wps.timestamp_nav_started != INVALID_TIMESTAMP)
546  {
547  wp = (!wps.waypoints.empty() && wps.waypoint_index_current_goal >= 0 &&
548  wps.waypoint_index_current_goal < (int)wps.waypoints.size())
549  ? &wps.waypoints[wps.waypoint_index_current_goal]
550  : nullptr;
551  ret =
552  (wp == nullptr &&
553  targetDist <= m_navigationParams->target.targetAllowedDistance) ||
554  (wp->reached);
555  }
556  else
557  {
558  ret = (targetDist <= m_navigationParams->target.targetAllowedDistance);
559  }
561  "CWaypointsNavigator::checkHasReachedTarget() called "
562  "with targetDist="
563  << targetDist << " return=" << ret
564  << " waypoint: " << (wp == nullptr ? std::string("") : wp->getAsText())
565  << "wps.timestamp_nav_started="
566  << (wps.timestamp_nav_started == INVALID_TIMESTAMP
567  ? "INVALID_TIMESTAMP"
569  wps.timestamp_nav_started)));
570  return ret;
571 }
void cancel() override
Cancel current navegation.
#define MRPT_LOG_THROTTLE_INFO_FMT(_PERIOD_SECONDS, _FMT_STRING,...)
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...
double x
X,Y coordinates.
Definition: TPoint2D.h:23
virtual void sendNewWaypointTargetEvent(int waypoint_index)
Callback: Heading towards a new intermediary/final waypoint in waypoint list navigation.
bool m_was_aligning
Whether the last timestep was "is_aligning" in a waypoint with heading.
TWaypointStatusSequence m_waypoint_nav_status
The latest waypoints navigation command and the up-to-date control status.
void navigationStep() override
This method must be called periodically in order to effectively run the navigation.
virtual void loadConfigFile(const mrpt::config::CConfigFileBase &c)
Loads all params from a file.
std::vector< mrpt::nav::CAbstractNavigator::TargetInfo > multiple_targets
If not empty, this will prevail over the base class single goal target.
#define MRPT_START
Definition: exceptions.h:241
mrpt::system::CTimeLogger m_timlog_delays
Time logger to collect delay-related stats.
std::string getAsText() const
Gets navigation params as a human-readable format.
Definition: TWaypoint.cpp:99
double RAD2DEG(const double x)
Radians to degrees.
double x
X,Y coordinates.
Definition: TPose2D.h:30
void onStartNewNavigation() override
Called whenever a new navigation has been started.
A safe way to call enter() and leave() of a mrpt::system::CTimeLogger upon construction and destructi...
GLenum GLint ref
Definition: glext.h:4062
double DEG2RAD(const double x)
Degrees to radians.
std::vector< TWaypoint > waypoints
Definition: TWaypoint.h:98
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.
std::recursive_mutex m_nav_waypoints_cs
T angDistance(T from, T to)
Computes the shortest angular increment (or distance) between two planar orientations, such that it is constrained to [-pi,pi] and is correct for any combination of angles (e.g.
Definition: wrap2pi.h:95
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
TWaypointsNavigatorParams params_waypoints_navigator
STL namespace.
virtual void updateCurrentPoseAndSpeeds()
Call to the robot getCurrentPoseAndSpeeds() and updates members m_curPoseVel accordingly.
TWaypointStatusSequence getWaypointNavStatus() const
Get a copy of the control structure which describes the progress status of the waypoint navigation...
bool targetIsRelative
(Default=false) Whether the target coordinates are in global coordinates (false) or are relative to t...
virtual mrpt::kinematics::CVehicleVelCmd::Ptr getAlignCmd(const double relative_heading_radians)
Gets a motion command to make the robot to align with a given relative heading, without translating...
The struct for requesting navigation requests for a sequence of waypoints.
Definition: TWaypoint.h:96
GLdouble s
Definition: glext.h:3682
virtual void navigationStep()
This method must be called periodically in order to effectively run the navigation.
bool isRelativePointReachable(const mrpt::math::TPoint2D &wp_local_wrt_robot) const
Returns true if, according to the information gathered at the last navigation step, there is a free path to the given point; false otherwise: if way is blocked or there is missing information, the point is out of range for the existing PTGs, etc.
virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd)
Default: forward call to m_robot.changeSpeed().
The struct for configuring navigation requests to CWaypointsNavigator and derived classes...
TState m_navigationState
Current internal state of navigator:
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
bool reached
Whether this waypoint has been reached already (to within the allowed distance as per user specificat...
Definition: TWaypoint.h:122
This class allows loading and storing values and vectors of different types from a configuration text...
2D segment, consisting of two points.
Definition: TSegment2D.h:20
mrpt::math::TPose2D last_robot_pose
Robot pose at last time step (has INVALID_NUM fields upon initialization)
Definition: TWaypoint.h:159
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()
std::string target_frame_id
(Default="map") Frame ID in which target is given.
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. ...
const GLubyte * c
Definition: glext.h:6406
The struct for querying the status of waypoints navigation.
Definition: TWaypoint.h:143
mrpt::system::TTimeStamp m_last_alignment_cmd
std::unique_ptr< TNavigationParams > m_navigationParams
Current navigation parameters.
Individual target info in CAbstractNavigator::TNavigationParamsBase and derived classes.
bool isEqual(const CAbstractNavigator::TNavigationParamsBase &o) const override
A waypoint with an execution status.
Definition: TWaypoint.h:118
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
bool final_goal_reached
Whether the final waypoint has been reached successfuly.
Definition: TWaypoint.h:150
TPoint2D point2
Destiny point.
Definition: TSegment2D.h:30
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
GLsizei const GLchar ** string
Definition: glext.h:4116
mrpt::math::TPose2D target_coords
Coordinates of desired target location.
~CWaypointsNavigator() override
dtor
virtual void sendWaypointReachedEvent(int waypoint_index, bool reached_nSkipped)
Callback: Reached an intermediary waypoint in waypoint list navigation.
bool checkHasReachedTarget(const double targetDist) const override
Default implementation: check if target_dist is below the accepted distance.
TPoint2D point1
Origin point.
Definition: TSegment2D.h:26
double norm() const
Point norm: |v| = sqrt(x^2+y^2)
Definition: TPoint2D.h:162
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
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.
std::string getAsText() const override
Gets navigation params as a human-readable format.
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const
Saves all current options to a config file.
int min_timesteps_confirm_skip_waypoints
How many times shall a future waypoint be seen as reachable to skip to it (Default: 1) ...
void loadConfigFile(const mrpt::config::CConfigFileBase &c) override
Loads all params from a file.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
virtual void navigateWaypoints(const TWaypointSequence &nav_request)
Waypoint navigation request.
virtual void processNavigateCommand(const TNavigationParams *params)
Does the job of navigate(), except the call to onNavigateCommandReceived()
virtual bool impl_waypoint_is_reachable(const mrpt::math::TPoint2D &wp_local_wrt_robot) const =0
Implements the way to waypoint is free function in children classes: true must be returned if...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
mrpt::math::TPoint2D target
[Must be set by the user] Coordinates of desired target location (world/global coordinates).
Definition: TWaypoint.h:28
CRobot2NavInterface & m_robot
The navigator-robot interface.
void onNavigateCommandReceived() override
Called after each call to CAbstractNavigator::navigate()
#define MRPT_END
Definition: exceptions.h:245
bool isEqual(const TNavigationParamsBase &o) const override
void inverseComposePoint(const double gx, const double gy, double &lx, double &ly) const
Computes the 2D point L such as .
Definition: CPose2D.cpp:236
double target_heading
[Default=any heading] Optionally, set to the desired orientation [radians] of the robot at this waypo...
Definition: TWaypoint.h:34
double allowed_distance
[Must be set by the user] How close should the robot get to this waypoint for it to be considered rea...
Definition: TWaypoint.h:42
virtual void cancel()
Cancel current navegation.
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
mrpt::system::TTimeStamp timestamp_nav_started
Timestamp of user navigation command.
Definition: TWaypoint.h:148
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.
#define MRPT_LOAD_CONFIG_VAR_DEGREES( variableName, configFileObject, sectionNameStr)
Loads a double variable, stored as radians but entered in the INI-file as degrees.
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
std::string dateTimeLocalToString(const mrpt::system::TTimeStamp t)
Convert a timestamp into this textual form (in local time): YEAR/MONTH/DAY,HH:MM:SS.MMM.
Definition: datetime.cpp:170
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
CWaypointsNavigator(CRobot2NavInterface &robot_interface_impl)
ctor
std::vector< TWaypointStatus > waypoints
Waypoints parameters and status (reached, skipped, etc.)
Definition: TWaypoint.h:146
Lightweight 2D point.
Definition: TPoint2D.h:31
#define MRPT_SAVE_CONFIG_VAR_DEGREES_COMMENT( __entryName, __variable, __comment)
static const int INVALID_NUM
The default value of fields (used to detect non-set values)
Definition: TWaypoint.h:75
int waypoint_index_current_goal
Index in waypoints of the waypoint the navigator is currently trying to reach.
Definition: TWaypoint.h:155
This is the base class for any reactive/planned navigation system.
double phi
Orientation (rads)
Definition: TPose2D.h:32
virtual void waypoints_navigationStep()
The waypoints-specific part of navigationStep()
int multitarget_look_ahead
>=0 number of waypoints to forward to the underlying navigation engine, to ease obstacles avoidance w...
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
double speed_ratio
(Default=1.0) Desired robot speed at the target, as a ratio of the full robot speed.
Definition: TWaypoint.h:50
virtual bool stop(bool isEmergencyStop)
Default: forward call to m_robot.stop().
std::string target_frame_id
(Default="map") Frame ID in which target is given.
Definition: TWaypoint.h:38
void saveConfigFile(mrpt::config::CConfigFileBase &c) const override
Saves all current options to a config file.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019