Main MRPT website > C++ reference for MRPT 1.5.6
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-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 
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  s += mrpt::format("target[%i]:\n",i++);
28  s += e.getAsText();
29  }
30  }
31  return s;
32 }
33 
35 {
36  auto o = dynamic_cast<const CWaypointsNavigator::TNavigationParamsWaypoints*>(&rhs);
37  return o!=nullptr &&
39  multiple_targets == o->multiple_targets;
40 }
41 
43  CAbstractNavigator(robot_if),
44  m_was_aligning(false),
45  m_is_aligning(false),
46  m_last_alignment_cmd(mrpt::system::now())
47 {
48 }
49 
51 {
52 }
53 
55 {
57 
59 
60  m_was_aligning = false;
63  m_waypoint_nav_status.waypoint_index_current_goal = -1; // Not started yet.
64 }
65 
67 {
69 
71 
73 
74  m_pending_events.clear();
75 
76  const size_t N = nav_request.waypoints.size();
77  ASSERTMSG_(N>0,"List of waypoints is empty!");
78 
80  // Copy waypoints fields data, leave status fields to defaults:
81  for (size_t i=0;i<N;i++)
82  {
83  ASSERT_( nav_request.waypoints[i].isValid() );
84  m_waypoint_nav_status.waypoints[i] = nav_request.waypoints[i];
85  }
87 
88  // The main loop navigationStep() will iterate over waypoints and send them to navigate()
89  MRPT_END
90 }
91 
93 {
94  // No need to lock mutex...
95  out_nav_status = m_waypoint_nav_status;
96 }
97 
99 {
100  {
103  }
105 }
106 
108 {
109  MRPT_START
110 
111  using mrpt::math::square;
112 
113  // --------------------------------------
114  // Waypoint navigation algorithm
115  // --------------------------------------
116  m_is_aligning = false; // the robot is aligning into a waypoint with a desired heading
117 
118  {
119  mrpt::utils::CTimeLoggerEntry tle(m_timlog_delays,"CWaypointsNavigator::navigationStep()");
121 
122  TWaypointStatusSequence &wps = m_waypoint_nav_status; // shortcut to save typing
123 
124  if (wps.waypoints.empty() || wps.final_goal_reached)
125  {
126  // No nav request is pending or it was canceled
127  }
128  else
129  {
130  // 0) Get current robot pose:
132 
133  // 1) default policy: go thru WPs one by one
134  const int prev_wp_index = wps.waypoint_index_current_goal;
135 
136  mrpt::math::TSegment2D robot_move_seg;
137  robot_move_seg.point1.x = m_curPoseVel.pose.x;
138  robot_move_seg.point1.y = m_curPoseVel.pose.y;
140  {
141  robot_move_seg.point2 = robot_move_seg.point1;
142  }
143  else
144  {
145  robot_move_seg.point2.x = wps.last_robot_pose.x;
146  robot_move_seg.point2.y = wps.last_robot_pose.y;
147  }
148  wps.last_robot_pose = m_curPoseVel.pose; // save for next iters
149 
150  if (wps.waypoint_index_current_goal >= 0)
151  {
152  auto &wp = wps.waypoints[wps.waypoint_index_current_goal];
153  const double dist2target = robot_move_seg.distance(wp.target);
154  if (dist2target < wp.allowed_distance || m_was_aligning /* we were already aligning at a WP */)
155  {
156  bool consider_wp_reached = false;
157  if (wp.target_heading == TWaypoint::INVALID_NUM)
158  {
159  consider_wp_reached = true;
160  }
161  else
162  {
163  // Handle pure-rotation robot interface to honor target_heading
164  const double ang_err = mrpt::math::angDistance(m_curPoseVel.pose.phi, wp.target_heading);
165  const double tim_since_last_align = mrpt::system::timeDifference(m_last_alignment_cmd, mrpt::system::now());
166  const double ALIGN_WAIT_TIME = 1.5; // seconds
167  if (std::abs(ang_err) <= params_waypoints_navigator.waypoint_angle_tolerance &&
168  tim_since_last_align>ALIGN_WAIT_TIME // give some time for the alignment (if supported in this robot) to finish
169  )
170  {
171  consider_wp_reached = true;
172  }
173  else
174  {
175  m_is_aligning = true;
176 
177  if (!m_was_aligning || tim_since_last_align> ALIGN_WAIT_TIME)
178  {
179  // we must align:
180  // Send vel_cmd to the robot:
181  mrpt::kinematics::CVehicleVelCmdPtr align_cmd = m_robot.getAlignCmd(ang_err);
182 
184  "[CWaypointsNavigator::navigationStep] Trying to align to heading: %.02f deg. "
185  "Relative heading: %.02f deg. "
186  "With motion cmd: %s",
187  mrpt::utils::RAD2DEG(wp.target_heading),
188  mrpt::utils::RAD2DEG(ang_err),
189  align_cmd ? align_cmd->asString().c_str() : "nullptr (operation not supported by this robot)");
190 
191  if (align_cmd)
192  {
194  this->changeSpeeds(*align_cmd);
195  }
196  else
197  {
198  this->stop(false /*not emergency*/); // In any case, do a "stop"
199  consider_wp_reached = true; // this robot does not support "in place" alignment
200  }
201  }
202  else
203  {
204  MRPT_LOG_THROTTLE_INFO_FMT(0.5, "[CWaypointsNavigator::navigationStep] Waiting for the robot to get aligned: current_heading=%.02f deg target_heading=%.02f deg", mrpt::utils::RAD2DEG(m_curPoseVel.pose.phi), mrpt::utils::RAD2DEG(wp.target_heading) );
205  }
206  }
207  }
208 
209  if (consider_wp_reached)
210  {
211  MRPT_LOG_DEBUG_STREAM("[CWaypointsNavigator::navigationStep] Waypoint " <<
212  (wps.waypoint_index_current_goal + 1) << "/" << wps.waypoints.size() << " reached."
213  " segment-to-target dist: " << dist2target << ", allowed_dist: " << wp.allowed_distance
214  );
215 
216  wp.reached = true;
217  wp.skipped = false;
218  wp.timestamp_reach = mrpt::system::now();
219  {
220  TPendingEvent ev;
221  ev.event_wp_reached = true;
223  ev.event_wp_reached_reached = true /* reason: really reached*/;
224  m_pending_events.push_back(ev);
225  }
226 
227  // Was this the final goal??
228  if (wps.waypoint_index_current_goal < int(wps.waypoints.size() - 1)) {
230  }
231  else {
232  wps.final_goal_reached = true;
233 
234  // Make sure the end-navigation event is issued,
235  // navigation state switches to IDLE, etc:
236  this->performNavigationStepNavigating(false);
237  }
238  }
239  }
240  }
241 
242  // 2) More advanced policy: if available, use children class methods to decide
243  // which is the best candidate for the next waypoint, if we can skip current one:
244  if (!wps.final_goal_reached &&
245  wps.waypoint_index_current_goal >= 0 &&
246  wps.waypoints[wps.waypoint_index_current_goal].allow_skip
247  )
248  {
249  const mrpt::poses::CPose2D robot_pose(m_curPoseVel.pose);
250  int most_advanced_wp = wps.waypoint_index_current_goal;
251  const int most_advanced_wp_at_begin = most_advanced_wp;
252 
253  for (int idx=wps.waypoint_index_current_goal;idx<(int)wps.waypoints.size();idx++)
254  {
255  if (idx<0) continue;
256  if (wps.waypoints[idx].reached) continue;
257 
258  // Is it reachable?
259  mrpt::math::TPoint2D wp_local_wrt_robot;
260  robot_pose.inverseComposePoint(wps.waypoints[idx].target, wp_local_wrt_robot);
261 
263  continue; // Skip this one, it is too far away
264 
265  const bool is_reachable = this->impl_waypoint_is_reachable(wp_local_wrt_robot);
266 
267  if (is_reachable) {
268  // Robustness filter: only skip to a future waypoint if it is seen as "reachable" during
269  // a given number of timesteps:
270  if (++wps.waypoints[idx].counter_seen_reachable > params_waypoints_navigator.min_timesteps_confirm_skip_waypoints) {
271  most_advanced_wp = idx;
272  }
273  }
274 
275  // Is allowed to skip it?
276  if (!wps.waypoints[idx].allow_skip)
277  break; // Do not keep trying, since we are now allowed to skip this one.
278  }
279 
280  if (most_advanced_wp>=0) {
281  wps.waypoint_index_current_goal = most_advanced_wp;
282  for (int k=most_advanced_wp_at_begin;k<most_advanced_wp;k++) {
283  auto &wp = wps.waypoints[k];
284  wp.reached = true;
285  wp.skipped = true;
286  wp.timestamp_reach = mrpt::system::now();
287 
288  {
289  TPendingEvent ev;
290  ev.event_wp_reached = true;
291  ev.event_wp_reached_index = k;
292  ev.event_wp_reached_reached = false /* reason: skipped */;
293  m_pending_events.push_back(ev);
294  }
295  }
296  }
297  }
298 
299  // Still not started and no better guess? Start with the first waypoint:
300  if (wps.waypoint_index_current_goal<0)
302 
303  // 3) Should I request a new (single target) navigation command?
304  // Only if the temporary goal changed:
305  if (wps.waypoint_index_current_goal>=0 && prev_wp_index!=wps.waypoint_index_current_goal)
306  {
307  ASSERT_( wps.waypoint_index_current_goal < int(wps.waypoints.size()) );
309 
310  // Notify we have a new "current waypoint"
311  {
312  TPendingEvent ev;
313  ev.event_new_wp = true;
315  m_pending_events.push_back(ev);
316  }
317 
318  // Send the current targets + "multitarget_look_ahead" additional ones to help the local planner.
320 
321  // Check skippable flag while traversing from current wp forward "multitarget_look_ahead" steps:
322  int wp_last_idx = wps.waypoint_index_current_goal;
323  for (int nstep = 0; wp_last_idx<int(wps.waypoints.size()) - 1 && nstep < params_waypoints_navigator.multitarget_look_ahead; ++nstep)
324  {
325  if (!m_waypoint_nav_status.waypoints[wp_last_idx].allow_skip)
326  break;
327  wp_last_idx++;
328  }
329 
330  for (int wp_idx = wps.waypoint_index_current_goal; wp_idx <= wp_last_idx; wp_idx++)
331  {
332  TWaypointStatus &wp = wps.waypoints[wp_idx];
333  const bool is_final_wp = ((wp_idx + 1) == int(wps.waypoints.size()));
334 
336 
337  ti.target_coords.x = wp.target.x;
338  ti.target_coords.y = wp.target.y;
342  ti.targetIsRelative = false;
343  ti.targetIsIntermediaryWaypoint = !is_final_wp;
345 
346  // For backwards compat. with single-target code, write single target info too for the first, next, waypoint:
347  if (wp_idx == wps.waypoint_index_current_goal) {
348  nav_cmd.target = ti;
349  }
350  // Append to list of targets:
351  nav_cmd.multiple_targets.emplace_back(ti);
352  }
353  this->processNavigateCommand( &nav_cmd );
354 
355  MRPT_LOG_DEBUG_STREAM( "[CWaypointsNavigator::navigationStep] Active waypoint changed. Current status:\n" << this->getWaypointNavStatus().getAsText());
356  }
357  }
358  }
359 
360  // Note: navigationStep() called *after* waypoints part to get end-of-navigation events *after*
361  // waypoints-related events:
362 
363  m_was_aligning = m_is_aligning; // Let the next timestep know about this
364 
365  MRPT_END
366 }
367 
368 
370 {
371  MRPT_START
372  m_is_aligning = false; // the robot is aligning into a waypoint with a desired heading
373 
374  // State can be NAVIGATING if we are already heading to a waypoint,
375  // or IDLE if navigateWaypoints() was called and this is the first
376  // navStep() afterwards:
378  {
380  }
381 
382  // Call base navigation step to execute one-single waypoint navigation, as usual:
383  if (!m_is_aligning)
384  {
385  CAbstractNavigator::navigationStep(); // This internally locks "m_nav_cs"
386  }
387  else
388  {
389  // otherwise, at least, process pending events:
391  }
392 
393  MRPT_END
394 }
395 
397 {
398 }
399 
401 {
402  return impl_waypoint_is_reachable(wp_local_wrt_robot);
403 }
404 
406 {
407  MRPT_START
408 
409  params_waypoints_navigator.loadFromConfigFile(c, "CWaypointsNavigator");
411 
412  MRPT_END
413 }
414 
416 {
418  params_waypoints_navigator.saveToConfigFile(c, "CWaypointsNavigator");
419 }
420 
422 {
423  MRPT_LOAD_CONFIG_VAR(max_distance_to_allow_skip_waypoint, double, c, s);
424  MRPT_LOAD_CONFIG_VAR(min_timesteps_confirm_skip_waypoints, int, c, s);
425  MRPT_LOAD_CONFIG_VAR_DEGREES(waypoint_angle_tolerance, c, s);
426  MRPT_LOAD_CONFIG_VAR(rel_speed_for_stop_waypoints, double, c, s);
427  MRPT_LOAD_CONFIG_VAR(multitarget_look_ahead, int, c, s);
428 }
429 
431 {
432  MRPT_SAVE_CONFIG_VAR_COMMENT(max_distance_to_allow_skip_waypoint, "Max distance to `foresee` waypoints [meters]. (<0: unlimited)");
433  MRPT_SAVE_CONFIG_VAR_COMMENT(min_timesteps_confirm_skip_waypoints, "Min timesteps a `future` waypoint must be seen as reachable to become the active one.");
434  MRPT_SAVE_CONFIG_VAR_DEGREES_COMMENT("waypoint_angle_tolerance", waypoint_angle_tolerance, "Angular error tolerance for waypoints with an assigned heading [deg] (Default: 5 deg)");
435  MRPT_SAVE_CONFIG_VAR_COMMENT(rel_speed_for_stop_waypoints, "[0,1] Relative speed when aiming at a stop-point waypoint (Default=0.10)");
436  MRPT_SAVE_CONFIG_VAR_COMMENT(multitarget_look_ahead, ">=0 number of waypoints to forward to the underlying navigation engine, to ease obstacles avoidance when a waypoint is blocked (Default=0 : none)");
437 }
438 
440  max_distance_to_allow_skip_waypoint(-1.0),
441  min_timesteps_confirm_skip_waypoints(1),
442  waypoint_angle_tolerance( mrpt::utils::DEG2RAD(5.0) ),
443  rel_speed_for_stop_waypoints(0.10),
444  multitarget_look_ahead(0)
445 {
446 }
447 
448 bool CWaypointsNavigator::checkHasReachedTarget(const double targetDist) const
449 {
450  bool ret;
451  const TWaypointStatus *wp = nullptr;
452  const auto &wps = m_waypoint_nav_status;
454  {
455  ret = false;
456  }
457  else if (wps.timestamp_nav_started != INVALID_TIMESTAMP)
458  {
459  wp = (!wps.waypoints.empty() &&
460  wps.waypoint_index_current_goal >= 0 &&
461  wps.waypoint_index_current_goal < (int)wps.waypoints.size()
462  )
463  ?
464  &wps.waypoints[wps.waypoint_index_current_goal]
465  :
466  nullptr;
467  ret = (wp == nullptr && targetDist <= m_navigationParams->target.targetAllowedDistance) ||
468  (wp->reached);
469  }
470  else
471  {
472  ret = (targetDist <= m_navigationParams->target.targetAllowedDistance);
473  }
474  MRPT_LOG_DEBUG_STREAM("CWaypointsNavigator::checkHasReachedTarget() called "
475  "with targetDist=" << targetDist << " return=" << ret << " waypoint: " <<
476  (wp == nullptr ? std::string("") : wp->getAsText()) <<
477  "wps.timestamp_nav_started=" <<
478  (wps.timestamp_nav_started==INVALID_TIMESTAMP ?
479  "INVALID_TIMESTAMP" : mrpt::system::dateTimeLocalToString(wps.timestamp_nav_started) )
480  );
481  return ret;
482 }
TRobotPoseVel m_curPoseVel
Current robot pose (updated in CAbstractNavigator::navigationStep() )
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.
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 bool isEqual(const CAbstractNavigator::TNavigationParamsBase &o) const MRPT_OVERRIDE
std::vector< mrpt::nav::CAbstractNavigator::TargetInfo > multiple_targets
If not empty, this will prevail over the base class single goal target.
double y
X,Y coordinates.
double waypoint_angle_tolerance
[rad] Angular error tolerance for waypoints with an assigned heading (Default: 5 deg) ...
static const double INVALID_NUM
The default value of fields (used to detect non-set values)
Definition: TWaypoint.h:51
const GLfloat * c
Definition: glew.h:10088
TNavigationParams * m_navigationParams
Current navigation parameters.
std::vector< TWaypoint > waypoints
Definition: TWaypoint.h:73
Base for all high-level navigation commands.
float targetAllowedDistance
(Default=0.5 meters) Allowed distance to target in order to end the navigation.
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:91
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
TWaypointsNavigatorParams params_waypoints_navigator
void updateCurrentPoseAndSpeeds()
Call to the robot getCurrentPoseAndSpeeds() and updates members m_curPoseVel accordingly.
virtual bool checkHasReachedTarget(const double targetDist) const MRPT_OVERRIDE
Default implementation: check if target_dist is below the accepted distance.
bool targetIsRelative
(Default=false) Whether the target coordinates are in global coordinates (false) or are relative to t...
The struct for requesting navigation requests for a sequence of waypoints.
Definition: TWaypoint.h:71
double norm() const
Point norm.
virtual void navigationStep()
This method must be called periodically in order to effectively run the navigation.
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.
The struct for configuring navigation requests to CWaypointsNavigator and derived classes...
This class allows loading and storing values and vectors of different types from a configuration text...
TState m_navigationState
Current internal state of navigator:
double max_distance_to_allow_skip_waypoint
In meters. <0: unlimited.
bool reached
Whether this waypoint has been reached already (to within the allowed distance as per user specificat...
Definition: TWaypoint.h:90
virtual void navigationStep() MRPT_OVERRIDE
This method must be called periodically in order to effectively run the navigation.
GLdouble s
Definition: glew.h:1295
2D segment, consisting of two points.
#define MRPT_LOG_THROTTLE_INFO_FMT(_PERIOD_SECONDS, _FMT_STRING,...)
virtual mrpt::kinematics::CVehicleVelCmdPtr getAlignCmd(const double relative_heading_radians)
Gets a motion command to make the robot to align with a given relative heading, without translating...
double RAD2DEG(const double x)
Radians to degrees.
mrpt::math::TPose2D last_robot_pose
Robot pose at last time step (has INVALID_NUM fields upon initialization)
Definition: TWaypoint.h:113
#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()
std::string target_frame_id
(Default="map") Frame ID in which target is given. Optional, use only for submapping applications...
virtual void cancel() MRPT_OVERRIDE
Cancel current navegation.
virtual void loadConfigFile(const mrpt::utils::CConfigFileBase &c) MRPT_OVERRIDE
Loads all params from a file.
The struct for querying the status of waypoints navigation.
Definition: TWaypoint.h:103
mrpt::system::TTimeStamp m_last_alignment_cmd
Individual target info in CAbstractNavigator::TNavigationParamsBase and derived classes.
A waypoint with an execution status.
Definition: TWaypoint.h:88
bool final_goal_reached
Whether the final waypoint has been reached successfuly.
Definition: TWaypoint.h:107
TWaypointStatusSequence getWaypointNavStatus() const
Get a copy of the control structure which describes the progress status of the waypoint navigation...
mrpt::synch::CCriticalSectionRecursive m_nav_waypoints_cs
TPoint2D point2
Destiny point.
virtual void onNavigateCommandReceived() MRPT_OVERRIDE
Called after each call to CAbstractNavigator::navigate()
#define DEG2RAD
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
typedef int(WINAPI *PFNWGLRELEASEPBUFFERDCARBPROC)(HPBUFFERARB hPbuffer
mrpt::math::TPose2D target_coords
Coordinates of desired target location. Heading may be ignored by some reactive implementations.
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:17
virtual std::string getAsText() const MRPT_OVERRIDE
Gets navigation params as a human-readable format.
TPoint2D point1
Origin point.
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.
double y
X,Y coordinates.
GLenum target
Definition: glew.h:5023
#define MRPT_START
#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...
virtual bool isEqual(const TNavigationParamsBase &o) const MRPT_OVERRIDE
virtual void performNavigationStepNavigating(bool call_virtual_nav_method=true)
Factorization of the part inside navigationStep(), for the case of state being NAVIGATING.
GLsizei const GLcharARB ** string
Definition: glew.h:3293
A safe way to call enter() and leave() of a mrpt::utils::CTimeLogger upon construction and destructio...
Definition: CTimeLogger.h:127
int min_timesteps_confirm_skip_waypoints
How many times shall a future waypoint be seen as reachable to skip to it (Default: 1) ...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
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()
#define MRPT_LOAD_CONFIG_VAR_DEGREES(variableName, configFileObject, sectionNameStr)
Loads a double variable, stored as radians but entered in the INI-file as degrees.
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...
T square(const T x)
Inline function for the square of a number.
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.
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
double target_heading
[Default=any heading] Optionally, set to the desired orientation [radians] of the robot at this waypo...
Definition: TWaypoint.h:33
#define ASSERT_(f)
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:37
virtual void saveConfigFile(mrpt::utils::CConfigFileBase &c) const
Saves all current options to a config file.
virtual void cancel()
Cancel current navegation.
void inverseComposePoint(const double gx, const double gy, double &lx, double &ly) const
Computes the 2D point L such as .
Definition: CPose2D.cpp:205
double distance(const TPoint2D &point) const
Distance to point.
mrpt::system::TTimeStamp timestamp_nav_started
Timestamp of user navigation command.
Definition: TWaypoint.h:106
std::string getAsText() const
Gets navigation params as a human-readable format.
Definition: TWaypoint.cpp:103
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.
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
std::string BASE_IMPEXP 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:274
#define MRPT_SAVE_CONFIG_VAR_DEGREES_COMMENT(__entryName, __variable, __comment)
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:105
virtual void saveConfigFile(mrpt::utils::CConfigFileBase &c) const MRPT_OVERRIDE
Saves all current options to a config file.
Lightweight 2D point.
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
#define ASSERTMSG_(f, __ERROR_MSG)
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. ...
int waypoint_index_current_goal
Index in waypoints of the waypoint the navigator is currently trying to reach.
Definition: TWaypoint.h:111
This is the base class for any reactive/planned navigation system.
virtual void onStartNewNavigation() MRPT_OVERRIDE
Called whenever a new navigation has been started.
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...
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...
std::string target_frame_id
(Default="map") Frame ID in which target is given. Optional, use only for submapping applications...
Definition: TWaypoint.h:35



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018