MRPT  2.0.4
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
mrpt::nav::CWaypointsNavigator::isRelativePointReachable
bool isRelativePointReachable(const mrpt::math::TPoint2D &wp_local_wrt_robot) const
Returns true if, according to the information gathered at the last navigation step,...
Definition: CWaypointsNavigator.cpp:476
TWaypoint.h
mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::waypoint_angle_tolerance
double waypoint_angle_tolerance
[rad] Angular error tolerance for waypoints with an assigned heading (Default: 5 deg)
Definition: CWaypointsNavigator.h:118
mrpt::nav::CWaypointsNavigator::impl_waypoint_is_reachable
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_< double >
mrpt::nav::CWaypointsNavigator::m_was_aligning
bool m_was_aligning
Whether the last timestep was "is_aligning" in a waypoint with heading.
Definition: CWaypointsNavigator.h:166
mrpt::nav
Definition: CAbstractHolonomicReactiveMethod.h:20
mrpt::nav::CWaypointsNavigator::m_last_alignment_cmd
mrpt::system::TTimeStamp m_last_alignment_cmd
Definition: CWaypointsNavigator.h:168
mrpt::nav::CWaypointsNavigator::navigationStep
void navigationStep() override
This method must be called periodically in order to effectively run the navigation.
Definition: CWaypointsNavigator.cpp:443
mrpt::nav::CWaypointsNavigator::checkHasReachedTarget
bool checkHasReachedTarget(const double targetDist) const override
Default implementation: check if target_dist is below the accepted distance.
Definition: CWaypointsNavigator.cpp:537
mrpt::nav::CWaypointsNavigator::~CWaypointsNavigator
~CWaypointsNavigator() override
dtor
mrpt::nav::CAbstractNavigator
This is the base class for any reactive/planned navigation system.
Definition: CAbstractNavigator.h:58
mrpt::nav::CWaypointsNavigator::TNavigationParamsWaypoints
The struct for configuring navigation requests to CWaypointsNavigator and derived classes.
Definition: CWaypointsNavigator.h:43
mrpt::nav::CWaypointsNavigator::getWaypointNavStatus
TWaypointStatusSequence getWaypointNavStatus() const
Get a copy of the control structure which describes the progress status of the waypoint navigation.
Definition: CWaypointsNavigator.h:93
mrpt::nav::CRobot2NavInterface
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
Definition: CRobot2NavInterface.h:43
mrpt::nav::CWaypointsNavigator
This class extends CAbstractNavigator with the capability of following a list of waypoints.
Definition: CWaypointsNavigator.h:38
mrpt::nav::CWaypointsNavigator::waypoints_navigationStep
virtual void waypoints_navigationStep()
The waypoints-specific part of navigationStep()
Definition: CWaypointsNavigator.cpp:112
mrpt::nav::CWaypointsNavigator::onNavigateCommandReceived
void onNavigateCommandReceived() override
Called after each call to CAbstractNavigator::navigate()
Definition: CWaypointsNavigator.cpp:54
mrpt::nav::CWaypointsNavigator::cancel
void cancel() override
Cancel current navegation.
Definition: CWaypointsNavigator.cpp:102
mrpt::nav::CAbstractNavigator::TNavigationParams
The struct for configuring navigation requests.
Definition: CAbstractNavigator.h:118
mrpt::nav::CWaypointsNavigator::waypoints_isAligning
bool waypoints_isAligning() const
Definition: CWaypointsNavigator.h:163
mrpt::nav::CWaypointsNavigator::params_waypoints_navigator
TWaypointsNavigatorParams params_waypoints_navigator
Definition: CWaypointsNavigator.h:133
mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::max_distance_to_allow_skip_waypoint
double max_distance_to_allow_skip_waypoint
In meters.
Definition: CWaypointsNavigator.h:112
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::config::CLoadableOptions
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
Definition: config/CLoadableOptions.h:26
mrpt::system::TTimeStamp
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
mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::saveToConfigFile
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.
Definition: CWaypointsNavigator.cpp:509
CAbstractNavigator.h
mrpt::nav::CWaypointsNavigator::TNavigationParamsWaypoints::multiple_targets
std::vector< mrpt::nav::CAbstractNavigator::TargetInfo > multiple_targets
If not empty, this will prevail over the base class single goal target.
Definition: CWaypointsNavigator.h:51
mrpt::nav::CWaypointsNavigator::m_nav_waypoints_cs
std::recursive_mutex m_nav_waypoints_cs
Definition: CWaypointsNavigator.h:144
mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::TWaypointsNavigatorParams
TWaypointsNavigatorParams()
Definition: CWaypointsNavigator.cpp:530
mrpt::nav::CWaypointsNavigator::TNavigationParamsWaypoints::isEqual
bool isEqual(const CAbstractNavigator::TNavigationParamsBase &o) const override
Definition: CWaypointsNavigator.cpp:36
mrpt::nav::TWaypointSequence
The struct for requesting navigation requests for a sequence of waypoints.
Definition: TWaypoint.h:96
mrpt::nav::CWaypointsNavigator::saveConfigFile
void saveConfigFile(mrpt::config::CConfigFileBase &c) const override
Saves all current options to a config file.
Definition: CWaypointsNavigator.cpp:492
mrpt::nav::CWaypointsNavigator::CWaypointsNavigator
CWaypointsNavigator(CRobot2NavInterface &robot_interface_impl)
ctor
Definition: CWaypointsNavigator.cpp:47
mrpt::nav::CWaypointsNavigator::TNavigationParamsWaypoints::clone
std::unique_ptr< TNavigationParams > clone() const override
Definition: CWaypointsNavigator.h:54
mrpt::nav::CWaypointsNavigator::TNavigationParamsWaypoints::getAsText
std::string getAsText() const override
Gets navigation params as a human-readable format.
Definition: CWaypointsNavigator.cpp:20
mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::min_timesteps_confirm_skip_waypoints
int min_timesteps_confirm_skip_waypoints
How many times shall a future waypoint be seen as reachable to skip to it (Default: 1)
Definition: CWaypointsNavigator.h:115
mrpt::nav::CWaypointsNavigator::navigateWaypoints
virtual void navigateWaypoints(const TWaypointSequence &nav_request)
Waypoint navigation request.
Definition: CWaypointsNavigator.cpp:66
mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams
Definition: CWaypointsNavigator.h:109
mrpt::nav::CWaypointsNavigator::m_is_aligning
bool m_is_aligning
Definition: CWaypointsNavigator.h:167
mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::multitarget_look_ahead
int multitarget_look_ahead
>=0 number of waypoints to forward to the underlying navigation engine, to ease obstacles avoidance w...
Definition: CWaypointsNavigator.h:122
mrpt::nav::CWaypointsNavigator::loadConfigFile
void loadConfigFile(const mrpt::config::CConfigFileBase &c) override
Loads all params from a file.
Definition: CWaypointsNavigator.cpp:482
mrpt::nav::TWaypointStatusSequence
The struct for querying the status of waypoints navigation.
Definition: TWaypoint.h:143
mrpt::nav::CWaypointsNavigator::m_waypoint_nav_status
TWaypointStatusSequence m_waypoint_nav_status
The latest waypoints navigation command and the up-to-date control status.
Definition: CWaypointsNavigator.h:143
mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::loadFromConfigFile
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.
Definition: CWaypointsNavigator.cpp:499
mrpt::nav::CAbstractNavigator::TNavigationParamsBase
Base for all high-level navigation commands.
Definition: CAbstractNavigator.h:104
mrpt::nav::CWaypointsNavigator::onStartNewNavigation
void onStartNewNavigation() override
Called whenever a new navigation has been started.
Definition: CWaypointsNavigator.cpp:474



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 17:54:30 UTC 2020