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 <mrpt/reactivenav/CParameterizedTrajectoryGenerator.h>
00039 #include "CLogFileRecord.h"
00040 #include "CAbstractHolonomicReactiveMethod.h"
00041 #include "CHolonomicVFF.h"
00042 #include "CHolonomicND.h"
00043
00044 namespace mrpt
00045 {
00046
00047
00048 namespace reactivenav
00049 {
00050
00051
00052 enum THolonomicMethod
00053 {
00054 hmVIRTUAL_FORCE_FIELDS = 0,
00055 hmSEARCH_FOR_BEST_GAP = 1
00056 };
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071 class RNAVDLLIMPEXP CReactiveNavigationSystem : public CAbstractReactiveNavigationSystem
00072 {
00073 public:
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
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
00094
00095
00096 virtual ~CReactiveNavigationSystem();
00097
00098
00099
00100 void loadConfigFile(const mrpt::utils::CConfigFileBase &ini, const mrpt::utils::CConfigFileBase &robotIni);
00101
00102
00103
00104
00105 void initialize();
00106
00107
00108
00109 float evaluate( TNavigationParams *params );
00110
00111
00112
00113 void navigate( TNavigationParams *params );
00114
00115
00116
00117 void setParams( TNavigationParams *params );
00118
00119
00120
00121
00122 void setHolonomicMethod(
00123 THolonomicMethod method,
00124 const char *config_INIfile = "./CONFIG_ReactiveNavigator.ini");
00125
00126
00127
00128
00129 void changeRobotShape( math::CPolygon &shape );
00130
00131
00132
00133
00134 void getLastLogRecord( CLogFileRecord &o );
00135
00136
00137
00138
00139 enum TNavigatorBehavior
00140 {
00141
00142
00143 beNormalNavigation = 0,
00144
00145
00146 beHeadDirection,
00147
00148
00149 beDoorCrosing1,
00150
00151
00152 beDoorCrosing2,
00153
00154
00155 beDoorCrosing3
00156 };
00157
00158
00159 private:
00160
00161
00162
00163
00164
00165
00166 TNavigatorBehavior navigatorBehavior;
00167
00168
00169
00170 float m_beHeadDirection_rad;
00171
00172
00173
00174 CPoint2D m_beAuxTarget;
00175
00176
00177
00178 CPoint2D m_bePassPoint1,m_bePassPoint2;
00179
00180
00181
00182 struct THolonomicMovement {
00183
00184
00185 CParameterizedTrajectoryGenerator *PTG;
00186
00187
00188 float direction, speed;
00189
00190
00191 float evaluation;
00192
00193
00194
00195 float securityDistance;
00196 };
00197
00198
00199
00200 CLogFileRecord lastLogRecord;
00201
00202
00203
00204 float last_cmd_v,last_cmd_w;
00205
00206
00207
00208 bool navigationEndEventSent;
00209
00210
00211
00212 synch::CCriticalSection critZoneLastLog,critZoneNavigating;
00213
00214
00215
00216
00217
00218
00219 void performNavigationStep( );
00220
00221
00222
00223 void enableLogFile(bool enable);
00224
00225
00226
00227
00228
00229
00230 CAbstractHolonomicReactiveMethod *holonomicMethod;
00231
00232
00233 utils::CStream *logFile;
00234
00235
00236 bool enableConsoleOutput;
00237
00238 CTicTac timerForExecutionPeriod;
00239
00240
00241 bool m_debugWindows;
00242 std::string robotName;
00243 float refDistance;
00244 float colGridRes_x,colGridRes_y;
00245 float robotMax_V_mps;
00246 float robotMax_W_degps;
00247 float ROBOTMODEL_TAU,ROBOTMODEL_DELAY;
00248 vector_float weights;
00249 float minObstaclesHeight, maxObstaclesHeight;
00250 float DIST_TO_TARGET_FOR_SENDING_EVENT;
00251
00252 float DOOR_CROSSING_SEARCH_TARGET_DISTANCEx2;
00253 float VORONOI_MINIMUM_CLEARANCE;
00254 float DISABLE_PERIOD_AFTER_FAIL;
00255 float VORONOI_PATH_DIST_FROM_DOORWAY;
00256 float DOORCROSSING_HEADING_ACCURACY_DEG;
00257 float DOORCROSSING_ROTATION_CTE_DEG;
00258 float DOOR_CROSSING_DIST_TO_AUX_TARGETS;
00259 float DOOR_CROOSING_BEH3_TIMEOUT;
00260 float DOOR_CROSSING_MAXIMUM_DOORWAY_SIZE;
00261
00262
00263
00264 unsigned long nIteration;
00265
00266
00267
00268 float meanExecutionPeriod;
00269
00270
00271
00272
00273 float badNavAlarm_minDistTarget;
00274 mrpt::system::TTimeStamp badNavAlarm_lastMinDistTime;
00275 float badNavAlarm_AlarmTimeout;
00276
00277
00278
00279
00280 math::CPolygon robotShape;
00281 bool collisionGridsMustBeUpdated;
00282
00283
00284
00285 std::vector<CParameterizedTrajectoryGenerator*> PTGs;
00286
00287
00288 mrpt::gui::CDisplayWindowPlotsPtr m_debugWin_WS;
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
00304 void STEP4_HolonomicMethod(
00305 vector_float &in_Obstacles,
00306 poses::CPoint2D &in_Target,
00307 float in_maxRobotSpeed,
00308 THolonomicMovement &out_selectedMovement,
00309 CHolonomicLogFileRecordPtr &in_HLFR );
00310
00311 void STEP5_Evaluator(
00312 THolonomicMovement &in_holonomicMovement,
00313 vector_float &in_TPObstacles,
00314 poses::CPoint2D &WS_Target,
00315 poses::CPoint2D &TP_Target,
00316 bool wasSelectedInLast,
00317 CLogFileRecord::TInfoPerPTG &log );
00318
00319 void STEP6_Selector(
00320 std::vector<THolonomicMovement> &in_holonomicMovements,
00321 THolonomicMovement &out_selectedHolonomicMovement,
00322 int &out_nSelectedPTG);
00323
00324 void STEP7_NonHolonomicMovement(
00325 THolonomicMovement &in_movement,
00326 float &out_v,
00327 float &out_w);
00328
00329
00330 bool CerrandoHilo;
00331
00332
00333 void Error_ParadaDeEmergencia( const char *msg );
00334
00335
00336
00337 class CDynamicWindow
00338 {
00339 public:
00340 float v_max, v_min;
00341 float w_max, w_min;
00342
00343 private:
00344 float c1,c2,c3,c4;
00345
00346 public:
00347
00348
00349
00350 void findMinMaxCurvatures(float &minCurv, float &maxCurv);
00351
00352
00353
00354 void findBestApproximation(float desV,float desW, float &outV,float &outW);
00355
00356 private:
00357
00358
00359 bool findClosestCut( float cmd_v, float cmd_w,
00360 float &out_v,float &out_w);
00361
00362 };
00363
00364 };
00365 }
00366 }
00367
00368
00369 #endif
00370
00371
00372
00373
00374