Main MRPT website > C++ reference for MRPT 1.9.9
CWaypointsNavigator.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 
13 #include <mrpt/poses/CPose2D.h>
14 #include <mrpt/math/wrap2pi.h>
15 
16 using namespace mrpt::nav;
17 using namespace std;
18 
20 {
21  std::string s = TNavigationParams::getAsText();
22  if (!multiple_targets.empty())
23  {
24  s += "multiple_targets:\n";
25  int i = 0;
26  for (const auto& e : multiple_targets)
27  {
28  s += mrpt::format("target[%i]:\n", i++);
29  s += e.getAsText();
30  }
31  }
32  return s;
33 }
34 
37 {
38  auto o =
40  &rhs);
41  return o != nullptr &&
43  multiple_targets == o->multiple_targets;
44 }
45 
47  : CAbstractNavigator(robot_if),
48  m_was_aligning(false),
49  m_is_aligning(false),
50  m_last_alignment_cmd(mrpt::system::now())
51 {
52 }
53 
56 {
58 
59  std::lock_guard<std::recursive_mutex> csl(m_nav_waypoints_cs);
60 
61  m_was_aligning = false;
64  m_waypoint_nav_status.waypoint_index_current_goal = -1; // Not started yet.
65 }
66 
68  const TWaypointSequence& nav_request)
69 {
71 
73 
74  std::lock_guard<std::recursive_mutex> csl(m_nav_waypoints_cs);
75 
76  m_pending_events.clear();
77 
78  const size_t N = nav_request.waypoints.size();
79  ASSERTMSG_(N > 0, "List of waypoints is empty!");
80 
82  // Copy waypoints fields data, leave status fields to defaults:
83  for (size_t i = 0; i < N; i++)
84  {
85  ASSERT_(nav_request.waypoints[i].isValid());
86  m_waypoint_nav_status.waypoints[i] = nav_request.waypoints[i];
87  }
89 
90  m_waypoint_nav_status.waypoint_index_current_goal = -1; // Not started yet.
91 
92  // The main loop navigationStep() will iterate over waypoints and send them
93  // to navigate()
94  MRPT_END
95 }
96 
98  TWaypointStatusSequence& out_nav_status) const
99 {
100  // No need to lock mutex...
101  out_nav_status = m_waypoint_nav_status;
102 }
103 
104 /** \callergraph */
106 {
107  {
108  std::lock_guard<std::recursive_mutex> csl(m_nav_waypoints_cs);
110  }
112 }
113 
114 /** \callergraph */
116 {
117  MRPT_START
118 
119  using mrpt::square;
120 
121  // --------------------------------------
122  // Waypoint navigation algorithm
123  // --------------------------------------
124  m_is_aligning =
125  false; // the robot is aligning into a waypoint with a desired heading
126 
127  {
129  m_timlog_delays, "CWaypointsNavigator::navigationStep()");
130  std::lock_guard<std::recursive_mutex> csl(m_nav_waypoints_cs);
131 
133  m_waypoint_nav_status; // shortcut to save typing
134 
135  if (wps.waypoints.empty() || wps.final_goal_reached)
136  {
137  // No nav request is pending or it was canceled
138  }
139  else
140  {
141  // 0) Get current robot pose:
143 
144  // 1) default policy: go thru WPs one by one
145  const int prev_wp_index = wps.waypoint_index_current_goal;
146 
147  mrpt::math::TSegment2D robot_move_seg;
148  robot_move_seg.point1.x = m_curPoseVel.pose.x;
149  robot_move_seg.point1.y = m_curPoseVel.pose.y;
151  {
152  robot_move_seg.point2 = robot_move_seg.point1;
153  }
154  else
155  {
156  robot_move_seg.point2.x = wps.last_robot_pose.x;
157  robot_move_seg.point2.y = wps.last_robot_pose.y;
158  }
159  wps.last_robot_pose = m_curPoseVel.pose; // save for next iters
160 
161  if (wps.waypoint_index_current_goal >= 0)
162  {
163  auto& wp = wps.waypoints[wps.waypoint_index_current_goal];
164  const double dist2target = robot_move_seg.distance(wp.target);
165  if (dist2target < wp.allowed_distance ||
166  m_was_aligning /* we were already aligning at a WP */)
167  {
168  bool consider_wp_reached = false;
169  if (wp.target_heading == TWaypoint::INVALID_NUM)
170  {
171  consider_wp_reached = true;
172  }
173  else
174  {
175  // Handle pure-rotation robot interface to honor
176  // target_heading
177  const double ang_err = mrpt::math::angDistance(
178  m_curPoseVel.pose.phi, wp.target_heading);
179  const double tim_since_last_align =
182  const double ALIGN_WAIT_TIME = 1.5; // seconds
183 
184  if (std::abs(ang_err) <=
186  .waypoint_angle_tolerance &&
187  /* give some time for the alignment (if supported in
188  this robot) to finish)*/
189  tim_since_last_align > ALIGN_WAIT_TIME)
190  {
191  consider_wp_reached = true;
192  }
193  else
194  {
195  m_is_aligning = true;
196 
197  if (!m_was_aligning)
198  {
199  // 1st time we are aligning:
200  // Send vel_cmd to the robot:
202  align_cmd = m_robot.getAlignCmd(ang_err);
203 
205  "[CWaypointsNavigator::navigationStep] "
206  "Trying to align to heading: %.02f deg. "
207  "Relative heading: %.02f deg. "
208  "With motion cmd: %s",
209  mrpt::RAD2DEG(wp.target_heading),
210  mrpt::RAD2DEG(ang_err),
211  align_cmd ? align_cmd->asString().c_str()
212  : "nullptr (operation not "
213  "supported by this robot)");
214 
215  this->stop(false /*not emergency*/); // In any
216  // case,
217  // do a
218  // "stop"
219  if (align_cmd)
220  {
221  this->changeSpeeds(*align_cmd);
222  }
223  else
224  {
225  consider_wp_reached =
226  true; // this robot does not support
227  // "in place" alignment
228  }
229  }
230  else
231  {
233  0.5,
234  "[CWaypointsNavigator::navigationStep] "
235  "Waiting for the robot to get aligned: "
236  "current_heading=%.02f deg "
237  "target_heading=%.02f deg",
239  mrpt::RAD2DEG(wp.target_heading));
240  }
241  }
242  }
243 
244  if (consider_wp_reached)
245  {
247  "[CWaypointsNavigator::navigationStep] Waypoint "
248  << (wps.waypoint_index_current_goal + 1) << "/"
249  << wps.waypoints.size()
250  << " reached."
251  " segment-to-target dist: "
252  << dist2target
253  << ", allowed_dist: " << wp.allowed_distance);
254 
255  wp.reached = true;
256  wp.skipped = false;
257  wp.timestamp_reach = mrpt::system::now();
258 
259  m_pending_events.push_back(
260  std::bind(
262  std::ref(m_robot),
264  true /*reason: really reached*/));
265 
266  // Was this the final goal??
268  int(wps.waypoints.size() - 1))
269  {
271  }
272  else
273  {
274  wps.final_goal_reached = true;
275  // Make sure the end-navigation event is issued,
276  // navigation state switches to IDLE, etc:
277  this->performNavigationStepNavigating(false);
278  }
279  }
280  }
281  }
282 
283  // 2) More advanced policy: if available, use children class methods
284  // to decide
285  // which is the best candidate for the next waypoint, if we can
286  // skip current one:
287  if (!wps.final_goal_reached &&
288  wps.waypoint_index_current_goal >= 0 &&
289  wps.waypoints[wps.waypoint_index_current_goal].allow_skip)
290  {
291  const mrpt::poses::CPose2D robot_pose(m_curPoseVel.pose);
292  int most_advanced_wp = wps.waypoint_index_current_goal;
293  const int most_advanced_wp_at_begin = most_advanced_wp;
294 
295  for (int idx = wps.waypoint_index_current_goal;
296  idx < (int)wps.waypoints.size(); idx++)
297  {
298  if (idx < 0) continue;
299  if (wps.waypoints[idx].reached) continue;
300 
301  // Is it reachable?
302  mrpt::math::TPoint2D wp_local_wrt_robot;
303  robot_pose.inverseComposePoint(
304  wps.waypoints[idx].target, wp_local_wrt_robot);
305 
307  .max_distance_to_allow_skip_waypoint > 0 &&
308  wp_local_wrt_robot.norm() >
311  continue; // Skip this one, it is too far away
312 
313  const bool is_reachable =
314  this->impl_waypoint_is_reachable(wp_local_wrt_robot);
315 
316  if (is_reachable)
317  {
318  // Robustness filter: only skip to a future waypoint if
319  // it is seen as "reachable" during
320  // a given number of timesteps:
321  if (++wps.waypoints[idx].counter_seen_reachable >
324  {
325  most_advanced_wp = idx;
326  }
327  }
328 
329  // Is allowed to skip it?
330  if (!wps.waypoints[idx].allow_skip)
331  break; // Do not keep trying, since we are now allowed
332  // to skip this one.
333  }
334 
335  if (most_advanced_wp >= 0)
336  {
337  wps.waypoint_index_current_goal = most_advanced_wp;
338  for (int k = most_advanced_wp_at_begin;
339  k < most_advanced_wp; k++)
340  {
341  auto& wp = wps.waypoints[k];
342  wp.reached = true;
343  wp.skipped = true;
344  wp.timestamp_reach = mrpt::system::now();
345 
346  m_pending_events.push_back(
347  std::bind(
349  std::ref(m_robot), k,
350  false /*reason: skipped*/));
351  }
352  }
353  }
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  m_pending_events.push_back(
372  std::bind(
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  (is_final_wp ||
418  : 1.;
419 
420  // For backwards compat. with single-target code, write
421  // single target info too for the first, next, waypoint:
422  if (wp_idx == wps.waypoint_index_current_goal)
423  {
424  nav_cmd.target = ti;
425  }
426  // Append to list of targets:
427  nav_cmd.multiple_targets.emplace_back(ti);
428  }
429  this->processNavigateCommand(&nav_cmd);
430 
432  "[CWaypointsNavigator::navigationStep] Active waypoint "
433  "changed. Current status:\n"
434  << this->getWaypointNavStatus().getAsText());
435  }
436  }
437  }
438 
439  // Note: navigationStep() called *after* waypoints part to get
440  // end-of-navigation events *after*
441  // waypoints-related events:
442 
443  m_was_aligning = m_is_aligning; // Let the next timestep know about this
444 
445  MRPT_END
446 }
447 
449 {
450  MRPT_START
451  m_is_aligning =
452  false; // the robot is aligning into a waypoint with a desired heading
453 
455  {
457  }
458 
459  // Call base navigation step to execute one-single waypoint navigation, as
460  // usual:
461  if (!m_is_aligning)
462  {
463  CAbstractNavigator::navigationStep(); // This internally locks
464  // "m_nav_cs"
465  }
466  else
467  {
468  // otherwise, at least, process pending events:
470  }
471 
472  MRPT_END
473 }
474 
475 /** \callergraph */
477 /** \callergraph */
479  const mrpt::math::TPoint2D& wp_local_wrt_robot) const
480 {
481  return impl_waypoint_is_reachable(wp_local_wrt_robot);
482 }
483 
485 {
486  MRPT_START
487 
488  params_waypoints_navigator.loadFromConfigFile(c, "CWaypointsNavigator");
490 
491  MRPT_END
492 }
493 
495 {
497  params_waypoints_navigator.saveToConfigFile(c, "CWaypointsNavigator");
498 }
499 
503 {
504  MRPT_LOAD_CONFIG_VAR(max_distance_to_allow_skip_waypoint, double, c, s);
505  MRPT_LOAD_CONFIG_VAR(min_timesteps_confirm_skip_waypoints, int, c, s);
506  MRPT_LOAD_CONFIG_VAR_DEGREES(waypoint_angle_tolerance, c, s);
507  MRPT_LOAD_CONFIG_VAR(rel_speed_for_stop_waypoints, double, c, s);
508  MRPT_LOAD_CONFIG_VAR(multitarget_look_ahead, int, c, s);
509 }
510 
514 {
516  max_distance_to_allow_skip_waypoint,
517  "Max distance to `foresee` waypoints [meters]. (<0: unlimited)");
519  min_timesteps_confirm_skip_waypoints,
520  "Min timesteps a `future` waypoint must be seen as reachable to become "
521  "the active one.");
523  "waypoint_angle_tolerance", waypoint_angle_tolerance,
524  "Angular error tolerance for waypoints with an assigned heading [deg] "
525  "(Default: 5 deg)");
527  rel_speed_for_stop_waypoints,
528  "[0,1] Relative speed when aiming at a stop-point waypoint "
529  "(Default=0.10)");
531  multitarget_look_ahead,
532  ">=0 number of waypoints to forward to the underlying navigation "
533  "engine, to ease obstacles avoidance when a waypoint is blocked "
534  "(Default=0 : none)");
535 }
536 
538  : max_distance_to_allow_skip_waypoint(-1.0),
539  min_timesteps_confirm_skip_waypoints(1),
540  waypoint_angle_tolerance(mrpt::DEG2RAD(5.0)),
541  rel_speed_for_stop_waypoints(0.10),
542  multitarget_look_ahead(0)
543 {
544 }
545 
546 /** \callergraph */
547 bool CWaypointsNavigator::checkHasReachedTarget(const double targetDist) const
548 {
549  bool ret;
550  const TWaypointStatus* wp = nullptr;
551  const auto& wps = m_waypoint_nav_status;
552  if (m_navigationParams->target.targetIsIntermediaryWaypoint)
553  {
554  ret = false;
555  }
556  else if (wps.timestamp_nav_started != INVALID_TIMESTAMP)
557  {
558  wp = (!wps.waypoints.empty() && wps.waypoint_index_current_goal >= 0 &&
559  wps.waypoint_index_current_goal < (int)wps.waypoints.size())
560  ? &wps.waypoints[wps.waypoint_index_current_goal]
561  : nullptr;
562  ret =
563  (wp == nullptr &&
564  targetDist <= m_navigationParams->target.targetAllowedDistance) ||
565  (wp->reached);
566  }
567  else
568  {
569  ret = (targetDist <= m_navigationParams->target.targetAllowedDistance);
570  }
572  "CWaypointsNavigator::checkHasReachedTarget() called "
573  "with targetDist="
574  << targetDist << " return=" << ret
575  << " waypoint: " << (wp == nullptr ? std::string("") : wp->getAsText())
576  << "wps.timestamp_nav_started="
577  << (wps.timestamp_nav_started == INVALID_TIMESTAMP
578  ? "INVALID_TIMESTAMP"
580  wps.timestamp_nav_started)));
581  return ret;
582 }
virtual 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() )
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.
virtual 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:262
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:102
double RAD2DEG(const double x)
Radians to degrees.
double x
X,Y coordinates.
double x
X,Y coordinates.
virtual 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:4050
double DEG2RAD(const double x)
Degrees to radians.
std::vector< TWaypoint > waypoints
Definition: TWaypoint.h:90
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:98
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:75
TWaypointsNavigatorParams params_waypoints_navigator
STL namespace.
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:88
GLdouble s
Definition: glext.h:3676
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:113
bool reached
Whether this waypoint has been reached already (to within the allowed distance as per user specificat...
Definition: TWaypoint.h:114
This class allows loading and storing values and vectors of different types from a configuration text...
2D segment, consisting of two points.
mrpt::math::TPose2D last_robot_pose
Robot pose at last time step (has INVALID_NUM fields upon initialization)
Definition: TWaypoint.h:151
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.
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. ...
const GLubyte * c
Definition: glext.h:6313
The struct for querying the status of waypoints navigation.
Definition: TWaypoint.h:135
mrpt::system::TTimeStamp m_last_alignment_cmd
std::unique_ptr< TNavigationParams > m_navigationParams
Current navigation parameters.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
Individual target info in CAbstractNavigator::TNavigationParamsBase and derived classes.
virtual bool isEqual(const CAbstractNavigator::TNavigationParamsBase &o) const override
A waypoint with an execution status.
Definition: TWaypoint.h:110
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
bool final_goal_reached
Whether the final waypoint has been reached successfuly.
Definition: TWaypoint.h:142
TPoint2D point2
Destiny point.
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::math::TPose2D target_coords
Coordinates of desired target location.
#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...
virtual void sendWaypointReachedEvent(int waypoint_index, bool reached_nSkipped)
Callback: Reached an intermediary waypoint in waypoint list navigation.
virtual bool checkHasReachedTarget(const double targetDist) const override
Default implementation: check if target_dist is below the accepted distance.
TPoint2D point1
Origin point.
double norm() const
Point norm.
#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.
virtual 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) ...
virtual 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:40
virtual void navigateWaypoints(const TWaypointSequence &nav_request)
Waypoint navigation request.
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:29
CRobot2NavInterface & m_robot
The navigator-robot interface.
virtual void onNavigateCommandReceived() override
Called after each call to CAbstractNavigator::navigate()
#define MRPT_END
Definition: exceptions.h:266
virtual 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:206
double target_heading
[Default=any heading] Optionally, set to the desired orientation [radians] of the robot at this waypo...
Definition: TWaypoint.h:35
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:43
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:140
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.
#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.cpp:209
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:270
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
CWaypointsNavigator(CRobot2NavInterface &robot_interface_impl)
ctor
double rel_speed_for_stop_waypoints
[0,1] Relative speed when aiming at a stop-point waypoint (Default=0.10)
std::vector< TWaypointStatus > waypoints
Waypoints parameters and status (reached, skipped, etc.)
Definition: TWaypoint.h:138
Lightweight 2D point.
#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:67
int waypoint_index_current_goal
Index in waypoints of the waypoint the navigator is currently trying to reach.
Definition: TWaypoint.h:147
This is the base class for any reactive/planned navigation system.
double phi
Orientation (rads)
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,...)
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:39
virtual 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: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019