00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://www.mrpt.org/ | 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 * \ingroup mrpt_reactivenav_grp 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 * 00063 * How to use: 00064 * - A class with callbacks must be defined by the user and provided to the constructor. 00065 * - loadConfigFile() must be called to set up the bunch of parameters from a config file (could be a memory-based virtual config file). 00066 * - 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. 00067 * 00068 * - 17/JUN/2004: First design. 00069 * - 16/SEP/2004: Totally redesigned, according to document "MultiParametric Based Space Transformation for Reactive Navigation" 00070 * - 29/SEP/2005: Totally rewritten again, for integration into MRPT library and according to the ICRA paper. 00071 * - 17/OCT/2007: Whole code updated to accomodate to MRPT 0.5 and make it portable to Linux. 00072 * 00073 * \sa CAbstractReactiveNavigationSystem, CParameterizedTrajectoryGenerator, CAbstractHolonomicReactiveMethod 00074 * \ingroup mrpt_reactivenav_grp 00075 */ 00076 class REACTIVENAV_IMPEXP CReactiveNavigationSystem : public CAbstractReactiveNavigationSystem 00077 { 00078 public: 00079 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00080 public: 00081 /** Constructor 00082 * \param configINIFile The file to load the configuration from. See loadConfigFile 00083 * \param robotConfigFile The file to load the robot specific configuration from. 00084 * \param rmc A set of wrappers that must be filled in. 00085 * \param sensors A set of wrappers that must be filled in. 00086 * \param dbg A set of wrappers that must be filled in. 00087 * \param evnts A set of wrappers that must be filled in. 00088 * \param enableConsoleOutput Set to false if console output is not desired. 00089 * \param enableLogFile Set to true to enable logging to file. 00090 */ 00091 CReactiveNavigationSystem( 00092 CReactiveInterfaceImplementation &react_iterf_impl, 00093 bool enableConsoleOutput = true, 00094 bool enableLogFile = false); 00095 00096 /** Destructor 00097 */ 00098 virtual ~CReactiveNavigationSystem(); 00099 00100 /** Reload the configuration from a file 00101 */ 00102 void loadConfigFile(const mrpt::utils::CConfigFileBase &ini, const mrpt::utils::CConfigFileBase &robotIni); 00103 00104 /** Must be called for loading collision grids, or the first navigation 00105 * command may last a long time to be executed. 00106 */ 00107 void initialize(); 00108 00109 /** Evaluate navigation hardness: 00110 */ 00111 float evaluate( TNavigationParams *params ); 00112 00113 /** Start navigation: 00114 */ 00115 void navigate( TNavigationParams *params ); 00116 00117 /** Change current navigation params: 00118 */ 00119 void setParams( TNavigationParams *params ); 00120 00121 /** Selects which one from the set of available holonomic methods will be used 00122 * into transformed TP-Space, and sets its configuration from a configuration file. 00123 */ 00124 void setHolonomicMethod( 00125 THolonomicMethod method, 00126 const char *config_INIfile = "./CONFIG_ReactiveNavigator.ini"); 00127 00128 /** Change the robot shape, which is taken into account for collision 00129 * grid building. 00130 */ 00131 void changeRobotShape( math::CPolygon &shape ); 00132 00133 /** Provides a copy of the last log record with information about execution. On any unexpected error "*o" will be NULL. 00134 * \param o An object where the log will be stored into. 00135 */ 00136 void getLastLogRecord( CLogFileRecord &o ); 00137 00138 /** Enables / disables the logging into a file. 00139 */ 00140 void enableLogFile(bool enable); 00141 00142 private: 00143 // ------------------------------------------------------ 00144 // PRIVATE DEFINITIONS 00145 // ------------------------------------------------------ 00146 /** The structure used for storing a movement generated by a holonomic-method . 00147 */ 00148 struct THolonomicMovement { 00149 CParameterizedTrajectoryGenerator *PTG; /// The associated PTG 00150 double direction, speed; /// The holonomic movement 00151 double evaluation; /// An evaluation in the range [0,1] for the goodness of the movement. 00152 }; 00153 00154 /** The last log. 00155 */ 00156 CLogFileRecord lastLogRecord; 00157 00158 /** For the histeresis: */ 00159 float last_cmd_v,last_cmd_w; 00160 00161 /** Will be false until the navigation end is sent, and it is reset with each new command 00162 */ 00163 bool navigationEndEventSent; 00164 00165 /** Critical zones: */ 00166 synch::CCriticalSection m_critZoneLastLog,m_critZoneNavigating; 00167 00168 // ------------------------------------------------------ 00169 // PRIVATE METHODS 00170 // ------------------------------------------------------ 00171 /** The main method for the navigator */ 00172 void performNavigationStep( ); 00173 00174 // ------------------------------------------------------ 00175 // PRIVATE VARIABLES 00176 // ------------------------------------------------------ 00177 00178 CAbstractHolonomicReactiveMethod *holonomicMethod; //!< The holonomic navigation algorithm. 00179 mrpt::utils::CStream *logFile; //!< The current log file stream, or NULL if not being used 00180 00181 bool m_enableConsoleOutput; //!< Enables / disables the console debug output. 00182 00183 bool m_init_done; //!< Whether \a loadConfigFile() has been called or not. 00184 00185 CTicTac timerForExecutionPeriod; 00186 00187 // Loaded from INI file: 00188 std::string robotName; // El nombre del robot donde estamos 00189 float refDistance; // "dmax" in papers. 00190 float colGridRes_x,colGridRes_y; // Resolucion de la rejilla de distancias de choque precalculadas 00191 float robotMax_V_mps; // Max. vel del robot en m/s 00192 float robotMax_W_degps; // Max. vel del robot en rad/s 00193 float ROBOTMODEL_TAU,ROBOTMODEL_DELAY; // Params for the motor system modelation 00194 std::vector<float> weights; // length: 6 [0,5] 00195 float minObstaclesHeight, maxObstaclesHeight; // The range of "z" coordinates for obstacles to be considered 00196 float DIST_TO_TARGET_FOR_SENDING_EVENT; 00197 00198 00199 unsigned long nIteration; //!< The iteration count. 00200 float meanExecutionPeriod; //!< Runtime estimation of execution period of the method. 00201 00202 00203 /** For sending an alarm (error event) when it seems that we are not approaching toward the target in a while... 00204 */ 00205 float badNavAlarm_minDistTarget; 00206 mrpt::system::TTimeStamp badNavAlarm_lastMinDistTime; 00207 float badNavAlarm_AlarmTimeout; 00208 00209 00210 /** The robot 2D shape model 00211 */ 00212 math::CPolygon robotShape; 00213 bool collisionGridsMustBeUpdated; 00214 00215 00216 /** @name Variables for CReactiveNavigationSystem::performNavigationStep 00217 @{ */ 00218 mrpt::utils::CTicTac totalExecutionTime, executionTime, tictac; 00219 std::vector<vector_double> TP_Obstacles; 00220 std::vector<poses::CPoint2D,Eigen::aligned_allocator<poses::CPoint2D> > TP_Targets; // Target location (x,y) in TP-Space 00221 std::vector<THolonomicMovement> holonomicMovements; 00222 std::vector<float> times_TP_transformations, times_HoloNav; 00223 std::vector<bool> valid_TP; 00224 float meanExecutionTime; 00225 float meanTotalExecutionTime; 00226 int nLastSelectedPTG; 00227 int m_decimateHeadingEstimate; 00228 /** @} */ 00229 00230 /** The set of transformations to be used: 00231 */ 00232 std::vector<CParameterizedTrajectoryGenerator*> PTGs; 00233 00234 00235 // Steps for the reactive navigation sytem. 00236 // ---------------------------------------------------------------------------- 00237 void STEP1_CollisionGridsBuilder(); 00238 00239 bool STEP2_Sense( 00240 mrpt::slam::CSimplePointsMap &out_obstacles); 00241 00242 void STEP3_SpaceTransformer( 00243 poses::CPointsMap &in_obstacles, 00244 CParameterizedTrajectoryGenerator *in_PTG, 00245 vector_double &out_TPObstacles); 00246 00247 void STEP4_HolonomicMethod( 00248 vector_double &in_Obstacles, 00249 poses::CPoint2D &in_Target, 00250 float in_maxRobotSpeed, 00251 THolonomicMovement &out_selectedMovement, 00252 CHolonomicLogFileRecordPtr &in_HLFR ); 00253 00254 void STEP5_Evaluator( 00255 THolonomicMovement &in_holonomicMovement, 00256 vector_double &in_TPObstacles, 00257 poses::CPoint2D &WS_Target, 00258 poses::CPoint2D &TP_Target, 00259 bool wasSelectedInLast, 00260 CLogFileRecord::TInfoPerPTG &log ); 00261 00262 void STEP6_Selector( 00263 std::vector<THolonomicMovement> &in_holonomicMovements, 00264 THolonomicMovement &out_selectedHolonomicMovement, 00265 int &out_nSelectedPTG); 00266 00267 void STEP7_NonHolonomicMovement( 00268 THolonomicMovement &in_movement, 00269 float &out_v, 00270 float &out_w); 00271 00272 // 00273 bool m_closing_navigator; 00274 00275 /** Stops the robot and set navigation state to error */ 00276 void doEmergencyStop( const char *msg ); 00277 00278 }; 00279 } 00280 } 00281 00282 00283 #endif 00284 00285 00286 00287 00288
| Page generated by Doxygen 1.7.5 for MRPT 0.9.5 SVN: at Thu Oct 13 21:25:36 UTC 2011 |