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/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
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 REACTIVENAV_IMPEXP CReactiveNavigationSystem : public CAbstractReactiveNavigationSystem
00072 {
00073 public:
00074 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00075 public:
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086 CReactiveNavigationSystem(
00087 CReactiveInterfaceImplementation &react_iterf_impl,
00088 bool enableConsoleOutput = true,
00089 bool enableLogFile = false);
00090
00091
00092
00093 virtual ~CReactiveNavigationSystem();
00094
00095
00096
00097 void loadConfigFile(const mrpt::utils::CConfigFileBase &ini, const mrpt::utils::CConfigFileBase &robotIni);
00098
00099
00100
00101
00102 void initialize();
00103
00104
00105
00106 float evaluate( TNavigationParams *params );
00107
00108
00109
00110 void navigate( TNavigationParams *params );
00111
00112
00113
00114 void setParams( TNavigationParams *params );
00115
00116
00117
00118
00119 void setHolonomicMethod(
00120 THolonomicMethod method,
00121 const char *config_INIfile = "./CONFIG_ReactiveNavigator.ini");
00122
00123
00124
00125
00126 void changeRobotShape( math::CPolygon &shape );
00127
00128
00129
00130
00131 void getLastLogRecord( CLogFileRecord &o );
00132
00133
00134
00135
00136 enum TNavigatorBehavior
00137 {
00138
00139
00140 beNormalNavigation = 0,
00141
00142
00143 beHeadDirection,
00144
00145
00146 beDoorCrosing1,
00147
00148
00149 beDoorCrosing2,
00150
00151
00152 beDoorCrosing3
00153 };
00154
00155
00156
00157 void enableLogFile(bool enable);
00158
00159 private:
00160
00161
00162
00163 std::vector<float> prevV,prevW,prevSelPTG;
00164
00165
00166
00167 TNavigatorBehavior navigatorBehavior;
00168
00169
00170
00171 float m_beHeadDirection_rad;
00172
00173
00174
00175 CPoint2D m_beAuxTarget;
00176
00177
00178
00179 CPoint2D m_bePassPoint1,m_bePassPoint2;
00180
00181
00182
00183 struct THolonomicMovement {
00184 CParameterizedTrajectoryGenerator *PTG;
00185 double direction, speed;
00186 double evaluation;
00187 };
00188
00189
00190
00191 CLogFileRecord lastLogRecord;
00192
00193
00194
00195 float last_cmd_v,last_cmd_w;
00196
00197
00198
00199 bool navigationEndEventSent;
00200
00201
00202
00203 synch::CCriticalSection m_critZoneLastLog,m_critZoneNavigating;
00204
00205
00206
00207
00208
00209
00210 void performNavigationStep( );
00211
00212
00213
00214
00215
00216
00217
00218 CAbstractHolonomicReactiveMethod *holonomicMethod;
00219
00220
00221 utils::CStream *logFile;
00222
00223
00224 bool enableConsoleOutput;
00225
00226 CTicTac timerForExecutionPeriod;
00227
00228
00229 std::string robotName;
00230 float refDistance;
00231 float colGridRes_x,colGridRes_y;
00232 float robotMax_V_mps;
00233 float robotMax_W_degps;
00234 float ROBOTMODEL_TAU,ROBOTMODEL_DELAY;
00235 std::vector<float> weights;
00236 float minObstaclesHeight, maxObstaclesHeight;
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
00250
00251 unsigned long nIteration;
00252
00253
00254
00255 float meanExecutionPeriod;
00256
00257
00258
00259
00260 float badNavAlarm_minDistTarget;
00261 mrpt::system::TTimeStamp badNavAlarm_lastMinDistTime;
00262 float badNavAlarm_AlarmTimeout;
00263
00264
00265
00266
00267 math::CPolygon robotShape;
00268 bool collisionGridsMustBeUpdated;
00269
00270
00271
00272 class CDynamicWindow
00273 {
00274 public:
00275 float v_max, v_min;
00276 float w_max, w_min;
00277
00278 private:
00279 float c1,c2,c3,c4;
00280
00281 public:
00282
00283
00284
00285 void findMinMaxCurvatures(float &minCurv, float &maxCurv);
00286
00287
00288
00289 void findBestApproximation(float desV,float desW, float &outV,float &outW);
00290
00291 private:
00292
00293
00294 bool findClosestCut( float cmd_v, float cmd_w,
00295 float &out_v,float &out_w);
00296
00297 };
00298
00299
00300
00301
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;
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
00317
00318 std::vector<CParameterizedTrajectoryGenerator*> PTGs;
00319
00320
00321
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
00362 void Error_ParadaDeEmergencia( const char *msg );
00363
00364 };
00365 }
00366 }
00367
00368
00369 #endif
00370
00371
00372
00373
00374