Main MRPT website > C++ reference
MRPT logo

CAbstractReactiveNavigationSystem.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef CAbstractReactiveNavigationSystem_H
00029 #define CAbstractReactiveNavigationSystem_H
00030 
00031 #include <mrpt/maps.h>
00032 #include <mrpt/poses.h>
00033 
00034 #include <mrpt/reactivenav/link_pragmas.h>
00035 
00036 
00037 #include <cstdarg>
00038 
00039 namespace mrpt
00040 {
00041   namespace reactivenav
00042   {
00043         using namespace mrpt;
00044         using namespace mrpt::slam;
00045         using namespace mrpt::poses;
00046 
00047         /** 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.
00048           *
00049           *  The user must define a new class derived from CReactiveInterfaceImplementation and reimplement
00050           *   all pure virtual and the desired virtual methods according to the documentation in this class.
00051           *
00052           * \sa CReactiveNavigationSystem, CAbstractReactiveNavigationSystem
00053           */
00054         class REACTIVENAV_IMPEXP CReactiveInterfaceImplementation
00055         {
00056         public:
00057                 /** Get the current pose and speeds of the robot.
00058                  *   \param curPose Current robot pose.
00059                  *   \param curV Current linear speed, in meters per second.
00060                  *       \param curW Current angular speed, in radians per second.
00061                  * \return false on any error.
00062                  */
00063                 virtual bool getCurrentPoseAndSpeeds( mrpt::poses::CPose2D &curPose, float &curV, float &curW) = 0;
00064 
00065                 /** Change the instantaneous speeds of robot.
00066                  *   \param v Linear speed, in meters per second.
00067                  *       \param w Angular speed, in radians per second.
00068                  * \return false on any error.
00069                  */
00070                 virtual bool changeSpeeds( float v, float w ) = 0;
00071 
00072                 /** Stop the robot right now.
00073                  * \return false on any error.
00074                  */
00075                 virtual bool stop() {
00076                         return changeSpeeds(0,0);
00077                 }
00078 
00079                 /** Start the watchdog timer of the robot platform, if any.
00080                  * \param T_ms Period, in ms.
00081                  * \return false on any error.
00082                  */
00083                 virtual bool startWatchdog(float T_ms) { return true; }
00084 
00085                 /** Stop the watchdog timer.
00086                  * \return false on any error.
00087                  */
00088                 virtual bool stopWatchdog() { return true; }
00089 
00090                 /** Return the current set of obstacle points.
00091                   * \return false on any error.
00092                   */
00093                 virtual bool senseObstacles( mrpt::slam::CSimplePointsMap               &obstacles ) = 0;
00094 
00095                 virtual void sendNavigationStartEvent () { std::cout << "[sendNavigationStartEvent] Not implemented by the user." << std::endl; }
00096 
00097                 virtual void sendNavigationEndEvent() { std::cout << "[sendNavigationEndEvent] Not implemented by the user." << std::endl; }
00098 
00099                 virtual void sendNavigationEndDueToErrorEvent() { std::cout << "[sendNavigationEndDueToErrorEvent] Not implemented by the user." << std::endl; }
00100 
00101                 virtual void sendWaySeemsBlockedEvent() { std::cout << "[sendWaySeemsBlockedEvent] Not implemented by the user." << std::endl; }
00102 
00103                 virtual void notifyHeadingDirection(const double heading_dir_angle) { }
00104 
00105         };
00106 
00107 
00108 
00109         /** This is the base class for any reactive navigation system. Here is defined
00110          *   the interface that users will use with derived classes where algorithms are really implemented.
00111          *
00112          * Changes history:
00113          *              - 30/JUN/2004: Creation (JLBC)
00114      *          - 16/SEP/2004: Totally redesigned.
00115          *              - 15/SEP/2005: Totally rewritten again, for integration into MRPT Applications Repository.
00116          *              -  3/NOV/2009: All functors are finally replaced by the new virtual class CReactiveInterfaceImplementation
00117          *
00118          * \sa CReactiveNavigationSystem, CReactiveInterfaceImplementation
00119          */
00120         class REACTIVENAV_IMPEXP CAbstractReactiveNavigationSystem : public mrpt::utils::CDebugOutputCapable
00121         {
00122         public:
00123                 struct TNavigationParams;
00124 
00125                 /** Constructor
00126                   */
00127                 CAbstractReactiveNavigationSystem( CReactiveInterfaceImplementation &react_iterf_impl );
00128 
00129         /** Destructor
00130           */
00131         virtual ~CAbstractReactiveNavigationSystem()
00132                 {
00133                 }
00134 
00135                 /** Cancel current navegacion.
00136                  */
00137                 void cancel();
00138 
00139                 /** Continues with suspended navigation.
00140                  * \sa suspend
00141                  */
00142                 void resume();
00143 
00144                 /** Evaluates the practicability of a navigation for given parameters:
00145                  * \returns An estimation in the range [0,1], for 0 being imposible and 1 being easy.
00146                  */
00147                 virtual float  evaluate( TNavigationParams *params )=0;
00148 
00149                 /** This method must be called periodically in order to effectively run the navigation.
00150                  */
00151                 void navigationStep();
00152 
00153                 /** Navigation request. It starts a new navigation.
00154                  */
00155                 virtual void  navigate( TNavigationParams *params )=0;
00156 
00157                 /** Changes the parameters for current navigation
00158                  */
00159                 virtual void  setParams( TNavigationParams *params)=0;
00160 
00161                 /** Suspend current navegation
00162                  * \sa resume
00163                  */
00164                 virtual void  suspend();
00165 
00166                 /** The struct for configuring the navigation request.
00167                  */
00168                 struct TNavigationParams
00169                 {
00170                         /** Coordinates of desired target location.
00171                          */
00172                         mrpt::poses::TPoint2D           target;
00173 
00174                         /** The allowed distance from target in order to end the navigation.
00175                          */
00176                         float           targetAllowedDistance;
00177 
00178                         /** The allowed distance from target in order to end the navigation.
00179                          */
00180                         bool            targetIsRelative;
00181                 };
00182 
00183                 /** The different states for the navigation system.
00184                  */
00185                 enum TState
00186                 {
00187                         IDLE=0,
00188                         NAVIGATING,
00189                         SUSPENDED,
00190                         NAV_ERROR
00191                 };
00192 
00193                 /** Returns the current navigator state.
00194                  */
00195                 TState getCurrentState() const { return m_navigationState; }
00196 
00197         private:
00198                 /** Last internal state of navigator:
00199                  */
00200                 TState          m_lastNavigationState;
00201 
00202         protected:
00203                 /** To be implemented in derived classes
00204                   */
00205                 virtual void  performNavigationStep( )=0;
00206 
00207                 /** Current internal state of navigator:
00208                  */
00209                 TState          m_navigationState;
00210 
00211                 /** Current navigation parameters:
00212                  */
00213                 TNavigationParams       m_navigationParams;
00214 
00215 
00216                 CReactiveInterfaceImplementation   &m_robot; //!< The navigator-robot interface.
00217 
00218         };
00219   }
00220 }
00221 
00222 
00223 #endif
00224 



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011