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