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-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "nav-precomp.h" // Precomp header
11 
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 {
51 }
52 
55 {
57 
58  std::lock_guard<std::recursive_mutex> csl(m_nav_waypoints_cs);
59 
60  m_was_aligning = false;
63  m_waypoint_nav_status.waypoint_index_current_goal = -1; // Not started yet.
64 }
65 
67  const TWaypointSequence& nav_request)
68 {
70 
72 
73  std::lock_guard<std::recursive_mutex> csl(m_nav_waypoints_cs);
74 
75  const size_t N = nav_request.waypoints.size();
76  ASSERTMSG_(N > 0, "List of waypoints is empty!");
77 
79  // Copy waypoints fields data, leave status fields to defaults:
80  for (size_t i = 0; i < N; i++)
81  {
82  ASSERT_(nav_request.waypoints[i].isValid());
83  m_waypoint_nav_status.waypoints[i] = nav_request.waypoints[i];
84  }
86 
87  m_waypoint_nav_status.waypoint_index_current_goal = -1; // Not started yet.
88 
89  // The main loop navigationStep() will iterate over waypoints and send them
90  // to navigate()
91  MRPT_END
92 }
93 
95  TWaypointStatusSequence& out_nav_status) const
96 {
97  // No need to lock mutex...
98  out_nav_status = m_waypoint_nav_status;
99 }
100 
101 /** \callergraph */
103 {
104  {
105  std::lock_guard<std::recursive_mutex> csl(m_nav_waypoints_cs);
107  }
109 }
110 
111 /** \callergraph */
113 {
114  MRPT_START
115 
116  using mrpt::square;
117 
118  // --------------------------------------
119  // Waypoint navigation algorithm
120  // --------------------------------------
121  m_is_aligning =
122  false; // the robot is aligning into a waypoint with a desired heading
123 
124  {
126  m_timlog_delays, "CWaypointsNavigator::navigationStep()");
127  std::lock_guard<std::recursive_mutex> csl(m_nav_waypoints_cs);
128 
130  m_waypoint_nav_status; // shortcut to save typing
131 
132  if (wps.waypoints.empty() || wps.final_goal_reached)
133  {
134  // No nav request is pending or it was canceled
135  }
136  else
137  {
138  // 0) Get current robot pose:
140 
141  // 1) default policy: go thru WPs one by one
142  const int prev_wp_index = wps.waypoint_index_current_goal;
143 
144  mrpt::math::TSegment2D robot_move_seg;
145  robot_move_seg.point1.x = m_curPoseVel.pose.x;
146  robot_move_seg.point1.y = m_curPoseVel.pose.y;
148  {
149  robot_move_seg.point2 = robot_move_seg.point1;
150  }
151  else
152  {
153  robot_move_seg.point2.x = wps.last_robot_pose.x;
154  robot_move_seg.point2.y = wps.last_robot_pose.y;
155  }
156  wps.last_robot_pose = m_curPoseVel.pose; // save for next iters
157 
158  decltype(m_pending_events) new_events;
159 
160  if (wps.waypoint_index_current_goal >= 0)
161  {
162  auto& wp = wps.waypoints[wps.waypoint_index_current_goal];
163  const double dist2target = robot_move_seg.distance(wp.target);
164  if (dist2target < wp.allowed_distance ||
165  m_was_aligning /* we were already aligning at a WP */)
166  {
167  bool consider_wp_reached = false;
168  if (wp.target_heading == TWaypoint::INVALID_NUM)
169  {
170  consider_wp_reached = true;
171  }
172  else
173  {
174  // Handle pure-rotation robot interface to honor
175  // target_heading
176  const double ang_err = mrpt::math::angDistance(
177  m_curPoseVel.pose.phi, wp.target_heading);
178  const double tim_since_last_align =
181  const double ALIGN_WAIT_TIME = 1.5; // seconds
182 
183  if (std::abs(ang_err) <=
185  .waypoint_angle_tolerance &&
186  /* give some time for the alignment (if supported in
187  this robot) to finish)*/
188  tim_since_last_align > ALIGN_WAIT_TIME)
189  {
190  consider_wp_reached = true;
191  }
192  else
193  {
194  m_is_aligning = true;
195 
196  if (!m_was_aligning)
197  {
198  // 1st time we are aligning:
199  // Send vel_cmd to the robot:
201  align_cmd = m_robot.getAlignCmd(ang_err);
202 
204  "[CWaypointsNavigator::navigationStep] "
205  "Trying to align to heading: %.02f deg. "
206  "Relative heading: %.02f deg. "
207  "With motion cmd: %s",
208  mrpt::RAD2DEG(wp.target_heading),
209  mrpt::RAD2DEG(ang_err),
210  align_cmd ? align_cmd->asString().c_str()
211  : "nullptr (operation not "
212  "supported by this robot)");
213 
214  this->stop(false /*not emergency*/); // In any
215  // case,
216  // do a
217  // "stop"
218  if (align_cmd)
219  {
220  this->changeSpeeds(*align_cmd);
221  }
222  else
223  {
224  consider_wp_reached =
225  true; // this robot does not support
226  // "in place" alignment
227  }
228  }
229  else
230  {
232  0.5,
233  "[CWaypointsNavigator::navigationStep] "
234  "Waiting for the robot to get aligned: "
235  "current_heading=%.02f deg "
236  "target_heading=%.02f deg",
238  mrpt::RAD2DEG(wp.target_heading));
239  }
240  }
241  }
242 
243  if (consider_wp_reached)
244  {
246  "[CWaypointsNavigator::navigationStep] Waypoint "
247  << (wps.waypoint_index_current_goal + 1) << "/"
248  << wps.waypoints.size()
249  << " reached."
250  " segment-to-target dist: "
251  << dist2target
252  << ", allowed_dist: " << wp.allowed_distance);
253 
254  wp.reached = true;
255  wp.skipped = false;
256  wp.timestamp_reach = mrpt::system::now();
257 
258  new_events.emplace_back(std::bind(
260  std::ref(m_robot), wps.waypoint_index_current_goal,
261  true /*reason: really reached*/));
262 
263  // Was this the final goal??
265  int(wps.waypoints.size() - 1))
266  {
268  }
269  else
270  {
271  wps.final_goal_reached = true;
272  // Make sure the end-navigation event is issued,
273  // navigation state switches to IDLE, etc:
274  this->performNavigationStepNavigating(false);
275  }
276  }
277  }
278  }
279 
280  // 2) More advanced policy: if available, use children class methods
281  // to decide
282  // which is the best candidate for the next waypoint, if we can
283  // skip current one:
284  if (!wps.final_goal_reached &&
285  wps.waypoint_index_current_goal >= 0 &&
286  wps.waypoints[wps.waypoint_index_current_goal].allow_skip)
287  {
288  const mrpt::poses::CPose2D robot_pose(m_curPoseVel.pose);
289  int most_advanced_wp = wps.waypoint_index_current_goal;
290  const int most_advanced_wp_at_begin = most_advanced_wp;
291 
292  for (int idx = wps.waypoint_index_current_goal;
293  idx < (int)wps.waypoints.size(); idx++)
294  {
295  if (idx < 0) continue;
296  if (wps.waypoints[idx].reached) continue;
297 
298  // Is it reachable?
299  mrpt::math::TPoint2D wp_local_wrt_robot;
300  robot_pose.inverseComposePoint(
301  wps.waypoints[idx].target, wp_local_wrt_robot);
302 
304  .max_distance_to_allow_skip_waypoint > 0 &&
305  wp_local_wrt_robot.norm() >
308  continue; // Skip this one, it is too far away
309 
310  const bool is_reachable =
311  this->impl_waypoint_is_reachable(wp_local_wrt_robot);
312 
313  if (is_reachable)
314  {
315  // Robustness filter: only skip to a future waypoint if
316  // it is seen as "reachable" during
317  // a given number of timesteps:
318  if (++wps.waypoints[idx].counter_seen_reachable >
321  {
322  most_advanced_wp = idx;
323  }
324  }
325 
326  // Is allowed to skip it?
327  if (!wps.waypoints[idx].allow_skip)
328  break; // Do not keep trying, since we are now allowed
329  // to skip this one.
330  }
331 
332  if (most_advanced_wp >= 0)
333  {
334  wps.waypoint_index_current_goal = most_advanced_wp;
335  for (int k = most_advanced_wp_at_begin;
336  k < most_advanced_wp; k++)
337  {
338  auto& wp = wps.waypoints[k];
339  wp.reached = true;
340  wp.skipped = true;
341  wp.timestamp_reach = mrpt::system::now();
342 
343  new_events.emplace_back(std::bind(
345  std::ref(m_robot), k, false /*reason: skipped*/));
346  }
347  }
348  }
349 
350  // Insert at the beginning, for these events to be dispatched
351  // *before* any "end of nav" event:
352  m_pending_events.insert(
353  m_pending_events.begin(), new_events.begin(), new_events.end());
354 
355  // Still not started and no better guess? Start with the first
356  // waypoint:
357  if (wps.waypoint_index_current_goal < 0)
359 
360  // 3) Should I request a new (single target) navigation command?
361  // Only if the temporary goal changed:
362  if (wps.waypoint_index_current_goal >= 0 &&
363  prev_wp_index != wps.waypoint_index_current_goal)
364  {
365  ASSERT_(
367  int(wps.waypoints.size()));
369 
370  // Notify we have a new "current waypoint"
371  // Push back so it's dispatched *after* the wp reached events:
372  m_pending_events.emplace_back(std::bind(
374  std::ref(m_robot), wps.waypoint_index_current_goal));
375 
376  // Send the current targets + "multitarget_look_ahead"
377  // additional ones to help the local planner.
379 
380  // Check skippable flag while traversing from current wp forward
381  // "multitarget_look_ahead" steps:
382  int wp_last_idx = wps.waypoint_index_current_goal;
383  for (int nstep = 0;
384  wp_last_idx < int(wps.waypoints.size()) - 1 &&
386  ++nstep)
387  {
388  if (!m_waypoint_nav_status.waypoints[wp_last_idx]
389  .allow_skip)
390  break;
391  wp_last_idx++;
392  }
393 
394  for (int wp_idx = wps.waypoint_index_current_goal;
395  wp_idx <= wp_last_idx; wp_idx++)
396  {
397  TWaypointStatus& wp = wps.waypoints[wp_idx];
398  const bool is_final_wp =
399  ((wp_idx + 1) == int(wps.waypoints.size()));
400 
402 
403  ti.target_coords.x = wp.target.x;
404  ti.target_coords.y = wp.target.y;
405  ti.target_coords.phi =
407  ? wp.target_heading
408  : .0);
411  ti.targetIsRelative = false;
412  ti.targetIsIntermediaryWaypoint = !is_final_wp;
414 
415  // For backwards compat. with single-target code, write
416  // single target info too for the first, next, waypoint:
417  if (wp_idx == wps.waypoint_index_current_goal)
418  {
419  nav_cmd.target = ti;
420  }
421  // Append to list of targets:
422  nav_cmd.multiple_targets.emplace_back(ti);
423  }
424  this->processNavigateCommand(&nav_cmd);
425 
427  "[CWaypointsNavigator::navigationStep] Active waypoint "
428  "changed. Current status:\n"
429  << this->getWaypointNavStatus().getAsText());
430  }
431  }
432  }
433 
434  // Note: navigationStep() called *after* waypoints part to get
435  // end-of-navigation events *after*
436  // waypoints-related events:
437 
438  m_was_aligning = m_is_aligning; // Let the next timestep know about this
439 
440  MRPT_END
441 }
442 
444 {
445  MRPT_START
446  // the robot is aligning into a waypoint with a desired heading
447  m_is_aligning = false;
448 
450  m_navProfiler, "CWaypointsNavigator::navigationStep()");
451 
453  {
455  }
456 
457  // Call base navigation step to execute one-single waypoint navigation, as
458  // usual:
459  if (!m_is_aligning)
460  {
461  CAbstractNavigator::navigationStep(); // This internally locks
462  // "m_nav_cs"
463  }
464  else
465  {
466  // otherwise, at least, process pending events:
468  }
469 
470  MRPT_END
471 }
472 
473 /** \callergraph */
475 /** \callergraph */
477  const mrpt::math::TPoint2D& wp_local_wrt_robot) const
478 {
479  return impl_waypoint_is_reachable(wp_local_wrt_robot);
480 }
481 
483 {
484  MRPT_START
485 
486  params_waypoints_navigator.loadFromConfigFile(c, "CWaypointsNavigator");
488 
489  MRPT_END
490 }
491 
493 {
495  params_waypoints_navigator.saveToConfigFile(c, "CWaypointsNavigator");
496 }
497 
500  const mrpt::config::CConfigFileBase& c, const std::string& s)
501 {
502  MRPT_LOAD_CONFIG_VAR(max_distance_to_allow_skip_waypoint, double, c, s);
503  MRPT_LOAD_CONFIG_VAR(min_timesteps_confirm_skip_waypoints, int, c, s);
504  MRPT_LOAD_CONFIG_VAR_DEGREES(waypoint_angle_tolerance, c, s);
505  MRPT_LOAD_CONFIG_VAR(multitarget_look_ahead, int, c, s);
506 }
507 
510  mrpt::config::CConfigFileBase& c, const std::string& s) const
511 {
513  max_distance_to_allow_skip_waypoint,
514  "Max distance to `foresee` waypoints [meters]. (<0: unlimited)");
516  min_timesteps_confirm_skip_waypoints,
517  "Min timesteps a `future` waypoint must be seen as reachable to become "
518  "the active one.");
520  "waypoint_angle_tolerance", waypoint_angle_tolerance,
521  "Angular error tolerance for waypoints with an assigned heading [deg] "
522  "(Default: 5 deg)");
524  multitarget_look_ahead,
525  ">=0 number of waypoints to forward to the underlying navigation "
526  "engine, to ease obstacles avoidance when a waypoint is blocked "
527  "(Default=0 : none)");
528 }
529 
531  : waypoint_angle_tolerance(mrpt::DEG2RAD(5.0))
532 
533 {
534 }
535 
536 /** \callergraph */
537 bool CWaypointsNavigator::checkHasReachedTarget(const double targetDist) const
538 {
539  bool ret;
540  const TWaypointStatus* wp = nullptr;
541  const auto& wps = m_waypoint_nav_status;
542  if (m_navigationParams->target.targetIsIntermediaryWaypoint)
543  {
544  ret = false;
545  }
546  else if (wps.timestamp_nav_started != INVALID_TIMESTAMP)
547  {
548  wp = (!wps.waypoints.empty() && wps.waypoint_index_current_goal >= 0 &&
549  wps.waypoint_index_current_goal < (int)wps.waypoints.size())
550  ? &wps.waypoints[wps.waypoint_index_current_goal]
551  : nullptr;
552  ret =
553  (wp == nullptr &&
554  targetDist <= m_navigationParams->target.targetAllowedDistance) ||
555  (wp->reached);
556  }
557  else
558  {
559  ret = (targetDist <= m_navigationParams->target.targetAllowedDistance);
560  }
562  "CWaypointsNavigator::checkHasReachedTarget() called "
563  "with targetDist="
564  << targetDist << " return=" << ret
565  << " waypoint: " << (wp == nullptr ? std::string("") : wp->getAsText())
566  << "wps.timestamp_nav_started="
567  << (wps.timestamp_nav_started == INVALID_TIMESTAMP
568  ? "INVALID_TIMESTAMP"
570  wps.timestamp_nav_started)));
571  return ret;
572 }
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...
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.
double distance(const TPoint2D &point) const
Distance to point.
Definition: TSegment2D.cpp:19
std::string getAsText() const
Gets navigation params as a human-readable format.
Definition: TWaypoint.cpp:99
double x
X,Y coordinates.
Definition: TPose2D.h:30
void onStartNewNavigation() override
Called whenever a new navigation has been started.
T x
X,Y coordinates.
Definition: TPoint2D.h:25
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
std::vector< TWaypoint > waypoints
Definition: TWaypoint.h:98
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
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:
#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. ...
constexpr double DEG2RAD(const double x)
Degrees to radians.
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);
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
return_t square(const num_t x)
Inline function for the square of a number.
#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...
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.
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...
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()
constexpr double RAD2DEG(const double x)
Radians to degrees.
#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
T norm() const
Point norm: |v| = sqrt(x^2+y^2)
Definition: TPoint2D.h:205
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:176
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
#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: 3a26b90fd Wed Mar 25 20:17:03 2020 +0100 at miƩ mar 25 23:05:41 CET 2020