00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef CReactiveNavigationSystem_H
00029 #define CReactiveNavigationSystem_H
00030
00031 #include <mrpt/slam.h>
00032 #include <mrpt/poses.h>
00033 #include <mrpt/math_mrpt.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 {
00048 namespace reactivenav
00049 {
00052 enum THolonomicMethod
00053 {
00054 hmVIRTUAL_FORCE_FIELDS = 0,
00055 hmSEARCH_FOR_BEST_GAP = 1
00056 };
00057
00071 class RNAVDLLIMPEXP CReactiveNavigationSystem : public CAbstractReactiveNavigationSystem
00072 {
00073 public:
00084 CReactiveNavigationSystem(
00085 TRobotMotionControl &rmc,
00086 TSensors &sensors,
00087 void (*emul_printf)(const char *s),
00088 TEventsLaunching &evnts,
00089 const mrpt::utils::CConfigFileBase &configIni,
00090 const mrpt::utils::CConfigFileBase &configRobotIni,
00091 bool enableConsoleOutput = true,
00092 bool enableLogFile = false);
00093
00096 virtual ~CReactiveNavigationSystem();
00097
00100 void loadConfigFile(const mrpt::utils::CConfigFileBase &ini, const mrpt::utils::CConfigFileBase &robotIni);
00101
00105 void initialize();
00106
00109 float evaluate( TNavigationParams *params );
00110
00113 void navigate( TNavigationParams *params );
00114
00117 void setParams( TNavigationParams *params );
00118
00122 void setHolonomicMethod(
00123 THolonomicMethod method,
00124 const char *config_INIfile = "./CONFIG_ReactiveNavigator.ini");
00125
00129 void changeRobotShape( math::CPolygon &shape );
00130
00134 void getLastLogRecord( CLogFileRecord &o );
00135
00136
00139 enum TNavigatorBehavior
00140 {
00143 beNormalNavigation = 0,
00146 beHeadDirection,
00149 beDoorCrosing1,
00152 beDoorCrosing2,
00155 beDoorCrosing3
00156 };
00157
00158
00159 private:
00160
00161
00162
00163
00166 TNavigatorBehavior navigatorBehavior;
00167
00170 float m_beHeadDirection_rad;
00171
00174 CPoint2D m_beAuxTarget;
00175
00178 CPoint2D m_bePassPoint1,m_bePassPoint2;
00179
00182 struct THolonomicMovement {
00185 CParameterizedTrajectoryGenerator *PTG;
00188 float direction, speed;
00191 float evaluation;
00192
00195 float securityDistance;
00196 };
00197
00200 CLogFileRecord lastLogRecord;
00201
00204 float last_cmd_v,last_cmd_w;
00205
00208 bool navigationEndEventSent;
00209
00212 synch::CCriticalSection critZoneLastLog,critZoneNavigating;
00213
00216 vector_float securityDistances;
00217
00218
00219
00220
00223 void performNavigationStep( );
00224
00227 void enableLogFile(bool enable);
00228
00229
00230
00231
00234 CAbstractHolonomicReactiveMethod *holonomicMethod;
00237 utils::CStream *logFile;
00240 bool enableConsoleOutput;
00241
00242 CTicTac timerForExecutionPeriod;
00243
00244
00245 std::string robotName;
00246 float refDistance;
00247 float colGridRes_x,colGridRes_y;
00248 float robotMax_V_mps;
00249 float robotMax_W_degps;
00250 float ROBOTMODEL_TAU,ROBOTMODEL_DELAY;
00251 vector_float weights;
00252 float minObstaclesHeight, maxObstaclesHeight;
00253 float DIST_TO_TARGET_FOR_SENDING_EVENT;
00254
00255 float DOOR_CROSSING_SEARCH_TARGET_DISTANCEx2;
00256 float VORONOI_MINIMUM_CLEARANCE;
00257 float DISABLE_PERIOD_AFTER_FAIL;
00258 float VORONOI_PATH_DIST_FROM_DOORWAY;
00259 float DOORCROSSING_HEADING_ACCURACY_DEG;
00260 float DOORCROSSING_ROTATION_CTE_DEG;
00261 float DOOR_CROSSING_DIST_TO_AUX_TARGETS;
00262 float DOOR_CROOSING_BEH3_TIMEOUT;
00263 float DOOR_CROSSING_MAXIMUM_DOORWAY_SIZE;
00264
00267 unsigned long nIteration;
00268
00271 float meanExecutionPeriod;
00272
00273
00276 float badNavAlarm_minDistTarget;
00277 mrpt::system::TTimeStamp badNavAlarm_lastMinDistTime;
00278 float badNavAlarm_AlarmTimeout;
00279
00280
00283 math::CPolygon robotShape;
00284 bool collisionGridsMustBeUpdated;
00285
00288 std::vector<CParameterizedTrajectoryGenerator*> PTGs;
00289
00290
00291
00292
00293 void STEP1_CollisionGridsBuilder();
00294
00295 bool STEP2_Sense(
00296 mrpt::slam::CSimplePointsMap &out_obstacles,
00297 mrpt::slam::COccupancyGridMap2D *out_obstaclesGridMap = NULL );
00298
00299 void STEP3_SpaceTransformer(
00300 poses::CPointsMap &in_obstacles,
00301 CParameterizedTrajectoryGenerator *in_PTG,
00302 vector_float &out_TPObstacles,
00303 unsigned int securityDistanceIndex);
00304
00305 void STEP4_HolonomicMethod(
00306 vector_float &in_Obstacles,
00307 poses::CPoint2D &in_Target,
00308 float in_maxRobotSpeed,
00309 THolonomicMovement &out_selectedMovement,
00310 CHolonomicLogFileRecordPtr &in_HLFR );
00311
00312 void STEP5_Evaluator(
00313 THolonomicMovement &in_holonomicMovement,
00314 vector_float &in_TPObstacles,
00315 poses::CPoint2D &WS_Target,
00316 poses::CPoint2D &TP_Target,
00317 bool wasSelectedInLast,
00318 CLogFileRecord::TInfoPerPTG &log,
00319 unsigned int securityDistanceIndex);
00320
00321 void STEP6_Selector(
00322 std::vector<THolonomicMovement> &in_holonomicMovements,
00323 THolonomicMovement &out_selectedHolonomicMovement,
00324 int &out_nSelectedPTG);
00325
00326 void STEP7_NonHolonomicMovement(
00327 THolonomicMovement &in_movement,
00328 float &out_v,
00329 float &out_w);
00330
00331 bool PointIsIntoPolygon(float x,float y,float v1x,float v1y,float v2x,float v2y,float v3x,float v3y,float v4x,float v4y );
00332
00333
00334 bool CerrandoHilo;
00335
00336
00337 void Error_ParadaDeEmergencia( const char *msg );
00338
00341 class CDynamicWindow
00342 {
00343 public:
00344 float v_max, v_min;
00345 float w_max, w_min;
00346
00347 private:
00348 float c1,c2,c3,c4;
00349
00350 public:
00351
00354 void findMinMaxCurvatures(float &minCurv, float &maxCurv);
00355
00358 void findBestApproximation(float desV,float desW, float &outV,float &outW);
00359
00360 private:
00363 bool findClosestCut( float cmd_v, float cmd_w,
00364 float &out_v,float &out_w);
00365
00366 };
00367
00368 };
00369 }
00370 }
00371
00372
00373 #endif
00374
00375
00376
00377
00378