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_NAVXYTHETALAT_H_
00030 #define __ENVIRONMENT_NAVXYTHETALAT_H_
00031
00032
00033
00034 #define NAVXYTHETALAT_DXYWIDTH 8
00035
00036 #define ENVNAVXYTHETALAT_DEFAULTOBSTHRESH 254 //see explanation of the value below
00037
00038 #define SBPL_XYTHETALAT_MAXSTATESFORLOOKUP 100000000 //maximum number of states for storing them into lookup (as opposed to hash)
00039
00040
00041
00042
00043
00044 #define NAVXYTHETALAT_THETADIRS 16
00045
00046
00047 #define NAVXYTHETALAT_DEFAULT_ACTIONWIDTH 5 //decrease, increase, same angle while moving plus decrease, increase angle while standing.
00048
00049 #define NAVXYTHETALAT_COSTMULT_MTOMM 1000
00050
00051 typedef struct{
00052 double x;
00053 double y;
00054 } EnvNAVXYTHETALAT2Dpt_t;
00055
00056 typedef struct{
00057 double x;
00058 double y;
00059 double theta;
00060 } EnvNAVXYTHETALAT3Dpt_t;
00061
00062
00063 typedef struct EnvNAVXYTHETALAT3DCELL{
00064 int x;
00065 int y;
00066 int theta;
00067 int iteration;
00068 public:
00069 bool operator == (EnvNAVXYTHETALAT3DCELL cell) {return (x==cell.x && y==cell.y && theta==cell.theta);}
00070 } EnvNAVXYTHETALAT3Dcell_t;
00071
00072
00073 typedef struct
00074 {
00075 unsigned char aind;
00076 char starttheta;
00077 char dX;
00078 char dY;
00079 char endtheta;
00080 unsigned int cost;
00081 vector<sbpl_2Dcell_t> intersectingcellsV;
00082
00083 vector<EnvNAVXYTHETALAT3Dpt_t> intermptV;
00084
00085 vector<EnvNAVXYTHETALAT3Dcell_t> interm3DcellsV;
00086 } EnvNAVXYTHETALATAction_t;
00087
00088
00089 typedef struct
00090 {
00091 int stateID;
00092 int X;
00093 int Y;
00094 char Theta;
00095 int iteration;
00096 } EnvNAVXYTHETALATHashEntry_t;
00097
00098
00099 typedef struct
00100 {
00101 int motprimID;
00102 unsigned char starttheta_c;
00103 int additionalactioncostmult;
00104 EnvNAVXYTHETALAT3Dcell_t endcell;
00105
00106 vector<EnvNAVXYTHETALAT3Dpt_t> intermptV;
00107 }SBPL_xytheta_mprimitive;
00108
00109
00110
00111 typedef struct
00112 {
00113
00114 int startstateid;
00115 int goalstateid;
00116
00117 bool bInitialized;
00118
00119
00120
00121
00122 }EnvironmentNAVXYTHETALAT_t;
00123
00124
00125 typedef struct ENV_NAVXYTHETALAT_CONFIG
00126 {
00127 int EnvWidth_c;
00128 int EnvHeight_c;
00129 int StartX_c;
00130 int StartY_c;
00131 int StartTheta;
00132 int EndX_c;
00133 int EndY_c;
00134 int EndTheta;
00135 unsigned char** Grid2D;
00136
00137
00138
00139 unsigned char obsthresh;
00140
00141
00142
00143
00144
00145 unsigned char cost_inscribed_thresh;
00146
00147
00148
00149
00150
00151 int cost_possibly_circumscribed_thresh;
00152
00153 double nominalvel_mpersecs;
00154 double timetoturn45degsinplace_secs;
00155 double cellsize_m;
00156
00157 int dXY[NAVXYTHETALAT_DXYWIDTH][2];
00158
00159 EnvNAVXYTHETALATAction_t** ActionsV;
00160 vector<EnvNAVXYTHETALATAction_t*>* PredActionsV;
00161
00162 int actionwidth;
00163 vector<SBPL_xytheta_mprimitive> mprimV;
00164
00165 vector<sbpl_2Dpt_t> FootprintPolygon;
00166 } EnvNAVXYTHETALATConfig_t;
00167
00168
00169
00170 class SBPL2DGridSearch;
00171
00176 class EnvironmentNAVXYTHETALATTICE : public DiscreteSpaceInformation
00177 {
00178
00179 public:
00180
00181 EnvironmentNAVXYTHETALATTICE();
00182
00190 bool InitializeEnv(const char* sEnvFile, const vector<sbpl_2Dpt_t>& perimeterptsV, const char* sMotPrimFile);
00194 bool InitializeEnv(const char* sEnvFile);
00198 virtual bool SetEnvParameter(const char* parameter, int value);
00202 virtual int GetEnvParameter(const char* parameter);
00206 bool InitializeMDPCfg(MDPConfig *MDPCfg);
00210 virtual int GetFromToHeuristic(int FromStateID, int ToStateID) = 0;
00214 virtual int GetGoalHeuristic(int stateID) = 0;
00218 virtual int GetStartHeuristic(int stateID) = 0;
00222 virtual void SetAllActionsandAllOutcomes(CMDPSTATE* state) = 0;
00226 virtual void SetAllPreds(CMDPSTATE* state);
00230 virtual void GetSuccs(int SourceStateID, vector<int>* SuccIDV, vector<int>* CostV);
00234 virtual void GetPreds(int TargetStateID, vector<int>* PredIDV, vector<int>* CostV) = 0;
00235
00239 virtual void EnsureHeuristicsUpdated(bool bGoalHeuristics);
00240
00244 void PrintEnv_Config(FILE* fOut);
00245
00263 bool InitializeEnv(int width, int height,
00265 const unsigned char* mapdata,
00266 double startx, double starty, double starttheta,
00267 double goalx, double goaly, double goaltheta,
00268 double goaltol_x, double goaltol_y, double goaltol_theta,
00269 const vector<sbpl_2Dpt_t> & perimeterptsV,
00270 double cellsize_m, double nominalvel_mpersecs, double timetoturn45degsinplace_secs,
00271 unsigned char obsthresh, const char* sMotPrimFile);
00275 bool UpdateCost(int x, int y, unsigned char newcost);
00276
00281 bool SetMap(const unsigned char* mapdata);
00282
00283
00289 virtual void GetPredsofChangedEdges(vector<nav2dcell_t> const * changedcellsV, vector<int> *preds_of_changededgesIDV) = 0;
00293 virtual void GetSuccsofChangedEdges(vector<nav2dcell_t> const * changedcellsV, vector<int> *succs_of_changededgesIDV) = 0;
00294
00298 bool IsObstacle(int x, int y);
00303 bool IsValidConfiguration(int X, int Y, int Theta);
00304
00308 void GetEnvParms(int *size_x, int *size_y, double* startx, double* starty, double* starttheta, double* goalx, double* goaly, double* goaltheta,
00309 double* cellsize_m, double* nominalvel_mpersecs, double* timetoturn45degsinplace_secs, unsigned char* obsthresh, vector<SBPL_xytheta_mprimitive>* motionprimitiveV);
00313 const EnvNAVXYTHETALATConfig_t* GetEnvNavConfig();
00314
00315
00316 virtual ~EnvironmentNAVXYTHETALATTICE();
00320 void PrintTimeStat(FILE* fOut);
00324 unsigned char GetMapCost(int x, int y);
00325
00329 bool IsWithinMapCell(int X, int Y);
00330
00343 bool PoseContToDisc(double px, double py, double pth,
00344 int &ix, int &iy, int &ith) const;
00345
00355 bool PoseDiscToCont(int ix, int iy, int ith,
00356 double &px, double &py, double &pth) const;
00357
00360 virtual void PrintVars(){};
00361
00362 protected:
00363
00364 virtual int GetActionCost(int SourceX, int SourceY, int SourceTheta, EnvNAVXYTHETALATAction_t* action);
00365
00366
00367
00368 EnvNAVXYTHETALATConfig_t EnvNAVXYTHETALATCfg;
00369 EnvironmentNAVXYTHETALAT_t EnvNAVXYTHETALAT;
00370 vector<EnvNAVXYTHETALAT3Dcell_t> affectedsuccstatesV;
00371 vector<EnvNAVXYTHETALAT3Dcell_t> affectedpredstatesV;
00372 int iteration;
00373
00374
00375 bool bNeedtoRecomputeStartHeuristics;
00376 bool bNeedtoRecomputeGoalHeuristics;
00377 SBPL2DGridSearch* grid2Dsearchfromstart;
00378 SBPL2DGridSearch* grid2Dsearchfromgoal;
00379
00380 virtual void ReadConfiguration(FILE* fCfg);
00381
00382 void InitializeEnvConfig(vector<SBPL_xytheta_mprimitive>* motionprimitiveV);
00383
00384
00385 bool CheckQuant(FILE* fOut);
00386
00387 void SetConfiguration(int width, int height,
00389 const unsigned char* mapdata,
00390 int startx, int starty, int starttheta,
00391 int goalx, int goaly, int goaltheta,
00392 double cellsize_m, double nominalvel_mpersecs, double timetoturn45degsinplace_secs, const vector<sbpl_2Dpt_t> & robot_perimeterV);
00393
00394 bool InitGeneral( vector<SBPL_xytheta_mprimitive>* motionprimitiveV);
00395 void PrecomputeActionswithBaseMotionPrimitive(vector<SBPL_xytheta_mprimitive>* motionprimitiveV);
00396 void PrecomputeActionswithCompleteMotionPrimitive(vector<SBPL_xytheta_mprimitive>* motionprimitiveV);
00397 void PrecomputeActions();
00398
00399 void CreateStartandGoalStates();
00400
00401 virtual void InitializeEnvironment() = 0;
00402
00403 void ComputeHeuristicValues();
00404
00405 virtual bool IsValidCell(int X, int Y);
00406
00407 void CalculateFootprintForPose(EnvNAVXYTHETALAT3Dpt_t pose, vector<sbpl_2Dcell_t>* footprint);
00408 void CalculateFootprintForPose(EnvNAVXYTHETALAT3Dpt_t pose, vector<sbpl_2Dcell_t>* footprint, const vector<sbpl_2Dpt_t>& FootprintPolygon);
00409 void RemoveSourceFootprint(EnvNAVXYTHETALAT3Dpt_t sourcepose, vector<sbpl_2Dcell_t>* footprint);
00410 void RemoveSourceFootprint(EnvNAVXYTHETALAT3Dpt_t sourcepose, vector<sbpl_2Dcell_t>* footprint, const vector<sbpl_2Dpt_t>& FootprintPolygon);
00411
00412 virtual void GetSuccs(int SourceStateID, vector<int>* SuccIDV, vector<int>* CostV, vector<EnvNAVXYTHETALATAction_t*>* actionindV=NULL) = 0;
00413
00414 double EuclideanDistance_m(int X1, int Y1, int X2, int Y2);
00415
00416 void ComputeReplanningData();
00417 void ComputeReplanningDataforAction(EnvNAVXYTHETALATAction_t* action);
00418
00419 bool ReadMotionPrimitives(FILE* fMotPrims);
00420 bool ReadinMotionPrimitive(SBPL_xytheta_mprimitive* pMotPrim, FILE* fIn);
00421 bool ReadinCell(EnvNAVXYTHETALAT3Dcell_t* cell, FILE* fIn);
00422 bool ReadinPose(EnvNAVXYTHETALAT3Dpt_t* pose, FILE* fIn);
00423
00424 void PrintHeuristicValues();
00425
00426 };
00427
00428
00429 class EnvironmentNAVXYTHETALAT : public EnvironmentNAVXYTHETALATTICE
00430 {
00431
00432 public:
00433 EnvironmentNAVXYTHETALAT()
00434 {
00435 HashTableSize = 0;
00436 Coord2StateIDHashTable = NULL;
00437 Coord2StateIDHashTable_lookup = NULL;
00438 };
00439
00440 ~EnvironmentNAVXYTHETALAT();
00441
00444 int SetStart(double x, double y, double theta);
00447 int SetGoal(double x, double y, double theta);
00450 void SetGoalTolerance(double tol_x, double tol_y, double tol_theta) { }
00453 void GetCoordFromState(int stateID, int& x, int& y, int& theta) const;
00456 int GetStateFromCoord(int x, int y, int theta);
00457
00461 void ConvertStateIDPathintoXYThetaPath(vector<int>* stateIDPath, vector<EnvNAVXYTHETALAT3Dpt_t>* xythetaPath);
00464 void PrintState(int stateID, bool bVerbose, FILE* fOut=NULL);
00467 virtual void GetPreds(int TargetStateID, vector<int>* PredIDV, vector<int>* CostV);
00471 virtual void GetSuccs(int SourceStateID, vector<int>* SuccIDV, vector<int>* CostV, vector<EnvNAVXYTHETALATAction_t*>* actionindV=NULL);
00472
00477 void GetPredsofChangedEdges(vector<nav2dcell_t> const * changedcellsV, vector<int> *preds_of_changededgesIDV);
00480 void GetSuccsofChangedEdges(vector<nav2dcell_t> const * changedcellsV, vector<int> *succs_of_changededgesIDV);
00481
00484 virtual void SetAllActionsandAllOutcomes(CMDPSTATE* state);
00485
00488 virtual int GetFromToHeuristic(int FromStateID, int ToStateID);
00491 virtual int GetGoalHeuristic(int stateID);
00494 virtual int GetStartHeuristic(int stateID);
00495
00498 virtual int SizeofCreatedEnv();
00501 virtual void PrintVars(){};
00502
00503 protected:
00504
00505
00506 int HashTableSize;
00507 vector<EnvNAVXYTHETALATHashEntry_t*>* Coord2StateIDHashTable;
00508
00509 vector<EnvNAVXYTHETALATHashEntry_t*> StateID2CoordTable;
00510
00511 EnvNAVXYTHETALATHashEntry_t** Coord2StateIDHashTable_lookup;
00512
00513 unsigned int GETHASHBIN(unsigned int X, unsigned int Y, unsigned int Theta);
00514
00515 EnvNAVXYTHETALATHashEntry_t* GetHashEntry_hash(int X, int Y, int Theta);
00516 EnvNAVXYTHETALATHashEntry_t* CreateNewHashEntry_hash(int X, int Y, int Theta);
00517 EnvNAVXYTHETALATHashEntry_t* GetHashEntry_lookup(int X, int Y, int Theta);
00518 EnvNAVXYTHETALATHashEntry_t* CreateNewHashEntry_lookup(int X, int Y, int Theta);
00519
00520
00521 EnvNAVXYTHETALATHashEntry_t* (EnvironmentNAVXYTHETALAT::*GetHashEntry)(int X, int Y, int Theta);
00522 EnvNAVXYTHETALATHashEntry_t* (EnvironmentNAVXYTHETALAT::*CreateNewHashEntry)(int X, int Y, int Theta);
00523
00524
00525 virtual void InitializeEnvironment();
00526
00527 void PrintHashTableHist(FILE* fOut);
00528
00529
00530 };
00531
00532
00533 #endif
00534