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
00061
00062
00063
00064
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
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
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
00199
00200 Coordinates::normalize(temp_theta);
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214 double x = (ranges[l] * cos(tempTheta)) / _physical_size;
00215 double y = (ranges[l] * sin(tempTheta)) / _physical_size;
00216
00217
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
00248
00249
00250 VisualLanes::line( 0, 0, (int)x, (int)y, (&VisualLanes::cellLighten));
00251
00252
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
00264
00265
00266 (*c) = LOGODDS_MAX_OCCUPANCY;
00267 }
00268 }
00269 }
00270
00271 }
00272 }
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
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
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
00333 if( *c >= _threshold )
00334 {
00335
00336
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
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
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
00385
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;
00397 dx <<= 1;
00398
00399 (this->*_fp)(x0, y0);
00400
00401
00402 if (dx > dy) {
00403 int fraction = dy - (dx >> 1);
00404 while (x0 != x1) {
00405 if (fraction >= 0) {
00406 y0 += stepy;
00407 fraction -= dx;
00408 }
00409 x0 += stepx;
00410 fraction += 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
00489
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
00503
00504 temp = VisualLanes::line(0, 0, (int)x, (int)y,
00505 (&VisualLanes::cellOccupiedRelative));
00506 if(temp != NULL)
00507 {
00508
00509
00510
00511
00512
00513 result->first = temp->first;
00514 result->second = temp->second;
00515
00516
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
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
00546
00547
00548
00549
00550
00551 double xGoalLocalTemp = 0;
00552 xGoalLocalTemp = ( (0 + _resolution / 2 ) + x_offset );
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
00564
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
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
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
00620
00621 }
00622 else
00623 result = NULL;
00624
00625 return result;
00626 }
00627
00628
00629
00630 void VisualLanes::setCellShift(int shift)
00631 { _shift = shift;}
00632
00633
00634
00635
00636
00637
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
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
00690
00691
00692
00693
00694
00695 if((localYGoal - localYobstacle) == 0)
00696 slop = -((localXGoal - localXobstacle)/(localYGoal+1 - localYobstacle));
00697 else
00698 slop = -((localXGoal - localXobstacle)/(localYGoal - localYobstacle));
00699
00700
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
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
00760
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
00771
00772
00773
00774
00775
00776
00777
00778
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
00795
00796 void VisualLanes::addMapLane(double w1lat, double w1long, double w2lat,
00797 double w2long, double w3lat, double w3long, double laneWidth){
00798
00799
00800
00801
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
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
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
00825 double theta = theta1;
00826 double laneMarkOffset = laneWidth/2;
00827
00828
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
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
00847 theta = theta2;
00848
00849
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
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
00868
00869 }
00870
00871
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
00881
00882 temp_theta=Coordinates::normalize(temp_theta);
00883
00884
00885
00886
00887
00888
00889
00890
00891
00892
00893
00894
00895
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
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
00957 VisualLanes::line((int)x2, (int)y2, (int)x3, (int)y3,
00958 (&VisualLanes::drawPointW));
00959
00960 VisualLanes::line((int)x4, (int)y4, (int)x1, (int)y1,
00961 (&VisualLanes::drawPointW));
00962 }
00963
00964 VisualLanes::line( (int)x1, (int)y1, (int)x2, (int)y2,
00965 (&VisualLanes::drawPointB));
00966
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
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
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