MRPT  1.9.9
CWaypointsNavigator.h
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 #pragma once
10 
13 
14 namespace mrpt::nav
15 {
16 /** This class extends `CAbstractNavigator` with the capability of following a
17  * list of waypoints. By default, waypoints are followed one by one,
18  * but, if they are tagged with `allow_skip=true` **and** the derived navigator
19  * class supports it, the navigator may choose to skip some to
20  * make a smoother, safer and shorter navigation.
21  *
22  * Waypoints have an optional `target_heading` field, which will be honored only
23  * for waypoints that are skipped, and if the underlying robot
24  * interface supports the pure-rotation methods.
25  *
26  * Notes on navigation status and event dispatchment:
27  * - Navigation state may briefly pass by the `IDLE` status between a waypoint
28  * is reached and a new navigation command is issued towards the next waypoint.
29  * - `sendNavigationEndEvent()` will be called only when the last waypoint is
30  * reached.
31  * - Reaching an intermediary waypoint (or skipping it if considered so by the
32  * navigator) generates a call to `sendWaypointReachedEvent()` instead.
33  *
34  * \sa Base class CAbstractNavigator, CWaypointsNavigator::navigateWaypoints(),
35  * and derived classes.
36  * \ingroup nav_reactive
37  */
39 {
40  public:
41  /** The struct for configuring navigation requests to CWaypointsNavigator
42  * and derived classes. */
45  {
46  /** If not empty, this will prevail over the base class single goal
47  * target.
48  * Semantic is: any of these targets will be good for heading the robot
49  * towards them,
50  * but the priority is for the latest ones in the sequence. */
51  std::vector<mrpt::nav::CAbstractNavigator::TargetInfo> multiple_targets;
52 
53  std::string getAsText() const override;
54  std::unique_ptr<TNavigationParams> clone() const override
55  {
56  return std::unique_ptr<TNavigationParams>(
57  new TNavigationParamsWaypoints(*this));
58  }
59 
60  protected:
61  bool isEqual(
62  const CAbstractNavigator::TNavigationParamsBase& o) const override;
63  };
64 
65  /** ctor */
66  CWaypointsNavigator(CRobot2NavInterface& robot_interface_impl);
67  /** dtor */
68  ~CWaypointsNavigator() override;
69 
70  // Overriden to call the general navigationStep(), plus waypoint selection
71  // logic.
72  void navigationStep() override;
73  /** Cancel current navegation. */
74  void cancel() override;
75 
76  /** \name Waypoint navigation control API
77  * @{ */
78 
79  /** Waypoint navigation request. This immediately cancels any other previous
80  * on-going navigation.
81  * \sa CAbstractNavigator::navigate() for single waypoint navigation
82  * requests.
83  */
84  virtual void navigateWaypoints(const TWaypointSequence& nav_request);
85 
86  /** Get a copy of the control structure which describes the progress status
87  * of the waypoint navigation. */
88  virtual void getWaypointNavStatus(
89  TWaypointStatusSequence& out_nav_status) const;
90 
91  /** Get a copy of the control structure which describes the progress status
92  * of the waypoint navigation. */
94  {
95  TWaypointStatusSequence nav_status;
96  this->getWaypointNavStatus(nav_status);
97  return nav_status;
98  }
99  /** @}*/
100 
101  /** Returns `true` if, according to the information gathered at the last
102  * navigation step,
103  * there is a free path to the given point; `false` otherwise: if way is
104  * blocked or there is missing information,
105  * the point is out of range for the existing PTGs, etc. */
107  const mrpt::math::TPoint2D& wp_local_wrt_robot) const;
108 
110  {
111  /** In meters. <0: unlimited */
113  /** How many times shall a future waypoint be seen as reachable to skip
114  * to it (Default: 1) */
116  /** [rad] Angular error tolerance for waypoints with an assigned heading
117  * (Default: 5 deg) */
119  /** >=0 number of waypoints to forward to the underlying navigation
120  * engine, to ease obstacles avoidance when a waypoint is blocked
121  * (Default=0 : none). */
123 
124  void loadFromConfigFile(
126  const std::string& s) override;
127  void saveToConfigFile(
129  const std::string& s) const override;
131  };
132 
134 
136  override; // See base class docs!
138  const override; // See base class docs!
139 
140  protected:
141  /** The latest waypoints navigation command and the up-to-date control
142  * status. */
144  std::recursive_mutex m_nav_waypoints_cs;
145 
146  /** Implements the way to waypoint is free function in children classes:
147  * `true` must be returned
148  * if, according to the information gathered at the last navigation step,
149  * there is a free path to
150  * the given point; `false` otherwise: if way is blocked or there is
151  * missing information, the point is out of range, etc. */
152  virtual bool impl_waypoint_is_reachable(
153  const mrpt::math::TPoint2D& wp_local_wrt_robot) const = 0;
154 
155  void onStartNewNavigation() override;
156 
157  void onNavigateCommandReceived() override;
158 
159  bool checkHasReachedTarget(const double targetDist) const override;
160  /** The waypoints-specific part of navigationStep() */
161  virtual void waypoints_navigationStep();
162 
163  bool waypoints_isAligning() const { return m_is_aligning; }
164  /** Whether the last timestep was "is_aligning" in a waypoint with heading
165  */
166  bool m_was_aligning{false};
167  bool m_is_aligning{false};
169 };
170 } // namespace mrpt::nav
void cancel() override
Cancel current navegation.
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.
std::vector< mrpt::nav::CAbstractNavigator::TargetInfo > multiple_targets
If not empty, this will prevail over the base class single goal target.
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
void onStartNewNavigation() override
Called whenever a new navigation has been started.
double waypoint_angle_tolerance
[rad] Angular error tolerance for waypoints with an assigned heading (Default: 5 deg) ...
Base for all high-level navigation commands.
std::recursive_mutex m_nav_waypoints_cs
std::unique_ptr< TNavigationParams > clone() const override
TWaypointsNavigatorParams params_waypoints_navigator
TWaypointStatusSequence getWaypointNavStatus() const
Get a copy of the control structure which describes the progress status of the waypoint navigation...
This class extends CAbstractNavigator with the capability of following a list of waypoints.
The struct for requesting navigation requests for a sequence of waypoints.
Definition: TWaypoint.h:96
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.
The struct for configuring navigation requests to CWaypointsNavigator and derived classes...
The struct for configuring navigation requests.
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
This class allows loading and storing values and vectors of different types from a configuration text...
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. ...
The struct for querying the status of waypoints navigation.
Definition: TWaypoint.h:143
mrpt::system::TTimeStamp m_last_alignment_cmd
bool isEqual(const CAbstractNavigator::TNavigationParamsBase &o) const override
~CWaypointsNavigator() override
dtor
bool checkHasReachedTarget(const double targetDist) const override
Default implementation: check if target_dist is below the accepted distance.
std::string getAsText() const override
Gets navigation params as a human-readable format.
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.
virtual void navigateWaypoints(const TWaypointSequence &nav_request)
Waypoint navigation request.
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...
void onNavigateCommandReceived() override
Called after each call to CAbstractNavigator::navigate()
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.
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
CWaypointsNavigator(CRobot2NavInterface &robot_interface_impl)
ctor
This is the base class for any reactive/planned navigation system.
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...
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: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020