Main MRPT website > C++ reference for MRPT 1.5.6
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  protected:
80  virtual bool isEqual(const TNavigationParamsBase& o) const =0;
81  };
82 
83  /** The struct for configuring navigation requests. Used in CAbstractPTGBasedReactive::navigate() */
85  {
86  TargetInfo target; //!< Navigation target
87 
88  virtual std::string getAsText() const MRPT_OVERRIDE; //!< Gets navigation params as a human-readable format
89  virtual TNavigationParams* clone() const { return new TNavigationParams(*this); }
90  protected:
91  virtual bool isEqual(const TNavigationParamsBase& o) const MRPT_OVERRIDE;
92  };
93 
94  /** \name Navigation control API
95  * @{ */
96 
97  /** Loads all params from a file. To be called before initialize().
98  * Each derived class *MUST* load its own parameters, and then call *ITS PARENT'S* overriden method to ensure all params are loaded. */
99  virtual void loadConfigFile(const mrpt::utils::CConfigFileBase &c);
100  /** Saves all current options to a config file.
101  * Each derived class *MUST* save its own parameters, and then call *ITS PARENT'S* overriden method to ensure all params are saved. */
102  virtual void saveConfigFile(mrpt::utils::CConfigFileBase &c) const;
103 
104  virtual void initialize() = 0; //!< Must be called before any other navigation command
105  virtual void navigationStep(); //!< This method must be called periodically in order to effectively run the navigation
106 
107  /** Navigation request to a single target location. It starts a new navigation.
108  * \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.)
109  * \note A pointer is used so the passed object can be polymorphic with derived types.
110  */
111  virtual void navigate( const TNavigationParams *params );
112 
113  virtual void cancel(); //!< Cancel current navegation.
114  virtual void resume(); //!< Continues with suspended navigation. \sa suspend
115  virtual void suspend(); //!< Suspend current navegation. \sa resume
116  virtual void resetNavError(); //!< Resets a `NAV_ERROR` state back to `IDLE`
117 
118  /** The different states for the navigation system. */
119  enum TState {
120  IDLE=0,
123  NAV_ERROR
124  };
125 
126  /** Returns the current navigator state. */
127  inline TState getCurrentState() const { return m_navigationState; }
128 
129  /** Sets a user-provided frame transformer object; used only if providing targets in a frame ID
130  * different than the one in which robot odometry is given (both IDs default to `"map"`).
131  * Ownership of the pointee object remains belonging to the user, which is responsible of deleting it
132  * and ensuring its a valid pointer during the lifetime of this navigator object.
133  * \todo [MRPT 2.0: Make this a weak_ptr]
134  */
135  void setFrameTF(mrpt::poses::FrameTransformer<2> *frame_tf);
136 
137  /** Get the current frame tf object (defaults to nullptr) \sa setFrameTF */
138  const mrpt::poses::FrameTransformer<2> * getFrameTF() const { return m_frame_tf; }
139 
140  /** @}*/
141 
143  {
144  double dist_to_target_for_sending_event; //!< Default value=0, means use the "targetAllowedDistance" passed by the user in the navigation request.
145  double alarm_seems_not_approaching_target_timeout; //!< navigator timeout (seconds) [Default=30 sec]
146  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.
147  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
148 
149  virtual void loadFromConfigFile(const mrpt::utils::CConfigFileBase &c, const std::string &s) MRPT_OVERRIDE;
150  virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &c, const std::string &s) const MRPT_OVERRIDE;
152  };
153 
155 
156  /** Gives access to a const-ref to the internal time logger used to estimate delays \sa getTimeLogger() in derived classes */
157  const mrpt::utils::CTimeLogger & getDelaysTimeLogger() const { return m_timlog_delays; }
158 
159  private:
160  TState m_lastNavigationState; //!< Last internal state of navigator:
161  bool m_navigationEndEventSent; //!< Will be false until the navigation end is sent, and it is reset with each new command
163 
164  /** Called before starting a new navigation. Internally, it calls to child-implemented onStartNewNavigation() */
165  void internal_onStartNewNavigation();
166 
167  protected:
169  {
170  typedef void (CRobot2NavInterface::*functor_event_void_t)();
171  functor_event_void_t event_noargs; //!< event w/o arguments
172 
176 
179 
181 
182  TPendingEvent();
183  };
184  /** Events generated during navigationStep(), enqueued to be called
185  * at the end of the method execution to avoid user code to change
186  * the navigator state. */
187  std::vector<TPendingEvent> m_pending_events;
188 
189  void dispatchPendingNavEvents();
190 
191  /** To be implemented in derived classes */
192  virtual void performNavigationStep( )=0;
193 
194  /** Called whenever a new navigation has been started. Can be used to reset state variables, etc. */
195  virtual void onStartNewNavigation() = 0;
196 
197  /** Called after each call to CAbstractNavigator::navigate() */
198  virtual void onNavigateCommandReceived();
199 
200  /** Does the job of navigate(), except the call to onNavigateCommandReceived() */
201  void processNavigateCommand(const TNavigationParams *params);
202 
203  /** Call to the robot getCurrentPoseAndSpeeds() and updates members m_curPoseVel accordingly.
204  * If an error is returned by the user callback, first, it calls robot.stop() ,then throws an std::runtime_error exception. */
205  void updateCurrentPoseAndSpeeds();
206 
207  /** Factorization of the part inside navigationStep(), for the case of state being NAVIGATING.
208  * Performs house-hold tasks like raising events in case of starting/ending navigation, timeout reaching destination, etc.
209  * `call_virtual_nav_method` can be set to false to avoid calling the virtual method performNavigationStep()
210  */
211  virtual void performNavigationStepNavigating(bool call_virtual_nav_method = true);
212 
213  /** Stops the robot and set navigation state to error */
214  void doEmergencyStop( const std::string &msg );
215 
216  virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd); //!< Default: forward call to m_robot.changeSpeed(). Can be overriden.
217  virtual bool changeSpeedsNOP(); //!< Default: forward call to m_robot.changeSpeedsNOP(). Can be overriden.
218  virtual bool stop(bool isEmergencyStop); //!< Default: forward call to m_robot.stop(). Can be overriden.
219 
220  /** Default implementation: check if target_dist is below the accepted distance.
221  * If true is returned here, the end-of-navigation event will be sent out (only for non-intermediary targets).
222  */
223  virtual bool checkHasReachedTarget(const double targetDist) const;
224 
225  /** Checks whether the robot shape, when placed at the given pose (relative to the current pose),
226  * is colliding with any of the latest known obstacles.
227  * Default implementation: always returns false. */
228  virtual bool checkCollisionWithLatestObstacles(const mrpt::math::TPose2D &relative_robot_pose) const;
229 
230  TState m_navigationState; //!< Current internal state of navigator:
231  TNavigationParams *m_navigationParams; //!< Current navigation parameters
232 
233  CRobot2NavInterface &m_robot; //!< The navigator-robot interface.
234 
235  /** Optional, user-provided frame transformer.
236  * Note: We dont have ownership of the pointee object! */
238 
239  mrpt::synch::CCriticalSectionRecursive m_nav_cs; //!< mutex for all navigation methods
240 
242  {
245  mrpt::math::TPose2D rawOdometry; //!< raw odometry (frame does not match to "pose", but is expected to be smoother in the short term).
247  std::string pose_frame_id; //!< map frame ID for `pose`
248  TRobotPoseVel();
249  };
250 
251  TRobotPoseVel m_curPoseVel; //!< Current robot pose (updated in CAbstractNavigator::navigationStep() )
254  mrpt::poses::CPose2DInterpolator m_latestPoses, m_latestOdomPoses; //!< Latest robot poses (updated in CAbstractNavigator::navigationStep() )
255 
256  mrpt::utils::CTimeLogger m_timlog_delays; //!< Time logger to collect delay-related stats
257 
258  /** For sending an alarm (error event) when it seems that we are not approaching toward the target in a while... */
261 
262  public:
264  };
265 
267 
268  }
269 
270  // Specializations MUST occur at the same namespace:
271  namespace utils
272  {
273  template <>
275  {
277  static void fill(bimap<enum_t, std::string> &m_map)
278  {
283  }
284  };
285  } // End of namespace
286 }
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...
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
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]
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
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)
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.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019