Main MRPT website > C++ reference for MRPT 1.5.3
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 
21 #include <mrpt/nav/link_pragmas.h>
22 
23 namespace mrpt
24 {
25  namespace nav
26  {
27  /** This is the base class for any reactive/planned navigation system. See derived classes.
28  *
29  * How to use:
30  * - A class derived from `CRobot2NavInterface` with callbacks must be defined by the user and provided to the constructor.
31  * - `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.
32  *
33  * It implements the following state machine (see CAbstractNavigator::getCurrentState() ), taking into account the extensions described in CWaypointsNavigator
34  * \dot
35  * digraph CAbstractNavigator_States {
36  * IDLE; NAVIGATING; SUSPENDED; NAV_ERROR;
37  * IDLE -> NAVIGATING [ label="CAbstractNavigator::navigate()"];
38  * IDLE -> NAVIGATING [ label="CWaypointsNavigator::navigateWaypoints()" ];
39  * NAVIGATING -> IDLE [ label="Final target reached" ];
40  * NAVIGATING -> IDLE [ label="CAbstractNavigator::cancel()" ];
41  * NAVIGATING -> NAV_ERROR [ label="Upon sensor errors, timeout,..." ];
42  * NAVIGATING -> SUSPENDED [ label="CAbstractNavigator::suspend()" ];
43  * SUSPENDED -> NAVIGATING [ label="CAbstractNavigator::resume()" ];
44  * NAV_ERROR -> IDLE [ label="CAbstractNavigator::resetNavError()" ];
45  * }
46  * \enddot
47  *
48  * \sa CWaypointsNavigator, CReactiveNavigationSystem, CRobot2NavInterface, all children classes
49  * \ingroup nav_reactive
50  */
51  class NAV_IMPEXP CAbstractNavigator : public mrpt::utils::COutputLogger
52  {
53  public:
54  CAbstractNavigator( CRobot2NavInterface &robot_interface_impl ); //!< ctor
55  virtual ~CAbstractNavigator(); //!< dtor
56 
57  /** Individual target info in CAbstractNavigator::TNavigationParamsBase and derived classes */
59  {
60  mrpt::math::TPose2D target_coords; //!< Coordinates of desired target location. Heading may be ignored by some reactive implementations.
61  std::string target_frame_id; //!< (Default="map") Frame ID in which target is given. Optional, use only for submapping applications.
62  float targetAllowedDistance; //!< (Default=0.5 meters) Allowed distance to target in order to end the navigation.
63  bool targetIsRelative; //!< (Default=false) Whether the \a target coordinates are in global coordinates (false) or are relative to the current robot pose (true).
64  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.
65  bool targetIsIntermediaryWaypoint; // !< (Default=false) If true, event callback `sendWaypointReachedEvent()` will be called instead of `sendNavigationEndEvent()`
66 
67  TargetInfo();
68  std::string getAsText() const; //!< Gets navigation params as a human-readable format
69  bool operator==(const TargetInfo&o) const;
70  bool operator!=(const TargetInfo&o) const { return !(*this==o); }
71  };
72 
73  /** Base for all high-level navigation commands. See derived classes */
75  {
77  virtual std::string getAsText() const =0; //!< Gets navigation params as a human-readable format
78  virtual TNavigationParamsBase* clone() const = 0;
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 TNavigationParamsBase* clone() const MRPT_OVERRIDE { 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
125  };
126 
127  /** Returns the current navigator state. */
128  inline TState getCurrentState() const { return m_navigationState; }
129 
130  /** Sets a user-provided frame transformer object; used only if providing targets in a frame ID
131  * different than the one in which robot odometry is given (both IDs default to `"map"`).
132  * Ownership of the pointee object remains belonging to the user, which is responsible of deleting it
133  * and ensuring its a valid pointer during the lifetime of this navigator object.
134  * \todo [MRPT 2.0: Make this a weak_ptr]
135  */
136  void setFrameTF(mrpt::poses::FrameTransformer<2> *frame_tf);
137 
138  /** Get the current frame tf object (defaults to nullptr) \sa setFrameTF */
139  const mrpt::poses::FrameTransformer<2> * getFrameTF() const { return m_frame_tf; }
140 
141  /** @}*/
142 
144  {
145  double dist_to_target_for_sending_event; //!< Default value=0, means use the "targetAllowedDistance" passed by the user in the navigation request.
146  double alarm_seems_not_approaching_target_timeout; //!< navigator timeout (seconds) [Default=30 sec]
147  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.
148  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
149 
150  virtual void loadFromConfigFile(const mrpt::utils::CConfigFileBase &c, const std::string &s) MRPT_OVERRIDE;
151  virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &c, const std::string &s) const MRPT_OVERRIDE;
153  };
154 
156 
157  /** Gives access to a const-ref to the internal time logger used to estimate delays \sa getTimeLogger() in derived classes */
158  const mrpt::utils::CTimeLogger & getDelaysTimeLogger() const { return m_timlog_delays; }
159 
160  private:
161  TState m_lastNavigationState; //!< Last internal state of navigator:
162  bool m_navigationEndEventSent; //!< Will be false until the navigation end is sent, and it is reset with each new command
164 
165  /** Called before starting a new navigation. Internally, it calls to child-implemented onStartNewNavigation() */
166  void internal_onStartNewNavigation();
167 
168  protected:
170  {
171  typedef void (CRobot2NavInterface::*functor_event_void_t)();
172  functor_event_void_t event_noargs; //!< event w/o arguments
173 
177 
180 
182 
183  TPendingEvent();
184  };
185  /** Events generated during navigationStep(), enqueued to be called
186  * at the end of the method execution to avoid user code to change
187  * the navigator state. */
188  std::vector<TPendingEvent> m_pending_events;
189 
190  void dispatchPendingNavEvents();
191 
192  /** To be implemented in derived classes */
193  virtual void performNavigationStep( )=0;
194 
195  /** Called whenever a new navigation has been started. Can be used to reset state variables, etc. */
196  virtual void onStartNewNavigation() = 0;
197 
198  /** Call to the robot getCurrentPoseAndSpeeds() and updates members m_curPoseVel accordingly.
199  * If an error is returned by the user callback, first, it calls robot.stop() ,then throws an std::runtime_error exception. */
200  void updateCurrentPoseAndSpeeds();
201 
202  /** Factorization of the part inside navigationStep(), for the case of state being NAVIGATING.
203  * Performs house-hold tasks like raising events in case of starting/ending navigation, timeout reaching destination, etc.
204  * `call_virtual_nav_method` can be set to false to avoid calling the virtual method performNavigationStep()
205  */
206  virtual void performNavigationStepNavigating(bool call_virtual_nav_method = true);
207 
208  /** Stops the robot and set navigation state to error */
209  void doEmergencyStop( const std::string &msg );
210 
211  virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd); //!< Default: forward call to m_robot.changeSpeed(). Can be overriden.
212  virtual bool changeSpeedsNOP(); //!< Default: forward call to m_robot.changeSpeedsNOP(). Can be overriden.
213  virtual bool stop(bool isEmergencyStop); //!< Default: forward call to m_robot.stop(). Can be overriden.
214 
215  /** Default implementation: check if target_dist is below the accepted distance.
216  * If true is returned here, the end-of-navigation event will be sent out (only for non-intermediary targets).
217  */
218  virtual bool checkHasReachedTarget(const double targetDist) const;
219 
220  /** Checks whether the robot shape, when placed at the given pose (relative to the current pose),
221  * is colliding with any of the latest known obstacles.
222  * Default implementation: always returns false. */
223  virtual bool checkCollisionWithLatestObstacles(const mrpt::math::TPose2D &relative_robot_pose) const;
224 
225  TState m_navigationState; //!< Current internal state of navigator:
226  TNavigationParams *m_navigationParams; //!< Current navigation parameters
227 
228  CRobot2NavInterface &m_robot; //!< The navigator-robot interface.
229 
230  /** Optional, user-provided frame transformer.
231  * Note: We dont have ownership of the pointee object! */
233 
234  mrpt::synch::CCriticalSectionRecursive m_nav_cs; //!< mutex for all navigation methods
235 
237  {
240  mrpt::math::TPose2D rawOdometry; //!< raw odometry (frame does not match to "pose", but is expected to be smoother in the short term).
242  std::string pose_frame_id; //!< map frame ID for `pose`
243  TRobotPoseVel();
244  };
245 
246  TRobotPoseVel m_curPoseVel; //!< Current robot pose (updated in CAbstractNavigator::navigationStep() )
249  mrpt::poses::CPose2DInterpolator m_latestPoses, m_latestOdomPoses; //!< Latest robot poses (updated in CAbstractNavigator::navigationStep() )
250 
251  mrpt::utils::CTimeLogger m_timlog_delays; //!< Time logger to collect delay-related stats
252 
253  /** For sending an alarm (error event) when it seems that we are not approaching toward the target in a while... */
256 
257  public:
259  };
260 
262 
263  }
264 
265  // Specializations MUST occur at the same namespace:
266  namespace utils
267  {
268  template <>
270  {
272  static void fill(bimap<enum_t, std::string> &m_map)
273  {
278  }
279  };
280  } // End of namespace
281 }
Recursive mutex: allow recursive locks by the owner thread.
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...
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)
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...
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.
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]
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
bool NAV_IMPEXP operator==(const CAbstractNavigator::TNavigationParamsBase &, const CAbstractNavigator::TNavigationParamsBase &)
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.
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)
std::vector< TPendingEvent > m_pending_events
Events generated during navigationStep(), enqueued to be called at the end of the method execution to...



Page generated by Doxygen 1.8.13 for MRPT 1.5.3 at Tue Aug 22 01:03:35 UTC 2017