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
00029 #ifndef __ENVIRONMENT_NAV2D_H_
00030 #define __ENVIRONMENT_NAV2D_H_
00031
00032
00033 #define ENVNAV2D_COSTMULT 1000
00034
00035
00036 #define ENVNAV2D_DEFAULTOBSTHRESH 1 //253-for willow garage //see explanation of the value below
00037
00038 #define ENVNAV2D_MAXDIRS 16 //TODO-debugmax - crashes for 8 in debug mode
00039
00040
00041 typedef struct ENV_NAV2D_CONFIG
00042 {
00043 int EnvWidth_c;
00044 int EnvHeight_c;
00045 int StartX_c;
00046 int StartY_c;
00047 int EndX_c;
00048 int EndY_c;
00049 unsigned char** Grid2D;
00050
00051
00052 unsigned char obsthresh;
00053
00054 int dx_[ENVNAV2D_MAXDIRS];
00055 int dy_[ENVNAV2D_MAXDIRS];
00056
00057 int dxintersects_[ENVNAV2D_MAXDIRS][2];
00058 int dyintersects_[ENVNAV2D_MAXDIRS][2];
00059
00060 int dxy_distance_mm_[ENVNAV2D_MAXDIRS];
00061
00062
00063
00064 int numofdirs;
00065
00066 } EnvNAV2DConfig_t;
00067
00068 typedef struct ENVHASHENTRY
00069 {
00070 int stateID;
00071 int X;
00072 int Y;
00073 } EnvNAV2DHashEntry_t;
00074
00075
00076 typedef struct NAV2D_CELL
00077 {
00078 int x;
00079 int y;
00080 }nav2dcell_t;
00081
00082
00083 typedef struct
00084 {
00085
00086 int startstateid;
00087 int goalstateid;
00088
00089 bool bInitialized;
00090
00091
00092 int HashTableSize;
00093 vector<EnvNAV2DHashEntry_t*>* Coord2StateIDHashTable;
00094
00095
00096 vector<EnvNAV2DHashEntry_t*> StateID2CoordTable;
00097
00098
00099
00100 }EnvironmentNAV2D_t;
00101
00102
00105 class EnvironmentNAV2D : public DiscreteSpaceInformation
00106 {
00107
00108 public:
00109
00112 bool InitializeEnv(const char* sEnvFile);
00115 bool InitializeMDPCfg(MDPConfig *MDPCfg);
00118 int GetFromToHeuristic(int FromStateID, int ToStateID);
00121 int GetGoalHeuristic(int stateID);
00124 int GetStartHeuristic(int stateID);
00127 void SetAllActionsandAllOutcomes(CMDPSTATE* state);
00130 void SetAllPreds(CMDPSTATE* state);
00133 void GetSuccs(int SourceStateID, vector<int>* SuccIDV, vector<int>* CostV);
00136 void GetPreds(int TargetStateID, vector<int>* PredIDV, vector<int>* CostV);
00137
00140 int SizeofCreatedEnv();
00143 void PrintState(int stateID, bool bVerbose, FILE* fOut=NULL);
00146 void PrintEnv_Config(FILE* fOut);
00147
00157 bool InitializeEnv(int width, int height,
00159 const unsigned char* mapdata,
00160 int startx, int starty,
00161 int goalx, int goaly, unsigned char obsthresh);
00164 bool InitializeEnv(int width, int height,
00166 const unsigned char* mapdata, unsigned char obsthresh);
00169 int SetStart(int x, int y);
00172 int SetGoal(int x, int y);
00175 void SetGoalTolerance(double tol_x, double tol_y, double tol_theta);
00178 bool UpdateCost(int x, int y, unsigned char newcost);
00179
00184 void GetPredsofChangedEdges(vector<nav2dcell_t> const * changedcellsV, vector<int> *preds_of_changededgesIDV);
00187 void GetSuccsofChangedEdges(vector<nav2dcell_t> const * changedcellsV, vector<int> *succs_of_changededgesIDV);
00188
00191 virtual bool AreEquivalent(int StateID1, int StateID2);
00192
00196 virtual void GetRandomSuccsatDistance(int SourceStateID, std::vector<int>* SuccIDV, std::vector<int>* CLowV);
00200 virtual void GetRandomPredsatDistance(int TargetStateID, std::vector<int>* PredIDV, std::vector<int>* CLowV);
00201
00205 virtual void GetRandomNeighs(int stateID, std::vector<int>* NeighIDV, std::vector<int>* CLowV, int nNumofNeighs, int nDist_c, bool bSuccs);
00206
00210 void SetConfiguration(int width, int height,
00212 const unsigned char* mapdata,
00213 int startx, int starty,
00214 int goalx, int goaly);
00215
00219 bool InitGeneral();
00220
00223 void GetCoordFromState(int stateID, int& x, int& y) const;
00224
00227 int GetStateFromCoord(int x, int y);
00228
00231 bool IsObstacle(int x, int y);
00232
00235 unsigned char GetMapCost(int x, int y);
00236
00239 void GetEnvParms(int *size_x, int *size_y, int* startx, int* starty, int* goalx, int* goaly, unsigned char* obsthresh);
00240
00243 bool SetEnvParameter(const char* parameter, int value);
00244
00247 const EnvNAV2DConfig_t* GetEnvNavConfig();
00248
00249 EnvironmentNAV2D();
00250 ~EnvironmentNAV2D();
00251
00254 void PrintTimeStat(FILE* fOut);
00255
00258 bool IsWithinMapCell(int X, int Y);
00259
00260
00261
00262 private:
00263
00264
00265 EnvNAV2DConfig_t EnvNAV2DCfg;
00266 EnvironmentNAV2D_t EnvNAV2D;
00267
00268
00269 void ReadConfiguration(FILE* fCfg);
00270
00271 void InitializeEnvConfig();
00272
00273 unsigned int GETHASHBIN(unsigned int X, unsigned int Y);
00274
00275 void PrintHashTableHist();
00276
00277
00278 EnvNAV2DHashEntry_t* GetHashEntry(int X, int Y);
00279
00280 EnvNAV2DHashEntry_t* CreateNewHashEntry(int X, int Y);
00281
00282
00283 void CreateStartandGoalStates();
00284
00285 void InitializeEnvironment();
00286
00287 void ComputeHeuristicValues();
00288
00289 bool IsValidCell(int X, int Y);
00290
00291 void Computedxy();
00292
00293
00294 };
00295
00296 #endif
00297