VisualLanes.cc
Go to the documentation of this file.
00001 #include <angles/angles.h>
00002 #include <art_map/coordinates.h>
00003 #include <art_map/euclidean_distance.h>
00004 #include <art_map/VisualLanes.h>
00005 
00006 #define ONE_GRID 0.9
00007 #define TWO_GRID 1.8
00008 
00009 VisualLanes::VisualLanes(double physical_size,
00010                              int resolution) :
00011   _m(NULL),
00012   _physical_size(physical_size),
00013   _resolution(resolution),
00014   _x(0),
00015   _y(0),
00016   _theta(0),
00017   _x_offset(0),
00018   _y_offset(0),
00019   _shift(0),
00020   scan_off_right_side(false),
00021   scan_off_left_side(false),
00022   scan_off_bottom_side(false),
00023   scan_off_top_side(false),
00024   count(0) {
00025   int r;
00026   _m = new cell*[_resolution];
00027   for(r = 0; r < _resolution; r++) {
00028     _m[r] = NULL;
00029     _m[r] = new cell[_resolution];
00030   }
00031 }
00032 
00033 VisualLanes::~VisualLanes() {
00034   int r;
00035   for(r = 0; r < _resolution; r++) {
00036     delete[] _m[r];
00037   }
00038   delete[] _m;
00039 }
00040 
00041 void VisualLanes::clear() {
00042   int r, c;
00043   for(r = 0; r < _resolution; r++) {
00044     for(c = 0; c < _resolution; c++) {
00045       _m[r][c] = OCCUPANCY_UNKNOWN;
00046     }
00047   }
00048 }
00049 
00050 void VisualLanes::initialize(double x, double y, double theta) {
00051   _theta = theta;
00052   _x = x;
00053   _y = y;
00054   _x_offset = 0;
00055   _y_offset = 0;
00056   clear();
00057 }
00058 
00059 /*
00060  * Note, that all of these fuctions require that _resolution/2 -
00061  * (laser_range/_physical_size) >= 0); Also, make sure that if a scan
00062  * range goes off... of a side that executing these searches on the
00063  * map don't overlap with the robot's position. This can be arranged
00064  * by using an appropriate size.
00065  */
00066 
00067 void VisualLanes::clearBottom()
00068 {
00069   for(int num = _resolution/2; num < _resolution; num++)
00070     for(int num2 = 0; num2 < _resolution; num2++)       
00071       _m[num][num2] = 0;
00072   
00073 }
00074 
00075 void VisualLanes::clearTop()
00076 {
00077   for(int num = 0; num < _resolution/2; num++)
00078     for(int num2 = 0; num2 < _resolution; num2++)
00079       _m[num][num2] = 0;
00080 }
00081 
00082 void VisualLanes::clearRight()
00083 {
00084   for(int num = 0; num < _resolution; num++)
00085     for(int num2 = _resolution/2; num2 < _resolution; num2++)
00086       _m[num][num2] = 0;
00087 }
00088 
00089 void VisualLanes::clearLeft()
00090 {
00091   for(int num = 0; num < _resolution; num++)
00092     for(int num2 = 0; num2 < _resolution/2; num2++)
00093       _m[num][num2] = 0;
00094 }
00095 
00096 void VisualLanes::setPosition(double x, double y, double theta) {
00097   _theta = theta;
00098   rX = x;
00099   rY = y;
00100   if(fabs(_x - x) >= _physical_size) {
00101     double _x_offset_temp = _x_offset + ((x - _x) / _physical_size);
00102     _x_offset = (int)_x_offset_temp;
00103     //printf("_x_offset %i\n", _x_offset);
00104     _x = x - fmod(x, _physical_size);
00105   }
00106   
00107   if(fabs(_y - y) >= _physical_size) {
00108     double _y_offset_temp = _y_offset + ((y - _y) / _physical_size);
00109     _y_offset = (int)_y_offset_temp;
00110     //printf("_y_offset %i\n", _y_offset);
00111     _y = y - fmod(y, _physical_size);
00112   }
00113   
00114   
00115   
00116   double cellX = _resolution / 2 + _x_offset;
00117   double cellY = _resolution / 2 + _y_offset;
00118   
00119   int cellXtemp = (int)(cellX) % _resolution;
00120   int cellYtemp = (int)(cellY) % _resolution;
00121   
00122   if(cellXtemp < 0)
00123     {
00124       printf("The value of cellX %f\n", cellX);
00125     }
00126   if(cellYtemp < 0)
00127     printf("The value of cellY %f\n", cellY);
00128   
00129   if( (_resolution/2 + _x_offset) > _resolution
00130       && (_resolution/2 + _y_offset)  < _resolution
00131       && (_resolution/2 + _y_offset)  >= 0)
00132     {
00133       clearBottom();
00134       _x_offset = -_resolution/2;
00135       printf("The bottom was cleared by position\n");
00136       scan_off_right_side = false;
00137       scan_off_bottom_side = false;
00138       scan_off_left_side = false;
00139       scan_off_top_side = false;
00140     }
00141   else if( (_resolution/2 + _x_offset) < 0
00142            && (_resolution/2 + _y_offset)  >= 0
00143            && (_resolution/2 + _y_offset)  < _resolution)
00144     {
00145       clearTop();
00146       _x_offset = (_resolution - 1) -  _resolution / 2;
00147       scan_off_top_side = false;
00148       scan_off_bottom_side = false;
00149       scan_off_left_side = false;
00150       scan_off_right_side = false;
00151     }
00152   else if( (_resolution/2 + _y_offset) < 0
00153            && (_resolution/2 + _x_offset) >= 0
00154            && (_resolution/2 + _x_offset) < _resolution)
00155     {
00156       clearLeft();
00157       _y_offset = (_resolution - 1) - _resolution / 2;
00158       printf("The _y_offset %i\n", _y_offset);
00159       scan_off_top_side = false;
00160       scan_off_bottom_side = false;
00161       scan_off_left_side = false;
00162       scan_off_right_side = false;
00163     }
00164   else if( (_resolution/2 + _x_offset) < _resolution
00165            && (_resolution/2 + _y_offset) > _resolution
00166            && (_resolution/2 + _x_offset) >= 0)
00167     {
00168       clearRight();
00169       printf("The right was cleared by position: %i\n", count);
00170       _y_offset = -_resolution/2;
00171       printf("value: %i \n", _resolution/2 + _y_offset);
00172       scan_off_right_side = false;
00173       scan_off_bottom_side = false;
00174       scan_off_left_side = false;
00175       scan_off_top_side = false;
00176       count++;
00177     }
00178   
00179 }
00180 
00181 std::vector<float>* VisualLanes::getPose()
00182 {
00183   std::vector<float>* temp = new std::vector<float>();
00184   temp->push_back(rX);
00185   temp->push_back(_x_offset);
00186   temp->push_back(rY);
00187   temp->push_back(_y_offset);
00188   temp->push_back(_theta);
00189   return temp;
00190 }
00191 
00192 void VisualLanes::addSickScan(std::vector<double> ranges) {
00193   int l;
00194   double temp_theta = _theta - angles::from_degrees(90);
00195   for (l = 0; l < 180; l++) {
00196     
00197     double tempTheta = temp_theta + angles::from_degrees(l);
00198     //if(l == 0)
00199     //printf("temp theta %f\n", temp_theta);
00200     Coordinates::normalize(temp_theta);
00201     
00202     /*double t = M_PI * (1.0 - l/180.0) + _theta;
00203       double x = (ranges[l] * sin(t)) / _physical_size;
00204       double y = (ranges[l] * cos(t)) / _physical_size;*/
00205     
00206     /*if(l == 90)
00207       {
00208       printf("Theta %f tempTheta %f\n", _theta, tempTheta);
00209       printf("Theta %f super Theta %f\n", _theta,
00210              (_theta - angles::from_degrees(90)) + angles::from_degrees(90));
00211       }*/
00212     //if(ranges[l] != 0)
00213     //{
00214     double x = (ranges[l] * cos(tempTheta)) / _physical_size;
00215     double y = (ranges[l] * sin(tempTheta)) / _physical_size;
00216     
00217     //lighten(x, y);
00218     if( ( x + ( _resolution/2 + _x_offset)%_resolution) > _resolution
00219         && y < _resolution && y >= 0 && !scan_off_bottom_side)
00220       {
00221         clearTop();
00222         scan_off_bottom_side = true;
00223       }
00224     if( (x < _resolution
00225          && (y + (_resolution/2 + _y_offset)%_resolution) > _resolution
00226          && x >= 0 && !scan_off_right_side))
00227       { 
00228         clearLeft();
00229         scan_off_right_side = true;
00230       }
00231     if( x < _resolution
00232         && (y + (_resolution/2 + _y_offset)%_resolution ) < 0
00233         && x >= 0 && !scan_off_left_side)
00234       {
00235         clearRight();
00236         scan_off_left_side = true;
00237       }
00238     if( (x + (_resolution/2 + _x_offset)%_resolution ) < 0
00239         && y >= 0
00240         && y < _resolution && !scan_off_top_side)
00241       {
00242         clearBottom();
00243         scan_off_top_side = true;
00244       }
00245     
00246     
00247     //fp = VisualLanes::cellLighten;
00248     
00249     
00250     VisualLanes::line( 0, 0, (int)x, (int)y, (&VisualLanes::cellLighten));
00251     
00252     //lighten(x, y);
00253     double distance = Euclidean::DistanceTo(x,y,0,0);
00254 if ( ( distance < (laser_range / _physical_size - EPSILON)) && ranges[l] != 0)
00255       {
00256         cell* c = at((int)x, (int)y);
00257         if(c != NULL)
00258           {
00259             if((*c) < 0)
00260               (*c) = 3.5;
00261             else
00262               {
00263                 //(*c) = std::min(LOGODDS_MAX_OCCUPANCY, (*c) +
00264                 //       LOGODDS_OCCUPANCY_INCREMENT);
00265                 //printf("moo\n");
00266                 (*c) = LOGODDS_MAX_OCCUPANCY;
00267               }
00268           }     
00269       }
00270     //}
00271   }
00272 }
00273 
00274 
00275 // This should be deleted but I am leaving it in so that everyone can
00276 // see the old way to do ligthen/line trace
00277 
00278 /*void VisualLanes::lighten(int x, int y) {
00279   int step_count = floor(sqrt(x * x + y * y)) + 2;
00280   double dx = x / (step_count - 1.0);
00281   double dy = y / (step_count - 1.0);
00282   int j;
00283   
00284   for (j = 0; j < step_count; j++) {
00285   int nx = dx * j;
00286   int ny = dy * j;
00287   
00288   cell* c = at(nx, ny);
00289   if(c != NULL) {
00290   //cellLighten(nx, ny);
00291   (*c) = std::max((*c)-LOGODDS_OCCUPANCY_DECREMENT, LOGODDS_MIN_OCCUPANCY);
00292   //if((*c - OCCUPANCY_DECREMENT) > MIN_OCCUPANCY)
00293   //    (*c) -= OCCUPANCY_DECREMENT;
00294   }
00295   }
00296   } */
00297 
00298 
00299 void VisualLanes::setThreshold(int threshold)
00300 { _threshold = threshold; }
00301 
00309 std::pair<int,int>* VisualLanes::cellLighten(int x, int y)
00310 {
00311   cell* c = at(x,y);
00312   if(c != NULL)
00313     {
00314       (*c) = std::max((*c)-LOGODDS_OCCUPANCY_DECREMENT, LOGODDS_MIN_OCCUPANCY);
00315       //(*c) = LOGODDS_MIN_OCCUPANCY;
00316     } 
00317   return NULL;  
00318 }
00319 
00327 std::pair<int,int>* VisualLanes::cellOccupied(int x, int y)
00328 {
00329   cell* c = atgoal(x,y);
00330   if(c != NULL)
00331     {
00332       //cellLightenDebug(x,y);
00333       if( *c >= _threshold )
00334         {
00335           //printf("x: %i, y: %i 's value: %i\n", x, y, *c);
00336           //printf("_threshold: %i: \n", _threshold);
00337           std::pair<int,int>* result = new std::pair<int,int>();
00338           result->first = x;
00339           result->second = y;
00340           return result;
00341         }
00342     }
00343   return NULL;
00344 }
00345 
00346 std::pair<int,int>* VisualLanes::cellOccupiedDebug(int x, int y)
00347 {
00348   cell* c = atgoal(x,y);
00349   if(c != NULL)
00350     {
00351       //printf("x: %i, y: %i 's value: %i\n", x, y, *c);
00352       cellLightenDebug(x,y);
00353       if( *c >= _threshold )
00354         {
00355           std::pair<int,int>* result = new std::pair<int,int>();
00356           result->first = x;
00357           result->second = y;
00358           return result;
00359         }
00360     }
00361   return NULL;
00362 }
00363 
00364 std::pair<int,int>* VisualLanes::cellLightenDebug(int x, int y)
00365 {
00366   cell* c = atgoal(x,y);
00367   if(c != NULL)
00368     {
00369       //(*c) = std::max((*c)-LOGODDS_OCCUPANCY_DECREMENT, LOGODDS_MIN_OCCUPANCY);
00370       (*c) = LOGODDS_MIN_OCCUPANCY;
00371     } 
00372   return NULL;  
00373 }
00374 
00380 std::pair<int,int>* VisualLanes::line(int x0, int y0, int x1, int y1,
00381                                       std::pair<int,int>*
00382                                       (VisualLanes::*_fp) (int,int))
00383 {
00384   //This is how you invoke a pointer to a memeber function!!!
00385   //(this->*_fp)(1,1);
00386   
00387   
00388   int dy = y1 - y0;
00389   int dx = x1 - x0;
00390   int stepx, stepy;
00391   
00392   std::pair<int,int> *result = NULL;
00393   
00394   if (dy < 0) { dy = -dy;  stepy = -1; } else { stepy = 1; }
00395   if (dx < 0) { dx = -dx;  stepx = -1; } else { stepx = 1; }
00396   dy <<= 1;                                                  // dy is now 2*dy
00397   dx <<= 1;                                                  // dx is now 2*dx
00398   
00399   (this->*_fp)(x0, y0); //never check for a return value on this
00400                         //cell... until its changed to where the car
00401                         //is not there, at this point in time.
00402   if (dx > dy) {
00403     int fraction = dy - (dx >> 1);      // same as 2*dy - dx
00404     while (x0 != x1) {
00405       if (fraction >= 0) {
00406         y0 += stepy;
00407         fraction -= dx;                 // same as fraction -= 2*dx
00408       }
00409       x0 += stepx;
00410       fraction += dy;                   // same as fraction -= 2*dy
00411       if( (result = (this->*(_fp))(x0, y0)) != NULL  )
00412         return result;
00413     }
00414   } else {
00415     int fraction = dx - (dy >> 1);
00416     while (y0 != y1) {
00417       if (fraction >= 0) {
00418         x0 += stepx;
00419         fraction -= dy;
00420       }
00421       y0 += stepy;
00422       fraction += dx;
00423       if( (result = (this->*(_fp))(x0, y0)) != NULL  )
00424         return result;
00425     }
00426   }
00427   return result;     
00428 }
00429 
00430 cell VisualLanes::value(int x, int y) {
00431   if(valid(x, y)) {
00432     return *(at(x,y));
00433   }
00434   else return OCCUPANCY_UNKNOWN;
00435 }
00436 
00437 bool VisualLanes::valid(int x, int y) {
00438   return (x < _resolution / 2 &&
00439           y < _resolution / 2 &&
00440           x > -_resolution / 2 &&
00441           y > -_resolution / 2);
00442 }
00443 
00444 void VisualLanes::savePGM(const char *filename) {
00445   int i, j;
00446   cell c;
00447   unsigned char d;
00448   FILE *file;
00449   
00450   file = fopen(filename, "w+");
00451   if (file == NULL) {
00452     fprintf(stderr, "error writing %s : %s", filename, strerror(errno));
00453     return;
00454   }
00455   
00456   fprintf(file, "P5 %d %d 255\n", _resolution, _resolution);
00457   for (j = _resolution - 1; j >= 0; j--) {
00458     for (i = 0; i < _resolution; i++) {
00459       c = _m[i][j];
00460       
00461       d = (unsigned char)((unsigned int)(((20 - c)*255/40)));
00462       
00463       fwrite(&d, 1, 1,  file);
00464     }
00465   }
00466   fclose(file);
00467 }
00468 
00469 
00470 
00471 void VisualLanes::padObstacles()
00472 {
00473   
00474 }
00475 
00476 std::pair<int,int>* VisualLanes::cellOccupiedRelative(int x, int y)
00477 {
00478   cell* c = at(x,y);
00479   if(c != NULL)
00480     {
00481       if( *c >= _threshold )
00482         {
00483           std::pair<int,int>* result = new std::pair<int,int>();
00484           result->first = x;
00485           result->second = y;
00486           return result;
00487         }
00488       //else
00489       //   printf("c value %f\n", *c);
00490     }
00491   return NULL;
00492 }
00493 
00494 std::pair<double,double>* VisualLanes::laserScan(double x, double y)
00495 {
00496   std::pair<double,double>* result = new std::pair<double,double>();
00497   
00498   result->first = 0;
00499   result->second = 0;
00500   
00501   std::pair<int,int>* temp;
00502   //uses a different occupied that looks locally to the robot rather than
00503   //some global point in the map!!
00504   temp = VisualLanes::line(0, 0, (int)x, (int)y,
00505                            (&VisualLanes::cellOccupiedRelative));
00506   if(temp != NULL)
00507     {
00508       //double x_offsetObstacle = (2 * temp->first - _resolution)
00509       //                           /(_physical_size + 2);
00510       //double y_offsetObstacle = (2 * temp->second - _resolution)
00511       //                           /(_physical_size + 2);
00512       
00513       result->first = temp->first;
00514       result->second = temp->second;
00515       //printf("result offset x: %f, offset y: %f\n",
00516       //       x_offsetObstacle, y_offsetObstacle);
00517     }
00518   else
00519     return NULL;
00520   
00521   return result;    
00522 }
00523 
00524 std::pair<double,double>* VisualLanes::isPathClear(double x, double y)
00525 {   
00526   std::pair<double,double>* result = new std::pair<double,double>();
00527   
00528   result->first = 0;
00529   result->second = 0;
00530   
00531   double x_offset = 0;
00532   double y_offset = 0;
00533   
00534   
00535   //printf("y value passed in %f\n", y);   
00536   if(fabs(0 - x) >= _physical_size) {
00537     x_offset = (x - 0) / _physical_size;
00538     x = x - fmod(x, _physical_size);
00539   }
00540   if(fabs(0 - y) >= _physical_size) {
00541     y_offset = (y - 0) / _physical_size;
00542     y = y - fmod(y, _physical_size);
00543   }
00544   
00545   //printf("y_offset %f should be 0:\n", y_offset);
00546   //printf("y value after modifications %f\n", y);
00547   
00548   //printf(" x cell without mod %i\n",
00549   //       (((int)x) + _resolution) / 2 + ((int)x_offset) );
00550   
00551   double xGoalLocalTemp = 0;
00552   xGoalLocalTemp = ( (0 + _resolution / 2 ) + x_offset );  //% _resolution;
00553   double yGoalLocalTemp = 0;
00554   yGoalLocalTemp = ( (0 + _resolution / 2 ) + y_offset );
00555   int xGoalLocal = (int)(xGoalLocalTemp);
00556   int yGoalLocal = (int)(yGoalLocalTemp);
00557   
00558   double tempXRobotLocal = ( (0 + _resolution / 2) + _x_offset );
00559   double tempYRobotLocal = ( (0 + _resolution / 2) + _y_offset );
00560   int xRobotLocal = (int)tempXRobotLocal;
00561   int yRobotLocal = (int)tempYRobotLocal;
00562   
00563   //printf("xGoalLocal: %i yGoalLocal: %i xRobotLocal: %i yRobotLocal: %i\n",
00564   //       xGoalLocal, yGoalLocal, xRobotLocal, yRobotLocal);
00565   std::pair<int,int>* center_collision;
00566   double offset_right = _theta-HALFPI;
00567   offset_right=Coordinates::normalize(offset_right);
00568   double offset_left = _theta+HALFPI;
00569   offset_left=Coordinates::normalize(offset_left);
00570   
00571   center_collision = (VisualLanes::line(xRobotLocal, yRobotLocal,
00572                                         xGoalLocal, yGoalLocal,
00573                                         (&VisualLanes::cellOccupied)));
00574   std::pair<int,int>* right_collision =
00575     (VisualLanes::line((int)(cos(offset_right) * ONE_GRID + xRobotLocal),
00576                        (int)(sin(offset_right) * ONE_GRID + yRobotLocal),
00577                        (int)(cos(offset_right) * ONE_GRID + xGoalLocal),
00578                        (int)(sin(offset_right) * ONE_GRID + yGoalLocal),
00579                        (&VisualLanes::cellOccupied)));
00580   std::pair<int,int>* right_collision2 =
00581     (VisualLanes::line((int)(cos(offset_right) * TWO_GRID + xRobotLocal),
00582                        (int)(sin(offset_right) * TWO_GRID + yRobotLocal),
00583                        (int)(cos(offset_right) * TWO_GRID + xGoalLocal),
00584                        (int)(sin(offset_right) * TWO_GRID + yGoalLocal),
00585                        (&VisualLanes::cellOccupied)));
00586   std::pair<int,int>* left_collision =
00587     (VisualLanes::line((int)(cos(offset_left) * ONE_GRID + xRobotLocal),
00588                        (int)(sin(offset_left) * ONE_GRID + yRobotLocal),
00589                        (int)(cos(offset_left) * ONE_GRID + xGoalLocal),
00590                        (int)(sin(offset_left) * ONE_GRID + yGoalLocal),
00591                        (&VisualLanes::cellOccupied)));
00592   std::pair<int,int>* left_collision2 =
00593     (VisualLanes::line((int)(cos(offset_left) * TWO_GRID + xRobotLocal),
00594                        (int)(sin(offset_left) * TWO_GRID + yRobotLocal),
00595                        (int)(cos(offset_left) * TWO_GRID + xGoalLocal),
00596                        (int)(sin(offset_left) * TWO_GRID + yGoalLocal),
00597                        (&VisualLanes::cellOccupied)));
00598   
00599   std::pair<int,int>* collision = center_collision;
00600   
00601   //FORGIVE US JACK!
00602   collision = (center_collision != NULL) ? center_collision : 
00603     (right_collision != NULL) ? right_collision :
00604     (right_collision2 != NULL) ? right_collision2 :
00605     (left_collision != NULL) ? left_collision :
00606     (left_collision2 != NULL) ? left_collision2 : NULL;
00607   
00608   if(collision != NULL)
00609     {
00610       //This conversion code should also be placed in a function!
00611       double x_offsetObstacle =
00612         (2 * collision->first - _resolution)/(_physical_size + 2);
00613       double y_offsetObstacle =
00614         (2 * collision->second -_resolution)/(_physical_size + 2);
00615       
00616       result->first =  (_physical_size * x_offsetObstacle);
00617       result->second = (_physical_size * y_offsetObstacle);
00618       
00619       //result->first = temp->first;
00620       //result->second = temp->second;
00621     }
00622   else
00623     result = NULL;
00624   
00625   return result;
00626 }
00627 
00628 //Note, this is the amout to shift on the perpendicular line from
00629 //an obstacle. (static only!)
00630 void VisualLanes::setCellShift(int shift)
00631 { _shift = shift;}
00632 
00633 
00634 //There is lots of room for expansion in how this class returns a point
00635 //This function is currently not working as intended; I will try my best
00636 //to debug it quickly, but it is possible to do some basic behaviors in
00637 //assignment 2 without it.
00638 std::pair<double,double>
00639 VisualLanes::nearestClearPath(std::pair<double,double> obstacle,
00640                               std::pair<double,double> original)
00641 {
00642   std::pair<int,int>* temp;
00643   
00644   double slop = 0;
00645   
00646   double localXobstacle = 0;
00647   double localYobstacle = 0;
00648   
00649   double localXGoal = 0;
00650   double localYGoal = 0;
00651   
00652   double xTempOffset = 0;
00653   double yTempOffset = 0;
00654   
00655   double xTemp = 0;
00656   double yTemp = 0;
00657   
00658   //This conversion code should really be placed in a function!
00659   if(fabs(0 - obstacle.first) >= _physical_size) {
00660     xTempOffset = (obstacle.first - 0) / _physical_size;
00661     xTemp = obstacle.first - fmod(obstacle.first, _physical_size);
00662   }
00663   if(fabs(0 - obstacle.second) >= _physical_size) {
00664     yTempOffset = (obstacle.second - 0) / _physical_size;
00665     yTemp = obstacle.second - fmod(obstacle.second, _physical_size);
00666   }
00667   
00668   localXobstacle = ( (((int)xTemp) + _resolution) / 2 + ((int)xTempOffset) );
00669   localYobstacle = ( (((int)yTemp) + _resolution) / 2 + ((int)yTempOffset) );
00670   
00671   xTempOffset = 0;
00672   yTempOffset = 0;    
00673   
00674   xTemp = 0;
00675   yTemp = 0;
00676   
00677   if(fabs(0 - original.first) >= _physical_size) {
00678     xTempOffset = (original.first - 0) / _physical_size;
00679     xTemp = original.first - fmod(original.first, _physical_size);
00680   }
00681   if(fabs(0 - original.second) >= _physical_size) {
00682     yTempOffset = (original.second - 0) / _physical_size;
00683     yTemp = original.second - fmod(original.second, _physical_size);
00684   }
00685   
00686   localXGoal = ( (((int)xTemp) + _resolution) / 2 + ((int)xTempOffset) );
00687   localYGoal = ( (((int)yTemp) + _resolution) / 2 + ((int)yTempOffset) );
00688   
00689   //The perpendicular slop -1/m
00690   
00691   //printf("LocalYGoal: %f LocalYobstacle: %f\n", localYGoal, localYobstacle);
00692   //printf("localXGoal: %f LocalXobstacle: %f\n", localXGoal, localXobstacle);
00693   
00694   
00695   if((localYGoal - localYobstacle) == 0)
00696     slop = -((localXGoal - localXobstacle)/(localYGoal+1 - localYobstacle));
00697   else
00698     slop = -((localXGoal - localXobstacle)/(localYGoal - localYobstacle));
00699   
00700   //printf("slop: %f\n", slop);
00701   
00702   bool validPointFound = false;
00703   int shiftScaler = 1;
00704   
00705   int localXshiftedUp = 0;
00706   int localYshiftedUp = 0;
00707   
00708   int localXshiftedDown = 0;
00709   int localYshiftedDown = 0;
00710   
00711   while(!validPointFound)
00712     {
00713       localXshiftedUp =
00714         (int)(((_shift * shiftScaler) + slop * localXobstacle)/slop);
00715       localYshiftedUp =
00716         (int)(_shift * shiftScaler + localYobstacle);
00717       
00718       localXshiftedDown =
00719         (int)((( -1 * _shift * shiftScaler) + slop * localXobstacle)/slop);
00720       localYshiftedDown =
00721         (int)(-1 * _shift * shiftScaler + localYobstacle);
00722       
00723       //Logic Flaw here to fix... at a later date
00724       temp = (VisualLanes::line(localXshiftedUp, localYshiftedUp,
00725                                 (int)localXGoal, (int)localYGoal,
00726                                 (&VisualLanes::cellOccupied)));
00727       if(temp == NULL)
00728         {
00729           temp = new std::pair<int,int>( (int)localXshiftedUp,
00730                                          (int)localYshiftedUp);
00731           break;
00732         }
00733       temp = (VisualLanes::line(localXshiftedDown, localYshiftedDown,
00734                                 (int)localXGoal, (int)localYGoal,
00735                                 (&VisualLanes::cellOccupied)));
00736       if(temp != NULL)
00737         {
00738           temp = new std::pair<int,int>( (int)localXshiftedDown,
00739                                          (int)localYshiftedDown);
00740           break;
00741         }
00742       
00743       shiftScaler++;
00744     }
00745   
00746   double x_offsetSubPoint = (2 * temp->first - _resolution)/(_physical_size + 2);
00747   double y_offsetSubPoint = (2 * temp->second -_resolution)/(_physical_size + 2);
00748   
00749   std::pair<double,double>* result =
00750     new std::pair<double,double>(_physical_size * x_offsetSubPoint,
00751                                  _physical_size * y_offsetSubPoint);
00752   
00753   return *result;
00754 }
00755 
00756 
00757 cell *VisualLanes::atgoal(int x, int y)
00758 {
00759   //printf(" cell x index %i\n",
00760   //       ((x + _resolution)/2 + x_offsetCurrentGoal) % _resolution);
00761   return &(_m[x % _resolution][y % _resolution]);
00762 }
00763 
00764 
00765 cell *VisualLanes::at(int x, int y) {
00766   if(valid(x,y)) {
00767     
00768     int cellX = (x + _resolution / 2 + _x_offset) % _resolution;
00769     int cellY = (y + _resolution / 2 + _y_offset) % _resolution;
00770     //return &(_m[(x + _resolution / 2 + _x_offset) % _resolution]
00771     //   [(y + _resolution / 2 + _y_offset) % _resolution]);
00772     
00773     //if(cellY == 0)
00774     //{
00775     //  printf("The Y offset: %i\n", _y_offset);
00776     //  printf("The Y value: %i\n", y);
00777     //  printf("Cell Value: %i\n",
00778     //         (y + _resolution/2 + _y_offset) % _resolution);
00779     //}
00780     if( cellX >= 0 && cellY >= 0 )
00781       return &_m[cellX][cellY];
00782     else if(cellY < 0 && cellX < 0)
00783       return &_m[_resolution+cellX][_resolution+cellY];
00784     else if(cellX < 0)
00785       return &_m[_resolution+cellX][cellY];
00786     else
00787       return &_m[cellX][_resolution+cellY];
00788     
00789     
00790   }
00791   return NULL;
00792 }
00793 
00794 //map lanes stuff
00795 //takes a couple of waypoints and makes a sick scan of the lane
00796 void VisualLanes::addMapLane(double w1lat, double w1long, double w2lat,
00797     double w2long, double w3lat, double w3long, double laneWidth){
00798         //store car's current position
00799         //double carX = _x;
00800         //double carY = _y;
00801         //double carTheta = _theta;
00802 
00803   double distance1 = Euclidean::DistanceTo(w1lat, w1long, w2lat, w2long);
00804   double distance2 = Euclidean::DistanceTo(w3lat, w3long, w2lat, w2long);
00805   double theta1 = atan2(w2long-w1long, w2lat-w1lat);
00806   double theta2 = atan2(w3long-w2long, w3lat-w2lat);
00807         
00808   if(distance1 > 5 && distance2 > 5){
00809     //calc first bisec
00810     double angle = theta1 - (theta2 - theta1)/4;
00811     double newDist = (distance1/2) * cos((theta2-theta1)/4);
00812     double mid1lat = w1lat + newDist*cos(angle);
00813     double mid1long = w1long + newDist*sin(angle);
00814     //calc second bisec
00815     angle = theta2 - (theta2 - theta1)/4;
00816     newDist = (distance2/2) * cos((theta2-theta1)/4);
00817     double mid2lat = w2lat + newDist*cos(angle);
00818     double mid2long = w2long + newDist*sin(angle);
00819     addMapLane(w1lat, w1long, mid1lat, mid1long, w2lat, w2long, laneWidth);
00820     addMapLane(mid1lat, mid1long, w2lat, w2long, mid2lat, mid2long, laneWidth);
00821     addMapLane(w2lat, w2long, mid2lat, mid2long, w3lat, w3long, laneWidth);
00822   }
00823   else{
00824     //add first segment
00825     double theta = theta1;
00826     double laneMarkOffset = laneWidth/2;
00827     
00828     //create laser array
00829     std::vector<double> ranges(180);
00830     for (int i = 0; i < 90; i++){
00831       if(laneMarkOffset*tan(angles::from_degrees(i)) <= distance1)
00832         ranges[i] = laneMarkOffset/cos(angles::from_degrees(i));
00833       else
00834         ranges[i] = -distance1/sin(angles::from_degrees(i));
00835     }
00836     for (int i = 90; i < 180; i++){
00837       ranges[i] = ranges[179-i];
00838     }
00839     
00840     //add lane to ogrid
00841     setPosition(w1lat, w1long, theta);
00842     addMapLane(ranges);
00843     setPosition(w2lat, w2long, (theta < 0 ? theta+M_PI : theta-M_PI));
00844     addMapLane(ranges);
00845     
00846     //add second segment
00847     theta = theta2;
00848     
00849     //create laser array
00850     for (int i = 0; i < 90; i++){
00851       if(laneMarkOffset*tan(angles::from_degrees(i)) <= distance2)
00852         ranges[i] = laneMarkOffset/cos(angles::from_degrees(i));
00853       else
00854         ranges[i] = -distance2/sin(angles::from_degrees(i));
00855     }
00856     for (int i = 90; i < 180; i++){
00857       ranges[i] = ranges[179-i];
00858     }
00859     
00860     //add lane to ogrid
00861     setPosition(w2lat, w2long, theta);
00862     addMapLane(ranges);
00863     setPosition(w3lat, w3long, (theta < 0 ? theta+M_PI : theta-M_PI));
00864     addMapLane(ranges);
00865   }
00866   
00867   //reset location to car
00868   //setPosition(carX, carY, carTheta);
00869 }
00870 
00871 //adds a lane segment to the ogrid
00872 void VisualLanes::addMapLane(std::vector<double> ranges)
00873 {
00874   int l;
00875   double temp_theta = _theta - angles::from_degrees(90);
00876   bool laneMark = true;
00877   for (l = 0; l < 180; l++) {
00878         
00879     double tempTheta = temp_theta + angles::from_degrees(l);
00880     //if(l == 0)
00881     //printf("temp theta %f\n", temp_theta);
00882     temp_theta=Coordinates::normalize(temp_theta);
00883 
00884     /*double t = M_PI * (1.0 - l/180.0) + _theta;
00885       double x = (ranges[l] * sin(t)) / _physical_size;
00886       double y = (ranges[l] * cos(t)) / _physical_size;*/
00887 
00888     /*if(l == 90)
00889       {
00890       printf("Theta %f tempTheta %f\n", _theta, tempTheta);
00891       printf("Theta %f super Theta %f\n", _theta,
00892       (_theta - angles::from_degrees(90))
00893       + angles::from_degrees(90));
00894       }*/
00895     //if(ranges[l] != 0)
00896     //{
00897     laneMark = ranges[l] > 0;
00898     if(!laneMark)
00899       ranges[l] = -ranges[l];
00900     double x = (ranges[l] * cos(tempTheta)) / _physical_size;
00901     double y = (ranges[l] * sin(tempTheta)) / _physical_size;
00902 
00903     //lighten(x, y);
00904     if(( x + ( _resolution/2 + _x_offset)%_resolution) > _resolution
00905        && y < _resolution && y >= 0 && !scan_off_bottom_side)
00906       {
00907         clearTop();
00908         scan_off_bottom_side = true;
00909       }
00910     if( (x < _resolution
00911          && (y + (_resolution/2 + _y_offset)%_resolution) > _resolution
00912          && x >= 0
00913          && !scan_off_right_side))
00914       { 
00915         clearLeft();
00916         scan_off_right_side = true;
00917       }
00918     if( x < _resolution
00919         && (y + (_resolution/2 + _y_offset)%_resolution ) < 0
00920         && x >= 0 && !scan_off_left_side)
00921       {
00922         clearRight();
00923         scan_off_left_side = true;
00924       }
00925     if( (x + (_resolution/2 + _x_offset)%_resolution ) < 0
00926         && y >= 0 && y < _resolution && !scan_off_top_side)
00927       {
00928         clearBottom();
00929         scan_off_top_side = true;
00930       }
00931 
00932     VisualLanes::line( 0, 0, (int)x, (int)y,
00933                        (&VisualLanes::drawPointW));
00934 
00935     if(laneMark){
00936       drawPointB((int)x, (int)y);
00937     }
00938   }
00939 }
00940 
00941 void VisualLanes::addPoly(double x1, double x2, double x3, double x4,
00942                           double y1, double y2, double y3, double y4,
00943                           bool is_stop)
00944 {
00945   x1 = x1 / _physical_size;
00946   x2 = x2 / _physical_size;
00947   x3 = x3 / _physical_size;
00948   x4 = x4 / _physical_size;
00949   y1 = y1 / _physical_size;
00950   y2 = y2 / _physical_size;
00951   y3 = y3 / _physical_size;
00952   y4 = y4 / _physical_size;
00953   
00954   if(is_stop)
00955     {
00956       //draw front
00957       VisualLanes::line((int)x2, (int)y2, (int)x3, (int)y3,
00958                         (&VisualLanes::drawPointW));
00959       //draw back
00960       VisualLanes::line((int)x4, (int)y4, (int)x1, (int)y1,
00961                         (&VisualLanes::drawPointW));
00962     }
00963   //draw left
00964   VisualLanes::line( (int)x1, (int)y1, (int)x2, (int)y2,
00965                      (&VisualLanes::drawPointB));
00966   //draw right
00967   VisualLanes::line((int)x3, (int)y3, (int)x4, (int)y4,
00968                     (&VisualLanes::drawPointB));
00969 }
00970 
00971 void VisualLanes::addTrace(double w1lat, double w1long,
00972                            double w2lat, double w2long)
00973 {
00974   double x1 = w1lat / _physical_size;
00975   double y1 = w1long / _physical_size;
00976   double x2 = w2lat / _physical_size;
00977   double y2 = w2long / _physical_size;
00978   VisualLanes::line( (int)x1, (int)y1, (int)x2, (int)y2,
00979                      (&VisualLanes::drawPointW));
00980 }
00981 
00982 //for map lanes: only makes cell black if not already white to avoid overdraw
00983 std::pair<int,int>* VisualLanes::drawPointB(int x, int y)
00984 {
00985   cell* c = at(x,y);
00986   if(c != NULL)
00987     {
00988       (*c) = LOGODDS_MAX_OCCUPANCY;;
00989     } 
00990   return NULL;  
00991 }
00992 
00993 //for map lanes
00994 std::pair<int,int>* VisualLanes::drawPointW(int x, int y)
00995 {
00996   cell* c = at(x,y);
00997   if(c != NULL)
00998     {
00999       (*c) = LOGODDS_MIN_OCCUPANCY;;
01000     } 
01001   return NULL;  
01002 }
01003 //end map lane stuff


art_map
Author(s): David Li, Patrick Beeson, Bartley Gillen, Tarun Nimmagadda, Mickey Ristroph, Michael Quinlan, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:08:34