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 CHolonomicND_H
00029 #define CHolonomicND_H
00030
00031 #include "CAbstractHolonomicReactiveMethod.h"
00032
00033 namespace mrpt
00034 {
00035 namespace reactivenav
00036 {
00037 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(CLogFileRecord_ND, CHolonomicLogFileRecord, RNAVDLLIMPEXP)
00038
00039
00047 class RNAVDLLIMPEXP CHolonomicND : public CAbstractHolonomicReactiveMethod
00048 {
00049 public:
00052 CHolonomicND( const mrpt::utils::CConfigFileBase *INI_FILE = NULL );
00053
00065 void navigate( poses::CPoint2D &target,
00066 vector_float &obstacles,
00067 float maxRobotSpeed,
00068 float &desiredDirection,
00069 float &desiredSpeed,
00070 CHolonomicLogFileRecordPtr &logRecord );
00071
00074 struct TGap
00075 {
00076 int ini;
00077 int end;
00078 float entranceDistance;
00079 float maxDistance;
00080 int representative_sector;
00081 };
00082
00083 typedef std::vector<TGap> TGapArray;
00084
00087 enum TSituations
00088 {
00089 SITUATION_TARGET_DIRECTLY = 1,
00090 SITUATION_SMALL_GAP,
00091 SITUATION_WIDE_GAP,
00092 SITUATION_NO_WAY_FOUND
00093 };
00094
00097 void initialize( const mrpt::utils::CConfigFileBase &INI_FILE );
00098
00099
00100
00101 private:
00102 int last_selected_sector;
00103
00104 int direction2sector(float a, int N);
00105
00108 float TOO_CLOSE_OBSTACLE,WIDE_GAP_SIZE_PERCENT,RISK_EVALUATION_SECTORS_PERCENT;
00109 float RISK_EVALUATION_DISTANCE,MAX_SECTOR_DIST_FOR_D2_PERCENT;
00110 float TARGET_SLOW_APPROACHING_DISTANCE;
00111
00112 vector_float factorWeights;
00113
00116 void gapsEstimator(
00117 vector_float &obstacles,
00118 poses::CPoint2D &in_target,
00119 TGapArray &gaps );
00120
00123 void searchBestGap(
00124 vector_float &in_obstacles,
00125 float in_maxObsRange,
00126 TGapArray &in_gaps,
00127 poses::CPoint2D &in_target,
00128 int &out_selDirection,
00129 float &out_selEvaluation,
00130 TSituations &out_situation,
00131 float &out_riskEvaluation,
00132 CLogFileRecord_NDPtr log);
00133
00136 void calcRepresentativeSectorForGap(
00137 TGap *gap,
00138 poses::CPoint2D &target,
00139 vector_float &obstacles);
00140
00143 void evaluateGaps(
00144 vector_float &in_obstacles,
00145 float in_maxObsRange,
00146 TGapArray &in_gaps,
00147 int TargetSector,
00148 float TargetDist,
00149 vector_float &out_gaps_evaluation );
00150
00151
00152
00153 };
00154
00159 class CLogFileRecord_ND : public CHolonomicLogFileRecord
00160 {
00161 DEFINE_SERIALIZABLE( CLogFileRecord_ND )
00162
00163 public:
00166 vector_int gaps_ini,gaps_end;
00167 vector_float gaps_eval;
00168 int32_t selectedSector;
00169 float evaluation;
00170 float riskEvaluation;
00171 CHolonomicND::TSituations situation;
00172 };
00173
00174 }
00175 }
00176
00177
00178 #endif
00179
00180
00181