Main MRPT website > C++ reference for MRPT 1.5.7
CAbstractNavigator.h
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 #pragma once
10 
13 #include <mrpt/utils/CTimeLogger.h>
14 #include <mrpt/utils/TEnumType.h>
19 #include <mrpt/obs/obs_frwds.h>
20 #include <list>
21 
22 #include <mrpt/nav/link_pragmas.h>
23 
24 namespace mrpt
25 {
26  namespace nav
27  {
28  /** This is the base class for any reactive/planned navigation system. See derived classes.
29  *
30  * How to use:
31  * - A class derived from `CRobot2NavInterface` with callbacks must be defined by the user and provided to the constructor.
32  * - `navigationStep()` must be called periodically in order to effectively run the navigation. This method will internally call the callbacks to gather sensor data and robot positioning data.
33  *
34  * It implements the following state machine (see CAbstractNavigator::getCurrentState() ), taking into account the extensions described in CWaypointsNavigator
35  * \dot
36  * digraph CAbstractNavigator_States {
37  * IDLE; NAVIGATING; SUSPENDED; NAV_ERROR;
38  * IDLE -> NAVIGATING [ label="CAbstractNavigator::navigate()"];
39  * IDLE -> NAVIGATING [ label="CWaypointsNavigator::navigateWaypoints()" ];
40  * NAVIGATING -> IDLE [ label="Final target reached" ];
41  * NAVIGATING -> IDLE [ label="CAbstractNavigator::cancel()" ];
42  * NAVIGATING -> NAV_ERROR [ label="Upon sensor errors, timeout,..." ];
43  * NAVIGATING -> SUSPENDED [ label="CAbstractNavigator::suspend()" ];
44  * SUSPENDED -> NAVIGATING [ label="CAbstractNavigator::resume()" ];
45  * NAV_ERROR -> IDLE [ label="CAbstractNavigator::resetNavError()" ];
46  * }
47  * \enddot
48  *
49  * \sa CWaypointsNavigator, CReactiveNavigationSystem, CRobot2NavInterface, all children classes
50  * \ingroup nav_reactive
51  */
52  class NAV_IMPEXP CAbstractNavigator : public mrpt::utils::COutputLogger
53  {
54  public:
55  CAbstractNavigator( CRobot2NavInterface &robot_interface_impl ); //!< ctor
56  virtual ~CAbstractNavigator(); //!< dtor
57 
58  /** Individual target info in CAbstractNavigator::TNavigationParamsBase and derived classes */
60  {
61  mrpt::math::TPose2D target_coords; //!< Coordinates of desired target location. Heading may be ignored by some reactive implementations.
62  std::string target_frame_id; //!< (Default="map") Frame ID in which target is given. Optional, use only for submapping applications.
63  float targetAllowedDistance; //!< (Default=0.5 meters) Allowed distance to target in order to end the navigation.
64  bool targetIsRelative; //!< (Default=false) Whether the \a target coordinates are in global coordinates (false) or are relative to the current robot pose (true).
65  double targetDesiredRelSpeed; //!< (Default=.05) Desired relative speed (wrt maximum speed), in range [0,1], of the vehicle at target. Holonomic nav methods will perform "slow down" approaching target only if this is "==.0". Intermediary values will be honored only by the higher-level navigator, based on straight-line Euclidean distances.
66  bool targetIsIntermediaryWaypoint; // !< (Default=false) If true, event callback `sendWaypointReachedEvent()` will be called instead of `sendNavigationEndEvent()`
67 
68  TargetInfo();
69  std::string getAsText() const; //!< Gets navigation params as a human-readable format
70  bool operator==(const TargetInfo&o) const;
71  bool operator!=(const TargetInfo&o) const { return !(*this==o); }
72  };
73 
74  /** Base for all high-level navigation commands. See derived classes */
76  {
78  virtual std::string getAsText() const =0; //!< Gets navigation params as a human-readable format
79  protected:
81  virtual bool isEqual(const TNavigationParamsBase& o) const =0;
82  };
83 
84  /** The struct for configuring navigation requests. Used in CAbstractPTGBasedReactive::navigate() */
86  {
87  TargetInfo target; //!< Navigation target
88 
89  virtual std::string getAsText() const MRPT_OVERRIDE; //!< Gets navigation params as a human-readable format
90  virtual TNavigationParams* clone() const { return new TNavigationParams(*this); }
91  protected:
92  virtual bool isEqual(const TNavigationParamsBase& o) const MRPT_OVERRIDE;
93  };
94 
95  /** \name Navigation control API
96  * @{ */
97 
98  /** Loads all params from a file. To be called before initialize().
99  * Each derived class *MUST* load its own parameters, and then call *ITS PARENT'S* overriden method to ensure all params are loaded. */
100  virtual void loadConfigFile(const mrpt::utils::CConfigFileBase &c);
101  /** Saves all current options to a config file.
102  * Each derived class *MUST* save its own parameters, and then call *ITS PARENT'S* overriden method to ensure all params are saved. */
103  virtual void saveConfigFile(mrpt::utils::CConfigFileBase &c) const;
104 
105  virtual void initialize() = 0; //!< Must be called before any other navigation command
106  virtual void navigationStep(); //!< This method must be called periodically in order to effectively run the navigation
107 
108  /** Navigation request to a single target location. It starts a new navigation.
109  * \param[in] params Pointer to structure with navigation info (its contents will be copied, so the original can be freely destroyed upon return if it was dynamically allocated.)
110  * \note A pointer is used so the passed object can be polymorphic with derived types.
111  */
112  virtual void navigate( const TNavigationParams *params );
113 
114  virtual void cancel(); //!< Cancel current navegation.
115  virtual void resume(); //!< Continues with suspended navigation. \sa suspend
116  virtual void suspend(); //!< Suspend current navegation. \sa resume
117  virtual void resetNavError(); //!< Resets a `NAV_ERROR` state back to `IDLE`
118 
119  /** The different states for the navigation system. */
120  enum TState {
121  IDLE=0,
124  NAV_ERROR //!< In this case, use getErrorReason()
125  };
126 
127  /** Returns the current navigator state. */
128  inline TState getCurrentState() const { return m_navigationState; }
129 
130  /** Explains the reason for the navigation error. */
131  enum TErrorCode {
132  ERR_NONE=0,
135  ERR_OTHER
136  };
138  {
139  TErrorReason() : error_code(ERR_NONE) {}
140 
142  /** Human friendly description of the error */
144  };
145 
146  /** In case of state=NAV_ERROR, this returns the reason for the error.
147  * Error state is reseted everytime a new navigation starts with
148  * a call to navigate(), or when resetNavError() is called.
149  */
150  inline const TErrorReason & getErrorReason() const { return m_navErrorReason; }
151 
152  /** Sets a user-provided frame transformer object; used only if providing targets in a frame ID
153  * different than the one in which robot odometry is given (both IDs default to `"map"`).
154  * Ownership of the pointee object remains belonging to the user, which is responsible of deleting it
155  * and ensuring its a valid pointer during the lifetime of this navigator object.
156  * \todo [MRPT 2.0: Make this a weak_ptr]
157  */
158  void setFrameTF(mrpt::poses::FrameTransformer<2> *frame_tf);
159 
160  /** Get the current frame tf object (defaults to nullptr) \sa setFrameTF */
161  const mrpt::poses::FrameTransformer<2> * getFrameTF() const { return m_frame_tf; }
162 
163  /** By default, error exceptions on navigationStep() will dump an error message
164  * to the output logger interface. If rethrow is enabled (default=false), the
165  * error message will be reported as well, but exceptions will be re-thrown.
166  */
167  void enableRethrowNavExceptions(const bool enable) {
168  m_rethrow_exceptions = enable;
169  }
171  return m_rethrow_exceptions;
172  }
173  /** @}*/
174 
176  {
177  double dist_to_target_for_sending_event; //!< Default value=0, means use the "targetAllowedDistance" passed by the user in the navigation request.
178  double alarm_seems_not_approaching_target_timeout; //!< navigator timeout (seconds) [Default=30 sec]
179  double dist_check_target_is_blocked; //!< (Default value=0.6) When closer than this distance, check if the target is blocked to abort navigation with an error.
180  int hysteresis_check_target_is_blocked; // (Default=3) How many steps should the condition for dist_check_target_is_blocked be fulfilled to raise an event
181 
182  virtual void loadFromConfigFile(const mrpt::utils::CConfigFileBase &c, const std::string &s) MRPT_OVERRIDE;
183  virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &c, const std::string &s) const MRPT_OVERRIDE;
185  };
186 
188 
189  /** Gives access to a const-ref to the internal time logger used to estimate delays \sa getTimeLogger() in derived classes */
190  const mrpt::utils::CTimeLogger & getDelaysTimeLogger() const { return m_timlog_delays; }
191 
192  private:
193  TState m_lastNavigationState; //!< Last internal state of navigator:
195  bool m_navigationEndEventSent; //!< Will be false until the navigation end is sent, and it is reset with each new command
198 
199  /** Called before starting a new navigation. Internally, it calls to child-implemented onStartNewNavigation() */
200  void internal_onStartNewNavigation();
201 
202  protected:
204  {
205  typedef void (CRobot2NavInterface::*functor_event_void_t)();
206  functor_event_void_t event_noargs; //!< event w/o arguments
207 
211 
214 
216 
217  TPendingEvent();
218  };
219  /** Events generated during navigationStep(), enqueued to be called
220  * at the end of the method execution to avoid user code to change
221  * the navigator state. */
222  std::list<TPendingEvent> m_pending_events;
223 
224  void dispatchPendingNavEvents();
225 
226  /** To be implemented in derived classes */
227  virtual void performNavigationStep( )=0;
228 
229  /** Called whenever a new navigation has been started. Can be used to reset state variables, etc. */
230  virtual void onStartNewNavigation() = 0;
231 
232  /** Called after each call to CAbstractNavigator::navigate() */
233  virtual void onNavigateCommandReceived();
234 
235  /** Does the job of navigate(), except the call to onNavigateCommandReceived() */
236  virtual void processNavigateCommand(const TNavigationParams *params);
237 
238  /** Call to the robot getCurrentPoseAndSpeeds() and updates members m_curPoseVel accordingly.
239  * If an error is returned by the user callback, first, it calls robot.stop() ,then throws an std::runtime_error exception. */
240  virtual void updateCurrentPoseAndSpeeds();
241 
242  /** Factorization of the part inside navigationStep(), for the case of state being NAVIGATING.
243  * Performs house-hold tasks like raising events in case of starting/ending navigation, timeout reaching destination, etc.
244  * `call_virtual_nav_method` can be set to false to avoid calling the virtual method performNavigationStep()
245  */
246  virtual void performNavigationStepNavigating(bool call_virtual_nav_method = true);
247 
248  /** Stops the robot and set navigation state to error */
249  virtual void doEmergencyStop( const std::string &msg );
250 
251  virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd); //!< Default: forward call to m_robot.changeSpeed(). Can be overriden.
252  virtual bool changeSpeedsNOP(); //!< Default: forward call to m_robot.changeSpeedsNOP(). Can be overriden.
253  virtual bool stop(bool isEmergencyStop); //!< Default: forward call to m_robot.stop(). Can be overriden.
254 
255  /** Default implementation: check if target_dist is below the accepted distance.
256  * If true is returned here, the end-of-navigation event will be sent out (only for non-intermediary targets).
257  */
258  virtual bool checkHasReachedTarget(const double targetDist) const;
259 
260  /** Checks whether the robot shape, when placed at the given pose (relative to the current pose),
261  * is colliding with any of the latest known obstacles.
262  * Default implementation: always returns false. */
263  virtual bool checkCollisionWithLatestObstacles(const mrpt::math::TPose2D &relative_robot_pose) const;
264 
265  TState m_navigationState; //!< Current internal state of navigator:
266  TNavigationParams *m_navigationParams; //!< Current navigation parameters
267 
268  CRobot2NavInterface &m_robot; //!< The navigator-robot interface.
269 
270  /** Optional, user-provided frame transformer.
271  * Note: We dont have ownership of the pointee object! */
273 
274  mrpt::synch::CCriticalSectionRecursive m_nav_cs; //!< mutex for all navigation methods
275 
277  {
280  mrpt::math::TPose2D rawOdometry; //!< raw odometry (frame does not match to "pose", but is expected to be smoother in the short term).
282  std::string pose_frame_id; //!< map frame ID for `pose`
283  TRobotPoseVel();
284  };
285 
286  TRobotPoseVel m_curPoseVel; //!< Current robot pose (updated in CAbstractNavigator::navigationStep() )
289  mrpt::poses::CPose2DInterpolator m_latestPoses, m_latestOdomPoses; //!< Latest robot poses (updated in CAbstractNavigator::navigationStep() )
290 
291  mrpt::utils::CTimeLogger m_timlog_delays; //!< Time logger to collect delay-related stats
292 
293  /** For sending an alarm (error event) when it seems that we are not approaching toward the target in a while... */
296 
297  public:
299  };
300 
302 
303  }
304 
305  // Specializations MUST occur at the same namespace:
306  namespace utils
307  {
308  template <>
310  {
312  static void fill(bimap<enum_t, std::string> &m_map)
313  {
318  }
319  };
320  } // End of namespace
321 }
Recursive mutex: allow recursive locks by the owner thread.
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...
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
const mrpt::utils::CTimeLogger & getDelaysTimeLogger() const
Gives access to a const-ref to the internal time logger used to estimate delays.
TRobotPoseVel m_curPoseVel
Current robot pose (updated in CAbstractNavigator::navigationStep() )
This class stores a time-stamped trajectory in SE(2) (mrpt::math::TPose2D poses). ...
mrpt::poses::FrameTransformer< 2 > * m_frame_tf
Optional, user-provided frame transformer.
#define MRPT_MAKE_ALIGNED_OPERATOR_NEW
Definition: memory.h:112
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
TNavigationParams * m_navigationParams
Current navigation parameters.
Base for all high-level navigation commands.
float targetAllowedDistance
(Default=0.5 meters) Allowed distance to target in order to end the navigation.
Only specializations of this class are defined for each enum type of interest.
Definition: TEnumType.h:23
functor_event_void_t event_noargs
event w/o arguments
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:3602
std::string pose_frame_id
map frame ID for pose
mrpt::utils::CTimeLogger m_timlog_delays
Time logger to collect delay-related stats.
This class allows loading and storing values and vectors of different types from a configuration text...
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)
bool NAV_IMPEXP operator==(const CAbstractNavigator::TNavigationParamsBase &, const CAbstractNavigator::TNavigationParamsBase &)
The struct for configuring navigation requests.
TState
The different states for the navigation system.
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.
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
void enableRethrowNavExceptions(const bool enable)
By default, error exceptions on navigationStep() will dump an error message to the output logger inte...
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
Definition: bimap.h:28
Individual target info in CAbstractNavigator::TNavigationParamsBase and derived classes.
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.
TState m_lastNavigationState
Last internal state of navigator:
double alarm_seems_not_approaching_target_timeout
navigator timeout (seconds) [Default=30 sec]
std::list< TPendingEvent > m_pending_events
Events generated during navigationStep(), enqueued to be called at the end of the method execution to...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
TAbstractNavigatorParams params_abstract_navigator
CRobot2NavInterface & m_robot
The navigator-robot interface.
Lightweight 2D pose.
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
Definition: CTimeLogger.h:41
In this case, use getErrorReason()
const TErrorReason & getErrorReason() const
In case of state=NAV_ERROR, this returns the reason for the error.
_u16 error_code
Definition: rplidar_cmd.h:22
std::string error_msg
Human friendly description of the error.
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:69
mrpt::synch::CCriticalSectionRecursive m_nav_cs
mutex for all navigation methods
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
This is the base class for any reactive/planned navigation system.
GLenum const GLfloat * params
Definition: glext.h:3514
mrpt::poses::CPose2DInterpolator m_latestPoses
bool operator!=(const TargetInfo &o) const
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
mrpt::math::TPose2D rawOdometry
raw odometry (frame does not match to "pose", but is expected to be smoother in the short term)...
const mrpt::poses::FrameTransformer< 2 > * getFrameTF() const
Get the current frame tf object (defaults to nullptr)



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