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-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 #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. */
75  std::string target_frame_id;
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 */
197  std::string error_msg;
198  };
199 
200  /** In case of state=NAV_ERROR, this returns the reason for the error.
201  * Error state is reseted every time 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  /** Publicly available time profiling object. Default: disabled */
269  "mrpt::nav::CAbstractNavigator"};
270 
271  private:
272  /** Last internal state of navigator: */
275  /** Will be false until the navigation end is sent, and it is reset with
276  * each new command */
279  bool m_rethrow_exceptions{false};
280 
281  /** Called before starting a new navigation. Internally, it calls to
282  * child-implemented onStartNewNavigation() */
284 
285  protected:
286  /** Events generated during navigationStep(), enqueued to be called at the
287  * end of the method execution to avoid user code to change the navigator
288  * state. */
289  std::list<std::function<void(void)>> m_pending_events;
290 
292 
293  /** To be implemented in derived classes */
294  virtual void performNavigationStep() = 0;
295 
296  /** Called whenever a new navigation has been started. Can be used to reset
297  * state variables, etc. */
298  virtual void onStartNewNavigation() = 0;
299 
300  /** Called after each call to CAbstractNavigator::navigate() */
301  virtual void onNavigateCommandReceived();
302 
303  /** Call to the robot getCurrentPoseAndSpeeds() and updates members
304  * m_curPoseVel accordingly.
305  * If an error is returned by the user callback, first, it calls
306  * robot.stop() ,then throws an std::runtime_error exception. */
307  virtual void updateCurrentPoseAndSpeeds();
308 
309  /** Factorization of the part inside navigationStep(), for the case of state
310  * being NAVIGATING.
311  * Performs house-hold tasks like raising events in case of starting/ending
312  * navigation, timeout reaching destination, etc.
313  * `call_virtual_nav_method` can be set to false to avoid calling the
314  * virtual method performNavigationStep()
315  */
316  virtual void performNavigationStepNavigating(
317  bool call_virtual_nav_method = true);
318 
319  /** Does the job of navigate(), except the call to
320  * onNavigateCommandReceived() */
321  virtual void processNavigateCommand(const TNavigationParams* params);
322 
323  /** Stops the robot and set navigation state to error */
324  virtual void doEmergencyStop(const std::string& msg);
325 
326  /** Default: forward call to m_robot.changeSpeed(). Can be overriden. */
327  virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd& vel_cmd);
328  /** Default: forward call to m_robot.changeSpeedsNOP(). Can be overriden. */
329  virtual bool changeSpeedsNOP();
330  /** Default: forward call to m_robot.stop(). Can be overriden. */
331  virtual bool stop(bool isEmergencyStop);
332 
333  /** Default implementation: check if target_dist is below the accepted
334  * distance.
335  * If true is returned here, the end-of-navigation event will be sent out
336  * (only for non-intermediary targets).
337  */
338  virtual bool checkHasReachedTarget(const double targetDist) const;
339 
340  /** Checks whether the robot shape, when placed at the given pose (relative
341  * to the current pose),
342  * is colliding with any of the latest known obstacles.
343  * Default implementation: always returns false. */
345  const mrpt::math::TPose2D& relative_robot_pose) const;
346 
347  /** Current internal state of navigator: */
349  /** Current navigation parameters */
350  std::unique_ptr<TNavigationParams> m_navigationParams;
351 
352  /** The navigator-robot interface. */
354 
355  /** Optional, user-provided frame transformer. */
356  std::weak_ptr<mrpt::poses::FrameTransformer<2>> m_frame_tf;
357 
358  /** mutex for all navigation methods */
359  std::recursive_mutex m_nav_cs;
360 
362  {
365  /** raw odometry (frame does not match to "pose", but is expected to be
366  * smoother in the short term). */
369  /** map frame ID for `pose` */
370  std::string pose_frame_id;
371  TRobotPoseVel();
372  };
373 
374  /** Current robot pose (updated in CAbstractNavigator::navigationStep() ) */
378  /** Latest robot poses (updated in CAbstractNavigator::navigationStep() ) */
380 
381  /** Time logger to collect delay-related stats */
383  true, "CAbstractNavigator::m_timlog_delays"};
384 
385  /** For sending an alarm (error event) when it seems that we are not
386  * approaching toward the target in a while... */
389 
390  public:
391 };
392 
393 bool operator==(
396 } // namespace mrpt::nav
398 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.
mrpt::vision::TStereoCalibParams params
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...
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.
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.
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]
mrpt::system::CTimeLogger m_navProfiler
Publicly available time profiling object.
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.
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: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020