Main MRPT website > C++ reference for MRPT 1.4.0
CAbstractReactiveNavigationSystem.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-2016, 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#ifndef CAbstractReactiveNavigationSystem_H
10#define CAbstractReactiveNavigationSystem_H
11
12#include <mrpt/poses/CPose2D.h>
14#include <mrpt/obs/obs_frwds.h>
15
17
18namespace mrpt
19{
20 namespace nav
21 {
22 /** The pure virtual class that a user of CAbstractReactiveNavigationSystem-derived classes must implement in order to allow the navigator sense the world and send motion commands to the robot.
23 *
24 * The user must define a new class derived from CReactiveInterfaceImplementation and reimplement
25 * all pure virtual and the desired virtual methods according to the documentation in this class.
26 *
27 * \sa CReactiveNavigationSystem, CAbstractReactiveNavigationSystem
28 * \ingroup nav_reactive
29 */
31 {
32 public:
33 /** Get the current pose and speeds of the robot.
34 * \param curPose Current robot pose.
35 * \param curV Current linear speed, in meters per second.
36 * \param curW Current angular speed, in radians per second.
37 * \return false on any error.
38 */
39 virtual bool getCurrentPoseAndSpeeds( mrpt::poses::CPose2D &curPose, float &curV, float &curW) = 0;
40
41 /** Change the instantaneous speeds of robot.
42 * \param v Linear speed, in meters per second.
43 * \param w Angular speed, in radians per second.
44 * \return false on any error.
45 */
46 virtual bool changeSpeeds( float v, float w ) = 0;
47
48 /** Stop the robot right now.
49 * \return false on any error.
50 */
51 virtual bool stop() {
52 return changeSpeeds(0,0);
53 }
54
55 /** Start the watchdog timer of the robot platform, if any.
56 * \param T_ms Period, in ms.
57 * \return false on any error.
58 */
59 virtual bool startWatchdog(float T_ms) {
61 return true;
62 }
63
64 /** Stop the watchdog timer.
65 * \return false on any error.
66 */
67 virtual bool stopWatchdog() { return true; }
68
69 /** Return the current set of obstacle points, as seen from the local coordinate frame of the robot.
70 * \return false on any error.
71 */
72 virtual bool senseObstacles( mrpt::maps::CSimplePointsMap &obstacles ) = 0;
73
74 virtual void sendNavigationStartEvent () { std::cout << "[sendNavigationStartEvent] Not implemented by the user." << std::endl; }
75 virtual void sendNavigationEndEvent() { std::cout << "[sendNavigationEndEvent] Not implemented by the user." << std::endl; }
76 virtual void sendNavigationEndDueToErrorEvent() { std::cout << "[sendNavigationEndDueToErrorEvent] Not implemented by the user." << std::endl; }
77 virtual void sendWaySeemsBlockedEvent() { std::cout << "[sendWaySeemsBlockedEvent] Not implemented by the user." << std::endl; }
78 virtual void notifyHeadingDirection(const double heading_dir_angle) {
79 MRPT_UNUSED_PARAM(heading_dir_angle);
80 }
81 };
82
83
84
85 /** This is the base class for any reactive navigation system. Here is defined
86 * the interface that users will use with derived classes where algorithms are really implemented.
87 *
88 * Changes history:
89 * - 30/JUN/2004: Creation (JLBC)
90 * - 16/SEP/2004: Totally redesigned.
91 * - 15/SEP/2005: Totally rewritten again, for integration into MRPT Applications Repository.
92 * - 3/NOV/2009: All functors are finally replaced by the new virtual class CReactiveInterfaceImplementation
93 * - 16/DEC/2013: Refactoring of code in 2D & 2.5D navigators.
94 *
95 * How to use:
96 * - A class with callbacks must be defined by the user and provided to the constructor.
97 * - 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.
98 *
99 * \sa CReactiveNavigationSystem, CReactiveInterfaceImplementation
100 * \ingroup nav_reactive
101 */
103 {
104 public:
105 /** The struct for configuring navigation requests. See also: CAbstractPTGBasedReactive::TNavigationParamsPTG */
107 {
108 mrpt::math::TPoint2D target; //!< Coordinates of desired target location.
109 double targetHeading; //!< Target location (heading, in radians).
110
111 float targetAllowedDistance; //!< Allowed distance to target in order to end the navigation.
112 bool targetIsRelative; //!< (Default=false) Whether the \a target coordinates are in global coordinates (false) or are relative to the current robot pose (true).
113
114 TNavigationParams(); //!< Ctor with default values
116 virtual std::string getAsText() const; //!< Gets navigation params as a human-readable format
117 virtual TNavigationParams* clone() const { return new TNavigationParams(*this); }
118 };
119
120
121 /** Constructor */
123
124 /** Destructor */
126
127 /** Cancel current navegacion. */
128 void cancel();
129
130 /** Continues with suspended navigation. \sa suspend */
131 void resume();
132
133 /** This method must be called periodically in order to effectively run the navigation. */
135
136 /** Navigation request. It starts a new navigation.
137 * \param[in] params Pointer to structure with navigation info (its contents will be copied, so the original can be freely destroyed upon return.)
138 */
139 virtual void navigate( const TNavigationParams *params )=0;
140
141 /** Suspend current navegation. \sa resume */
142 virtual void suspend();
143
144 /** The different states for the navigation system. */
146 {
147 IDLE=0,
150 NAV_ERROR
151 };
152
153 /** Returns the current navigator state. */
154 inline TState getCurrentState() const { return m_navigationState; }
155
156 private:
157 TState m_lastNavigationState; //!< Last internal state of navigator:
158
159 protected:
160 /** To be implemented in derived classes */
161 virtual void performNavigationStep( )=0;
162
163 TState m_navigationState; //!< Current internal state of navigator:
164 TNavigationParams *m_navigationParams; //!< Current navigation parameters
165
166
167 CReactiveInterfaceImplementation &m_robot; //!< The navigator-robot interface.
168
169 public:
171 };
172 }
173}
174
175
176#endif
177
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
This is the base class for any reactive navigation system.
TNavigationParams * m_navigationParams
Current navigation parameters.
virtual void suspend()
Suspend current navegation.
virtual ~CAbstractReactiveNavigationSystem()
Destructor.
TState getCurrentState() const
Returns the current navigator state.
CAbstractReactiveNavigationSystem(CReactiveInterfaceImplementation &react_iterf_impl)
Constructor.
void cancel()
Cancel current navegacion.
void resume()
Continues with suspended navigation.
TState m_lastNavigationState
Last internal state of navigator:
virtual void performNavigationStep()=0
To be implemented in derived classes.
virtual void navigate(const TNavigationParams *params)=0
Navigation request.
void navigationStep()
This method must be called periodically in order to effectively run the navigation.
TState m_navigationState
Current internal state of navigator:
TState
The different states for the navigation system.
CReactiveInterfaceImplementation & m_robot
The navigator-robot interface.
The pure virtual class that a user of CAbstractReactiveNavigationSystem-derived classes must implemen...
virtual bool changeSpeeds(float v, float w)=0
Change the instantaneous speeds of robot.
virtual bool getCurrentPoseAndSpeeds(mrpt::poses::CPose2D &curPose, float &curV, float &curW)=0
Get the current pose and speeds of the robot.
virtual bool startWatchdog(float T_ms)
Start the watchdog timer of the robot platform, if any.
virtual bool senseObstacles(mrpt::maps::CSimplePointsMap &obstacles)=0
Return the current set of obstacle points, as seen from the local coordinate frame of the robot.
virtual void notifyHeadingDirection(const double heading_dir_angle)
A class used to store a 2D pose.
Definition CPose2D.h:37
This base class provides a common printf-like method to send debug information to std::cout,...
#define MRPT_MAKE_ALIGNED_OPERATOR_NEW
Definition memory.h:112
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Lightweight 2D point.
virtual std::string getAsText() const
Gets navigation params as a human-readable format.
float targetAllowedDistance
Allowed distance to target in order to end the navigation.
mrpt::math::TPoint2D target
Coordinates of desired target location.
bool targetIsRelative
(Default=false) Whether the target coordinates are in global coordinates (false) or are relative to t...



Page generated by Doxygen 1.9.7 for MRPT 1.4.0 SVN: at Tue Jun 13 14:27:49 UTC 2023