Main MRPT website > C++ reference for MRPT 1.5.7
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  const size_t N = nav_request.waypoints.size();
75  ASSERTMSG_(N>0,"List of waypoints is empty!");
76 
78  // Copy waypoints fields data, leave status fields to defaults:
79  for (size_t i=0;i<N;i++)
80  {
81  ASSERT_( nav_request.waypoints[i].isValid() );
82  m_waypoint_nav_status.waypoints[i] = nav_request.waypoints[i];
83  }
85 
86  // The main loop navigationStep() will iterate over waypoints and send them to navigate()
87  MRPT_END
88 }
89 
91 {
92  // No need to lock mutex...
93  out_nav_status = m_waypoint_nav_status;
94 }
95 
97 {
98  {
101  }
103 }
104 
106 {
107  MRPT_START
108 
109  using mrpt::math::square;
110 
111  // --------------------------------------
112  // Waypoint navigation algorithm
113  // --------------------------------------
114  m_is_aligning = false; // the robot is aligning into a waypoint with a desired heading
115 
116  {
117  mrpt::utils::CTimeLoggerEntry tle(m_timlog_delays,"CWaypointsNavigator::navigationStep()");
119 
120  TWaypointStatusSequence &wps = m_waypoint_nav_status; // shortcut to save typing
121 
122  if (wps.waypoints.empty() || wps.final_goal_reached)
123  {
124  // No nav request is pending or it was canceled
125  }
126  else
127  {
128  // 0) Get current robot pose:
130 
131  // 1) default policy: go thru WPs one by one
132  const int prev_wp_index = wps.waypoint_index_current_goal;
133 
134  mrpt::math::TSegment2D robot_move_seg;
135  robot_move_seg.point1.x = m_curPoseVel.pose.x;
136  robot_move_seg.point1.y = m_curPoseVel.pose.y;
138  {
139  robot_move_seg.point2 = robot_move_seg.point1;
140  }
141  else
142  {
143  robot_move_seg.point2.x = wps.last_robot_pose.x;
144  robot_move_seg.point2.y = wps.last_robot_pose.y;
145  }
146  wps.last_robot_pose = m_curPoseVel.pose; // save for next iters
147 
148  decltype(m_pending_events) new_events;
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  new_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  new_events.push_back(ev);
294  }
295  }
296  }
297  }
298 
299  // Insert at the beginning, for these events to be dispatched *before* any "end of nav" event:
300  m_pending_events.insert(m_pending_events.begin(), new_events.begin(), new_events.end());
301 
302  // Still not started and no better guess? Start with the first waypoint:
303  if (wps.waypoint_index_current_goal<0)
305 
306  // 3) Should I request a new (single target) navigation command?
307  // Only if the temporary goal changed:
308  if (wps.waypoint_index_current_goal>=0 && prev_wp_index!=wps.waypoint_index_current_goal)
309  {
310  ASSERT_( wps.waypoint_index_current_goal < int(wps.waypoints.size()) );
312 
313  // Notify we have a new "current waypoint"
314  {
315  TPendingEvent ev;
316  ev.event_new_wp = true;
318  // Push back so it's dispatched *after* the wp reached events:
319  m_pending_events.push_back(ev);
320  }
321 
322  // Send the current targets + "multitarget_look_ahead" additional ones to help the local planner.
324 
325  // Check skippable flag while traversing from current wp forward "multitarget_look_ahead" steps:
326  int wp_last_idx = wps.waypoint_index_current_goal;
327  for (int nstep = 0; wp_last_idx<int(wps.waypoints.size()) - 1 && nstep < params_waypoints_navigator.multitarget_look_ahead; ++nstep)
328  {
329  if (!m_waypoint_nav_status.waypoints[wp_last_idx].allow_skip)
330  break;
331  wp_last_idx++;
332  }
333 
334  for (int wp_idx = wps.waypoint_index_current_goal; wp_idx <= wp_last_idx; wp_idx++)
335  {
336  TWaypointStatus &wp = wps.waypoints[wp_idx];
337  const bool is_final_wp = ((wp_idx + 1) == int(wps.waypoints.size()));
338 
340 
341  ti.target_coords.x = wp.target.x;
342  ti.target_coords.y = wp.target.y;
346  ti.targetIsRelative = false;
347  ti.targetIsIntermediaryWaypoint = !is_final_wp;
349 
350  // For backwards compat. with single-target code, write single target info too for the first, next, waypoint:
351  if (wp_idx == wps.waypoint_index_current_goal) {
352  nav_cmd.target = ti;
353  }
354  // Append to list of targets:
355  nav_cmd.multiple_targets.emplace_back(ti);
356  }
357  this->processNavigateCommand( &nav_cmd );
358 
359  MRPT_LOG_DEBUG_STREAM( "[CWaypointsNavigator::navigationStep] Active waypoint changed. Current status:\n" << this->getWaypointNavStatus().getAsText());
360  }
361  }
362  }
363 
364  // Note: navigationStep() called *after* waypoints part to get end-of-navigation events *after*
365  // waypoints-related events:
366 
367  m_was_aligning = m_is_aligning; // Let the next timestep know about this
368 
369  MRPT_END
370 }
371 
372 
374 {
375  MRPT_START
376  m_is_aligning = false; // the robot is aligning into a waypoint with a desired heading
377 
378  // State can be NAVIGATING if we are already heading to a waypoint,
379  // or IDLE if navigateWaypoints() was called and this is the first
380  // navStep() afterwards:
382  {
384  }
385 
386  // Call base navigation step to execute one-single waypoint navigation, as usual:
387  if (!m_is_aligning)
388  {
389  CAbstractNavigator::navigationStep(); // This internally locks "m_nav_cs"
390  }
391  else
392  {
393  // otherwise, at least, process pending events:
395  }
396 
397  MRPT_END
398 }
399 
401 {
402 }
403 
405 {
406  return impl_waypoint_is_reachable(wp_local_wrt_robot);
407 }
408 
410 {
411  MRPT_START
412 
413  params_waypoints_navigator.loadFromConfigFile(c, "CWaypointsNavigator");
415 
416  MRPT_END
417 }
418 
420 {
422  params_waypoints_navigator.saveToConfigFile(c, "CWaypointsNavigator");
423 }
424 
426 {
427  MRPT_LOAD_CONFIG_VAR(max_distance_to_allow_skip_waypoint, double, c, s);
428  MRPT_LOAD_CONFIG_VAR(min_timesteps_confirm_skip_waypoints, int, c, s);
429  MRPT_LOAD_CONFIG_VAR_DEGREES(waypoint_angle_tolerance, c, s);
430  MRPT_LOAD_CONFIG_VAR(multitarget_look_ahead, int, c, s);
431 }
432 
434 {
435  MRPT_SAVE_CONFIG_VAR_COMMENT(max_distance_to_allow_skip_waypoint, "Max distance to `foresee` waypoints [meters]. (<0: unlimited)");
436  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.");
437  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)");
438  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)");
439 }
440 
442  max_distance_to_allow_skip_waypoint(-1.0),
443  min_timesteps_confirm_skip_waypoints(1),
444  waypoint_angle_tolerance( mrpt::utils::DEG2RAD(5.0) ),
445  multitarget_look_ahead(0)
446 {
447 }
448 
449 bool CWaypointsNavigator::checkHasReachedTarget(const double targetDist) const
450 {
451  bool ret;
452  const TWaypointStatus *wp = nullptr;
453  const auto &wps = m_waypoint_nav_status;
455  {
456  ret = false;
457  }
458  else if (wps.timestamp_nav_started != INVALID_TIMESTAMP)
459  {
460  wp = (!wps.waypoints.empty() &&
461  wps.waypoint_index_current_goal >= 0 &&
462  wps.waypoint_index_current_goal < (int)wps.waypoints.size()
463  )
464  ?
465  &wps.waypoints[wps.waypoint_index_current_goal]
466  :
467  nullptr;
468  ret = (wp == nullptr && targetDist <= m_navigationParams->target.targetAllowedDistance) ||
469  (wp->reached);
470  }
471  else
472  {
473  ret = (targetDist <= m_navigationParams->target.targetAllowedDistance);
474  }
475  MRPT_LOG_DEBUG_STREAM("CWaypointsNavigator::checkHasReachedTarget() called "
476  "with targetDist=" << targetDist << " return=" << ret << " waypoint: " <<
477  (wp == nullptr ? std::string("") : wp->getAsText()) <<
478  "wps.timestamp_nav_started=" <<
479  (wps.timestamp_nav_started==INVALID_TIMESTAMP ?
480  "INVALID_TIMESTAMP" : mrpt::system::dateTimeLocalToString(wps.timestamp_nav_started) )
481  );
482  return ret;
483 }
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.
virtual void saveConfigFile(mrpt::utils::CConfigFileBase &c) const
Saves all current options to a config file.
double y
X,Y coordinates.
std::string getAsText() const
Gets navigation params as a human-readable format.
Definition: TWaypoint.cpp:105
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:58
TNavigationParams * m_navigationParams
Current navigation parameters.
std::vector< TWaypoint > waypoints
Definition: TWaypoint.h:80
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.
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
STL namespace.
virtual 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.
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...
The struct for requesting navigation requests for a sequence of waypoints.
Definition: TWaypoint.h:78
GLdouble s
Definition: glext.h:3602
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.
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(). 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:97
virtual void navigationStep() MRPT_OVERRIDE
This method must be called periodically in order to effectively run the navigation.
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:120
#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...
const GLubyte * c
Definition: glext.h:5590
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:110
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:95
bool final_goal_reached
Whether the final waypoint has been reached successfuly.
Definition: TWaypoint.h:114
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.
GLsizei const GLchar ** string
Definition: glext.h:3919
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 norm() const
Point norm.
std::list< TPendingEvent > m_pending_events
Events generated during navigationStep(), enqueued to be called at the end of the method execution to...
double y
X,Y coordinates.
#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
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.
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.
virtual 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.
void inverseComposePoint(const double gx, const double gy, double &lx, double &ly) const
Computes the 2D point L such as .
Definition: CPose2D.cpp:205
#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 cancel()
Cancel current navegation.
mrpt::system::TTimeStamp timestamp_nav_started
Timestamp of user navigation command.
Definition: TWaypoint.h:113
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
std::vector< TWaypointStatus > waypoints
Waypoints parameters and status (reached, skipped, etc.)
Definition: TWaypoint.h:112
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:118
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...
double speed_ratio
(Default=1.0) Desired robot speed at the target, as a ratio of the full robot speed.
Definition: TWaypoint.h:44
virtual bool stop(bool isEmergencyStop)
Default: forward call to m_robot.stop(). Can be overriden.
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.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019