Go to the documentation of this file.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_NAV2DUU_H_
00030 #define __ENVIRONMENT_NAV2DUU_H_
00031
00032 #define ENVNAV2DUU_COSTMULT 1000
00033
00034 #define NAV2DUU_MAXACTIONSWIDTH 9
00035
00036 #define ENVNAV2DUU_MAXDIRS 8
00037
00038 typedef struct ENV_NAV2DUU_CONFIG
00039 {
00040
00041 int EnvWidth_c;
00042 int EnvHeight_c;
00043 int StartX_c;
00044 int StartY_c;
00045 int EndX_c;
00046 int EndY_c;
00047
00048 unsigned char** Grid2D;
00049
00050
00051 unsigned char obsthresh;
00052
00053 float** UncertaintyGrid2D;
00054
00055 int** HiddenVariableXY2ID;
00056
00057
00058
00059
00060 int dx_[ENVNAV2DUU_MAXDIRS];
00061 int dy_[ENVNAV2DUU_MAXDIRS];
00062
00063 int dxy_distance_mm_[ENVNAV2DUU_MAXDIRS];
00064
00065 int dxintersects_[ENVNAV2D_MAXDIRS][2];
00066 int dyintersects_[ENVNAV2D_MAXDIRS][2];
00067 int numofdirs;
00068
00069
00070 int sizeofS;
00071 int sizeofH;
00072
00073 } EnvNAV2DUUConfig_t;
00074
00075 #define NAVNAV2DUU_MAXWIDTHHEIGH 1024
00076 #define ENVNAV2DUU_STATEIDTOY(stateID) (stateID%NAVNAV2DUU_MAXWIDTHHEIGH)
00077 #define ENVNAV2DUU_STATEIDTOX(stateID) (stateID/NAVNAV2DUU_MAXWIDTHHEIGH)
00078 #define ENVNAV2DUU_XYTOSTATEID(X, Y) (X*NAVNAV2DUU_MAXWIDTHHEIGH + Y)
00079
00080
00081 typedef struct
00082 {
00083
00084 int startstateid;
00085 int goalstateid;
00086
00087
00088
00089 bool bInitialized;
00090
00091 }EnvironmentNAV2DUU_t;
00092
00093
00096 class EnvironmentNAV2DUU : public DiscreteSpaceInformation
00097 {
00098
00099 public:
00100
00103 bool InitializeEnv(const char* sEnvFile);
00104
00116 bool InitializeEnv(int width, int height,
00117 const unsigned char* mapdata, const float* uncertaintymapdata, unsigned char obsthresh);
00120 int SetStart(int x, int y);
00123 int SetGoal(int x, int y);
00126 bool UpdateCost(int x, int y, unsigned char newcost);
00127
00128
00131 bool InitializeMDPCfg(MDPConfig *MDPCfg);
00134 int GetFromToHeuristic(int FromStateID, int ToStateID);
00137 int GetGoalHeuristic(int stateID);
00140 int GetStartHeuristic(int stateID);
00141
00144 void PrintState(int stateID, bool bVerbose, FILE* fOut=NULL);
00147 void PrintEnv_Config(FILE* fOut);
00148
00149
00150 EnvironmentNAV2DUU();
00151 ~EnvironmentNAV2DUU(){};
00152
00155 void GetPreds(int stateID, const vector<sbpl_BinaryHiddenVar_t>* updatedhvaluesV, vector<CMDPACTION>* IncomingDetActionV,
00156 vector<CMDPACTION>* IncomingStochActionV, vector<sbpl_BinaryHiddenVar_t>* StochActionNonpreferredOutcomeV);
00157
00158
00161 void SetAllActionsandAllOutcomes(CMDPSTATE* state){
00162 SBPL_ERROR("ERROR: SetAllActionsandAllOutcomes not supported in NAV2D UNDER UNCERTAINTY\n");
00163 throw new SBPL_Exception();
00164 };
00167 void SetAllPreds(CMDPSTATE* state){
00168 SBPL_ERROR("ERROR: SetAllPreds not supported in NAV2D UNDER UNCERTAINTY\n");
00169 throw new SBPL_Exception();
00170 };
00173 void GetSuccs(int SourceStateID, vector<int>* SuccIDV, vector<int>* CostV){
00174 SBPL_ERROR("ERROR: GetSuccs not supported in NAV2D UNDER UNCERTAINTY\n");
00175 throw new SBPL_Exception();
00176 };
00179 void GetPreds(int TargetStateID, vector<int>* PredIDV, vector<int>* CostV){
00180 SBPL_ERROR("ERROR: GetPreds not supported in NAV2D UNDER UNCERTAINTY\n");
00181 throw new SBPL_Exception();
00182 };
00183
00186 int SizeofCreatedEnv();
00189 int SizeofH();
00190
00191
00192
00193 private:
00194
00195
00196 EnvNAV2DUUConfig_t EnvNAV2DUUCfg;
00197 EnvironmentNAV2DUU_t EnvNAV2DUU;
00198
00199
00200
00201 void SetConfiguration(int width, int height, const unsigned char* mapdata, const float* uncertaintymapdata);
00202
00203 void ReadConfiguration(FILE* fCfg);
00204 void InitializeEnvConfig();
00205 void InitializeEnvironment();
00206 void ComputeHeuristicValues();
00207 bool InitGeneral();
00208
00209 bool IsValidRobotPosition(int X, int Y);
00210 bool IsWithinMapCell(int X, int Y);
00211
00212 void Computedxy();
00213
00214
00215 };
00216
00217
00218 #endif
00219