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 #include <iostream>
00030 using namespace std;
00031
00032 #include "../../sbpl/headers.h"
00033
00034
00035 #if TIME_DEBUG
00036 static clock_t time3_addallout = 0;
00037 static clock_t time_gethash = 0;
00038 static clock_t time_createhash = 0;
00039 static clock_t time_getsuccs = 0;
00040 #endif
00041
00042 static long int checks = 0;
00043
00044 #define XYTHETA2INDEX(X,Y,THETA) (THETA + X*NAVXYTHETALAT_THETADIRS + Y*EnvNAVXYTHETALATCfg.EnvWidth_c*NAVXYTHETALAT_THETADIRS)
00045
00046
00047 EnvironmentNAVXYTHETALATTICE::EnvironmentNAVXYTHETALATTICE()
00048 {
00049 EnvNAVXYTHETALATCfg.obsthresh = ENVNAVXYTHETALAT_DEFAULTOBSTHRESH;
00050 EnvNAVXYTHETALATCfg.cost_inscribed_thresh = EnvNAVXYTHETALATCfg.obsthresh;
00051 EnvNAVXYTHETALATCfg.cost_possibly_circumscribed_thresh = -1;
00052
00053 grid2Dsearchfromstart = NULL;
00054 grid2Dsearchfromgoal = NULL;
00055 bNeedtoRecomputeStartHeuristics = true;
00056 bNeedtoRecomputeGoalHeuristics = true;
00057 iteration = 0;
00058
00059 EnvNAVXYTHETALAT.bInitialized = false;
00060
00061 EnvNAVXYTHETALATCfg.actionwidth = NAVXYTHETALAT_DEFAULT_ACTIONWIDTH;
00062
00063
00064 EnvNAVXYTHETALATCfg.Grid2D = NULL;
00065 EnvNAVXYTHETALATCfg.ActionsV = NULL;
00066 EnvNAVXYTHETALATCfg.PredActionsV = NULL;
00067
00068
00069 }
00070
00071 EnvironmentNAVXYTHETALATTICE::~EnvironmentNAVXYTHETALATTICE()
00072 {
00073 SBPL_PRINTF("destroying XYTHETALATTICE\n");
00074 if(grid2Dsearchfromstart != NULL)
00075 delete grid2Dsearchfromstart;
00076 grid2Dsearchfromstart = NULL;
00077
00078 if(grid2Dsearchfromgoal != NULL)
00079 delete grid2Dsearchfromgoal;
00080 grid2Dsearchfromgoal = NULL;
00081
00082 if(EnvNAVXYTHETALATCfg.Grid2D != NULL)
00083 {
00084 for (int x = 0; x < EnvNAVXYTHETALATCfg.EnvWidth_c; x++)
00085 delete [] EnvNAVXYTHETALATCfg.Grid2D[x];
00086 delete [] EnvNAVXYTHETALATCfg.Grid2D;
00087 EnvNAVXYTHETALATCfg.Grid2D = NULL;
00088 }
00089
00090
00091 if(EnvNAVXYTHETALATCfg.ActionsV != NULL)
00092 {
00093 for(int tind = 0; tind < NAVXYTHETALAT_THETADIRS; tind++)
00094 delete [] EnvNAVXYTHETALATCfg.ActionsV[tind];
00095 delete [] EnvNAVXYTHETALATCfg.ActionsV;
00096 EnvNAVXYTHETALATCfg.ActionsV = NULL;
00097 }
00098 if(EnvNAVXYTHETALATCfg.PredActionsV != NULL)
00099 {
00100 delete [] EnvNAVXYTHETALATCfg.PredActionsV;
00101 EnvNAVXYTHETALATCfg.PredActionsV = NULL;
00102 }
00103 }
00104
00105
00106
00107
00108
00109
00110
00111 static unsigned int inthash(unsigned int key)
00112 {
00113 key += (key << 12);
00114 key ^= (key >> 22);
00115 key += (key << 4);
00116 key ^= (key >> 9);
00117 key += (key << 10);
00118 key ^= (key >> 2);
00119 key += (key << 7);
00120 key ^= (key >> 12);
00121 return key;
00122 }
00123
00124
00125
00126 void EnvironmentNAVXYTHETALATTICE::SetConfiguration(int width, int height,
00127 const unsigned char* mapdata,
00128 int startx, int starty, int starttheta,
00129 int goalx, int goaly, int goaltheta,
00130 double cellsize_m, double nominalvel_mpersecs, double timetoturn45degsinplace_secs, const vector<sbpl_2Dpt_t> & robot_perimeterV) {
00131 EnvNAVXYTHETALATCfg.EnvWidth_c = width;
00132 EnvNAVXYTHETALATCfg.EnvHeight_c = height;
00133 EnvNAVXYTHETALATCfg.StartX_c = startx;
00134 EnvNAVXYTHETALATCfg.StartY_c = starty;
00135 EnvNAVXYTHETALATCfg.StartTheta = starttheta;
00136
00137 if(EnvNAVXYTHETALATCfg.StartX_c < 0 || EnvNAVXYTHETALATCfg.StartX_c >= EnvNAVXYTHETALATCfg.EnvWidth_c) {
00138 SBPL_ERROR("ERROR: illegal start coordinates\n");
00139 throw new SBPL_Exception();
00140 }
00141 if(EnvNAVXYTHETALATCfg.StartY_c < 0 || EnvNAVXYTHETALATCfg.StartY_c >= EnvNAVXYTHETALATCfg.EnvHeight_c) {
00142 SBPL_ERROR("ERROR: illegal start coordinates\n");
00143 throw new SBPL_Exception();
00144 }
00145 if(EnvNAVXYTHETALATCfg.StartTheta < 0 || EnvNAVXYTHETALATCfg.StartTheta >= NAVXYTHETALAT_THETADIRS) {
00146 SBPL_ERROR("ERROR: illegal start coordinates for theta\n");
00147 throw new SBPL_Exception();
00148 }
00149
00150 EnvNAVXYTHETALATCfg.EndX_c = goalx;
00151 EnvNAVXYTHETALATCfg.EndY_c = goaly;
00152 EnvNAVXYTHETALATCfg.EndTheta = goaltheta;
00153
00154 if(EnvNAVXYTHETALATCfg.EndX_c < 0 || EnvNAVXYTHETALATCfg.EndX_c >= EnvNAVXYTHETALATCfg.EnvWidth_c) {
00155 SBPL_ERROR("ERROR: illegal goal coordinates\n");
00156 throw new SBPL_Exception();
00157 }
00158 if(EnvNAVXYTHETALATCfg.EndY_c < 0 || EnvNAVXYTHETALATCfg.EndY_c >= EnvNAVXYTHETALATCfg.EnvHeight_c) {
00159 SBPL_ERROR("ERROR: illegal goal coordinates\n");
00160 throw new SBPL_Exception();
00161 }
00162 if(EnvNAVXYTHETALATCfg.EndTheta < 0 || EnvNAVXYTHETALATCfg.EndTheta >= NAVXYTHETALAT_THETADIRS) {
00163 SBPL_ERROR("ERROR: illegal goal coordinates for theta\n");
00164 throw new SBPL_Exception();
00165 }
00166
00167 EnvNAVXYTHETALATCfg.FootprintPolygon = robot_perimeterV;
00168
00169 EnvNAVXYTHETALATCfg.nominalvel_mpersecs = nominalvel_mpersecs;
00170 EnvNAVXYTHETALATCfg.cellsize_m = cellsize_m;
00171 EnvNAVXYTHETALATCfg.timetoturn45degsinplace_secs = timetoturn45degsinplace_secs;
00172
00173
00174
00175 EnvNAVXYTHETALATCfg.Grid2D = new unsigned char* [EnvNAVXYTHETALATCfg.EnvWidth_c];
00176 for (int x = 0; x < EnvNAVXYTHETALATCfg.EnvWidth_c; x++) {
00177 EnvNAVXYTHETALATCfg.Grid2D[x] = new unsigned char [EnvNAVXYTHETALATCfg.EnvHeight_c];
00178 }
00179
00180
00181 if (0 == mapdata) {
00182 for (int y = 0; y < EnvNAVXYTHETALATCfg.EnvHeight_c; y++) {
00183 for (int x = 0; x < EnvNAVXYTHETALATCfg.EnvWidth_c; x++) {
00184 EnvNAVXYTHETALATCfg.Grid2D[x][y] = 0;
00185 }
00186 }
00187 }
00188 else {
00189 for (int y = 0; y < EnvNAVXYTHETALATCfg.EnvHeight_c; y++) {
00190 for (int x = 0; x < EnvNAVXYTHETALATCfg.EnvWidth_c; x++) {
00191 EnvNAVXYTHETALATCfg.Grid2D[x][y] = mapdata[x+y*width];
00192 }
00193 }
00194 }
00195 }
00196
00197 void EnvironmentNAVXYTHETALATTICE::ReadConfiguration(FILE* fCfg)
00198 {
00199
00200 char sTemp[1024], sTemp1[1024];
00201 int dTemp;
00202 int x, y;
00203
00204
00205 if(fscanf(fCfg, "%s", sTemp) != 1){
00206 SBPL_ERROR("ERROR: ran out of env file early\n");
00207 throw new SBPL_Exception();
00208 }
00209 strcpy(sTemp1, "discretization(cells):");
00210 if(strcmp(sTemp1, sTemp) != 0)
00211 {
00212 SBPL_ERROR("ERROR: configuration file has incorrect format\n");
00213 SBPL_PRINTF("Expected %s got %s\n", sTemp1, sTemp);
00214 throw new SBPL_Exception();
00215 }
00216 if(fscanf(fCfg, "%s", sTemp) != 1){
00217 SBPL_ERROR("ERROR: ran out of env file early\n");
00218 throw new SBPL_Exception();
00219 }
00220 EnvNAVXYTHETALATCfg.EnvWidth_c = atoi(sTemp);
00221 if(fscanf(fCfg, "%s", sTemp) != 1){
00222 SBPL_ERROR("ERROR: ran out of env file early\n");
00223 throw new SBPL_Exception();
00224 }
00225 EnvNAVXYTHETALATCfg.EnvHeight_c = atoi(sTemp);
00226
00227
00228 if(fscanf(fCfg, "%s", sTemp) != 1){
00229 SBPL_ERROR("ERROR: ran out of env file early\n");
00230 throw new SBPL_Exception();
00231 }
00232 strcpy(sTemp1, "obsthresh:");
00233 if(strcmp(sTemp1, sTemp) != 0)
00234 {
00235 SBPL_ERROR("ERROR: configuration file has incorrect format\n");
00236 SBPL_PRINTF("Expected %s got %s\n", sTemp1, sTemp);
00237 SBPL_PRINTF("see existing examples of env files for the right format of heading\n");
00238 throw new SBPL_Exception();
00239 }
00240 if(fscanf(fCfg, "%s", sTemp) != 1){
00241 SBPL_ERROR("ERROR: ran out of env file early\n");
00242 throw new SBPL_Exception();
00243 }
00244 EnvNAVXYTHETALATCfg.obsthresh = atoi(sTemp);
00245 SBPL_PRINTF("obsthresh = %d\n", EnvNAVXYTHETALATCfg.obsthresh);
00246
00247
00248 if(fscanf(fCfg, "%s", sTemp) != 1){
00249 SBPL_ERROR("ERROR: ran out of env file early\n");
00250 throw new SBPL_Exception();
00251 }
00252 strcpy(sTemp1, "cost_inscribed_thresh:");
00253 if(strcmp(sTemp1, sTemp) != 0)
00254 {
00255 SBPL_ERROR("ERROR: configuration file has incorrect format\n");
00256 SBPL_PRINTF("Expected %s got %s\n", sTemp1, sTemp);
00257 SBPL_PRINTF("see existing examples of env files for the right format of heading\n");
00258 throw new SBPL_Exception();
00259 }
00260 if(fscanf(fCfg, "%s", sTemp) != 1){
00261 SBPL_ERROR("ERROR: ran out of env file early\n");
00262 throw new SBPL_Exception();
00263 }
00264 EnvNAVXYTHETALATCfg.cost_inscribed_thresh = atoi(sTemp);
00265 SBPL_PRINTF("cost_inscribed_thresh = %d\n", EnvNAVXYTHETALATCfg.cost_inscribed_thresh);
00266
00267
00268
00269 if(fscanf(fCfg, "%s", sTemp) != 1){
00270 SBPL_ERROR("ERROR: ran out of env file early\n");
00271 throw new SBPL_Exception();
00272 }
00273 strcpy(sTemp1, "cost_possibly_circumscribed_thresh:");
00274 if(strcmp(sTemp1, sTemp) != 0)
00275 {
00276 SBPL_ERROR("ERROR: configuration file has incorrect format\n");
00277 SBPL_PRINTF("Expected %s got %s\n", sTemp1, sTemp);
00278 SBPL_PRINTF("see existing examples of env files for the right format of heading\n");
00279 throw new SBPL_Exception();
00280 }
00281 if(fscanf(fCfg, "%s", sTemp) != 1){
00282 SBPL_ERROR("ERROR: ran out of env file early\n");
00283 throw new SBPL_Exception();
00284 }
00285 EnvNAVXYTHETALATCfg.cost_possibly_circumscribed_thresh = atoi(sTemp);
00286 SBPL_PRINTF("cost_possibly_circumscribed_thresh = %d\n", EnvNAVXYTHETALATCfg.cost_possibly_circumscribed_thresh);
00287
00288
00289
00290 if(fscanf(fCfg, "%s", sTemp) != 1){
00291 SBPL_ERROR("ERROR: ran out of env file early\n");
00292 throw new SBPL_Exception();
00293 }
00294 strcpy(sTemp1, "cellsize(meters):");
00295 if(strcmp(sTemp1, sTemp) != 0)
00296 {
00297 SBPL_ERROR("ERROR: configuration file has incorrect format\n");
00298 SBPL_PRINTF("Expected %s got %s\n", sTemp1, sTemp);
00299 throw new SBPL_Exception();
00300 }
00301 if(fscanf(fCfg, "%s", sTemp) != 1){
00302 SBPL_ERROR("ERROR: ran out of env file early\n");
00303 throw new SBPL_Exception();
00304 }
00305 EnvNAVXYTHETALATCfg.cellsize_m = atof(sTemp);
00306
00307
00308 if(fscanf(fCfg, "%s", sTemp) != 1){
00309 SBPL_ERROR("ERROR: ran out of env file early\n");
00310 throw new SBPL_Exception();
00311 }
00312 strcpy(sTemp1, "nominalvel(mpersecs):");
00313 if(strcmp(sTemp1, sTemp) != 0)
00314 {
00315 SBPL_ERROR("ERROR: configuration file has incorrect format\n");
00316 SBPL_PRINTF("Expected %s got %s\n", sTemp1, sTemp);
00317 throw new SBPL_Exception();
00318 }
00319 if(fscanf(fCfg, "%s", sTemp) != 1){
00320 SBPL_ERROR("ERROR: ran out of env file early\n");
00321 throw new SBPL_Exception();
00322 }
00323 EnvNAVXYTHETALATCfg.nominalvel_mpersecs = atof(sTemp);
00324 if(fscanf(fCfg, "%s", sTemp) != 1){
00325 SBPL_ERROR("ERROR: ran out of env file early\n");
00326 throw new SBPL_Exception();
00327 }
00328 strcpy(sTemp1, "timetoturn45degsinplace(secs):");
00329 if(strcmp(sTemp1, sTemp) != 0)
00330 {
00331 SBPL_ERROR("ERROR: configuration file has incorrect format\n");
00332 SBPL_PRINTF("Expected %s got %s\n", sTemp1, sTemp);
00333 throw new SBPL_Exception();
00334 }
00335 if(fscanf(fCfg, "%s", sTemp) != 1){
00336 SBPL_ERROR("ERROR: ran out of env file early\n");
00337 throw new SBPL_Exception();
00338 }
00339 EnvNAVXYTHETALATCfg.timetoturn45degsinplace_secs = atof(sTemp);
00340
00341
00342
00343 if(fscanf(fCfg, "%s", sTemp) != 1){
00344 SBPL_ERROR("ERROR: ran out of env file early\n");
00345 throw new SBPL_Exception();
00346 }
00347 if(fscanf(fCfg, "%s", sTemp) != 1){
00348 SBPL_ERROR("ERROR: ran out of env file early\n");
00349 throw new SBPL_Exception();
00350 }
00351 EnvNAVXYTHETALATCfg.StartX_c = CONTXY2DISC(atof(sTemp),EnvNAVXYTHETALATCfg.cellsize_m);
00352 if(fscanf(fCfg, "%s", sTemp) != 1){
00353 SBPL_ERROR("ERROR: ran out of env file early\n");
00354 throw new SBPL_Exception();
00355 }
00356 EnvNAVXYTHETALATCfg.StartY_c = CONTXY2DISC(atof(sTemp),EnvNAVXYTHETALATCfg.cellsize_m);
00357 if(fscanf(fCfg, "%s", sTemp) != 1){
00358 SBPL_ERROR("ERROR: ran out of env file early\n");
00359 throw new SBPL_Exception();
00360 }
00361 EnvNAVXYTHETALATCfg.StartTheta = ContTheta2Disc(atof(sTemp), NAVXYTHETALAT_THETADIRS);
00362
00363
00364 if(EnvNAVXYTHETALATCfg.StartX_c < 0 || EnvNAVXYTHETALATCfg.StartX_c >= EnvNAVXYTHETALATCfg.EnvWidth_c)
00365 {
00366 SBPL_ERROR("ERROR: illegal start coordinates\n");
00367 throw new SBPL_Exception();
00368 }
00369 if(EnvNAVXYTHETALATCfg.StartY_c < 0 || EnvNAVXYTHETALATCfg.StartY_c >= EnvNAVXYTHETALATCfg.EnvHeight_c)
00370 {
00371 SBPL_ERROR("ERROR: illegal start coordinates\n");
00372 throw new SBPL_Exception();
00373 }
00374 if(EnvNAVXYTHETALATCfg.StartTheta < 0 || EnvNAVXYTHETALATCfg.StartTheta >= NAVXYTHETALAT_THETADIRS) {
00375 SBPL_ERROR("ERROR: illegal start coordinates for theta\n");
00376 throw new SBPL_Exception();
00377 }
00378
00379
00380 if(fscanf(fCfg, "%s", sTemp) != 1){
00381 SBPL_ERROR("ERROR: ran out of env file early\n");
00382 throw new SBPL_Exception();
00383 }
00384 if(fscanf(fCfg, "%s", sTemp) != 1){
00385 SBPL_ERROR("ERROR: ran out of env file early\n");
00386 throw new SBPL_Exception();
00387 }
00388 EnvNAVXYTHETALATCfg.EndX_c = CONTXY2DISC(atof(sTemp),EnvNAVXYTHETALATCfg.cellsize_m);
00389 if(fscanf(fCfg, "%s", sTemp) != 1){
00390 SBPL_ERROR("ERROR: ran out of env file early\n");
00391 throw new SBPL_Exception();
00392 }
00393 EnvNAVXYTHETALATCfg.EndY_c = CONTXY2DISC(atof(sTemp),EnvNAVXYTHETALATCfg.cellsize_m);
00394 if(fscanf(fCfg, "%s", sTemp) != 1){
00395 SBPL_ERROR("ERROR: ran out of env file early\n");
00396 throw new SBPL_Exception();
00397 }
00398 EnvNAVXYTHETALATCfg.EndTheta = ContTheta2Disc(atof(sTemp), NAVXYTHETALAT_THETADIRS);;
00399
00400 if(EnvNAVXYTHETALATCfg.EndX_c < 0 || EnvNAVXYTHETALATCfg.EndX_c >= EnvNAVXYTHETALATCfg.EnvWidth_c)
00401 {
00402 SBPL_ERROR("ERROR: illegal end coordinates\n");
00403 throw new SBPL_Exception();
00404 }
00405 if(EnvNAVXYTHETALATCfg.EndY_c < 0 || EnvNAVXYTHETALATCfg.EndY_c >= EnvNAVXYTHETALATCfg.EnvHeight_c)
00406 {
00407 SBPL_ERROR("ERROR: illegal end coordinates\n");
00408 throw new SBPL_Exception();
00409 }
00410 if(EnvNAVXYTHETALATCfg.EndTheta < 0 || EnvNAVXYTHETALATCfg.EndTheta >= NAVXYTHETALAT_THETADIRS) {
00411 SBPL_ERROR("ERROR: illegal goal coordinates for theta\n");
00412 throw new SBPL_Exception();
00413 }
00414
00415
00416
00417 EnvNAVXYTHETALATCfg.Grid2D = new unsigned char* [EnvNAVXYTHETALATCfg.EnvWidth_c];
00418 for (x = 0; x < EnvNAVXYTHETALATCfg.EnvWidth_c; x++)
00419 {
00420 EnvNAVXYTHETALATCfg.Grid2D[x] = new unsigned char [EnvNAVXYTHETALATCfg.EnvHeight_c];
00421 }
00422
00423
00424 if(fscanf(fCfg, "%s", sTemp) != 1){
00425 SBPL_ERROR("ERROR: ran out of env file early\n");
00426 throw new SBPL_Exception();
00427 }
00428 for (y = 0; y < EnvNAVXYTHETALATCfg.EnvHeight_c; y++)
00429 for (x = 0; x < EnvNAVXYTHETALATCfg.EnvWidth_c; x++)
00430 {
00431 if(fscanf(fCfg, "%d", &dTemp) != 1)
00432 {
00433 SBPL_ERROR("ERROR: incorrect format of config file\n");
00434 throw new SBPL_Exception();
00435 }
00436 EnvNAVXYTHETALATCfg.Grid2D[x][y] = dTemp;
00437 }
00438
00439 }
00440
00441 bool EnvironmentNAVXYTHETALATTICE::ReadinCell(EnvNAVXYTHETALAT3Dcell_t* cell, FILE* fIn)
00442 {
00443 char sTemp[60];
00444
00445 if(fscanf(fIn, "%s", sTemp) == 0)
00446 return false;
00447 cell->x = atoi(sTemp);
00448 if(fscanf(fIn, "%s", sTemp) == 0)
00449 return false;
00450 cell->y = atoi(sTemp);
00451 if(fscanf(fIn, "%s", sTemp) == 0)
00452 return false;
00453 cell->theta = atoi(sTemp);
00454
00455
00456 cell->theta = NORMALIZEDISCTHETA(cell->theta, NAVXYTHETALAT_THETADIRS);
00457
00458 return true;
00459 }
00460
00461 bool EnvironmentNAVXYTHETALATTICE::ReadinPose(EnvNAVXYTHETALAT3Dpt_t* pose, FILE* fIn)
00462 {
00463 char sTemp[60];
00464
00465 if(fscanf(fIn, "%s", sTemp) == 0)
00466 return false;
00467 pose->x = atof(sTemp);
00468 if(fscanf(fIn, "%s", sTemp) == 0)
00469 return false;
00470 pose->y = atof(sTemp);
00471 if(fscanf(fIn, "%s", sTemp) == 0)
00472 return false;
00473 pose->theta = atof(sTemp);
00474
00475 pose->theta = normalizeAngle(pose->theta);
00476
00477 return true;
00478 }
00479
00480 bool EnvironmentNAVXYTHETALATTICE::ReadinMotionPrimitive(SBPL_xytheta_mprimitive* pMotPrim, FILE* fIn)
00481 {
00482 char sTemp[1024];
00483 int dTemp;
00484 char sExpected[1024];
00485 int numofIntermPoses;
00486
00487
00488 strcpy(sExpected, "primID:");
00489 if(fscanf(fIn, "%s", sTemp) == 0)
00490 return false;
00491 if(strcmp(sTemp, sExpected) != 0){
00492 SBPL_ERROR("ERROR: expected %s but got %s\n", sExpected, sTemp);
00493 return false;
00494 }
00495 if(fscanf(fIn, "%d", &pMotPrim->motprimID) != 1)
00496 return false;
00497
00498
00499 strcpy(sExpected, "startangle_c:");
00500 if(fscanf(fIn, "%s", sTemp) == 0)
00501 return false;
00502 if(strcmp(sTemp, sExpected) != 0){
00503 SBPL_ERROR("ERROR: expected %s but got %s\n", sExpected, sTemp);
00504 return false;
00505 }
00506 if(fscanf(fIn, "%d", &dTemp) == 0)
00507 {
00508 SBPL_ERROR("ERROR reading startangle\n");
00509 return false;
00510 }
00511 pMotPrim->starttheta_c = dTemp;
00512
00513
00514 strcpy(sExpected, "endpose_c:");
00515 if(fscanf(fIn, "%s", sTemp) == 0)
00516 return false;
00517 if(strcmp(sTemp, sExpected) != 0){
00518 SBPL_ERROR("ERROR: expected %s but got %s\n", sExpected, sTemp);
00519 return false;
00520 }
00521
00522 if(ReadinCell(&pMotPrim->endcell, fIn) == false){
00523 SBPL_ERROR("ERROR: failed to read in endsearchpose\n");
00524 return false;
00525 }
00526
00527
00528 strcpy(sExpected, "additionalactioncostmult:");
00529 if(fscanf(fIn, "%s", sTemp) == 0)
00530 return false;
00531 if(strcmp(sTemp, sExpected) != 0){
00532 SBPL_ERROR("ERROR: expected %s but got %s\n", sExpected, sTemp);
00533 return false;
00534 }
00535 if(fscanf(fIn, "%d", &dTemp) != 1)
00536 return false;
00537 pMotPrim->additionalactioncostmult = dTemp;
00538
00539
00540 strcpy(sExpected, "intermediateposes:");
00541 if(fscanf(fIn, "%s", sTemp) == 0)
00542 return false;
00543 if(strcmp(sTemp, sExpected) != 0){
00544 SBPL_ERROR("ERROR: expected %s but got %s\n", sExpected, sTemp);
00545 return false;
00546 }
00547 if(fscanf(fIn, "%d", &numofIntermPoses) != 1)
00548 return false;
00549
00550
00551 for(int i = 0; i < numofIntermPoses; i++){
00552 EnvNAVXYTHETALAT3Dpt_t intermpose;
00553 if(ReadinPose(&intermpose, fIn) == false){
00554 SBPL_ERROR("ERROR: failed to read in intermediate poses\n");
00555 return false;
00556 }
00557 pMotPrim->intermptV.push_back(intermpose);
00558 }
00559
00560
00561 EnvNAVXYTHETALAT3Dpt_t sourcepose;
00562 sourcepose.x = DISCXY2CONT(0, EnvNAVXYTHETALATCfg.cellsize_m);
00563 sourcepose.y = DISCXY2CONT(0, EnvNAVXYTHETALATCfg.cellsize_m);
00564 sourcepose.theta = DiscTheta2Cont(pMotPrim->starttheta_c, NAVXYTHETALAT_THETADIRS);
00565 double mp_endx_m = sourcepose.x + pMotPrim->intermptV[pMotPrim->intermptV.size()-1].x;
00566 double mp_endy_m = sourcepose.y + pMotPrim->intermptV[pMotPrim->intermptV.size()-1].y;
00567 double mp_endtheta_rad = pMotPrim->intermptV[pMotPrim->intermptV.size()-1].theta;
00568 int endx_c = CONTXY2DISC(mp_endx_m, EnvNAVXYTHETALATCfg.cellsize_m);
00569 int endy_c = CONTXY2DISC(mp_endy_m, EnvNAVXYTHETALATCfg.cellsize_m);
00570 int endtheta_c = ContTheta2Disc(mp_endtheta_rad, NAVXYTHETALAT_THETADIRS);
00571 if(endx_c != pMotPrim->endcell.x || endy_c != pMotPrim->endcell.y || endtheta_c != pMotPrim->endcell.theta)
00572 {
00573 SBPL_ERROR("ERROR: incorrect primitive %d with startangle=%d last interm point %f %f %f does not match end pose %d %d %d\n",
00574 pMotPrim->motprimID, pMotPrim->starttheta_c,
00575 pMotPrim->intermptV[pMotPrim->intermptV.size()-1].x, pMotPrim->intermptV[pMotPrim->intermptV.size()-1].y, pMotPrim->intermptV[pMotPrim->intermptV.size()-1].theta,
00576 pMotPrim->endcell.x, pMotPrim->endcell.y,pMotPrim->endcell.theta);
00577 return false;
00578 }
00579
00580
00581 return true;
00582 }
00583
00584
00585
00586 bool EnvironmentNAVXYTHETALATTICE::ReadMotionPrimitives(FILE* fMotPrims)
00587 {
00588 char sTemp[1024], sExpected[1024];
00589 float fTemp;
00590 int dTemp;
00591 int totalNumofActions = 0;
00592
00593 SBPL_PRINTF("Reading in motion primitives...");
00594
00595
00596 strcpy(sExpected, "resolution_m:");
00597 if(fscanf(fMotPrims, "%s", sTemp) == 0)
00598 return false;
00599 if(strcmp(sTemp, sExpected) != 0){
00600 SBPL_ERROR("ERROR: expected %s but got %s\n", sExpected, sTemp);
00601 return false;
00602 }
00603 if(fscanf(fMotPrims, "%f", &fTemp) == 0)
00604 return false;
00605 if(fabs(fTemp-EnvNAVXYTHETALATCfg.cellsize_m) > ERR_EPS){
00606 SBPL_ERROR("ERROR: invalid resolution %f (instead of %f) in the dynamics file\n",
00607 fTemp, EnvNAVXYTHETALATCfg.cellsize_m);
00608 return false;
00609 }
00610
00611
00612 strcpy(sExpected, "numberofangles:");
00613 if(fscanf(fMotPrims, "%s", sTemp) == 0)
00614 return false;
00615 if(strcmp(sTemp, sExpected) != 0){
00616 SBPL_ERROR("ERROR: expected %s but got %s\n", sExpected, sTemp);
00617 return false;
00618 }
00619 if(fscanf(fMotPrims, "%d", &dTemp) == 0)
00620 return false;
00621 if(dTemp != NAVXYTHETALAT_THETADIRS){
00622 SBPL_ERROR("ERROR: invalid angular resolution %d angles (instead of %d angles) in the motion primitives file\n",
00623 dTemp, NAVXYTHETALAT_THETADIRS);
00624 return false;
00625 }
00626
00627
00628
00629 strcpy(sExpected, "totalnumberofprimitives:");
00630 if(fscanf(fMotPrims, "%s", sTemp) == 0)
00631 return false;
00632 if(strcmp(sTemp, sExpected) != 0){
00633 SBPL_ERROR("ERROR: expected %s but got %s\n", sExpected, sTemp);
00634 return false;
00635 }
00636 if(fscanf(fMotPrims, "%d", &totalNumofActions) == 0){
00637 return false;
00638 }
00639
00640 for(int i = 0; i < totalNumofActions; i++){
00641 SBPL_xytheta_mprimitive motprim;
00642
00643 if(EnvironmentNAVXYTHETALATTICE::ReadinMotionPrimitive(&motprim, fMotPrims) == false)
00644 return false;
00645
00646 EnvNAVXYTHETALATCfg.mprimV.push_back(motprim);
00647
00648 }
00649 SBPL_PRINTF("done ");
00650
00651 return true;
00652 }
00653
00654
00655 void EnvironmentNAVXYTHETALATTICE::ComputeReplanningDataforAction(EnvNAVXYTHETALATAction_t* action)
00656 {
00657 int j;
00658
00659
00660 EnvNAVXYTHETALAT3Dcell_t startcell3d, endcell3d;
00661 for(int i = 0; i < (int)action->intersectingcellsV.size(); i++)
00662 {
00663
00664
00665 startcell3d.theta = action->starttheta;
00666 startcell3d.x = - action->intersectingcellsV.at(i).x;
00667 startcell3d.y = - action->intersectingcellsV.at(i).y;
00668
00669
00670 endcell3d.theta = NORMALIZEDISCTHETA(action->endtheta, NAVXYTHETALAT_THETADIRS);
00671 endcell3d.x = startcell3d.x + action->dX;
00672 endcell3d.y = startcell3d.y + action->dY;
00673
00674
00675 for(j = 0; j < (int)affectedsuccstatesV.size(); j++)
00676 {
00677 if(affectedsuccstatesV.at(j) == endcell3d)
00678 break;
00679 }
00680 if (j == (int)affectedsuccstatesV.size())
00681 affectedsuccstatesV.push_back(endcell3d);
00682
00683 for(j = 0; j < (int)affectedpredstatesV.size(); j++)
00684 {
00685 if(affectedpredstatesV.at(j) == startcell3d)
00686 break;
00687 }
00688 if (j == (int)affectedpredstatesV.size())
00689 affectedpredstatesV.push_back(startcell3d);
00690
00691 }
00692
00693
00694
00695
00696
00697
00698 startcell3d.theta = action->starttheta;
00699 startcell3d.x = - 0;
00700 startcell3d.y = - 0;
00701
00702
00703 endcell3d.theta = NORMALIZEDISCTHETA(action->endtheta, NAVXYTHETALAT_THETADIRS);
00704 endcell3d.x = startcell3d.x + action->dX;
00705 endcell3d.y = startcell3d.y + action->dY;
00706
00707
00708 for(j = 0; j < (int)affectedsuccstatesV.size(); j++)
00709 {
00710 if(affectedsuccstatesV.at(j) == endcell3d)
00711 break;
00712 }
00713 if (j == (int)affectedsuccstatesV.size())
00714 affectedsuccstatesV.push_back(endcell3d);
00715
00716 for(j = 0; j < (int)affectedpredstatesV.size(); j++)
00717 {
00718 if(affectedpredstatesV.at(j) == startcell3d)
00719 break;
00720 }
00721 if (j == (int)affectedpredstatesV.size())
00722 affectedpredstatesV.push_back(startcell3d);
00723
00724
00725
00726
00727 startcell3d.theta = action->starttheta;
00728 startcell3d.x = - action->dX;
00729 startcell3d.y = - action->dY;
00730
00731
00732 endcell3d.theta = NORMALIZEDISCTHETA(action->endtheta, NAVXYTHETALAT_THETADIRS);
00733 endcell3d.x = startcell3d.x + action->dX;
00734 endcell3d.y = startcell3d.y + action->dY;
00735
00736 for(j = 0; j < (int)affectedsuccstatesV.size(); j++)
00737 {
00738 if(affectedsuccstatesV.at(j) == endcell3d)
00739 break;
00740 }
00741 if (j == (int)affectedsuccstatesV.size())
00742 affectedsuccstatesV.push_back(endcell3d);
00743
00744 for(j = 0; j < (int)affectedpredstatesV.size(); j++)
00745 {
00746 if(affectedpredstatesV.at(j) == startcell3d)
00747 break;
00748 }
00749 if (j == (int)affectedpredstatesV.size())
00750 affectedpredstatesV.push_back(startcell3d);
00751
00752
00753 }
00754
00755
00756
00757
00758 void EnvironmentNAVXYTHETALATTICE::ComputeReplanningData()
00759 {
00760
00761
00762
00763 for(int tind = 0; tind < NAVXYTHETALAT_THETADIRS; tind++)
00764 {
00765
00766 for(int aind = 0; aind < EnvNAVXYTHETALATCfg.actionwidth; aind++)
00767 {
00768
00769 ComputeReplanningDataforAction(&EnvNAVXYTHETALATCfg.ActionsV[tind][aind]);
00770 }
00771 }
00772 }
00773
00774
00775 void EnvironmentNAVXYTHETALATTICE::PrecomputeActionswithBaseMotionPrimitive(vector<SBPL_xytheta_mprimitive>* motionprimitiveV)
00776 {
00777
00778 SBPL_PRINTF("Pre-computing action data using base motion primitives...\n");
00779 EnvNAVXYTHETALATCfg.ActionsV = new EnvNAVXYTHETALATAction_t* [NAVXYTHETALAT_THETADIRS];
00780 EnvNAVXYTHETALATCfg.PredActionsV = new vector<EnvNAVXYTHETALATAction_t*> [NAVXYTHETALAT_THETADIRS];
00781 vector<sbpl_2Dcell_t> footprint;
00782
00783
00784 for(int tind = 0; tind < NAVXYTHETALAT_THETADIRS; tind++)
00785 {
00786 SBPL_PRINTF("pre-computing for angle %d out of %d angles\n", tind, NAVXYTHETALAT_THETADIRS);
00787 EnvNAVXYTHETALATCfg.ActionsV[tind] = new EnvNAVXYTHETALATAction_t[motionprimitiveV->size()];
00788
00789
00790 EnvNAVXYTHETALAT3Dpt_t sourcepose;
00791 sourcepose.x = DISCXY2CONT(0, EnvNAVXYTHETALATCfg.cellsize_m);
00792 sourcepose.y = DISCXY2CONT(0, EnvNAVXYTHETALATCfg.cellsize_m);
00793 sourcepose.theta = DiscTheta2Cont(tind, NAVXYTHETALAT_THETADIRS);
00794
00795
00796 for(size_t aind = 0; aind < motionprimitiveV->size(); aind++)
00797 {
00798 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].aind = aind;
00799 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].starttheta = tind;
00800 double mp_endx_m = motionprimitiveV->at(aind).intermptV[motionprimitiveV->at(aind).intermptV.size()-1].x;
00801 double mp_endy_m = motionprimitiveV->at(aind).intermptV[motionprimitiveV->at(aind).intermptV.size()-1].y;
00802 double mp_endtheta_rad = motionprimitiveV->at(aind).intermptV[motionprimitiveV->at(aind).intermptV.size()-1].theta;
00803
00804 double endx = sourcepose.x + (mp_endx_m*cos(sourcepose.theta) - mp_endy_m*sin(sourcepose.theta));
00805 double endy = sourcepose.y + (mp_endx_m*sin(sourcepose.theta) + mp_endy_m*cos(sourcepose.theta));
00806
00807 int endx_c = CONTXY2DISC(endx, EnvNAVXYTHETALATCfg.cellsize_m);
00808 int endy_c = CONTXY2DISC(endy, EnvNAVXYTHETALATCfg.cellsize_m);
00809
00810
00811 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta = ContTheta2Disc(mp_endtheta_rad+sourcepose.theta, NAVXYTHETALAT_THETADIRS);
00812 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dX = endx_c;
00813 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dY = endy_c;
00814 if(EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dY != 0 || EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dX != 0)
00815 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].cost = (int)(ceil(NAVXYTHETALAT_COSTMULT_MTOMM*EnvNAVXYTHETALATCfg.cellsize_m/EnvNAVXYTHETALATCfg.nominalvel_mpersecs*
00816 sqrt((double)(EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dX*EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dX +
00817 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dY*EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dY))));
00818 else
00819 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].cost = (int)(NAVXYTHETALAT_COSTMULT_MTOMM*
00820 EnvNAVXYTHETALATCfg.timetoturn45degsinplace_secs*fabs(computeMinUnsignedAngleDiff(mp_endtheta_rad,0))/(PI_CONST/4.0));
00821
00822
00823 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intersectingcellsV.clear();
00824 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intermptV.clear();
00825 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].interm3DcellsV.clear();
00826 EnvNAVXYTHETALAT3Dcell_t previnterm3Dcell;
00827 previnterm3Dcell.theta = previnterm3Dcell.x = previnterm3Dcell.y = 0;
00828 for (int pind = 0; pind < (int)motionprimitiveV->at(aind).intermptV.size(); pind++)
00829 {
00830 EnvNAVXYTHETALAT3Dpt_t intermpt = motionprimitiveV->at(aind).intermptV[pind];
00831
00832
00833 double rotx = intermpt.x*cos(sourcepose.theta) - intermpt.y*sin(sourcepose.theta);
00834 double roty = intermpt.x*sin(sourcepose.theta) + intermpt.y*cos(sourcepose.theta);
00835 intermpt.x = rotx;
00836 intermpt.y = roty;
00837 intermpt.theta = normalizeAngle(sourcepose.theta + intermpt.theta);
00838
00839
00840 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intermptV.push_back(intermpt);
00841
00842
00843 EnvNAVXYTHETALAT3Dpt_t pose;
00844 pose = intermpt;
00845 pose.x += sourcepose.x;
00846 pose.y += sourcepose.y;
00847 CalculateFootprintForPose(pose, &EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intersectingcellsV);
00848
00849
00850 EnvNAVXYTHETALAT3Dcell_t interm3Dcell;
00851 interm3Dcell.x = CONTXY2DISC(pose.x, EnvNAVXYTHETALATCfg.cellsize_m);
00852 interm3Dcell.y = CONTXY2DISC(pose.y, EnvNAVXYTHETALATCfg.cellsize_m);
00853 interm3Dcell.theta = ContTheta2Disc(pose.theta, NAVXYTHETALAT_THETADIRS);
00854 if(EnvNAVXYTHETALATCfg.ActionsV[tind][aind].interm3DcellsV.size() == 0 ||
00855 previnterm3Dcell.theta != interm3Dcell.theta || previnterm3Dcell.x != interm3Dcell.x || previnterm3Dcell.y != interm3Dcell.y)
00856 {
00857 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].interm3DcellsV.push_back(interm3Dcell);
00858 }
00859 previnterm3Dcell = interm3Dcell;
00860
00861 }
00862
00863
00864 RemoveSourceFootprint(sourcepose, &EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intersectingcellsV);
00865
00866 #if DEBUG
00867 SBPL_FPRINTF(fDeb, "action tind=%d aind=%d: dX=%d dY=%d endtheta=%d (%.2f degs -> %.2f degs) cost=%d (mprim: %.2f %.2f %.2f)\n",
00868 tind, aind,
00869 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dX, EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dY,
00870 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta, sourcepose.theta*180/PI_CONST,
00871 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intermptV[EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intermptV.size()-1].theta*180/PI_CONST,
00872 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].cost,
00873 mp_endx_m, mp_endy_m, mp_endtheta_rad);
00874 #endif
00875
00876
00877 int targettheta = EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta;
00878 if (targettheta < 0)
00879 targettheta = targettheta + NAVXYTHETALAT_THETADIRS;
00880 EnvNAVXYTHETALATCfg.PredActionsV[targettheta].push_back(&(EnvNAVXYTHETALATCfg.ActionsV[tind][aind]));
00881
00882 }
00883 }
00884
00885
00886 EnvNAVXYTHETALATCfg.actionwidth = motionprimitiveV->size();
00887
00888
00889
00890 ComputeReplanningData();
00891
00892 SBPL_PRINTF("done pre-computing action data based on motion primitives\n");
00893
00894
00895 }
00896
00897
00898
00899 void EnvironmentNAVXYTHETALATTICE::PrecomputeActionswithCompleteMotionPrimitive(vector<SBPL_xytheta_mprimitive>* motionprimitiveV)
00900 {
00901
00902 SBPL_PRINTF("Pre-computing action data using motion primitives for every angle...\n");
00903 EnvNAVXYTHETALATCfg.ActionsV = new EnvNAVXYTHETALATAction_t* [NAVXYTHETALAT_THETADIRS];
00904 EnvNAVXYTHETALATCfg.PredActionsV = new vector<EnvNAVXYTHETALATAction_t*> [NAVXYTHETALAT_THETADIRS];
00905 vector<sbpl_2Dcell_t> footprint;
00906
00907 if(motionprimitiveV->size()%NAVXYTHETALAT_THETADIRS != 0)
00908 {
00909 SBPL_ERROR("ERROR: motionprimitives should be uniform across actions\n");
00910 throw new SBPL_Exception();
00911 }
00912
00913 EnvNAVXYTHETALATCfg.actionwidth = ((int)motionprimitiveV->size())/NAVXYTHETALAT_THETADIRS;
00914
00915
00916 int maxnumofactions = 0;
00917 for(int tind = 0; tind < NAVXYTHETALAT_THETADIRS; tind++)
00918 {
00919 SBPL_PRINTF("pre-computing for angle %d out of %d angles\n", tind, NAVXYTHETALAT_THETADIRS);
00920
00921 EnvNAVXYTHETALATCfg.ActionsV[tind] = new EnvNAVXYTHETALATAction_t[EnvNAVXYTHETALATCfg.actionwidth];
00922
00923
00924 EnvNAVXYTHETALAT3Dpt_t sourcepose;
00925 sourcepose.x = DISCXY2CONT(0, EnvNAVXYTHETALATCfg.cellsize_m);
00926 sourcepose.y = DISCXY2CONT(0, EnvNAVXYTHETALATCfg.cellsize_m);
00927 sourcepose.theta = DiscTheta2Cont(tind, NAVXYTHETALAT_THETADIRS);
00928
00929
00930
00931 int numofactions = 0;
00932 int aind = -1;
00933 for(int mind = 0; mind < (int)motionprimitiveV->size(); mind++)
00934 {
00935
00936 if(motionprimitiveV->at(mind).starttheta_c != tind)
00937 continue;
00938
00939 aind++;
00940 numofactions++;
00941
00942
00943 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].aind = aind;
00944
00945
00946 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].starttheta = tind;
00947
00948
00949 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta = motionprimitiveV->at(mind).endcell.theta;
00950 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dX = motionprimitiveV->at(mind).endcell.x;
00951 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dY = motionprimitiveV->at(mind).endcell.y;
00952
00953
00954 if(EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dY != 0 || EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dX != 0)
00955 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].cost = (int)(ceil(NAVXYTHETALAT_COSTMULT_MTOMM*EnvNAVXYTHETALATCfg.cellsize_m/EnvNAVXYTHETALATCfg.nominalvel_mpersecs*
00956 sqrt((double)(EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dX*EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dX +
00957 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dY*EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dY))));
00958 else
00959 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].cost = (int)(NAVXYTHETALAT_COSTMULT_MTOMM*
00960 EnvNAVXYTHETALATCfg.timetoturn45degsinplace_secs*
00961 fabs(computeMinUnsignedAngleDiff(DiscTheta2Cont(EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta, NAVXYTHETALAT_THETADIRS),
00962 DiscTheta2Cont(EnvNAVXYTHETALATCfg.ActionsV[tind][aind].starttheta, NAVXYTHETALAT_THETADIRS)))/(PI_CONST/4.0));
00963
00964 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].cost *= motionprimitiveV->at(mind).additionalactioncostmult;
00965
00966
00967 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intersectingcellsV.clear();
00968 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intermptV.clear();
00969 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].interm3DcellsV.clear();
00970 EnvNAVXYTHETALAT3Dcell_t previnterm3Dcell;
00971 previnterm3Dcell.theta = 0; previnterm3Dcell.x = 0; previnterm3Dcell.y = 0;
00972 for (int pind = 0; pind < (int)motionprimitiveV->at(mind).intermptV.size(); pind++)
00973 {
00974 EnvNAVXYTHETALAT3Dpt_t intermpt = motionprimitiveV->at(mind).intermptV[pind];
00975
00976
00977 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intermptV.push_back(intermpt);
00978
00979
00980 EnvNAVXYTHETALAT3Dpt_t pose;
00981 pose = intermpt;
00982 pose.x += sourcepose.x;
00983 pose.y += sourcepose.y;
00984 CalculateFootprintForPose(pose, &EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intersectingcellsV);
00985
00986
00987 EnvNAVXYTHETALAT3Dcell_t interm3Dcell;
00988 interm3Dcell.x = CONTXY2DISC(pose.x, EnvNAVXYTHETALATCfg.cellsize_m);
00989 interm3Dcell.y = CONTXY2DISC(pose.y, EnvNAVXYTHETALATCfg.cellsize_m);
00990 interm3Dcell.theta = ContTheta2Disc(pose.theta, NAVXYTHETALAT_THETADIRS);
00991 if(EnvNAVXYTHETALATCfg.ActionsV[tind][aind].interm3DcellsV.size() == 0 ||
00992 previnterm3Dcell.theta != interm3Dcell.theta || previnterm3Dcell.x != interm3Dcell.x || previnterm3Dcell.y != interm3Dcell.y)
00993 {
00994 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].interm3DcellsV.push_back(interm3Dcell);
00995 }
00996 previnterm3Dcell = interm3Dcell;
00997 }
00998
00999
01000 RemoveSourceFootprint(sourcepose, &EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intersectingcellsV);
01001
01002 #if DEBUG
01003 SBPL_FPRINTF(fDeb, "action tind=%d aind=%d: dX=%d dY=%d endtheta=%d (%.2f degs -> %.2f degs) cost=%d (mprimID %d: %d %d %d) numofintermcells = %d numofintercells=%d\n",
01004 tind, aind,
01005 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dX, EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dY,
01006 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta,
01007 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intermptV[0].theta*180/PI_CONST,
01008 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intermptV[EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intermptV.size()-1].theta*180/PI_CONST,
01009 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].cost,
01010 motionprimitiveV->at(mind).motprimID,
01011 motionprimitiveV->at(mind).endcell.x, motionprimitiveV->at(mind).endcell.y, motionprimitiveV->at(mind).endcell.theta,
01012 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].interm3DcellsV.size(),
01013 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intersectingcellsV.size());
01014 #endif
01015
01016
01017 int targettheta = EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta;
01018 if (targettheta < 0)
01019 targettheta = targettheta + NAVXYTHETALAT_THETADIRS;
01020 EnvNAVXYTHETALATCfg.PredActionsV[targettheta].push_back(&(EnvNAVXYTHETALATCfg.ActionsV[tind][aind]));
01021
01022 }
01023
01024 if(maxnumofactions < numofactions)
01025 maxnumofactions = numofactions;
01026 }
01027
01028
01029
01030
01031 if(motionprimitiveV->size() != (size_t)(NAVXYTHETALAT_THETADIRS*maxnumofactions))
01032 {
01033 SBPL_ERROR("ERROR: nonuniform number of actions is not supported (maxnumofactions=%d while motprims=%d thetas=%d\n",
01034 maxnumofactions, (unsigned int)motionprimitiveV->size(), NAVXYTHETALAT_THETADIRS);
01035 throw new SBPL_Exception();
01036 }
01037
01038
01039 ComputeReplanningData();
01040
01041 SBPL_PRINTF("done pre-computing action data based on motion primitives\n");
01042
01043
01044 }
01045
01046 void EnvironmentNAVXYTHETALATTICE::PrecomputeActions()
01047 {
01048
01049
01050 SBPL_PRINTF("Pre-computing action data using the motion primitives for a 3D kinematic planning...\n");
01051 EnvNAVXYTHETALATCfg.ActionsV = new EnvNAVXYTHETALATAction_t* [NAVXYTHETALAT_THETADIRS];
01052 EnvNAVXYTHETALATCfg.PredActionsV = new vector<EnvNAVXYTHETALATAction_t*> [NAVXYTHETALAT_THETADIRS];
01053 vector<sbpl_2Dcell_t> footprint;
01054
01055 for(int tind = 0; tind < NAVXYTHETALAT_THETADIRS; tind++)
01056 {
01057 SBPL_PRINTF("processing angle %d\n", tind);
01058 EnvNAVXYTHETALATCfg.ActionsV[tind] = new EnvNAVXYTHETALATAction_t[EnvNAVXYTHETALATCfg.actionwidth];
01059
01060
01061 EnvNAVXYTHETALAT3Dpt_t sourcepose;
01062 sourcepose.x = DISCXY2CONT(0, EnvNAVXYTHETALATCfg.cellsize_m);
01063 sourcepose.y = DISCXY2CONT(0, EnvNAVXYTHETALATCfg.cellsize_m);
01064 sourcepose.theta = DiscTheta2Cont(tind, NAVXYTHETALAT_THETADIRS);
01065
01066
01067 int aind = 0;
01068 for(; aind < 3; aind++)
01069 {
01070 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].aind = aind;
01071 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].starttheta = tind;
01072 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta = (tind + aind - 1)%NAVXYTHETALAT_THETADIRS;
01073 double angle = DiscTheta2Cont(EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta, NAVXYTHETALAT_THETADIRS);
01074 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dX = (int)(cos(angle) + 0.5*(cos(angle)>0?1:-1));
01075 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dY = (int)(sin(angle) + 0.5*(sin(angle)>0?1:-1));
01076 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].cost = (int)(ceil(NAVXYTHETALAT_COSTMULT_MTOMM*EnvNAVXYTHETALATCfg.cellsize_m/EnvNAVXYTHETALATCfg.nominalvel_mpersecs*sqrt((double)(EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dX*EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dX +
01077 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dY*EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dY))));
01078
01079
01080 EnvNAVXYTHETALAT3Dpt_t pose;
01081 pose.x = DISCXY2CONT(EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dX, EnvNAVXYTHETALATCfg.cellsize_m);
01082 pose.y = DISCXY2CONT(EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dY, EnvNAVXYTHETALATCfg.cellsize_m);
01083 pose.theta = angle;
01084 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intermptV.clear();
01085 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intersectingcellsV.clear();
01086 CalculateFootprintForPose(pose, &EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intersectingcellsV);
01087 RemoveSourceFootprint(sourcepose, &EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intersectingcellsV);
01088
01089 #if DEBUG
01090 SBPL_PRINTF("action tind=%d aind=%d: endtheta=%d (%f) dX=%d dY=%d cost=%d\n",
01091 tind, aind, EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta, angle,
01092 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dX, EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dY,
01093 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].cost);
01094 #endif
01095
01096
01097 int targettheta = EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta;
01098 if (targettheta < 0)
01099 targettheta = targettheta + NAVXYTHETALAT_THETADIRS;
01100 EnvNAVXYTHETALATCfg.PredActionsV[targettheta].push_back(&(EnvNAVXYTHETALATCfg.ActionsV[tind][aind]));
01101
01102 }
01103
01104
01105 aind = 3;
01106 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].aind = aind;
01107 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].starttheta = tind;
01108 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta = tind-1;
01109 if(EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta < 0) EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta += NAVXYTHETALAT_THETADIRS;
01110 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dX = 0;
01111 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dY = 0;
01112 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].cost = (int)(NAVXYTHETALAT_COSTMULT_MTOMM*EnvNAVXYTHETALATCfg.timetoturn45degsinplace_secs);
01113
01114
01115 EnvNAVXYTHETALAT3Dpt_t pose;
01116 pose.x = DISCXY2CONT(EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dX, EnvNAVXYTHETALATCfg.cellsize_m);
01117 pose.y = DISCXY2CONT(EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dY, EnvNAVXYTHETALATCfg.cellsize_m);
01118 pose.theta = DiscTheta2Cont(EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta, NAVXYTHETALAT_THETADIRS);
01119 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intermptV.clear();
01120 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intersectingcellsV.clear();
01121 CalculateFootprintForPose(pose, &EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intersectingcellsV);
01122 RemoveSourceFootprint(sourcepose, &EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intersectingcellsV);
01123
01124 #if DEBUG
01125 SBPL_PRINTF("action tind=%d aind=%d: endtheta=%d (%f) dX=%d dY=%d cost=%d\n",
01126 tind, aind, EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta, DiscTheta2Cont(EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta, NAVXYTHETALAT_THETADIRS),
01127 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dX, EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dY,
01128 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].cost);
01129 #endif
01130
01131
01132 int targettheta = EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta;
01133 if (targettheta < 0)
01134 targettheta = targettheta + NAVXYTHETALAT_THETADIRS;
01135 EnvNAVXYTHETALATCfg.PredActionsV[targettheta].push_back(&(EnvNAVXYTHETALATCfg.ActionsV[tind][aind]));
01136
01137
01138 aind = 4;
01139 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].aind = aind;
01140 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].starttheta = tind;
01141 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta = (tind + 1)%NAVXYTHETALAT_THETADIRS;
01142 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dX = 0;
01143 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dY = 0;
01144 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].cost = (int)(NAVXYTHETALAT_COSTMULT_MTOMM*EnvNAVXYTHETALATCfg.timetoturn45degsinplace_secs);
01145
01146
01147 pose.x = DISCXY2CONT(EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dX, EnvNAVXYTHETALATCfg.cellsize_m);
01148 pose.y = DISCXY2CONT(EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dY, EnvNAVXYTHETALATCfg.cellsize_m);
01149 pose.theta = DiscTheta2Cont(EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta, NAVXYTHETALAT_THETADIRS);
01150 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intermptV.clear();
01151 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intersectingcellsV.clear();
01152 CalculateFootprintForPose(pose, &EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intersectingcellsV);
01153 RemoveSourceFootprint(sourcepose, &EnvNAVXYTHETALATCfg.ActionsV[tind][aind].intersectingcellsV);
01154
01155
01156 #if DEBUG
01157 SBPL_PRINTF("action tind=%d aind=%d: endtheta=%d (%f) dX=%d dY=%d cost=%d\n",
01158 tind, aind, EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta, DiscTheta2Cont(EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta, NAVXYTHETALAT_THETADIRS),
01159 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dX, EnvNAVXYTHETALATCfg.ActionsV[tind][aind].dY,
01160 EnvNAVXYTHETALATCfg.ActionsV[tind][aind].cost);
01161 #endif
01162
01163
01164 targettheta = EnvNAVXYTHETALATCfg.ActionsV[tind][aind].endtheta;
01165 if (targettheta < 0)
01166 targettheta = targettheta + NAVXYTHETALAT_THETADIRS;
01167 EnvNAVXYTHETALATCfg.PredActionsV[targettheta].push_back(&(EnvNAVXYTHETALATCfg.ActionsV[tind][aind]));
01168
01169 }
01170
01171
01172 ComputeReplanningData();
01173
01174 SBPL_PRINTF("done pre-computing action data\n");
01175
01176
01177 }
01178
01179
01180
01181 void EnvironmentNAVXYTHETALATTICE::InitializeEnvConfig(vector<SBPL_xytheta_mprimitive>* motionprimitiveV)
01182 {
01183
01184
01185
01186 EnvNAVXYTHETALATCfg.dXY[0][0] = -1;
01187 EnvNAVXYTHETALATCfg.dXY[0][1] = -1;
01188 EnvNAVXYTHETALATCfg.dXY[1][0] = -1;
01189 EnvNAVXYTHETALATCfg.dXY[1][1] = 0;
01190 EnvNAVXYTHETALATCfg.dXY[2][0] = -1;
01191 EnvNAVXYTHETALATCfg.dXY[2][1] = 1;
01192 EnvNAVXYTHETALATCfg.dXY[3][0] = 0;
01193 EnvNAVXYTHETALATCfg.dXY[3][1] = -1;
01194 EnvNAVXYTHETALATCfg.dXY[4][0] = 0;
01195 EnvNAVXYTHETALATCfg.dXY[4][1] = 1;
01196 EnvNAVXYTHETALATCfg.dXY[5][0] = 1;
01197 EnvNAVXYTHETALATCfg.dXY[5][1] = -1;
01198 EnvNAVXYTHETALATCfg.dXY[6][0] = 1;
01199 EnvNAVXYTHETALATCfg.dXY[6][1] = 0;
01200 EnvNAVXYTHETALATCfg.dXY[7][0] = 1;
01201 EnvNAVXYTHETALATCfg.dXY[7][1] = 1;
01202
01203
01204 EnvNAVXYTHETALAT3Dpt_t temppose;
01205 temppose.x = 0.0;
01206 temppose.y = 0.0;
01207 temppose.theta = 0.0;
01208 vector<sbpl_2Dcell_t> footprint;
01209 CalculateFootprintForPose(temppose, &footprint);
01210 SBPL_PRINTF("number of cells in footprint of the robot = %d\n", (unsigned int)footprint.size());
01211
01212 #if DEBUG
01213 SBPL_FPRINTF(fDeb, "footprint cells (size=%d):\n", footprint.size());
01214 for(int i = 0; i < (int) footprint.size(); i++)
01215 {
01216 SBPL_FPRINTF(fDeb, "%d %d (cont: %.3f %.3f)\n", footprint.at(i).x, footprint.at(i).y,
01217 DISCXY2CONT(footprint.at(i).x, EnvNAVXYTHETALATCfg.cellsize_m),
01218 DISCXY2CONT(footprint.at(i).y, EnvNAVXYTHETALATCfg.cellsize_m));
01219 }
01220 #endif
01221
01222
01223 if(motionprimitiveV == NULL)
01224 PrecomputeActions();
01225 else
01226 PrecomputeActionswithCompleteMotionPrimitive(motionprimitiveV);
01227
01228
01229 }
01230
01231
01232
01233 bool EnvironmentNAVXYTHETALATTICE::IsValidCell(int X, int Y)
01234 {
01235 return (X >= 0 && X < EnvNAVXYTHETALATCfg.EnvWidth_c &&
01236 Y >= 0 && Y < EnvNAVXYTHETALATCfg.EnvHeight_c &&
01237 EnvNAVXYTHETALATCfg.Grid2D[X][Y] < EnvNAVXYTHETALATCfg.obsthresh);
01238 }
01239
01240 bool EnvironmentNAVXYTHETALATTICE::IsWithinMapCell(int X, int Y)
01241 {
01242 return (X >= 0 && X < EnvNAVXYTHETALATCfg.EnvWidth_c &&
01243 Y >= 0 && Y < EnvNAVXYTHETALATCfg.EnvHeight_c);
01244 }
01245
01246 bool EnvironmentNAVXYTHETALATTICE::IsValidConfiguration(int X, int Y, int Theta)
01247 {
01248 vector<sbpl_2Dcell_t> footprint;
01249 EnvNAVXYTHETALAT3Dpt_t pose;
01250
01251
01252 pose.x = DISCXY2CONT(X, EnvNAVXYTHETALATCfg.cellsize_m);
01253 pose.y = DISCXY2CONT(Y, EnvNAVXYTHETALATCfg.cellsize_m);
01254 pose.theta = DiscTheta2Cont(Theta, NAVXYTHETALAT_THETADIRS);
01255
01256
01257 CalculateFootprintForPose(pose, &footprint);
01258
01259
01260 for(int find = 0; find < (int)footprint.size(); find++)
01261 {
01262 int x = footprint.at(find).x;
01263 int y = footprint.at(find).y;
01264
01265 if (x < 0 || x >= EnvNAVXYTHETALATCfg.EnvWidth_c ||
01266 y < 0 || y >= EnvNAVXYTHETALATCfg.EnvHeight_c ||
01267 EnvNAVXYTHETALATCfg.Grid2D[x][y] >= EnvNAVXYTHETALATCfg.obsthresh)
01268 {
01269 return false;
01270 }
01271 }
01272
01273 return true;
01274 }
01275
01276
01277 int EnvironmentNAVXYTHETALATTICE::GetActionCost(int SourceX, int SourceY, int SourceTheta, EnvNAVXYTHETALATAction_t* action)
01278 {
01279 sbpl_2Dcell_t cell;
01280 EnvNAVXYTHETALAT3Dcell_t interm3Dcell;
01281 int i;
01282
01283
01284
01285 if(!IsValidCell(SourceX, SourceY))
01286 return INFINITECOST;
01287 if(!IsValidCell(SourceX + action->dX, SourceY + action->dY))
01288 return INFINITECOST;
01289
01290 if(EnvNAVXYTHETALATCfg.Grid2D[SourceX + action->dX][SourceY + action->dY] >= EnvNAVXYTHETALATCfg.cost_inscribed_thresh)
01291 return INFINITECOST;
01292
01293
01294 unsigned char maxcellcost = 0;
01295 for(i = 0; i < (int)action->interm3DcellsV.size(); i++)
01296 {
01297 interm3Dcell = action->interm3DcellsV.at(i);
01298 interm3Dcell.x = interm3Dcell.x + SourceX;
01299 interm3Dcell.y = interm3Dcell.y + SourceY;
01300
01301 if(interm3Dcell.x < 0 || interm3Dcell.x >= EnvNAVXYTHETALATCfg.EnvWidth_c ||
01302 interm3Dcell.y < 0 || interm3Dcell.y >= EnvNAVXYTHETALATCfg.EnvHeight_c)
01303 return INFINITECOST;
01304
01305 maxcellcost = __max(maxcellcost, EnvNAVXYTHETALATCfg.Grid2D[interm3Dcell.x][interm3Dcell.y]);
01306
01307
01308 if(maxcellcost >= EnvNAVXYTHETALATCfg.cost_inscribed_thresh)
01309 return INFINITECOST;
01310 }
01311
01312
01313
01314 if(EnvNAVXYTHETALATCfg.FootprintPolygon.size() > 1 && (int)maxcellcost >= EnvNAVXYTHETALATCfg.cost_possibly_circumscribed_thresh)
01315 {
01316 checks++;
01317
01318 for(i = 0; i < (int)action->intersectingcellsV.size(); i++)
01319 {
01320
01321 cell = action->intersectingcellsV.at(i);
01322 cell.x = cell.x + SourceX;
01323 cell.y = cell.y + SourceY;
01324
01325
01326 if(!IsValidCell(cell.x, cell.y))
01327 return INFINITECOST;
01328
01329
01330
01331 }
01332 }
01333
01334
01335 maxcellcost = __max(maxcellcost, EnvNAVXYTHETALATCfg.Grid2D[SourceX][SourceY]);
01336 int currentmaxcost = (int)__max(maxcellcost, EnvNAVXYTHETALATCfg.Grid2D[SourceX + action->dX][SourceY + action->dY]);
01337
01338
01339 return action->cost*(currentmaxcost+1);
01340
01341 }
01342
01343
01344
01345 double EnvironmentNAVXYTHETALATTICE::EuclideanDistance_m(int X1, int Y1, int X2, int Y2)
01346 {
01347 int sqdist = ((X1-X2)*(X1-X2)+(Y1-Y2)*(Y1-Y2));
01348 return EnvNAVXYTHETALATCfg.cellsize_m*sqrt((double)sqdist);
01349
01350 }
01351
01352
01353
01354 void EnvironmentNAVXYTHETALATTICE::CalculateFootprintForPose(EnvNAVXYTHETALAT3Dpt_t pose, vector<sbpl_2Dcell_t>* footprint, const vector<sbpl_2Dpt_t>& FootprintPolygon)
01355 {
01356
01357 int pind;
01358
01359 #if DEBUG
01360
01361
01362 #endif
01363
01364
01365 if(FootprintPolygon.size() <= 1){
01366 sbpl_2Dcell_t cell;
01367 cell.x = CONTXY2DISC(pose.x, EnvNAVXYTHETALATCfg.cellsize_m);
01368 cell.y = CONTXY2DISC(pose.y, EnvNAVXYTHETALATCfg.cellsize_m);
01369
01370 for(pind = 0; pind < (int)footprint->size(); pind++)
01371 {
01372 if(cell.x == footprint->at(pind).x && cell.y == footprint->at(pind).y)
01373 break;
01374 }
01375 if(pind == (int)footprint->size()) footprint->push_back(cell);
01376 return;
01377 }
01378
01379 vector<sbpl_2Dpt_t> bounding_polygon;
01380 unsigned int find;
01381 double max_x = -INFINITECOST, min_x = INFINITECOST, max_y = -INFINITECOST, min_y = INFINITECOST;
01382 sbpl_2Dpt_t pt = {0,0};
01383 for(find = 0; find < FootprintPolygon.size(); find++){
01384
01385
01386 pt = FootprintPolygon[find];
01387
01388
01389 sbpl_2Dpt_t corner;
01390 corner.x = cos(pose.theta)*pt.x - sin(pose.theta)*pt.y + pose.x;
01391 corner.y = sin(pose.theta)*pt.x + cos(pose.theta)*pt.y + pose.y;
01392 bounding_polygon.push_back(corner);
01393 #if DEBUG
01394
01395 #endif
01396 if(corner.x < min_x || find==0){
01397 min_x = corner.x;
01398 }
01399 if(corner.x > max_x || find==0){
01400 max_x = corner.x;
01401 }
01402 if(corner.y < min_y || find==0){
01403 min_y = corner.y;
01404 }
01405 if(corner.y > max_y || find==0){
01406 max_y = corner.y;
01407 }
01408
01409 }
01410
01411 #if DEBUG
01412
01413 #endif
01414
01415 int prev_discrete_x = CONTXY2DISC(pt.x, EnvNAVXYTHETALATCfg.cellsize_m) + 1;
01416 int prev_discrete_y = CONTXY2DISC(pt.y, EnvNAVXYTHETALATCfg.cellsize_m) + 1;
01417 int prev_inside = 0;
01418 int discrete_x;
01419 int discrete_y;
01420
01421 for(double x=min_x; x<=max_x; x+=EnvNAVXYTHETALATCfg.cellsize_m/3){
01422 for(double y=min_y; y<=max_y; y+=EnvNAVXYTHETALATCfg.cellsize_m/3){
01423 pt.x = x;
01424 pt.y = y;
01425 discrete_x = CONTXY2DISC(pt.x, EnvNAVXYTHETALATCfg.cellsize_m);
01426 discrete_y = CONTXY2DISC(pt.y, EnvNAVXYTHETALATCfg.cellsize_m);
01427
01428
01429 if(discrete_x != prev_discrete_x || discrete_y != prev_discrete_y || prev_inside==0){
01430
01431 #if DEBUG
01432
01433 #endif
01434
01435 if(IsInsideFootprint(pt, &bounding_polygon)){
01436
01437
01438 #if DEBUG
01439
01440 #endif
01441
01442 sbpl_2Dcell_t cell;
01443 cell.x = discrete_x;
01444 cell.y = discrete_y;
01445
01446
01447 int pind = 0;
01448 for(pind = 0; pind < (int)footprint->size(); pind++)
01449 {
01450 if(cell.x == footprint->at(pind).x && cell.y == footprint->at(pind).y)
01451 break;
01452 }
01453 if(pind == (int)footprint->size()) footprint->push_back(cell);
01454
01455 prev_inside = 1;
01456
01457 #if DEBUG
01458
01459 #endif
01460 }
01461 else{
01462 prev_inside = 0;
01463 }
01464
01465 }
01466 else
01467 {
01468 #if DEBUG
01469
01470 #endif
01471 }
01472
01473 prev_discrete_x = discrete_x;
01474 prev_discrete_y = discrete_y;
01475
01476 }
01477 }
01478
01479
01480
01481 }
01482
01483
01484
01485 void EnvironmentNAVXYTHETALATTICE::CalculateFootprintForPose(EnvNAVXYTHETALAT3Dpt_t pose, vector<sbpl_2Dcell_t>* footprint)
01486 {
01487 CalculateFootprintForPose(pose, footprint, EnvNAVXYTHETALATCfg.FootprintPolygon);
01488 }
01489
01490
01491
01492 void EnvironmentNAVXYTHETALATTICE::RemoveSourceFootprint(EnvNAVXYTHETALAT3Dpt_t sourcepose, vector<sbpl_2Dcell_t>* footprint, const vector<sbpl_2Dpt_t>& FootprintPolygon)
01493 {
01494 vector<sbpl_2Dcell_t> sourcefootprint;
01495
01496
01497 CalculateFootprintForPose(sourcepose, &sourcefootprint, FootprintPolygon);
01498
01499
01500 for(int sind = 0; sind < (int)sourcefootprint.size(); sind++)
01501 {
01502 for(int find = 0; find < (int)footprint->size(); find++)
01503 {
01504 if(sourcefootprint.at(sind).x == footprint->at(find).x && sourcefootprint.at(sind).y == footprint->at(find).y)
01505 {
01506 footprint->erase(footprint->begin() + find);
01507 break;
01508 }
01509 }
01510 }
01511
01512
01513
01514 }
01515
01516
01517
01518 void EnvironmentNAVXYTHETALATTICE::RemoveSourceFootprint(EnvNAVXYTHETALAT3Dpt_t sourcepose, vector<sbpl_2Dcell_t>* footprint)
01519 {
01520 RemoveSourceFootprint(sourcepose, footprint, EnvNAVXYTHETALATCfg.FootprintPolygon);
01521 }
01522
01523
01524
01525
01526
01527
01528
01529 void EnvironmentNAVXYTHETALATTICE::EnsureHeuristicsUpdated(bool bGoalHeuristics)
01530 {
01531
01532 if(bNeedtoRecomputeStartHeuristics && !bGoalHeuristics)
01533 {
01534 grid2Dsearchfromstart->search(EnvNAVXYTHETALATCfg.Grid2D, EnvNAVXYTHETALATCfg.cost_inscribed_thresh,
01535 EnvNAVXYTHETALATCfg.StartX_c, EnvNAVXYTHETALATCfg.StartY_c, EnvNAVXYTHETALATCfg.EndX_c, EnvNAVXYTHETALATCfg.EndY_c,
01536 SBPL_2DGRIDSEARCH_TERM_CONDITION_TWOTIMESOPTPATH);
01537 bNeedtoRecomputeStartHeuristics = false;
01538 SBPL_PRINTF("2dsolcost_infullunits=%d\n", (int)(grid2Dsearchfromstart->getlowerboundoncostfromstart_inmm(EnvNAVXYTHETALATCfg.EndX_c, EnvNAVXYTHETALATCfg.EndY_c)
01539 /EnvNAVXYTHETALATCfg.nominalvel_mpersecs));
01540
01541 }
01542
01543
01544 if(bNeedtoRecomputeGoalHeuristics && bGoalHeuristics)
01545 {
01546 grid2Dsearchfromgoal->search(EnvNAVXYTHETALATCfg.Grid2D, EnvNAVXYTHETALATCfg.cost_inscribed_thresh,
01547 EnvNAVXYTHETALATCfg.EndX_c, EnvNAVXYTHETALATCfg.EndY_c, EnvNAVXYTHETALATCfg.StartX_c, EnvNAVXYTHETALATCfg.StartY_c,
01548 SBPL_2DGRIDSEARCH_TERM_CONDITION_TWOTIMESOPTPATH);
01549 bNeedtoRecomputeGoalHeuristics = false;
01550 SBPL_PRINTF("2dsolcost_infullunits=%d\n", (int)(grid2Dsearchfromgoal->getlowerboundoncostfromstart_inmm(EnvNAVXYTHETALATCfg.StartX_c, EnvNAVXYTHETALATCfg.StartY_c)
01551 /EnvNAVXYTHETALATCfg.nominalvel_mpersecs));
01552
01553 }
01554
01555
01556 }
01557
01558
01559
01560 void EnvironmentNAVXYTHETALATTICE::ComputeHeuristicValues()
01561 {
01562
01563 SBPL_PRINTF("Precomputing heuristics...\n");
01564
01565
01566 grid2Dsearchfromstart = new SBPL2DGridSearch(EnvNAVXYTHETALATCfg.EnvWidth_c, EnvNAVXYTHETALATCfg.EnvHeight_c, (float)EnvNAVXYTHETALATCfg.cellsize_m);
01567 grid2Dsearchfromgoal = new SBPL2DGridSearch(EnvNAVXYTHETALATCfg.EnvWidth_c, EnvNAVXYTHETALATCfg.EnvHeight_c, (float)EnvNAVXYTHETALATCfg.cellsize_m);
01568
01569
01570 grid2Dsearchfromstart->setOPENdatastructure(SBPL_2DGRIDSEARCH_OPENTYPE_SLIDINGBUCKETS);
01571 grid2Dsearchfromgoal->setOPENdatastructure(SBPL_2DGRIDSEARCH_OPENTYPE_SLIDINGBUCKETS);
01572
01573 SBPL_PRINTF("done\n");
01574
01575 }
01576
01577
01578 bool EnvironmentNAVXYTHETALATTICE::CheckQuant(FILE* fOut)
01579 {
01580
01581 for(double theta = -10; theta < 10; theta += 2.0*PI_CONST/NAVXYTHETALAT_THETADIRS*0.01)
01582 {
01583 int nTheta = ContTheta2Disc(theta, NAVXYTHETALAT_THETADIRS);
01584 double newTheta = DiscTheta2Cont(nTheta, NAVXYTHETALAT_THETADIRS);
01585 int nnewTheta = ContTheta2Disc(newTheta, NAVXYTHETALAT_THETADIRS);
01586
01587 SBPL_FPRINTF(fOut, "theta=%f(%f)->%d->%f->%d\n", theta, theta*180/PI_CONST, nTheta, newTheta, nnewTheta);
01588
01589 if(nTheta != nnewTheta)
01590 {
01591 SBPL_ERROR("ERROR: invalid quantization\n");
01592 return false;
01593 }
01594 }
01595
01596 return true;
01597 }
01598
01599
01600
01601
01602
01603
01604 bool EnvironmentNAVXYTHETALATTICE::InitializeEnv(const char* sEnvFile, const vector<sbpl_2Dpt_t>& perimeterptsV, const char* sMotPrimFile)
01605 {
01606 EnvNAVXYTHETALATCfg.FootprintPolygon = perimeterptsV;
01607
01608 FILE* fCfg = fopen(sEnvFile, "r");
01609 if(fCfg == NULL)
01610 {
01611 SBPL_ERROR("ERROR: unable to open %s\n", sEnvFile);
01612 throw new SBPL_Exception();
01613 }
01614 ReadConfiguration(fCfg);
01615 fclose(fCfg);
01616
01617 if(sMotPrimFile != NULL)
01618 {
01619 FILE* fMotPrim = fopen(sMotPrimFile, "r");
01620 if(fMotPrim == NULL)
01621 {
01622 SBPL_ERROR("ERROR: unable to open %s\n", sMotPrimFile);
01623 throw new SBPL_Exception();
01624 }
01625 if(ReadMotionPrimitives(fMotPrim) == false)
01626 {
01627 SBPL_ERROR("ERROR: failed to read in motion primitive file\n");
01628 throw new SBPL_Exception();
01629 }
01630 InitGeneral(&EnvNAVXYTHETALATCfg.mprimV);
01631 fclose(fMotPrim);
01632 }
01633 else
01634 InitGeneral(NULL);
01635
01636 SBPL_PRINTF("size of env: %d by %d\n", EnvNAVXYTHETALATCfg.EnvWidth_c, EnvNAVXYTHETALATCfg.EnvHeight_c);
01637
01638 return true;
01639 }
01640
01641
01642 bool EnvironmentNAVXYTHETALATTICE::InitializeEnv(const char* sEnvFile)
01643 {
01644
01645 FILE* fCfg = fopen(sEnvFile, "r");
01646 if(fCfg == NULL)
01647 {
01648 SBPL_ERROR("ERROR: unable to open %s\n", sEnvFile);
01649 throw new SBPL_Exception();
01650 }
01651 ReadConfiguration(fCfg);
01652 fclose(fCfg);
01653
01654 InitGeneral(NULL);
01655
01656
01657 return true;
01658 }
01659
01660
01661
01662 bool EnvironmentNAVXYTHETALATTICE::InitializeEnv(int width, int height,
01663 const unsigned char* mapdata,
01664 double startx, double starty, double starttheta,
01665 double goalx, double goaly, double goaltheta,
01666 double goaltol_x, double goaltol_y, double goaltol_theta,
01667 const vector<sbpl_2Dpt_t> & perimeterptsV,
01668 double cellsize_m, double nominalvel_mpersecs, double timetoturn45degsinplace_secs,
01669 unsigned char obsthresh, const char* sMotPrimFile)
01670 {
01671
01672 SBPL_PRINTF("env: initialize with width=%d height=%d start=%.3f %.3f %.3f goalx=%.3f %.3f %.3f cellsize=%.3f nomvel=%.3f timetoturn=%.3f, obsthresh=%d\n",
01673 width, height, startx, starty, starttheta, goalx, goaly, goaltheta, cellsize_m, nominalvel_mpersecs, timetoturn45degsinplace_secs, obsthresh);
01674
01675 SBPL_PRINTF("perimeter has size=%d\n", (unsigned int)perimeterptsV.size());
01676
01677 for(int i = 0; i < (int)perimeterptsV.size(); i++)
01678 {
01679 SBPL_PRINTF("perimeter(%d) = %.4f %.4f\n", i, perimeterptsV.at(i).x, perimeterptsV.at(i).y);
01680 }
01681
01682
01683 EnvNAVXYTHETALATCfg.obsthresh = obsthresh;
01684
01685
01686
01687 SetConfiguration(width, height,
01688 mapdata,
01689 CONTXY2DISC(startx, cellsize_m), CONTXY2DISC(starty, cellsize_m), ContTheta2Disc(starttheta, NAVXYTHETALAT_THETADIRS),
01690 CONTXY2DISC(goalx, cellsize_m), CONTXY2DISC(goaly, cellsize_m), ContTheta2Disc(goaltheta, NAVXYTHETALAT_THETADIRS),
01691 cellsize_m, nominalvel_mpersecs, timetoturn45degsinplace_secs, perimeterptsV);
01692
01693 if(sMotPrimFile != NULL)
01694 {
01695 FILE* fMotPrim = fopen(sMotPrimFile, "r");
01696 if(fMotPrim == NULL)
01697 {
01698 SBPL_ERROR("ERROR: unable to open %s\n", sMotPrimFile);
01699 throw new SBPL_Exception();
01700 }
01701
01702 if(ReadMotionPrimitives(fMotPrim) == false)
01703 {
01704 SBPL_ERROR("ERROR: failed to read in motion primitive file\n");
01705 throw new SBPL_Exception();
01706 }
01707 fclose(fMotPrim);
01708 }
01709
01710 if(EnvNAVXYTHETALATCfg.mprimV.size() != 0)
01711 {
01712 InitGeneral(&EnvNAVXYTHETALATCfg.mprimV);
01713 }
01714 else
01715 InitGeneral(NULL);
01716
01717 return true;
01718 }
01719
01720
01721 bool EnvironmentNAVXYTHETALATTICE::InitGeneral(vector<SBPL_xytheta_mprimitive>* motionprimitiveV) {
01722
01723
01724
01725 InitializeEnvConfig(motionprimitiveV);
01726
01727
01728 InitializeEnvironment();
01729
01730
01731 ComputeHeuristicValues();
01732
01733 return true;
01734 }
01735
01736 bool EnvironmentNAVXYTHETALATTICE::InitializeMDPCfg(MDPConfig *MDPCfg)
01737 {
01738
01739 MDPCfg->goalstateid = EnvNAVXYTHETALAT.goalstateid;
01740 MDPCfg->startstateid = EnvNAVXYTHETALAT.startstateid;
01741
01742 return true;
01743 }
01744
01745
01746 void EnvironmentNAVXYTHETALATTICE::PrintHeuristicValues()
01747 {
01748 #ifndef ROS
01749 const char* heur = "heur.txt";
01750 #endif
01751 FILE* fHeur = SBPL_FOPEN(heur, "w");
01752 if(fHeur == NULL){
01753 SBPL_ERROR("ERROR: could not open debug file to write heuristic\n");
01754 throw new SBPL_Exception();
01755 }
01756 SBPL2DGridSearch* grid2Dsearch = NULL;
01757
01758 for(int i = 0; i < 2; i++)
01759 {
01760 if(i == 0 && grid2Dsearchfromstart != NULL)
01761 {
01762 grid2Dsearch = grid2Dsearchfromstart;
01763 SBPL_FPRINTF(fHeur, "start heuristics:\n");
01764 }
01765 else if(i == 1 && grid2Dsearchfromgoal != NULL)
01766 {
01767 grid2Dsearch = grid2Dsearchfromgoal;
01768 SBPL_FPRINTF(fHeur, "goal heuristics:\n");
01769 }
01770 else
01771 continue;
01772
01773 for (int y = 0; y < EnvNAVXYTHETALATCfg.EnvHeight_c; y++) {
01774 for (int x = 0; x < EnvNAVXYTHETALATCfg.EnvWidth_c; x++) {
01775 if(grid2Dsearch->getlowerboundoncostfromstart_inmm(x, y) < INFINITECOST)
01776 SBPL_FPRINTF(fHeur, "%5d ", grid2Dsearch->getlowerboundoncostfromstart_inmm(x, y));
01777 else
01778 SBPL_FPRINTF(fHeur, "XXXXX ");
01779 }
01780 SBPL_FPRINTF(fHeur, "\n");
01781 }
01782 }
01783 SBPL_FCLOSE(fHeur);
01784 }
01785
01786
01787
01788
01789 void EnvironmentNAVXYTHETALATTICE::SetAllPreds(CMDPSTATE* state)
01790 {
01791
01792
01793 SBPL_ERROR("ERROR in EnvNAVXYTHETALAT... function: SetAllPreds is undefined\n");
01794 throw new SBPL_Exception();
01795 }
01796
01797
01798 void EnvironmentNAVXYTHETALATTICE::GetSuccs(int SourceStateID, vector<int>* SuccIDV, vector<int>* CostV)
01799 {
01800 GetSuccs(SourceStateID, SuccIDV, CostV, NULL);
01801 }
01802
01803
01804
01805
01806
01807 const EnvNAVXYTHETALATConfig_t* EnvironmentNAVXYTHETALATTICE::GetEnvNavConfig() {
01808 return &EnvNAVXYTHETALATCfg;
01809 }
01810
01811
01812
01813 bool EnvironmentNAVXYTHETALATTICE::UpdateCost(int x, int y, unsigned char newcost)
01814 {
01815
01816 #if DEBUG
01817
01818 #endif
01819
01820 EnvNAVXYTHETALATCfg.Grid2D[x][y] = newcost;
01821
01822 bNeedtoRecomputeStartHeuristics = true;
01823 bNeedtoRecomputeGoalHeuristics = true;
01824
01825 return true;
01826 }
01827
01828
01829 bool EnvironmentNAVXYTHETALATTICE::SetMap(const unsigned char* mapdata)
01830 {
01831 int xind=-1, yind=-1;
01832
01833 for (xind = 0; xind < EnvNAVXYTHETALATCfg.EnvWidth_c; xind++) {
01834 for(yind = 0; yind < EnvNAVXYTHETALATCfg.EnvHeight_c; yind++) {
01835 EnvNAVXYTHETALATCfg.Grid2D[xind][yind] = mapdata[xind+yind*EnvNAVXYTHETALATCfg.EnvWidth_c];
01836 }
01837 }
01838
01839 bNeedtoRecomputeStartHeuristics = true;
01840 bNeedtoRecomputeGoalHeuristics = true;
01841
01842 return true;
01843
01844 }
01845
01846
01847
01848 void EnvironmentNAVXYTHETALATTICE::PrintEnv_Config(FILE* fOut)
01849 {
01850
01851
01852
01853 SBPL_ERROR("ERROR in EnvNAVXYTHETALAT... function: PrintEnv_Config is undefined\n");
01854 throw new SBPL_Exception();
01855
01856 }
01857
01858 void EnvironmentNAVXYTHETALATTICE::PrintTimeStat(FILE* fOut)
01859 {
01860
01861 #if TIME_DEBUG
01862 SBPL_FPRINTF(fOut, "time3_addallout = %f secs, time_gethash = %f secs, time_createhash = %f secs, time_getsuccs = %f\n",
01863 time3_addallout/(double)CLOCKS_PER_SEC, time_gethash/(double)CLOCKS_PER_SEC,
01864 time_createhash/(double)CLOCKS_PER_SEC, time_getsuccs/(double)CLOCKS_PER_SEC);
01865 #endif
01866 }
01867
01868
01869
01870 bool EnvironmentNAVXYTHETALATTICE::IsObstacle(int x, int y)
01871 {
01872
01873 #if DEBUG
01874 SBPL_FPRINTF(fDeb, "Status of cell %d %d is queried. Its cost=%d\n", x,y,EnvNAVXYTHETALATCfg.Grid2D[x][y]);
01875 #endif
01876
01877
01878 return (EnvNAVXYTHETALATCfg.Grid2D[x][y] >= EnvNAVXYTHETALATCfg.obsthresh);
01879
01880 }
01881
01882 void EnvironmentNAVXYTHETALATTICE::GetEnvParms(int *size_x, int *size_y, double* startx, double* starty, double*starttheta, double* goalx, double* goaly, double* goaltheta,
01883 double* cellsize_m, double* nominalvel_mpersecs, double* timetoturn45degsinplace_secs, unsigned char* obsthresh,
01884 vector<SBPL_xytheta_mprimitive>* mprimitiveV)
01885 {
01886 *size_x = EnvNAVXYTHETALATCfg.EnvWidth_c;
01887 *size_y = EnvNAVXYTHETALATCfg.EnvHeight_c;
01888
01889 *startx = DISCXY2CONT(EnvNAVXYTHETALATCfg.StartX_c, EnvNAVXYTHETALATCfg.cellsize_m);
01890 *starty = DISCXY2CONT(EnvNAVXYTHETALATCfg.StartY_c, EnvNAVXYTHETALATCfg.cellsize_m);
01891 *starttheta = DiscTheta2Cont(EnvNAVXYTHETALATCfg.StartTheta, NAVXYTHETALAT_THETADIRS);
01892 *goalx = DISCXY2CONT(EnvNAVXYTHETALATCfg.EndX_c, EnvNAVXYTHETALATCfg.cellsize_m);
01893 *goaly = DISCXY2CONT(EnvNAVXYTHETALATCfg.EndY_c, EnvNAVXYTHETALATCfg.cellsize_m);
01894 *goaltheta = DiscTheta2Cont(EnvNAVXYTHETALATCfg.EndTheta, NAVXYTHETALAT_THETADIRS);;
01895
01896 *cellsize_m = EnvNAVXYTHETALATCfg.cellsize_m;
01897 *nominalvel_mpersecs = EnvNAVXYTHETALATCfg.nominalvel_mpersecs;
01898 *timetoturn45degsinplace_secs = EnvNAVXYTHETALATCfg.timetoturn45degsinplace_secs;
01899
01900 *obsthresh = EnvNAVXYTHETALATCfg.obsthresh;
01901
01902 *mprimitiveV = EnvNAVXYTHETALATCfg.mprimV;
01903 }
01904
01905
01906 bool EnvironmentNAVXYTHETALATTICE::PoseContToDisc(double px, double py, double pth,
01907 int &ix, int &iy, int &ith) const
01908 {
01909 ix = CONTXY2DISC(px, EnvNAVXYTHETALATCfg.cellsize_m);
01910 iy = CONTXY2DISC(py, EnvNAVXYTHETALATCfg.cellsize_m);
01911 ith = ContTheta2Disc(pth, NAVXYTHETALAT_THETADIRS);
01912 return (pth >= -2*PI_CONST) && (pth <= 2*PI_CONST)
01913 && (ix >= 0) && (ix < EnvNAVXYTHETALATCfg.EnvWidth_c)
01914 && (iy >= 0) && (iy < EnvNAVXYTHETALATCfg.EnvHeight_c);
01915 }
01916
01917
01918 bool EnvironmentNAVXYTHETALATTICE::PoseDiscToCont(int ix, int iy, int ith,
01919 double &px, double &py, double &pth) const
01920 {
01921 px = DISCXY2CONT(ix, EnvNAVXYTHETALATCfg.cellsize_m);
01922 py = DISCXY2CONT(iy, EnvNAVXYTHETALATCfg.cellsize_m);
01923 pth = normalizeAngle(DiscTheta2Cont(ith, NAVXYTHETALAT_THETADIRS));
01924 return (ith >= 0) && (ith < NAVXYTHETALAT_THETADIRS)
01925 && (ix >= 0) && (ix < EnvNAVXYTHETALATCfg.EnvWidth_c)
01926 && (iy >= 0) && (iy < EnvNAVXYTHETALATCfg.EnvHeight_c);
01927 }
01928
01929 unsigned char EnvironmentNAVXYTHETALATTICE::GetMapCost(int x, int y)
01930 {
01931 return EnvNAVXYTHETALATCfg.Grid2D[x][y];
01932 }
01933
01934
01935
01936 bool EnvironmentNAVXYTHETALATTICE::SetEnvParameter(const char* parameter, int value)
01937 {
01938
01939 if(EnvNAVXYTHETALAT.bInitialized == true)
01940 {
01941 SBPL_ERROR("ERROR: all parameters must be set before initialization of the environment\n");
01942 return false;
01943 }
01944
01945 SBPL_PRINTF("setting parameter %s to %d\n", parameter, value);
01946
01947 if(strcmp(parameter, "cost_inscribed_thresh") == 0)
01948 {
01949 if(value < 0 || value > 255)
01950 {
01951 SBPL_ERROR("ERROR: invalid value %d for parameter %s\n", value, parameter);
01952 return false;
01953 }
01954 EnvNAVXYTHETALATCfg.cost_inscribed_thresh = (unsigned char)value;
01955 }
01956 else if(strcmp(parameter, "cost_possibly_circumscribed_thresh") == 0)
01957 {
01958 if(value < 0 || value > 255)
01959 {
01960 SBPL_ERROR("ERROR: invalid value %d for parameter %s\n", value, parameter);
01961 return false;
01962 }
01963 EnvNAVXYTHETALATCfg.cost_possibly_circumscribed_thresh = value;
01964 }
01965 else if(strcmp(parameter, "cost_obsthresh") == 0)
01966 {
01967 if(value < 0 || value > 255)
01968 {
01969 SBPL_ERROR("ERROR: invalid value %d for parameter %s\n", value, parameter);
01970 return false;
01971 }
01972 EnvNAVXYTHETALATCfg.obsthresh = (unsigned char)value;
01973 }
01974 else
01975 {
01976 SBPL_ERROR("ERROR: invalid parameter %s\n", parameter);
01977 return false;
01978 }
01979
01980 return true;
01981 }
01982
01983 int EnvironmentNAVXYTHETALATTICE::GetEnvParameter(const char* parameter)
01984 {
01985
01986 if(strcmp(parameter, "cost_inscribed_thresh") == 0)
01987 {
01988 return (int) EnvNAVXYTHETALATCfg.cost_inscribed_thresh;
01989 }
01990 else if(strcmp(parameter, "cost_possibly_circumscribed_thresh") == 0)
01991 {
01992 return (int) EnvNAVXYTHETALATCfg.cost_possibly_circumscribed_thresh;
01993 }
01994 else if(strcmp(parameter, "cost_obsthresh") == 0)
01995 {
01996 return (int) EnvNAVXYTHETALATCfg.obsthresh;
01997 }
01998 else
01999 {
02000 SBPL_ERROR("ERROR: invalid parameter %s\n", parameter);
02001 throw new SBPL_Exception();
02002 }
02003
02004 }
02005
02006
02007
02008
02009
02010
02011
02012 EnvironmentNAVXYTHETALAT::~EnvironmentNAVXYTHETALAT()
02013 {
02014 SBPL_PRINTF("destroying XYTHETALAT\n");
02015
02016
02017 for (int i = 0; i < (int)StateID2CoordTable.size(); i++)
02018 {
02019 delete StateID2CoordTable.at(i);
02020 StateID2CoordTable.at(i) = NULL;
02021 }
02022 StateID2CoordTable.clear();
02023
02024
02025 if(Coord2StateIDHashTable != NULL)
02026 {
02027 delete [] Coord2StateIDHashTable;
02028 Coord2StateIDHashTable = NULL;
02029 }
02030 if(Coord2StateIDHashTable_lookup != NULL)
02031 {
02032 delete [] Coord2StateIDHashTable_lookup;
02033 Coord2StateIDHashTable_lookup = NULL;
02034 }
02035
02036 }
02037
02038
02039 void EnvironmentNAVXYTHETALAT::GetCoordFromState(int stateID, int& x, int& y, int& theta) const {
02040 EnvNAVXYTHETALATHashEntry_t* HashEntry = StateID2CoordTable[stateID];
02041 x = HashEntry->X;
02042 y = HashEntry->Y;
02043 theta = HashEntry->Theta;
02044 }
02045
02046 int EnvironmentNAVXYTHETALAT::GetStateFromCoord(int x, int y, int theta) {
02047
02048 EnvNAVXYTHETALATHashEntry_t* OutHashEntry;
02049 if((OutHashEntry = (this->*GetHashEntry)(x, y, theta)) == NULL){
02050
02051 OutHashEntry = (this->*CreateNewHashEntry)(x, y, theta);
02052 }
02053 return OutHashEntry->stateID;
02054 }
02055
02056 void EnvironmentNAVXYTHETALAT::ConvertStateIDPathintoXYThetaPath(vector<int>* stateIDPath, vector<EnvNAVXYTHETALAT3Dpt_t>* xythetaPath)
02057 {
02058 vector<EnvNAVXYTHETALATAction_t*> actionV;
02059 vector<int> CostV;
02060 vector<int> SuccIDV;
02061 int targetx_c, targety_c, targettheta_c;
02062 int sourcex_c, sourcey_c, sourcetheta_c;
02063
02064 SBPL_PRINTF("checks=%ld\n", checks);
02065
02066 xythetaPath->clear();
02067
02068 #if DEBUG
02069 SBPL_FPRINTF(fDeb, "converting stateid path into coordinates:\n");
02070 #endif
02071
02072 for(int pind = 0; pind < (int)(stateIDPath->size())-1; pind++)
02073 {
02074 int sourceID = stateIDPath->at(pind);
02075 int targetID = stateIDPath->at(pind+1);
02076
02077 #if DEBUG
02078 GetCoordFromState(sourceID, sourcex_c, sourcey_c, sourcetheta_c);
02079 #endif
02080
02081
02082
02083 SuccIDV.clear();
02084 CostV.clear();
02085 actionV.clear();
02086 GetSuccs(sourceID, &SuccIDV, &CostV, &actionV);
02087
02088 int bestcost = INFINITECOST;
02089 int bestsind = -1;
02090
02091 #if DEBUG
02092 GetCoordFromState(sourceID, sourcex_c, sourcey_c, sourcetheta_c);
02093 GetCoordFromState(targetID, targetx_c, targety_c, targettheta_c);
02094 SBPL_FPRINTF(fDeb, "looking for %d %d %d -> %d %d %d (numofsuccs=%d)\n", sourcex_c, sourcey_c, sourcetheta_c,
02095 targetx_c, targety_c, targettheta_c, SuccIDV.size());
02096
02097 #endif
02098
02099 for(int sind = 0; sind < (int)SuccIDV.size(); sind++)
02100 {
02101
02102 #if DEBUG
02103 int x_c, y_c, theta_c;
02104 GetCoordFromState(SuccIDV[sind], x_c, y_c, theta_c);
02105 SBPL_FPRINTF(fDeb, "succ: %d %d %d\n", x_c, y_c, theta_c);
02106 #endif
02107
02108 if(SuccIDV[sind] == targetID && CostV[sind] <= bestcost)
02109 {
02110 bestcost = CostV[sind];
02111 bestsind = sind;
02112 }
02113 }
02114 if(bestsind == -1)
02115 {
02116 SBPL_ERROR("ERROR: successor not found for transition:\n");
02117 GetCoordFromState(sourceID, sourcex_c, sourcey_c, sourcetheta_c);
02118 GetCoordFromState(targetID, targetx_c, targety_c, targettheta_c);
02119 SBPL_PRINTF("%d %d %d -> %d %d %d\n", sourcex_c, sourcey_c, sourcetheta_c,
02120 targetx_c, targety_c, targettheta_c);
02121 throw new SBPL_Exception();
02122 }
02123
02124
02125 int sourcex_c, sourcey_c, sourcetheta_c;
02126 GetCoordFromState(sourceID, sourcex_c, sourcey_c, sourcetheta_c);
02127 double sourcex, sourcey;
02128 sourcex = DISCXY2CONT(sourcex_c, EnvNAVXYTHETALATCfg.cellsize_m);
02129 sourcey = DISCXY2CONT(sourcey_c, EnvNAVXYTHETALATCfg.cellsize_m);
02130
02131 for(int ipind = 0; ipind < ((int)actionV[bestsind]->intermptV.size())-1; ipind++)
02132 {
02133
02134 EnvNAVXYTHETALAT3Dpt_t intermpt = actionV[bestsind]->intermptV[ipind];
02135 intermpt.x += sourcex;
02136 intermpt.y += sourcey;
02137
02138 #if DEBUG
02139 int nx = CONTXY2DISC(intermpt.x, EnvNAVXYTHETALATCfg.cellsize_m);
02140 int ny = CONTXY2DISC(intermpt.y, EnvNAVXYTHETALATCfg.cellsize_m);
02141 SBPL_FPRINTF(fDeb, "%.3f %.3f %.3f (%d %d %d cost=%d) ",
02142 intermpt.x, intermpt.y, intermpt.theta,
02143 nx, ny,
02144 ContTheta2Disc(intermpt.theta, NAVXYTHETALAT_THETADIRS), EnvNAVXYTHETALATCfg.Grid2D[nx][ny]);
02145 if(ipind == 0) SBPL_FPRINTF(fDeb, "first (heur=%d)\n", GetStartHeuristic(sourceID));
02146 else SBPL_FPRINTF(fDeb, "\n");
02147 #endif
02148
02149
02150 xythetaPath->push_back(intermpt);
02151 }
02152 }
02153 }
02154
02155
02156
02157 int EnvironmentNAVXYTHETALAT::SetGoal(double x_m, double y_m, double theta_rad){
02158
02159 int x = CONTXY2DISC(x_m, EnvNAVXYTHETALATCfg.cellsize_m);
02160 int y = CONTXY2DISC(y_m, EnvNAVXYTHETALATCfg.cellsize_m);
02161 int theta = ContTheta2Disc(theta_rad, NAVXYTHETALAT_THETADIRS);
02162
02163 SBPL_PRINTF("env: setting goal to %.3f %.3f %.3f (%d %d %d)\n", x_m, y_m, theta_rad, x, y, theta);
02164
02165 if(!IsWithinMapCell(x,y))
02166 {
02167 SBPL_ERROR("ERROR: trying to set a goal cell %d %d that is outside of map\n", x,y);
02168 return -1;
02169 }
02170
02171 if(!IsValidConfiguration(x,y,theta))
02172 {
02173 SBPL_PRINTF("WARNING: goal configuration is invalid\n");
02174 }
02175
02176 EnvNAVXYTHETALATHashEntry_t* OutHashEntry;
02177 if((OutHashEntry = (this->*GetHashEntry)(x, y, theta)) == NULL){
02178
02179 OutHashEntry = (this->*CreateNewHashEntry)(x, y, theta);
02180 }
02181
02182
02183 if(EnvNAVXYTHETALAT.goalstateid != OutHashEntry->stateID)
02184 {
02185 bNeedtoRecomputeStartHeuristics = true;
02186 bNeedtoRecomputeGoalHeuristics = true;
02187 }
02188
02189
02190
02191 EnvNAVXYTHETALAT.goalstateid = OutHashEntry->stateID;
02192
02193 EnvNAVXYTHETALATCfg.EndX_c = x;
02194 EnvNAVXYTHETALATCfg.EndY_c = y;
02195 EnvNAVXYTHETALATCfg.EndTheta = theta;
02196
02197
02198 return EnvNAVXYTHETALAT.goalstateid;
02199
02200 }
02201
02202
02203
02204 int EnvironmentNAVXYTHETALAT::SetStart(double x_m, double y_m, double theta_rad){
02205
02206 int x = CONTXY2DISC(x_m, EnvNAVXYTHETALATCfg.cellsize_m);
02207 int y = CONTXY2DISC(y_m, EnvNAVXYTHETALATCfg.cellsize_m);
02208 int theta = ContTheta2Disc(theta_rad, NAVXYTHETALAT_THETADIRS);
02209
02210 if(!IsWithinMapCell(x,y))
02211 {
02212 SBPL_ERROR("ERROR: trying to set a start cell %d %d that is outside of map\n", x,y);
02213 return -1;
02214 }
02215
02216 SBPL_PRINTF("env: setting start to %.3f %.3f %.3f (%d %d %d)\n", x_m, y_m, theta_rad, x, y, theta);
02217
02218 if(!IsValidConfiguration(x,y,theta))
02219 {
02220 SBPL_PRINTF("WARNING: start configuration %d %d %d is invalid\n", x,y,theta);
02221 }
02222
02223 EnvNAVXYTHETALATHashEntry_t* OutHashEntry;
02224 if((OutHashEntry = (this->*GetHashEntry)(x, y, theta)) == NULL){
02225
02226 OutHashEntry = (this->*CreateNewHashEntry)(x, y, theta);
02227 }
02228
02229
02230 if(EnvNAVXYTHETALAT.startstateid != OutHashEntry->stateID)
02231 {
02232 bNeedtoRecomputeStartHeuristics = true;
02233 bNeedtoRecomputeGoalHeuristics = true;
02234 }
02235
02236
02237 EnvNAVXYTHETALAT.startstateid = OutHashEntry->stateID;
02238 EnvNAVXYTHETALATCfg.StartX_c = x;
02239 EnvNAVXYTHETALATCfg.StartY_c = y;
02240 EnvNAVXYTHETALATCfg.StartTheta = theta;
02241
02242 return EnvNAVXYTHETALAT.startstateid;
02243
02244 }
02245
02246 void EnvironmentNAVXYTHETALAT::PrintState(int stateID, bool bVerbose, FILE* fOut )
02247 {
02248 #if DEBUG
02249 if(stateID >= (int)StateID2CoordTable.size())
02250 {
02251 SBPL_ERROR("ERROR in EnvNAVXYTHETALAT... function: stateID illegal (2)\n");
02252 throw new SBPL_Exception();
02253 }
02254 #endif
02255
02256 if(fOut == NULL)
02257 fOut = stdout;
02258
02259 EnvNAVXYTHETALATHashEntry_t* HashEntry = StateID2CoordTable[stateID];
02260
02261 if(stateID == EnvNAVXYTHETALAT.goalstateid && bVerbose)
02262 {
02263 SBPL_FPRINTF(fOut, "the state is a goal state\n");
02264 }
02265
02266 if(bVerbose)
02267 SBPL_FPRINTF(fOut, "X=%d Y=%d Theta=%d\n", HashEntry->X, HashEntry->Y, HashEntry->Theta);
02268 else
02269 SBPL_FPRINTF(fOut, "%.3f %.3f %.3f\n", DISCXY2CONT(HashEntry->X, EnvNAVXYTHETALATCfg.cellsize_m), DISCXY2CONT(HashEntry->Y,EnvNAVXYTHETALATCfg.cellsize_m),
02270 DiscTheta2Cont(HashEntry->Theta, NAVXYTHETALAT_THETADIRS));
02271
02272 }
02273
02274
02275 EnvNAVXYTHETALATHashEntry_t* EnvironmentNAVXYTHETALAT::GetHashEntry_lookup(int X, int Y, int Theta)
02276 {
02277
02278 int index = XYTHETA2INDEX(X,Y,Theta);
02279 return Coord2StateIDHashTable_lookup[index];
02280
02281 }
02282
02283
02284 EnvNAVXYTHETALATHashEntry_t* EnvironmentNAVXYTHETALAT::GetHashEntry_hash(int X, int Y, int Theta)
02285 {
02286
02287 #if TIME_DEBUG
02288 clock_t currenttime = clock();
02289 #endif
02290
02291 int binid = GETHASHBIN(X, Y, Theta);
02292
02293 #if DEBUG
02294 if ((int)Coord2StateIDHashTable[binid].size() > 5)
02295 {
02296 SBPL_FPRINTF(fDeb, "WARNING: Hash table has a bin %d (X=%d Y=%d) of size %d\n",
02297 binid, X, Y, Coord2StateIDHashTable[binid].size());
02298
02299 PrintHashTableHist(fDeb);
02300 }
02301 #endif
02302
02303
02304 vector<EnvNAVXYTHETALATHashEntry_t*>* binV = &Coord2StateIDHashTable[binid];
02305 for(int ind = 0; ind < (int)binV->size(); ind++)
02306 {
02307 EnvNAVXYTHETALATHashEntry_t* hashentry = binV->at(ind);
02308 if( hashentry->X == X && hashentry->Y == Y && hashentry->Theta == Theta)
02309 {
02310 #if TIME_DEBUG
02311 time_gethash += clock()-currenttime;
02312 #endif
02313 return hashentry;
02314 }
02315 }
02316
02317 #if TIME_DEBUG
02318 time_gethash += clock()-currenttime;
02319 #endif
02320
02321 return NULL;
02322 }
02323
02324 EnvNAVXYTHETALATHashEntry_t* EnvironmentNAVXYTHETALAT::CreateNewHashEntry_lookup(int X, int Y, int Theta)
02325 {
02326 int i;
02327
02328 #if TIME_DEBUG
02329 clock_t currenttime = clock();
02330 #endif
02331
02332 EnvNAVXYTHETALATHashEntry_t* HashEntry = new EnvNAVXYTHETALATHashEntry_t;
02333
02334 HashEntry->X = X;
02335 HashEntry->Y = Y;
02336 HashEntry->Theta = Theta;
02337 HashEntry->iteration = 0;
02338
02339 HashEntry->stateID = StateID2CoordTable.size();
02340
02341
02342 StateID2CoordTable.push_back(HashEntry);
02343
02344 int index = XYTHETA2INDEX(X,Y,Theta);
02345
02346 #if DEBUG
02347 if(Coord2StateIDHashTable_lookup[index] != NULL)
02348 {
02349 SBPL_ERROR("ERROR: creating hash entry for non-NULL hashentry\n");
02350 throw new SBPL_Exception();
02351 }
02352 #endif
02353
02354 Coord2StateIDHashTable_lookup[index] = HashEntry;
02355
02356
02357 int* entry = new int [NUMOFINDICES_STATEID2IND];
02358 StateID2IndexMapping.push_back(entry);
02359 for(i = 0; i < NUMOFINDICES_STATEID2IND; i++)
02360 {
02361 StateID2IndexMapping[HashEntry->stateID][i] = -1;
02362 }
02363
02364 if(HashEntry->stateID != (int)StateID2IndexMapping.size()-1)
02365 {
02366 SBPL_ERROR("ERROR in Env... function: last state has incorrect stateID\n");
02367 throw new SBPL_Exception();
02368 }
02369
02370 #if TIME_DEBUG
02371 time_createhash += clock()-currenttime;
02372 #endif
02373
02374 return HashEntry;
02375 }
02376
02377
02378
02379
02380 EnvNAVXYTHETALATHashEntry_t* EnvironmentNAVXYTHETALAT::CreateNewHashEntry_hash(int X, int Y, int Theta)
02381 {
02382 int i;
02383
02384 #if TIME_DEBUG
02385 clock_t currenttime = clock();
02386 #endif
02387
02388 EnvNAVXYTHETALATHashEntry_t* HashEntry = new EnvNAVXYTHETALATHashEntry_t;
02389
02390 HashEntry->X = X;
02391 HashEntry->Y = Y;
02392 HashEntry->Theta = Theta;
02393 HashEntry->iteration = 0;
02394
02395 HashEntry->stateID = StateID2CoordTable.size();
02396
02397
02398 StateID2CoordTable.push_back(HashEntry);
02399
02400
02401
02402 i = GETHASHBIN(HashEntry->X, HashEntry->Y, HashEntry->Theta);
02403
02404
02405 Coord2StateIDHashTable[i].push_back(HashEntry);
02406
02407
02408 int* entry = new int [NUMOFINDICES_STATEID2IND];
02409 StateID2IndexMapping.push_back(entry);
02410 for(i = 0; i < NUMOFINDICES_STATEID2IND; i++)
02411 {
02412 StateID2IndexMapping[HashEntry->stateID][i] = -1;
02413 }
02414
02415 if(HashEntry->stateID != (int)StateID2IndexMapping.size()-1)
02416 {
02417 SBPL_ERROR("ERROR in Env... function: last state has incorrect stateID\n");
02418 throw new SBPL_Exception();
02419 }
02420
02421 #if TIME_DEBUG
02422 time_createhash += clock()-currenttime;
02423 #endif
02424
02425 return HashEntry;
02426 }
02427
02428
02429
02430 void EnvironmentNAVXYTHETALAT::GetSuccs(int SourceStateID, vector<int>* SuccIDV, vector<int>* CostV, vector<EnvNAVXYTHETALATAction_t*>* actionV )
02431 {
02432 int aind;
02433
02434 #if TIME_DEBUG
02435 clock_t currenttime = clock();
02436 #endif
02437
02438
02439 SuccIDV->clear();
02440 CostV->clear();
02441 SuccIDV->reserve(EnvNAVXYTHETALATCfg.actionwidth);
02442 CostV->reserve(EnvNAVXYTHETALATCfg.actionwidth);
02443 if(actionV != NULL)
02444 {
02445 actionV->clear();
02446 actionV->reserve(EnvNAVXYTHETALATCfg.actionwidth);
02447 }
02448
02449
02450 if(SourceStateID == EnvNAVXYTHETALAT.goalstateid)
02451 return;
02452
02453
02454 EnvNAVXYTHETALATHashEntry_t* HashEntry = StateID2CoordTable[SourceStateID];
02455
02456
02457 for (aind = 0; aind < EnvNAVXYTHETALATCfg.actionwidth; aind++)
02458 {
02459 EnvNAVXYTHETALATAction_t* nav3daction = &EnvNAVXYTHETALATCfg.ActionsV[(unsigned int)HashEntry->Theta][aind];
02460 int newX = HashEntry->X + nav3daction->dX;
02461 int newY = HashEntry->Y + nav3daction->dY;
02462 int newTheta = NORMALIZEDISCTHETA(nav3daction->endtheta, NAVXYTHETALAT_THETADIRS);
02463
02464
02465 if(!IsValidCell(newX, newY))
02466 continue;
02467
02468
02469 int cost = GetActionCost(HashEntry->X, HashEntry->Y, HashEntry->Theta, nav3daction);
02470 if(cost >= INFINITECOST)
02471 continue;
02472
02473 EnvNAVXYTHETALATHashEntry_t* OutHashEntry;
02474 if((OutHashEntry = (this->*GetHashEntry)(newX, newY, newTheta)) == NULL)
02475 {
02476
02477 OutHashEntry = (this->*CreateNewHashEntry)(newX, newY, newTheta);
02478 }
02479
02480 SuccIDV->push_back(OutHashEntry->stateID);
02481 CostV->push_back(cost);
02482 if(actionV != NULL)
02483 actionV->push_back(nav3daction);
02484 }
02485
02486 #if TIME_DEBUG
02487 time_getsuccs += clock()-currenttime;
02488 #endif
02489
02490 }
02491
02492 void EnvironmentNAVXYTHETALAT::GetPreds(int TargetStateID, vector<int>* PredIDV, vector<int>* CostV)
02493 {
02494
02495
02496
02497
02498 int aind;
02499
02500 #if TIME_DEBUG
02501 clock_t currenttime = clock();
02502 #endif
02503
02504
02505 EnvNAVXYTHETALATHashEntry_t* HashEntry = StateID2CoordTable[TargetStateID];
02506
02507
02508 PredIDV->clear();
02509 CostV->clear();
02510 PredIDV->reserve(EnvNAVXYTHETALATCfg.PredActionsV[(unsigned int)HashEntry->Theta].size());
02511 CostV->reserve(EnvNAVXYTHETALATCfg.PredActionsV[(unsigned int)HashEntry->Theta].size());
02512
02513
02514 vector<EnvNAVXYTHETALATAction_t*>* actionsV = &EnvNAVXYTHETALATCfg.PredActionsV[(unsigned int)HashEntry->Theta];
02515 for (aind = 0; aind < (int)EnvNAVXYTHETALATCfg.PredActionsV[(unsigned int)HashEntry->Theta].size(); aind++)
02516 {
02517
02518 EnvNAVXYTHETALATAction_t* nav3daction = actionsV->at(aind);
02519
02520 int predX = HashEntry->X - nav3daction->dX;
02521 int predY = HashEntry->Y - nav3daction->dY;
02522 int predTheta = nav3daction->starttheta;
02523
02524
02525
02526 if(!IsValidCell(predX, predY))
02527 continue;
02528
02529
02530 int cost = GetActionCost(predX, predY, predTheta, nav3daction);
02531 if(cost >= INFINITECOST)
02532 continue;
02533
02534 EnvNAVXYTHETALATHashEntry_t* OutHashEntry;
02535 if((OutHashEntry = (this->*GetHashEntry)(predX, predY, predTheta)) == NULL)
02536 {
02537
02538 OutHashEntry = (this->*CreateNewHashEntry)(predX, predY, predTheta);
02539 }
02540
02541 PredIDV->push_back(OutHashEntry->stateID);
02542 CostV->push_back(cost);
02543 }
02544
02545 #if TIME_DEBUG
02546 time_getsuccs += clock()-currenttime;
02547 #endif
02548
02549
02550 }
02551
02552 void EnvironmentNAVXYTHETALAT::SetAllActionsandAllOutcomes(CMDPSTATE* state)
02553 {
02554
02555 int cost;
02556
02557 #if DEBUG
02558 if(state->StateID >= (int)StateID2CoordTable.size())
02559 {
02560 SBPL_ERROR("ERROR in Env... function: stateID illegal\n");
02561 throw new SBPL_Exception();
02562 }
02563
02564 if((int)state->Actions.size() != 0)
02565 {
02566 SBPL_ERROR("ERROR in Env_setAllActionsandAllOutcomes: actions already exist for the state\n");
02567 throw new SBPL_Exception();
02568 }
02569 #endif
02570
02571
02572
02573 if(state->StateID == EnvNAVXYTHETALAT.goalstateid)
02574 return;
02575
02576
02577 EnvNAVXYTHETALATHashEntry_t* HashEntry = StateID2CoordTable[state->StateID];
02578
02579
02580 for (int aind = 0; aind < EnvNAVXYTHETALATCfg.actionwidth; aind++)
02581 {
02582 EnvNAVXYTHETALATAction_t* nav3daction = &EnvNAVXYTHETALATCfg.ActionsV[(unsigned int)HashEntry->Theta][aind];
02583 int newX = HashEntry->X + nav3daction->dX;
02584 int newY = HashEntry->Y + nav3daction->dY;
02585 int newTheta = NORMALIZEDISCTHETA(nav3daction->endtheta, NAVXYTHETALAT_THETADIRS);
02586
02587
02588 if(!IsValidCell(newX, newY))
02589 continue;
02590
02591
02592 cost = GetActionCost(HashEntry->X, HashEntry->Y, HashEntry->Theta, nav3daction);
02593 if(cost >= INFINITECOST)
02594 continue;
02595
02596
02597 CMDPACTION* action = state->AddAction(aind);
02598
02599 #if TIME_DEBUG
02600 clock_t currenttime = clock();
02601 #endif
02602
02603 EnvNAVXYTHETALATHashEntry_t* OutHashEntry;
02604 if((OutHashEntry = (this->*GetHashEntry)(newX, newY, newTheta)) == NULL)
02605 {
02606
02607 OutHashEntry = (this->*CreateNewHashEntry)(newX, newY, newTheta);
02608 }
02609 action->AddOutcome(OutHashEntry->stateID, cost, 1.0);
02610
02611 #if TIME_DEBUG
02612 time3_addallout += clock()-currenttime;
02613 #endif
02614
02615 }
02616 }
02617
02618
02619 void EnvironmentNAVXYTHETALAT::GetPredsofChangedEdges(vector<nav2dcell_t> const * changedcellsV, vector<int> *preds_of_changededgesIDV)
02620 {
02621 nav2dcell_t cell;
02622 EnvNAVXYTHETALAT3Dcell_t affectedcell;
02623 EnvNAVXYTHETALATHashEntry_t* affectedHashEntry;
02624
02625
02626 iteration++;
02627
02628 for(int i = 0; i < (int)changedcellsV->size(); i++)
02629 {
02630 cell = changedcellsV->at(i);
02631
02632
02633 for(int sind = 0; sind < (int)affectedpredstatesV.size(); sind++)
02634 {
02635 affectedcell = affectedpredstatesV.at(sind);
02636
02637
02638 affectedcell.x = affectedcell.x + cell.x;
02639 affectedcell.y = affectedcell.y + cell.y;
02640
02641
02642 affectedHashEntry = (this->*GetHashEntry)(affectedcell.x, affectedcell.y, affectedcell.theta);
02643 if(affectedHashEntry != NULL && affectedHashEntry->iteration < iteration)
02644 {
02645 preds_of_changededgesIDV->push_back(affectedHashEntry->stateID);
02646 affectedHashEntry->iteration = iteration;
02647 }
02648 }
02649 }
02650 }
02651
02652 void EnvironmentNAVXYTHETALAT::GetSuccsofChangedEdges(vector<nav2dcell_t> const * changedcellsV, vector<int> *succs_of_changededgesIDV)
02653 {
02654 nav2dcell_t cell;
02655 EnvNAVXYTHETALAT3Dcell_t affectedcell;
02656 EnvNAVXYTHETALATHashEntry_t* affectedHashEntry;
02657
02658 SBPL_ERROR("ERROR: getsuccs is not supported currently\n");
02659 throw new SBPL_Exception();
02660
02661
02662 iteration++;
02663
02664
02665 for(int i = 0; i < (int)changedcellsV->size(); i++)
02666 {
02667 cell = changedcellsV->at(i);
02668
02669
02670 for(int sind = 0; sind < (int)affectedsuccstatesV.size(); sind++)
02671 {
02672 affectedcell = affectedsuccstatesV.at(sind);
02673
02674
02675 affectedcell.x = affectedcell.x + cell.x;
02676 affectedcell.y = affectedcell.y + cell.y;
02677
02678
02679 affectedHashEntry = (this->*GetHashEntry)(affectedcell.x, affectedcell.y, affectedcell.theta);
02680 if(affectedHashEntry != NULL && affectedHashEntry->iteration < iteration)
02681 {
02682 succs_of_changededgesIDV->push_back(affectedHashEntry->stateID);
02683 affectedHashEntry->iteration = iteration;
02684 }
02685 }
02686 }
02687 }
02688
02689 void EnvironmentNAVXYTHETALAT::InitializeEnvironment()
02690 {
02691 EnvNAVXYTHETALATHashEntry_t* HashEntry;
02692
02693 int maxsize = EnvNAVXYTHETALATCfg.EnvWidth_c*EnvNAVXYTHETALATCfg.EnvHeight_c*NAVXYTHETALAT_THETADIRS;
02694
02695 if(maxsize <= SBPL_XYTHETALAT_MAXSTATESFORLOOKUP)
02696 {
02697 SBPL_PRINTF("environment stores states in lookup table\n");
02698
02699 Coord2StateIDHashTable_lookup = new EnvNAVXYTHETALATHashEntry_t*[maxsize];
02700 for(int i = 0; i < maxsize; i++)
02701 Coord2StateIDHashTable_lookup[i] = NULL;
02702 GetHashEntry = &EnvironmentNAVXYTHETALAT::GetHashEntry_lookup;
02703 CreateNewHashEntry = &EnvironmentNAVXYTHETALAT::CreateNewHashEntry_lookup;
02704
02705
02706 HashTableSize = 0;
02707 Coord2StateIDHashTable = NULL;
02708 }
02709 else
02710 {
02711 SBPL_PRINTF("environment stores states in hashtable\n");
02712
02713
02714 HashTableSize = 4*1024*1024;
02715 Coord2StateIDHashTable = new vector<EnvNAVXYTHETALATHashEntry_t*>[HashTableSize];
02716 GetHashEntry = &EnvironmentNAVXYTHETALAT::GetHashEntry_hash;
02717 CreateNewHashEntry = &EnvironmentNAVXYTHETALAT::CreateNewHashEntry_hash;
02718
02719
02720 Coord2StateIDHashTable_lookup = NULL;
02721 }
02722
02723
02724
02725 StateID2CoordTable.clear();
02726
02727
02728 if((HashEntry = (this->*GetHashEntry)(EnvNAVXYTHETALATCfg.StartX_c, EnvNAVXYTHETALATCfg.StartY_c, EnvNAVXYTHETALATCfg.StartTheta)) == NULL){
02729
02730 HashEntry = (this->*CreateNewHashEntry)(EnvNAVXYTHETALATCfg.StartX_c, EnvNAVXYTHETALATCfg.StartY_c, EnvNAVXYTHETALATCfg.StartTheta);
02731 }
02732 EnvNAVXYTHETALAT.startstateid = HashEntry->stateID;
02733
02734
02735 if((HashEntry = (this->*GetHashEntry)(EnvNAVXYTHETALATCfg.EndX_c, EnvNAVXYTHETALATCfg.EndY_c, EnvNAVXYTHETALATCfg.EndTheta)) == NULL){
02736
02737 HashEntry = (this->*CreateNewHashEntry)(EnvNAVXYTHETALATCfg.EndX_c, EnvNAVXYTHETALATCfg.EndY_c, EnvNAVXYTHETALATCfg.EndTheta);
02738 }
02739 EnvNAVXYTHETALAT.goalstateid = HashEntry->stateID;
02740
02741
02742 EnvNAVXYTHETALAT.bInitialized = true;
02743
02744 }
02745
02746
02747
02748
02749
02750 unsigned int EnvironmentNAVXYTHETALAT::GETHASHBIN(unsigned int X1, unsigned int X2, unsigned int Theta)
02751 {
02752
02753 return inthash(inthash(X1)+(inthash(X2)<<1)+(inthash(Theta)<<2)) & (HashTableSize-1);
02754 }
02755
02756 void EnvironmentNAVXYTHETALAT::PrintHashTableHist(FILE* fOut)
02757 {
02758 int s0=0, s1=0, s50=0, s100=0, s200=0, s300=0, slarge=0;
02759
02760 for(int j = 0; j < HashTableSize; j++)
02761 {
02762 if((int)Coord2StateIDHashTable[j].size() == 0)
02763 s0++;
02764 else if((int)Coord2StateIDHashTable[j].size() < 5)
02765 s1++;
02766 else if((int)Coord2StateIDHashTable[j].size() < 25)
02767 s50++;
02768 else if((int)Coord2StateIDHashTable[j].size() < 50)
02769 s100++;
02770 else if((int)Coord2StateIDHashTable[j].size() < 100)
02771 s200++;
02772 else if((int)Coord2StateIDHashTable[j].size() < 400)
02773 s300++;
02774 else
02775 slarge++;
02776 }
02777 SBPL_FPRINTF(fOut, "hash table histogram: 0:%d, <5:%d, <25:%d, <50:%d, <100:%d, <400:%d, >400:%d\n",
02778 s0,s1, s50, s100, s200,s300,slarge);
02779 }
02780
02781 int EnvironmentNAVXYTHETALAT::GetFromToHeuristic(int FromStateID, int ToStateID)
02782 {
02783
02784 #if USE_HEUR==0
02785 return 0;
02786 #endif
02787
02788
02789 #if DEBUG
02790 if(FromStateID >= (int)StateID2CoordTable.size()
02791 || ToStateID >= (int)StateID2CoordTable.size())
02792 {
02793 SBPL_ERROR("ERROR in EnvNAVXYTHETALAT... function: stateID illegal\n");
02794 throw new SBPL_Exception();
02795 }
02796 #endif
02797
02798
02799 EnvNAVXYTHETALATHashEntry_t* FromHashEntry = StateID2CoordTable[FromStateID];
02800 EnvNAVXYTHETALATHashEntry_t* ToHashEntry = StateID2CoordTable[ToStateID];
02801
02802
02803
02804
02805 return (int)(NAVXYTHETALAT_COSTMULT_MTOMM*EuclideanDistance_m(FromHashEntry->X, FromHashEntry->Y, ToHashEntry->X, ToHashEntry->Y)/EnvNAVXYTHETALATCfg.nominalvel_mpersecs);
02806
02807 }
02808
02809
02810 int EnvironmentNAVXYTHETALAT::GetGoalHeuristic(int stateID)
02811 {
02812 #if USE_HEUR==0
02813 return 0;
02814 #endif
02815
02816 #if DEBUG
02817 if(stateID >= (int)StateID2CoordTable.size())
02818 {
02819 SBPL_ERROR("ERROR in EnvNAVXYTHETALAT... function: stateID illegal\n");
02820 throw new SBPL_Exception();
02821 }
02822 #endif
02823
02824 EnvNAVXYTHETALATHashEntry_t* HashEntry = StateID2CoordTable[stateID];
02825 int h2D = grid2Dsearchfromgoal->getlowerboundoncostfromstart_inmm(HashEntry->X, HashEntry->Y);
02826 int hEuclid = (int)(NAVXYTHETALAT_COSTMULT_MTOMM*EuclideanDistance_m(HashEntry->X, HashEntry->Y, EnvNAVXYTHETALATCfg.EndX_c, EnvNAVXYTHETALATCfg.EndY_c));
02827
02828
02829 return (int)(((double)__max(h2D,hEuclid))/EnvNAVXYTHETALATCfg.nominalvel_mpersecs);
02830
02831 }
02832
02833
02834 int EnvironmentNAVXYTHETALAT::GetStartHeuristic(int stateID)
02835 {
02836
02837
02838 #if USE_HEUR==0
02839 return 0;
02840 #endif
02841
02842
02843 #if DEBUG
02844 if(stateID >= (int)StateID2CoordTable.size())
02845 {
02846 SBPL_ERROR("ERROR in EnvNAVXYTHETALAT... function: stateID illegal\n");
02847 throw new SBPL_Exception();
02848 }
02849 #endif
02850
02851 EnvNAVXYTHETALATHashEntry_t* HashEntry = StateID2CoordTable[stateID];
02852 int h2D = grid2Dsearchfromstart->getlowerboundoncostfromstart_inmm(HashEntry->X, HashEntry->Y);
02853 int hEuclid = (int)(NAVXYTHETALAT_COSTMULT_MTOMM*EuclideanDistance_m(EnvNAVXYTHETALATCfg.StartX_c, EnvNAVXYTHETALATCfg.StartY_c, HashEntry->X, HashEntry->Y));
02854
02855
02856 return (int)(((double)__max(h2D,hEuclid))/EnvNAVXYTHETALATCfg.nominalvel_mpersecs);
02857
02858 }
02859
02860 int EnvironmentNAVXYTHETALAT::SizeofCreatedEnv()
02861 {
02862 return (int)StateID2CoordTable.size();
02863
02864 }
02865
