Main MRPT website > C++ reference
MRPT logo

CReactiveNavigationSystem.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 CReactiveNavigationSystem_H
00029 #define CReactiveNavigationSystem_H
00030 
00031 #include <mrpt/maps.h>
00032 #include <mrpt/poses.h>
00033 #include <mrpt/math.h>
00034 #include <mrpt/synch.h>
00035 #include <mrpt/reactivenav/link_pragmas.h>
00036 
00037 #include "CAbstractReactiveNavigationSystem.h"
00038 #include "CParameterizedTrajectoryGenerator.h"
00039 #include "CLogFileRecord.h"
00040 #include "CAbstractHolonomicReactiveMethod.h"
00041 #include "CHolonomicVFF.h"
00042 #include "CHolonomicND.h"
00043 
00044 namespace mrpt
00045 {
00046         /** This namespace contains classes for building a TP-Space Reactive Navigation System.
00047         */
00048   namespace reactivenav
00049   {
00050           /**  The implemented reactive navigation methods
00051             */
00052           enum THolonomicMethod
00053           {
00054                   hmVIRTUAL_FORCE_FIELDS = 0,
00055                   hmSEARCH_FOR_BEST_GAP = 1
00056           };
00057 
00058           /** Implements a reactive navigation system based on TP-Space, with an arbitrary holonomic
00059                         reactive method running on it, and any desired number of PTG for transforming the navigation space.
00060                         Both, the holonomic method and the PTGs can be customized by the apropriate user derived classes.
00061                         For running it, the method NavigateStep must be invoked periodically.
00062 
00063                                 - 17/JUN/2004: First design.
00064                                 - 16/SEP/2004: Totally redesigned, according to document "MultiParametric Based Space
00065                      Transformation for Reactive Navigation"
00066                                 - 29/SEP/2005: Totally rewritten again, for integration into MRPT library and according to the ICRA paper.
00067                                 - 17/OCT/2007: Whole code updated to accomodate to MRPT 0.5 and make it portable to Linux.
00068 
00069                         \sa CAbstractReactiveNavigationSystem, CParameterizedTrajectoryGenerator, CAbstractHolonomicReactiveMethod
00070                 */
00071                 class REACTIVENAV_IMPEXP  CReactiveNavigationSystem : public CAbstractReactiveNavigationSystem
00072                 {
00073                 public:
00074                         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00075                 public:
00076                         /** Constructor
00077                          *  \param configINIFile The file to load the configuration from. See loadConfigFile
00078                          *  \param robotConfigFile The file to load the robot specific configuration from.
00079                          *      \param rmc A set of wrappers that must be filled in.
00080                          *      \param sensors A set of wrappers that must be filled in.
00081                          *      \param dbg A set of wrappers that must be filled in.
00082                          *      \param evnts A set of wrappers that must be filled in.
00083                          *  \param enableConsoleOutput Set to false if console output is not desired.
00084                          *  \param enableLogFile Set to true to enable logging to file.
00085                          */
00086                         CReactiveNavigationSystem(
00087                                 CReactiveInterfaceImplementation   &react_iterf_impl,
00088                                 bool                                    enableConsoleOutput = true,
00089                                 bool                                    enableLogFile = false);
00090 
00091                         /** Destructor
00092                          */
00093                         virtual ~CReactiveNavigationSystem();
00094 
00095                         /** Reload the configuration from a file
00096                          */
00097                         void loadConfigFile(const mrpt::utils::CConfigFileBase &ini, const mrpt::utils::CConfigFileBase &robotIni);
00098 
00099                         /** Must be called for loading collision grids, or the first navigation
00100                           *   command may last a long time to be executed.
00101                           */
00102                         void initialize();
00103 
00104                         /** Evaluate navigation hardness:
00105                           */
00106                         float  evaluate( TNavigationParams *params );
00107 
00108                         /** Start navigation:
00109                           */
00110                         void  navigate( TNavigationParams *params );
00111 
00112                         /** Change current navigation params:
00113                           */
00114                         void  setParams( TNavigationParams *params );
00115 
00116                         /** Selects which one from the set of available holonomic methods will be used
00117                           *  into transformed TP-Space, and sets its configuration from a configuration file.
00118                           */
00119                         void setHolonomicMethod(
00120                                 THolonomicMethod        method,
00121                                 const char                      *config_INIfile = "./CONFIG_ReactiveNavigator.ini");
00122 
00123                         /** Change the robot shape, which is taken into account for collision
00124                           *  grid building.
00125                           */
00126                         void changeRobotShape( math::CPolygon &shape );
00127 
00128                         /** Provides a copy of the last log record with information about execution. On any unexpected error "*o" will be NULL.
00129                           * \param o An object where the log will be stored into.
00130                           */
00131                         void getLastLogRecord( CLogFileRecord &o );
00132 
00133 
00134             /** See 'navigatorBehavior'
00135               */
00136             enum TNavigatorBehavior
00137             {
00138                 /** The robot tries to get to the given target point
00139                   */
00140                 beNormalNavigation = 0,
00141                 /** The robot rotates to head the direction "m_beHeadDirection_rad", then goes into normal behavior
00142                   */
00143                 beHeadDirection,
00144                 /** The robot tries to get to a given auxiliar target "m_beAuxTarget", then goes into behavior 'beDoorCrosing2'
00145                   */
00146                 beDoorCrosing1,
00147                 /** The robot rotates to head the direction "m_beHeadDirection_rad", then goes into behavior 'beDoorCrosing3'
00148                   */
00149                 beDoorCrosing2,
00150                 /** The robot rotates tries to get to a given auxiliar target "m_beAuxTarget", then goes into normal behavior
00151                   */
00152                 beDoorCrosing3
00153             };
00154 
00155                         /** Enables / disables the logging into a file.
00156                           */
00157                         void enableLogFile(bool enable);
00158 
00159                 private:
00160                         // ------------------------------------------------------
00161                         //                                      PRIVATE DEFINITIONS
00162                         // ------------------------------------------------------
00163                         std::vector<float>              prevV,prevW,prevSelPTG;
00164 
00165             /** This defines the 'behavior' of the navigator (see posible values in TNavigatorBehavior)
00166               */
00167             TNavigatorBehavior          navigatorBehavior;
00168 
00169             /** The desired heading direction (in rads), for behaviors 'beHeadDirection' and 'beDoorCrosing2'.
00170               */
00171             float                                       m_beHeadDirection_rad;
00172 
00173             /** Auxiliary target position, for behaviors 'beDoorCrosing1' and 'beDoorCrosing3'.
00174               */
00175             CPoint2D                            m_beAuxTarget;
00176 
00177             /**  This is the desired "path" for passing a door (in GLOBAL coordinates!)
00178               */
00179             CPoint2D                            m_bePassPoint1,m_bePassPoint2;
00180 
00181                         /** The structure used for storing a movement generated by a holonomic-method .
00182                           */
00183                         struct THolonomicMovement {
00184                                         CParameterizedTrajectoryGenerator               *PTG;   /// The associated PTG
00185                                         double  direction, speed;       /// The holonomic movement
00186                                         double  evaluation;     /// An evaluation in the range [0,1] for the goodness of the movement.
00187                         };
00188 
00189                         /** The last log.
00190                           */
00191                         CLogFileRecord          lastLogRecord;
00192 
00193                         /** For the histeresis:
00194                          */
00195                         float           last_cmd_v,last_cmd_w;
00196 
00197                         /** Will be false until the navigation end is sent, and it is reset with each new command
00198                           */
00199                         bool            navigationEndEventSent;
00200 
00201                         /** Critical zones:
00202                           */
00203             synch::CCriticalSection  m_critZoneLastLog,m_critZoneNavigating;
00204 
00205                         // ------------------------------------------------------
00206                         //                                      PRIVATE METHODS
00207                         // ------------------------------------------------------
00208                         /** The main method for the navigator
00209                           */
00210                         void  performNavigationStep( );
00211 
00212 
00213                         // ------------------------------------------------------
00214                         //                                      PRIVATE VARIABLES
00215                         // ------------------------------------------------------
00216                         /** The current log file stream, or NULL if not being used
00217                           */
00218                         CAbstractHolonomicReactiveMethod        *holonomicMethod;
00219                         /** The current log file stream, or NULL if not being used
00220                           */
00221                         utils::CStream                                          *logFile;
00222                         /** Enables / disables the console debug output.
00223                           */
00224                         bool enableConsoleOutput;
00225 
00226                         CTicTac timerForExecutionPeriod;
00227 
00228                         // Loaded from INI file:
00229                         std::string robotName;                          // El nombre del robot donde estamos
00230                         float   refDistance;                            // "dmax" in papers.
00231                         float   colGridRes_x,colGridRes_y;  // Resolucion de la rejilla de distancias de choque precalculadas
00232                         float   robotMax_V_mps;                         // Max. vel del robot en m/s
00233                         float   robotMax_W_degps;              // Max. vel del robot en rad/s
00234                         float   ROBOTMODEL_TAU,ROBOTMODEL_DELAY; // Params for the motor system modelation
00235                         std::vector<float> weights; // length: 6 [0,5]
00236                         float   minObstaclesHeight, maxObstaclesHeight; // The range of "z" coordinates for obstacles to be considered
00237                         float   DIST_TO_TARGET_FOR_SENDING_EVENT;
00238 
00239             float       DOOR_CROSSING_SEARCH_TARGET_DISTANCEx2;
00240             float       VORONOI_MINIMUM_CLEARANCE;
00241             float       DISABLE_PERIOD_AFTER_FAIL;
00242             float       VORONOI_PATH_DIST_FROM_DOORWAY;
00243             float       DOORCROSSING_HEADING_ACCURACY_DEG;
00244             float       DOORCROSSING_ROTATION_CTE_DEG;
00245                         float   DOOR_CROSSING_DIST_TO_AUX_TARGETS;
00246                         float   DOOR_CROOSING_BEH3_TIMEOUT;
00247                         float   DOOR_CROSSING_MAXIMUM_DOORWAY_SIZE;
00248 
00249                         /** The iterations count.
00250                           */
00251                         unsigned long   nIteration;
00252 
00253                         /** Runtime estimation of execution period of the method.
00254                           */
00255                         float                   meanExecutionPeriod;
00256 
00257 
00258                         /** For sending an alarm (error event) when it seems that we are not approaching toward the target in a while...
00259                           */
00260                         float                       badNavAlarm_minDistTarget;
00261                         mrpt::system::TTimeStamp        badNavAlarm_lastMinDistTime;
00262                         float                       badNavAlarm_AlarmTimeout;
00263 
00264 
00265                         /** The robot 2D shape model
00266                           */
00267                         math::CPolygon          robotShape;
00268                         bool                            collisionGridsMustBeUpdated;
00269 
00270                         /** For taken the dynamics of the robot into account.
00271                           */
00272                         class CDynamicWindow
00273                         {
00274                         public:
00275                                 float   v_max, v_min;   //  m/sec
00276                                 float   w_max, w_min;   //  rad/sec
00277 
00278                         private:
00279                                 float   c1,c2,c3,c4;    // Curvature of corners.
00280 
00281                         public:
00282 
00283                                 /** Finds the max/min curvatures in the DW.
00284                                   */
00285                                 void  findMinMaxCurvatures(float &minCurv, float &maxCurv);
00286 
00287                                 /** Returns the corner which is closer (in curvature, and abs. values) to the desired command.
00288                                   */
00289                                 void  findBestApproximation(float desV,float desW, float &outV,float &outW);
00290 
00291                         private:
00292                                 /** Find the closest cut of a line with the DW
00293                                   */
00294                                 bool  findClosestCut( float cmd_v, float cmd_w, // IN
00295                                                                                                 float &out_v,float &out_w);     // OUT
00296 
00297                         };
00298 
00299 
00300 
00301                         /** @name Variables for CReactiveNavigationSystem::performNavigationStep 
00302                             @{ */
00303                         mrpt::utils::CTicTac                            totalExecutionTime, executionTime, tictac;
00304                         std::vector<vector_double>                      TP_Obstacles;
00305                         std::vector<poses::CPoint2D,Eigen::aligned_allocator<poses::CPoint2D> >                 TP_Targets;             // Target location (x,y) in TP-Space
00306                         std::vector<THolonomicMovement>         holonomicMovements;
00307                         std::vector<float>                                      times_TP_transformations, times_HoloNav;
00308                         std::vector<bool>                                       valid_TP;
00309                         float                                                           meanExecutionTime;
00310                         float                                                           meanTotalExecutionTime;
00311                         int                                                                     nLastSelectedPTG;
00312                         CDynamicWindow                                          DW;
00313                         int                                 m_decimateHeadingEstimate;
00314                         /** @} */
00315 
00316                         /** The set of transformations to be used:
00317                           */
00318                         std::vector<CParameterizedTrajectoryGenerator*> PTGs;
00319 
00320 
00321                         // Steps for the reactive navigation sytem.
00322                         // ----------------------------------------------------------------------------
00323                         void            STEP1_CollisionGridsBuilder();
00324 
00325                         bool            STEP2_Sense(
00326                                                                         mrpt::slam::CSimplePointsMap                                    &out_obstacles);
00327 
00328                         void            STEP3_SpaceTransformer(
00329                                                                         poses::CPointsMap                                       &in_obstacles,
00330                                                                         CParameterizedTrajectoryGenerator       *in_PTG,
00331                                                                         vector_double                                           &out_TPObstacles);
00332 
00333                         void            STEP4_HolonomicMethod(
00334                                                                         vector_double                                           &in_Obstacles,
00335                                                                         poses::CPoint2D                                         &in_Target,
00336                                                                         float                                                           in_maxRobotSpeed,
00337                                                                         THolonomicMovement                                      &out_selectedMovement,
00338                                                                         CHolonomicLogFileRecordPtr                      &in_HLFR );
00339 
00340                         void            STEP5_Evaluator(
00341                                                                         THolonomicMovement                                      &in_holonomicMovement,
00342                                                                         vector_double                                           &in_TPObstacles,
00343                                                                         poses::CPoint2D                                         &WS_Target,
00344                                                                         poses::CPoint2D                                         &TP_Target,
00345                                                                         bool                                                            wasSelectedInLast,
00346                                                                         CLogFileRecord::TInfoPerPTG                     &log );
00347 
00348                         void            STEP6_Selector(
00349                                                                         std::vector<THolonomicMovement>         &in_holonomicMovements,
00350                                                                         THolonomicMovement                                      &out_selectedHolonomicMovement,
00351                                                                         int                                                                     &out_nSelectedPTG);
00352 
00353                         void                    STEP7_NonHolonomicMovement(
00354                                                                         THolonomicMovement                                      &in_movement,
00355                                                                         float                                                           &out_v,
00356                                                                         float                                                           &out_w);
00357 
00358                         //
00359                         bool                    CerrandoHilo;
00360 
00361                         // Para casos de errores:
00362                         void                    Error_ParadaDeEmergencia( const char *msg );
00363 
00364                 };
00365         }
00366 }
00367 
00368 
00369 #endif
00370 
00371 
00372 
00373 
00374 



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011