MRPT  1.9.9
CAbstractNavigator.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-2019, 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 #include <mrpt/obs/obs_frwds.h>
19 
20 #include <functional>
21 #include <list>
22 #include <memory> // unique_ptr
23 #include <mutex>
24 
25 namespace mrpt::nav
26 {
27 /** This is the base class for any reactive/planned navigation system. See
28  * derived classes.
29  *
30  * How to use:
31  * - A class derived from `CRobot2NavInterface` with callbacks must be defined
32  * by the user and provided to the constructor.
33  * - `navigationStep()` must be called periodically in order to effectively run
34  * the navigation. This method will internally call the callbacks to gather
35  * sensor data and robot positioning data.
36  *
37  * It implements the following state machine (see
38  * CAbstractNavigator::getCurrentState() ), taking into account the extensions
39  * described in CWaypointsNavigator
40  * \dot
41  * digraph CAbstractNavigator_States {
42  * IDLE; NAVIGATING; SUSPENDED; NAV_ERROR;
43  * IDLE -> NAVIGATING [ label="CAbstractNavigator::navigate()"];
44  * IDLE -> NAVIGATING [ label="CWaypointsNavigator::navigateWaypoints()" ];
45  * NAVIGATING -> IDLE [ label="Final target reached" ];
46  * NAVIGATING -> IDLE [ label="CAbstractNavigator::cancel()" ];
47  * NAVIGATING -> NAV_ERROR [ label="Upon sensor errors, timeout,..." ];
48  * NAVIGATING -> SUSPENDED [ label="CAbstractNavigator::suspend()" ];
49  * SUSPENDED -> NAVIGATING [ label="CAbstractNavigator::resume()" ];
50  * NAV_ERROR -> IDLE [ label="CAbstractNavigator::resetNavError()" ];
51  * }
52  * \enddot
53  *
54  * \sa CWaypointsNavigator, CReactiveNavigationSystem, CRobot2NavInterface, all
55  * children classes
56  * \ingroup nav_reactive
57  */
59 {
60  public:
61  /** ctor */
62  CAbstractNavigator(CRobot2NavInterface& robot_interface_impl);
63  /** dtor */
64  ~CAbstractNavigator() override;
65 
66  /** Individual target info in CAbstractNavigator::TNavigationParamsBase and
67  * derived classes */
68  struct TargetInfo
69  {
70  /** Coordinates of desired target location. Heading may be ignored by
71  * some reactive implementations. */
73  /** (Default="map") Frame ID in which target is given. Optional, use
74  * only for submapping applications. */
76  /** (Default=0.5 meters) Allowed distance to target in order to end the
77  * navigation. */
79  /** (Default=false) Whether the \a target coordinates are in global
80  * coordinates (false) or are relative to the current robot pose (true).
81  */
82  bool targetIsRelative{false};
83  /** (Default=.05) Desired relative speed (wrt maximum speed), in range
84  * [0,1], of the vehicle at target. Holonomic nav methods will perform
85  * "slow down" approaching target only if this is "==.0". Intermediary
86  * values will be honored only by the higher-level navigator, based on
87  * straight-line Euclidean distances. */
88  double targetDesiredRelSpeed{.05};
90  false}; // !< (Default=false) If true, event
91  // callback
92  // `sendWaypointReachedEvent()` will
93  // be called instead of
94  // `sendNavigationEndEvent()`
95 
96  TargetInfo();
97  /** Gets navigation params as a human-readable format */
98  std::string getAsText() const;
99  bool operator==(const TargetInfo& o) const;
100  bool operator!=(const TargetInfo& o) const { return !(*this == o); }
101  };
102 
103  /** Base for all high-level navigation commands. See derived classes */
105  {
106  virtual ~TNavigationParamsBase() = default;
107  /** Gets navigation params as a human-readable format */
108  virtual std::string getAsText() const = 0;
109 
110  protected:
111  friend bool operator==(
113  virtual bool isEqual(const TNavigationParamsBase& o) const = 0;
114  };
115 
116  /** The struct for configuring navigation requests. Used in
117  * CAbstractPTGBasedReactive::navigate() */
119  {
120  /** Navigation target */
122 
123  /** Gets navigation params as a human-readable format */
124  std::string getAsText() const override;
125  virtual std::unique_ptr<TNavigationParams> clone() const
126  {
127  return std::make_unique<TNavigationParams>(*this);
128  }
129 
130  protected:
131  bool isEqual(const TNavigationParamsBase& o) const override;
132  };
133 
134  /** \name Navigation control API
135  * @{ */
136 
137  /** Loads all params from a file. To be called before initialize().
138  * Each derived class *MUST* load its own parameters, and then call *ITS
139  * PARENT'S* overriden method to ensure all params are loaded. */
140  virtual void loadConfigFile(const mrpt::config::CConfigFileBase& c);
141  /** Saves all current options to a config file.
142  * Each derived class *MUST* save its own parameters, and then call *ITS
143  * PARENT'S* overriden method to ensure all params are saved. */
144  virtual void saveConfigFile(mrpt::config::CConfigFileBase& c) const;
145 
146  /** Must be called before any other navigation command */
147  virtual void initialize() = 0;
148  /** This method must be called periodically in order to effectively run the
149  * navigation */
150  virtual void navigationStep();
151 
152  /** Navigation request to a single target location. It starts a new
153  * navigation.
154  * \param[in] params Pointer to structure with navigation info (its
155  * contents will be copied, so the original can be freely destroyed upon
156  * return if it was dynamically allocated.)
157  * \note A pointer is used so the passed object can be polymorphic with
158  * derived types.
159  */
160  virtual void navigate(const TNavigationParams* params);
161 
162  /** Cancel current navegation. */
163  virtual void cancel();
164  /** Continues with suspended navigation. \sa suspend */
165  virtual void resume();
166  /** Suspend current navegation. \sa resume */
167  virtual void suspend();
168  /** Resets a `NAV_ERROR` state back to `IDLE` */
169  virtual void resetNavError();
170 
171  /** The different states for the navigation system. */
172  enum TState
173  {
174  IDLE = 0,
178  };
179 
180  /** Returns the current navigator state. */
181  inline TState getCurrentState() const { return m_navigationState; }
182 
183  /** Explains the reason for the navigation error. */
185  {
186  ERR_NONE = 0,
190  };
192  {
193  TErrorReason() = default;
194 
196  /** Human friendly description of the error */
198  };
199 
200  /** In case of state=NAV_ERROR, this returns the reason for the error.
201  * Error state is reseted everytime a new navigation starts with
202  * a call to navigate(), or when resetNavError() is called.
203  */
204  inline const TErrorReason& getErrorReason() const
205  {
206  return m_navErrorReason;
207  }
208 
209  /** Sets a user-provided frame transformer object; used only if providing
210  * targets in a frame ID
211  * different than the one in which robot odometry is given (both IDs
212  * default to `"map"`).
213  */
214  void setFrameTF(
215  const std::weak_ptr<mrpt::poses::FrameTransformer<2>>& frame_tf);
216 
217  /** Get the current frame tf object (defaults to nullptr) \sa setFrameTF */
218  std::weak_ptr<mrpt::poses::FrameTransformer<2>> getFrameTF() const
219  {
220  return m_frame_tf;
221  }
222 
223  /** By default, error exceptions on navigationStep() will dump an error
224  * message to the output logger interface. If rethrow is enabled
225  * (default=false), the error message will be reported as well, but
226  * exceptions will be re-thrown.
227  */
228  void enableRethrowNavExceptions(const bool enable)
229  {
230  m_rethrow_exceptions = enable;
231  }
233  /** @}*/
234 
236  {
237  /** Default value=0, means use the "targetAllowedDistance" passed by the
238  * user in the navigation request. */
240  /** navigator timeout (seconds) [Default=30 sec] */
242  /** (Default value=0.6) When closer than this distance, check if the
243  * target is blocked to abort navigation with an error. */
245  /** (Default=3) How many steps should the condition for
246  * dist_check_target_is_blocked be fulfilled to raise an event */
248 
249  void loadFromConfigFile(
251  const std::string& s) override;
252  void saveToConfigFile(
254  const std::string& s) const override;
256  };
257 
259 
260  /** Gives access to a const-ref to the internal time logger used to estimate
261  * delays \sa getTimeLogger() in derived classes */
263  {
264  return m_timlog_delays;
265  }
266 
267  private:
268  /** Last internal state of navigator: */
271  /** Will be false until the navigation end is sent, and it is reset with
272  * each new command */
275  bool m_rethrow_exceptions{false};
276 
277  /** Called before starting a new navigation. Internally, it calls to
278  * child-implemented onStartNewNavigation() */
280 
281  protected:
282  /** Events generated during navigationStep(), enqueued to be called at the
283  * end of the method execution to avoid user code to change the navigator
284  * state. */
285  std::list<std::function<void(void)>> m_pending_events;
286 
288 
289  /** To be implemented in derived classes */
290  virtual void performNavigationStep() = 0;
291 
292  /** Called whenever a new navigation has been started. Can be used to reset
293  * state variables, etc. */
294  virtual void onStartNewNavigation() = 0;
295 
296  /** Called after each call to CAbstractNavigator::navigate() */
297  virtual void onNavigateCommandReceived();
298 
299  /** Call to the robot getCurrentPoseAndSpeeds() and updates members
300  * m_curPoseVel accordingly.
301  * If an error is returned by the user callback, first, it calls
302  * robot.stop() ,then throws an std::runtime_error exception. */
303  virtual void updateCurrentPoseAndSpeeds();
304 
305  /** Factorization of the part inside navigationStep(), for the case of state
306  * being NAVIGATING.
307  * Performs house-hold tasks like raising events in case of starting/ending
308  * navigation, timeout reaching destination, etc.
309  * `call_virtual_nav_method` can be set to false to avoid calling the
310  * virtual method performNavigationStep()
311  */
312  virtual void performNavigationStepNavigating(
313  bool call_virtual_nav_method = true);
314 
315  /** Does the job of navigate(), except the call to
316  * onNavigateCommandReceived() */
317  virtual void processNavigateCommand(const TNavigationParams* params);
318 
319  /** Stops the robot and set navigation state to error */
320  virtual void doEmergencyStop(const std::string& msg);
321 
322  /** Default: forward call to m_robot.changeSpeed(). Can be overriden. */
323  virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd& vel_cmd);
324  /** Default: forward call to m_robot.changeSpeedsNOP(). Can be overriden. */
325  virtual bool changeSpeedsNOP();
326  /** Default: forward call to m_robot.stop(). Can be overriden. */
327  virtual bool stop(bool isEmergencyStop);
328 
329  /** Default implementation: check if target_dist is below the accepted
330  * distance.
331  * If true is returned here, the end-of-navigation event will be sent out
332  * (only for non-intermediary targets).
333  */
334  virtual bool checkHasReachedTarget(const double targetDist) const;
335 
336  /** Checks whether the robot shape, when placed at the given pose (relative
337  * to the current pose),
338  * is colliding with any of the latest known obstacles.
339  * Default implementation: always returns false. */
341  const mrpt::math::TPose2D& relative_robot_pose) const;
342 
343  /** Current internal state of navigator: */
345  /** Current navigation parameters */
346  std::unique_ptr<TNavigationParams> m_navigationParams;
347 
348  /** The navigator-robot interface. */
350 
351  /** Optional, user-provided frame transformer. */
352  std::weak_ptr<mrpt::poses::FrameTransformer<2>> m_frame_tf;
353 
354  /** mutex for all navigation methods */
355  std::recursive_mutex m_nav_cs;
356 
358  {
361  /** raw odometry (frame does not match to "pose", but is expected to be
362  * smoother in the short term). */
365  /** map frame ID for `pose` */
367  TRobotPoseVel();
368  };
369 
370  /** Current robot pose (updated in CAbstractNavigator::navigationStep() ) */
374  /** Latest robot poses (updated in CAbstractNavigator::navigationStep() ) */
376 
377  /** Time logger to collect delay-related stats */
379 
380  /** For sending an alarm (error event) when it seems that we are not
381  * approaching toward the target in a while... */
384 
385  public:
386 };
387 
388 bool operator==(
391 } // namespace mrpt::nav
393 using namespace mrpt::nav;
TErrorCode
Explains the reason for the navigation error.
double m_badNavAlarm_minDistTarget
For sending an alarm (error event) when it seems that we are not approaching toward the target in a w...
std::recursive_mutex m_nav_cs
mutex for all navigation methods
virtual std::unique_ptr< TNavigationParams > clone() const
TRobotPoseVel m_curPoseVel
Current robot pose (updated in CAbstractNavigator::navigationStep() )
std::list< 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 loadConfigFile(const mrpt::config::CConfigFileBase &c)
Loads all params from a file.
mrpt::system::CTimeLogger m_timlog_delays
Time logger to collect delay-related stats.
This class stores a time-stamped trajectory in SE(2) (mrpt::math::TPose2D poses). ...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
void setFrameTF(const std::weak_ptr< mrpt::poses::FrameTransformer< 2 >> &frame_tf)
Sets a user-provided frame transformer object; used only if providing targets in a frame ID different...
~CAbstractNavigator() override
dtor
Base for all high-level navigation commands.
float targetAllowedDistance
(Default=0.5 meters) Allowed distance to target in order to end the navigation.
virtual void updateCurrentPoseAndSpeeds()
Call to the robot getCurrentPoseAndSpeeds() and updates members m_curPoseVel accordingly.
bool targetIsRelative
(Default=false) Whether the target coordinates are in global coordinates (false) or are relative to t...
double dist_check_target_is_blocked
(Default value=0.6) When closer than this distance, check if the target is blocked to abort navigatio...
GLdouble s
Definition: glext.h:3682
virtual void navigationStep()
This method must be called periodically in order to effectively run the navigation.
std::string pose_frame_id
map frame ID for pose
virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd)
Default: forward call to m_robot.changeSpeed().
std::weak_ptr< mrpt::poses::FrameTransformer< 2 > > getFrameTF() const
Get the current frame tf object (defaults to nullptr)
virtual bool changeSpeedsNOP()
Default: forward call to m_robot.changeSpeedsNOP().
virtual bool isEqual(const TNavigationParamsBase &o) const =0
TState m_navigationState
Current internal state of navigator:
Virtual base for velocity commands of different kinematic models of planar mobile robot...
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
Definition: TTwist2D.h:19
virtual void performNavigationStep()=0
To be implemented in derived classes.
bool operator==(const CAbstractNavigator::TNavigationParamsBase &, const CAbstractNavigator::TNavigationParamsBase &)
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
CAbstractNavigator(CRobot2NavInterface &robot_interface_impl)
ctor
This class allows loading and storing values and vectors of different types from a configuration text...
TState
The different states for the navigation system.
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.
double dist_to_target_for_sending_event
Default value=0, means use the "targetAllowedDistance" passed by the user in the navigation request...
double targetDesiredRelSpeed
(Default=.05) Desired relative speed (wrt maximum speed), in range [0,1], of the vehicle at target...
TState getCurrentState() const
Returns the current navigator state.
virtual void onNavigateCommandReceived()
Called after each call to CAbstractNavigator::navigate()
const mrpt::system::CTimeLogger & getDelaysTimeLogger() const
Gives access to a const-ref to the internal time logger used to estimate delays.
std::string target_frame_id
(Default="map") Frame ID in which target is given.
const GLubyte * c
Definition: glext.h:6406
Versatile class for consistent logging and management of output messages.
void enableRethrowNavExceptions(const bool enable)
By default, error exceptions on navigationStep() will dump an error message to the output logger inte...
virtual void resetNavError()
Resets a NAV_ERROR state back to IDLE
std::unique_ptr< TNavigationParams > m_navigationParams
Current navigation parameters.
void internal_onStartNewNavigation()
Called before starting a new navigation.
Individual target info in CAbstractNavigator::TNavigationParamsBase and derived classes.
#define MRPT_ENUM_TYPE_END()
Definition: TEnumType.h:78
virtual void onStartNewNavigation()=0
Called whenever a new navigation has been started.
GLsizei const GLchar ** string
Definition: glext.h:4116
virtual void resume()
Continues with suspended navigation.
mrpt::math::TPose2D target_coords
Coordinates of desired target location.
TState m_lastNavigationState
Last internal state of navigator:
double alarm_seems_not_approaching_target_timeout
navigator timeout (seconds) [Default=30 sec]
virtual std::string getAsText() const =0
Gets navigation params as a human-readable format.
virtual void performNavigationStepNavigating(bool call_virtual_nav_method=true)
Factorization of the part inside navigationStep(), for the case of state being NAVIGATING.
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const
Saves all current options to a config file.
std::string getAsText() const
Gets navigation params as a human-readable format.
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
virtual void doEmergencyStop(const std::string &msg)
Stops the robot and set navigation state to error.
virtual void processNavigateCommand(const TNavigationParams *params)
Does the job of navigate(), except the call to onNavigateCommandReceived()
TAbstractNavigatorParams params_abstract_navigator
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. ...
virtual void initialize()=0
Must be called before any other navigation command.
CRobot2NavInterface & m_robot
The navigator-robot interface.
bool isEqual(const TNavigationParamsBase &o) const override
virtual bool checkCollisionWithLatestObstacles(const mrpt::math::TPose2D &relative_robot_pose) const
Checks whether the robot shape, when placed at the given pose (relative to the current pose)...
Lightweight 2D pose.
Definition: TPose2D.h:22
virtual void cancel()
Cancel current navegation.
const TErrorReason & getErrorReason() const
In case of state=NAV_ERROR, this returns the reason for the error.
std::string error_msg
Human friendly description of the error.
bool operator==(const TargetInfo &o) const
virtual void suspend()
Suspend current navegation.
std::weak_ptr< mrpt::poses::FrameTransformer< 2 > > m_frame_tf
Optional, user-provided frame transformer.
friend bool operator==(const TNavigationParamsBase &, const TNavigationParamsBase &)
int hysteresis_check_target_is_blocked
(Default=3) How many steps should the condition for dist_check_target_is_blocked be fulfilled to rais...
virtual bool checkHasReachedTarget(const double targetDist) const
Default implementation: check if target_dist is below the accepted distance.
mrpt::poses::CPose2DInterpolator m_latestOdomPoses
bool m_navigationEndEventSent
Will be false until the navigation end is sent, and it is reset with each new command.
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
mrpt::system::TTimeStamp m_badNavAlarm_lastMinDistTime
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
Definition: TEnumType.h:62
This is the base class for any reactive/planned navigation system.
GLenum const GLfloat * params
Definition: glext.h:3538
mrpt::poses::CPose2DInterpolator m_latestPoses
Latest robot poses (updated in CAbstractNavigator::navigationStep() )
std::string getAsText() const override
Gets navigation params as a human-readable format.
bool operator!=(const TargetInfo &o) const
mrpt::math::TPose2D rawOdometry
raw odometry (frame does not match to "pose", but is expected to be smoother in the short term)...
virtual bool stop(bool isEmergencyStop)
Default: forward call to m_robot.stop().
MRPT_FILL_ENUM_MEMBER(CAbstractNavigator, IDLE)
virtual void navigate(const TNavigationParams *params)
Navigation request to a single target location.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019