00001
00002
00003
00004
00005
00006
00007
00008
00017 #include <iostream>
00018 #include <iomanip>
00019 #include <art/epsilon.h>
00020 #include <vector>
00021
00022 #include <art_map/gaussian.h>
00023 #include <art_map/MapLanes.h>
00024 #include <art_map/euclidean_distance.h>
00025
00026 #include <art_map/rotate_translate_transform.h>
00027
00028
00029
00030
00031
00032
00033 #define way_poly_size 0.5 // half of length of polygon that goes
00034
00035 int writecounter=0;
00036 int aCount=0;
00037 int bCount=0;
00038 int cCount=0;
00039
00040 int MapLanes::MapRNDF(Graph* _graph, float _max_poly_size)
00041 {
00042 graph=_graph;
00043
00044 max_poly_size=fmaxf(_max_poly_size, MIN_POLY_SIZE);
00045
00046 allPolys.clear();
00047
00048
00049
00050 poly_id_counter = 0;
00051
00052 ROS_INFO_STREAM("Starting with " << graph->nodes_size << " nodes in graph");
00053
00054 rX=0.0;
00055 rY=0.0;
00056 rOri=0.0;
00057
00058 cX=0.0;
00059 cX=0.0;
00060 #ifdef DEBUGMAP
00061 debugFile=fopen("mapDebug.txt","wb");
00062 #endif
00063
00064 MakePolygons();
00065 SetFilteredPolygons();
00066
00067 ROS_DEBUG("MapLanes constructed successfully");
00068 return 0;
00069 }
00070
00071 void MapLanes::MakePolygons()
00072 {
00073
00074 std::vector<WayPointNode> lane;
00075 std::vector<Point2f> lane_pt;
00076 std::vector<int> lane_map;
00077
00078 SmoothCurve lc,rc;
00079
00080 ElementID prev_lane;
00081
00082
00083
00084
00085
00086
00087
00088 for(uint j = 0; j < graph->edges_size; j++)
00089 {
00090 WayPointEdge e=graph->edges[j];
00091 if (e.is_implicit)
00092 continue;
00093 WayPointNode w1=graph->nodes[e.startnode_index];
00094 WayPointNode w2=graph->nodes[e.endnode_index];
00095
00096 if ((!e.is_exit || (w1.id.same_lane(w2.id) &&
00097 w1.id.pt+1==w2.id.pt))
00098 && !w1.is_perimeter && !w1.is_spot &&
00099 !w2.is_perimeter && !w2.is_spot)
00100
00101 {
00102 if (w1.id.seg != prev_lane.seg ||
00103 w1.id.lane != prev_lane.lane ||
00104 w1.id.pt != prev_lane.pt)
00105
00106 {
00107
00108 if (lane.size()>1)
00109 {
00110 Point2f diff_pt=lane_pt[1]-lane_pt[0];
00111 Point2f diff_pt2=lane_pt[lane_pt.size()-1]-
00112 lane_pt[lane_pt.size()-2];
00113 SmoothCurve c=
00114 SmoothCurve(lane_pt,
00115 atan2f(diff_pt[1],diff_pt[0]), 1,
00116 atan2f(diff_pt2[1],diff_pt2[0]), 1);
00117
00118 for (uint i=0;i<lane.size()-1;i++)
00119 {
00120 MakeLanePolygon(lane[i],lane[i+1],e,
00121 c.knots[lane_map[i]],
00122 c.knots[lane_map[i+1]],
00123 c, true,
00124 0,0,lc,
00125 0,0,rc);
00126 }
00127 }
00128
00129
00130 lane.clear();
00131 lane_pt.clear();
00132 lane_map.clear();
00133
00134 lane.push_back(w1);
00135 Point2f pt1(w1.map.x,w1.map.y);
00136 lane_pt.push_back(pt1);
00137 lane_map.push_back(lane_pt.size()-1);
00138 }
00139
00140
00141 lane.push_back(w2);
00142 Point2f pt2(w2.map.x,w2.map.y);
00143 lane_pt.push_back(pt2);
00144 lane_map.push_back(lane_pt.size()-1);
00145
00146 prev_lane=w2.id;
00147 }
00148 else if (!w1.is_spot &&
00149 !w2.is_spot &&
00150 (!w1.is_perimeter || !w2.is_perimeter) &&
00151 (w1.id.seg != w2.id.seg ||
00152 w1.id.lane == w2.id.lane))
00153
00154 {
00155 if (lane.size()>1)
00156
00157 {
00158 Point2f diff_pt=lane_pt[1]-lane_pt[0];
00159 Point2f diff_pt2=lane_pt[lane_pt.size()-1]-
00160 lane_pt[lane_pt.size()-2];
00161
00162 SmoothCurve c=
00163 SmoothCurve(lane_pt,
00164 atan2f(diff_pt[1],diff_pt[0]), 1,
00165 atan2f(diff_pt2[1],diff_pt2[0]), 1);
00166
00167 for (uint i=0;i<lane.size()-1;i++)
00168 MakeLanePolygon(lane[i],lane[i+1],e,
00169 c.knots[lane_map[i]],
00170 c.knots[lane_map[i+1]],
00171 c, true,
00172 0,0,lc,0,0,rc);
00173 }
00174
00175
00176 lane.clear();
00177 lane_pt.clear();
00178 lane_map.clear();
00179
00180 int base_ind=0;
00181
00182
00183
00184
00185
00186 ElementID pre=w1.id;
00187 pre.pt--;
00188
00189 WayPointNode* w0=graph->get_node_by_id(pre);
00190
00191 if (w0 != NULL && w0->id.lane != 0) {
00192 lane.push_back(*w0);
00193 Point2f pt(w0->map.x,w0->map.y);
00194 lane_pt.push_back(pt);
00195 lane_map.push_back(lane_pt.size()-1);
00196 base_ind=1;
00197 }
00198
00199
00200
00201 lane.push_back(w1);
00202 Point2f pt1(w1.map.x,w1.map.y);
00203 lane_pt.push_back(pt1);
00204 lane_map.push_back(lane_pt.size()-1);
00205
00206 lane.push_back(w2);
00207 Point2f pt2(w2.map.x,w2.map.y);
00208 lane_pt.push_back(pt2);
00209 lane_map.push_back(lane_pt.size()-1);
00210
00211 ElementID post=w2.id;
00212 post.pt++;
00213
00214 WayPointNode* w3=graph->get_node_by_id(post);
00215
00216 if (w3!=NULL && w3->id.lane != 0)
00217 {
00218 lane.push_back(*w3);
00219 Point2f pt(w3->map.x,w3->map.y);
00220 lane_pt.push_back(pt);
00221 lane_map.push_back(lane_pt.size()-1);
00222 }
00223
00224
00225 if (lane_pt.size()<2)
00226 {
00227 WayPointNode w4=w2;
00228 w4.map=w2.map+w2.map-w1.map;
00229 lane.push_back(w4);
00230 Point2f pt(w4.map.x,w4.map.y);
00231 lane_pt.push_back(pt);
00232 lane_map.push_back(lane_pt.size()-1);
00233 }
00234
00235
00236 Point2f diff_pt=lane_pt[1]-lane_pt[0];
00237 Point2f diff_pt2=lane_pt[lane_pt.size()-1]-
00238 lane_pt[lane_pt.size()-2];
00239
00240 SmoothCurve c=
00241 SmoothCurve(lane_pt,
00242 atan2f(diff_pt[1],diff_pt[0]), 1,
00243 atan2f(diff_pt2[1],diff_pt2[0]), 1);
00244
00245 MakeTransitionPolygon(lane[base_ind],lane[base_ind+1],e,
00246 c.knots[lane_map[base_ind]],c.knots[lane_map[base_ind+1]],c);
00247
00248
00249 lane.clear();
00250 lane_pt.clear();
00251 lane_map.clear();
00252 prev_lane=ElementID();
00253 }
00254 }
00255
00256
00257 if (lane.size()>1)
00258 {
00259
00260 Point2f diff_pt=lane_pt[1]-lane_pt[0];
00261 Point2f diff_pt2=lane_pt[lane_pt.size()-1]-
00262 lane_pt[lane_pt.size()-2];
00263 SmoothCurve c=
00264 SmoothCurve(lane_pt,
00265 atan2f(diff_pt[1],diff_pt[0]), 1,
00266 atan2f(diff_pt2[1],diff_pt2[0]), 1);
00267
00268 for (uint i=0;i<lane.size()-1;i++) {
00269
00270 WayPointEdge e;
00271 for(uint j = 0; j < graph->edges_size; j++)
00272 {
00273 e=graph->edges[j];
00274
00275 if(graph->nodes[e.startnode_index].id.pt==lane[i].id.pt
00276 && graph->nodes[e.startnode_index].id.lane==lane[i].id.lane
00277 && graph->nodes[e.startnode_index].id.seg==lane[i].id.seg
00278 && graph->nodes[e.endnode_index].id.pt==lane[i+1].id.pt
00279 && graph->nodes[e.endnode_index].id.lane==lane[i+1].id.lane
00280 && graph->nodes[e.endnode_index].id.seg==lane[i+1].id.seg)
00281
00282 break;
00283 }
00284
00285 MakeLanePolygon(lane[i],lane[i+1],e,
00286 c.knots[lane_map[i]],
00287 c.knots[lane_map[i+1]],
00288 c, true,0,0,lc,0,0,rc);
00289 }
00290 }
00291
00292
00293 lane.clear();
00294 lane_pt.clear();
00295 lane_map.clear();
00296 }
00297
00298 void MapLanes::SetFilteredPolygons()
00299 {
00300 for (int i=0; i<(int)allPolys.size(); i++)
00301 {
00302 FilteredPolygon p;
00303 p.SetPolygon(allPolys.at(i));
00304 filtPolys.push_back(p);
00305 }
00306
00307 #ifdef DEBUGMAP
00308 for (int i=0; i<(int)filtPolys.size(); i++) {
00309 WritePolygonToDebugFile(i);
00310 }
00311 #endif
00312 }
00313
00314 poly MapLanes::build_waypoint_poly(const WayPointNode& w1,
00315 const WayPointEdge& e,
00316 const Point2f& _pt,
00317 float time,
00318 SmoothCurve& c)
00319 {
00320
00321
00322
00323 rotate_translate_transform trans;
00324 posetype origin(0,0,0);
00325
00326
00327 Point2f back_pt=c.evaluatePoint(time+way_poly_size);
00328 Point2f front_pt=c.evaluatePoint(time-way_poly_size);
00329 Point2f pt=_pt;
00330
00331
00332
00333
00334 if (back_pt[0]==pt[0] &&
00335 back_pt[1]==pt[1])
00336 back_pt=pt+(pt-front_pt);
00337
00338 if (front_pt[0]==pt[0] &&
00339 front_pt[1]==pt[1])
00340 front_pt=pt-(back_pt-pt);
00341
00342
00343 Point2f diff_back=back_pt-pt;
00344 Point2f diff_front=pt-front_pt;
00345
00346 float back_angle=atan2f(diff_back[1],diff_back[0]);
00347 float front_angle=atan2f(diff_front[1],diff_front[0]);
00348
00349
00350 posetype refway1(back_pt[0],back_pt[1],back_angle);
00351 trans.find_transform(origin,refway1);
00352 posetype p2=trans.apply_transform(posetype(0, w1.lane_width/2,0));
00353 posetype p3=trans.apply_transform(posetype(0,-w1.lane_width/2,0));
00354
00355 posetype refway2(front_pt[0],front_pt[1],front_angle);
00356 trans.find_transform(origin,refway2);
00357 posetype p1=trans.apply_transform(posetype(0, w1.lane_width/2,0));
00358 posetype p4=trans.apply_transform(posetype(0,-w1.lane_width/2,0));
00359
00360 poly newPoly;
00361
00362 newPoly.p1.x=p1.x;
00363 newPoly.p1.y=p1.y;
00364
00365 newPoly.p2.x=p2.x;
00366 newPoly.p2.y=p2.y;
00367
00368 newPoly.p3.x=p3.x;
00369 newPoly.p3.y=p3.y;
00370
00371 newPoly.p4.x=p4.x;
00372 newPoly.p4.y=p4.y;
00373
00374
00375 newPoly.start_way.seg = w1.id.seg;
00376 newPoly.start_way.lane = w1.id.lane;
00377 newPoly.start_way.pt = w1.id.pt;
00378 newPoly.end_way.seg = w1.id.seg;
00379 newPoly.end_way.lane = w1.id.lane;
00380 newPoly.end_way.pt = w1.id.pt;
00381 newPoly.is_stop = w1.is_stop;
00382 newPoly.is_transition = false;
00383 newPoly.contains_way = true;
00384 #if 0 //TODO
00385 newPoly.left_boundary.lane_marking = e.left_boundary.lane_marking;
00386 newPoly.right_boundary.lane_marking = e.right_boundary.lane_marking;
00387 #endif
00388
00389
00390 newPoly.heading = ops.PolyHeading(newPoly);
00391 MapXY cpt = ops.centerpoint(newPoly);
00392 newPoly.midpoint.x = cpt.x;
00393 newPoly.midpoint.y = cpt.y;
00394 newPoly.length = ops.getLength(newPoly);
00395 return newPoly;
00396 }
00397
00398
00399 void MapLanes::MakeLanePolygon(WayPointNode w1, WayPointNode w2,
00400 WayPointEdge e,
00401 float time1, float time2,
00402 SmoothCurve& c,
00403 bool new_edge,
00404 float ltime1, float ltime2,
00405 SmoothCurve& lc,
00406 float rtime1, float rtime2,
00407 SmoothCurve& rc)
00408 {
00409
00410 if (time2 <= time1)
00411 return;
00412
00413
00414
00415 if (poly_id_counter==0)
00416 new_edge=true;
00417
00418 Point2f w1_pt=c.evaluatePoint(time1);
00419 Point2f w2_pt=c.evaluatePoint(time2);
00420
00421
00422
00423
00424
00425 if (new_edge)
00426 {
00427 poly poly_w1;
00428
00429
00430 if (poly_id_counter == 0 ||
00431 ElementID(allPolys[poly_id_counter-1].end_way) != w1.id)
00432 {
00433 poly_w1=build_waypoint_poly(w1, e, w1_pt, time1, c);
00434
00435 poly_w1.poly_id = poly_id_counter;
00436
00437
00438 poly_id_counter++;
00439 allPolys.push_back(poly_w1);
00440
00441 }
00442 else
00443
00444
00445 poly_w1=allPolys[poly_id_counter-1];
00446
00447
00448
00449 poly poly_w2=build_waypoint_poly(w2,e, w2_pt, time2, c);
00450
00451
00452 time1=time1+way_poly_size;
00453 time2=time2-way_poly_size;
00454
00455 w1.map=ops.midpoint(poly_w1.p2,poly_w1.p3);
00456 w2.map=ops.midpoint(poly_w2.p1,poly_w2.p4);
00457
00458 float edist=
00459 Euclidean::DistanceTo(w1.map,w2.map);
00460
00461 float cdist = fmax(time2-time1,0.0);
00462
00463
00464
00465 if (cdist>Epsilon::float_value && edist>Epsilon::float_value)
00466 {
00467 std::vector<Point2f> left_curve_pts;
00468 left_curve_pts.push_back(Point2f(poly_w1.p2.x,poly_w1.p2.y));
00469 left_curve_pts.push_back(Point2f(poly_w2.p1.x,poly_w2.p1.y));
00470
00471 bool straight=false;
00472
00473
00474
00475 if (straight) {
00476 lc=
00477 SmoothCurve(left_curve_pts,
00478 atan2f(poly_w1.p2.y-poly_w1.p1.y,
00479 poly_w1.p2.x-poly_w1.p1.x), 1,
00480 atan2f(poly_w1.p2.y-poly_w1.p1.y,
00481 poly_w1.p2.x-poly_w1.p1.x), 1);
00482
00483 ltime1=lc.knots[0];
00484 ltime2=lc.knots[1];
00485
00486 std::vector<Point2f> right_curve_pts;
00487 right_curve_pts.push_back(Point2f(poly_w1.p3.x,poly_w1.p3.y));
00488 right_curve_pts.push_back(Point2f(poly_w2.p4.x,poly_w2.p4.y));
00489
00490 rc=
00491 SmoothCurve(right_curve_pts,
00492 atan2f(poly_w1.p3.y-poly_w1.p4.y,
00493 poly_w1.p3.x-poly_w1.p4.x), 1,
00494 atan2f(poly_w1.p3.y-poly_w1.p4.y,
00495 poly_w1.p3.x-poly_w1.p4.x), 1);
00496
00497 } else {
00498 lc=
00499 SmoothCurve(left_curve_pts,
00500 atan2f(poly_w1.p2.y-poly_w1.p1.y,
00501 poly_w1.p2.x-poly_w1.p1.x), 1,
00502 atan2f(poly_w2.p2.y-poly_w2.p1.y,
00503 poly_w2.p2.x-poly_w2.p1.x), 1);
00504
00505 ltime1=lc.knots[0];
00506 ltime2=lc.knots[1];
00507
00508 std::vector<Point2f> right_curve_pts;
00509 right_curve_pts.push_back(Point2f(poly_w1.p3.x,poly_w1.p3.y));
00510 right_curve_pts.push_back(Point2f(poly_w2.p4.x,poly_w2.p4.y));
00511
00512 rc=
00513 SmoothCurve(right_curve_pts,
00514 atan2f(poly_w1.p3.y-poly_w1.p4.y,
00515 poly_w1.p3.x-poly_w1.p4.x), 1,
00516 atan2f(poly_w2.p3.y-poly_w2.p4.y,
00517 poly_w2.p3.x-poly_w2.p4.x), 1);
00518
00519 }
00520 rtime1=rc.knots[0];
00521 rtime2=rc.knots[1];
00522
00523
00524 WayPointNode midpoint=w2;
00525 midpoint.lane_width=(w1.lane_width + w2.lane_width)/2.0;
00526
00527
00528 midpoint.map=ops.midpoint(w1.map,w2.map);
00529
00530 float midtime=(time1+time2)/2;
00531
00532 float lmidtime=(ltime1+ltime2)/2;
00533 float rmidtime=(rtime1+rtime2)/2;
00534
00535 MakeLanePolygon(w1, midpoint, e, time1, midtime, c, false,
00536 ltime1, lmidtime, lc, rtime1, rmidtime, rc);
00537
00538 midpoint.id=w1.id;
00539
00540 MakeLanePolygon(midpoint, w2, e, midtime, time2, c, false,
00541 lmidtime, ltime2, lc, rmidtime, rtime2, rc);
00542 }
00543
00544
00545 poly_w2.poly_id = poly_id_counter;
00546
00547
00548 allPolys[poly_id_counter-1].p2=poly_w2.p1;
00549 allPolys[poly_id_counter-1].p3=poly_w2.p4;
00550
00551
00552 poly_id_counter++;
00553 allPolys.push_back(poly_w2);
00554
00555 }
00556 else
00557
00558
00559 {
00560
00561
00562
00563
00564
00565
00566
00567
00568 float edist=
00569 Euclidean::DistanceTo(w1.map,w2.map);
00570
00571 float cdist = fmax(time2-time1,0.0);
00572
00573 if(Epsilon::equal(cdist,0.0) ||
00574 Epsilon::equal(edist,0.0) ||
00575 cdist <= max_poly_size*edist/cdist)
00576 {
00577
00578 poly newPoly;
00579
00580
00581 newPoly.p1=allPolys[poly_id_counter-1].p2;
00582 newPoly.p4=allPolys[poly_id_counter-1].p3;
00583
00584 Point2f point2=lc.evaluatePoint(ltime2);
00585 Point2f point3=rc.evaluatePoint(rtime2);
00586
00587 newPoly.p2.x=point2[0];
00588 newPoly.p2.y=point2[1];
00589
00590 newPoly.p3.x=point3[0];
00591 newPoly.p3.y=point3[1];
00592
00593
00594
00595 newPoly.start_way.seg = w1.id.seg;
00596 newPoly.start_way.lane = w1.id.lane;
00597 newPoly.start_way.pt = w1.id.pt;
00598 newPoly.end_way.seg = w2.id.seg;
00599 newPoly.end_way.lane = w2.id.lane;
00600 newPoly.end_way.pt = w2.id.pt;
00601 newPoly.is_stop = false;
00602 newPoly.is_transition = false;
00603 newPoly.contains_way = false;
00604 newPoly.poly_id = poly_id_counter;
00605 #if 0 //TODO
00606 newPoly.left_boundary=e.left_boundary;
00607 newPoly.right_boundary=e.right_boundary;
00608 #endif
00609
00610
00611 newPoly.heading = ops.PolyHeading(newPoly);
00612 MapXY cpt = ops.centerpoint(newPoly);
00613 newPoly.midpoint.x = cpt.x;
00614 newPoly.midpoint.y = cpt.y;
00615 newPoly.length = ops.getLength(newPoly);
00616
00617
00618 poly_id_counter++;
00619 allPolys.push_back(newPoly);
00620
00621 }
00622 else
00623 {
00624
00625
00626 w1.map.x=w1_pt[0];
00627 w1.map.y=w1_pt[1];
00628 w2.map.x=w2_pt[0];
00629 w2.map.y=w2_pt[1];
00630
00631 WayPointNode midpoint=w2;
00632 midpoint.lane_width=(w1.lane_width + w2.lane_width)/2.0;
00633
00634
00635 midpoint.map.x=(w1.map.x+w2.map.x)/2;
00636 midpoint.map.y=(w1.map.y+w2.map.y)/2;
00637
00638 float midtime=(time2+time1)/2;
00639 float lmidtime=(ltime1+ltime2)/2;
00640 float rmidtime=(rtime1+rtime2)/2;
00641
00642 MakeLanePolygon(w1, midpoint, e, time1, midtime, c, false,
00643 ltime1,lmidtime,lc,rtime1,rmidtime,rc);
00644 midpoint.id=w1.id;
00645 MakeLanePolygon(midpoint, w2, e, midtime, time2, c, false,
00646 lmidtime,ltime2,lc,rmidtime,rtime2,rc);
00647 }
00648 }
00649 }
00650
00651
00652 void MapLanes::MakeTransitionPolygon(WayPointNode w1, WayPointNode w2,
00653 WayPointEdge e,
00654 float time1, float time2,
00655 SmoothCurve& c)
00656 {
00657
00658 poly poly_w1, poly_w2;
00659
00660 int index_w1=-1;
00661 int index_w2=-1;
00662
00663
00664
00665
00666 for (uint i=0; i<allPolys.size() && (index_w1<0 || index_w2<0); i++)
00667 if (allPolys[i].contains_way) {
00668 if (ElementID(allPolys[i].start_way) == w1.id)
00669 {
00670 poly_w1=allPolys[i];
00671 index_w1=i;
00672 }
00673 else if (ElementID(allPolys[i].start_way) == w2.id)
00674 {
00675 poly_w2=allPolys[i];
00676 index_w2=i;
00677 }
00678 }
00679
00680 if (index_w1<0)
00681 {
00682 Point2f w1_pt=c.evaluatePoint(time1);
00683 poly_w1=build_waypoint_poly(w1, e, w1_pt, time1, c);
00684 }
00685
00686 time1+=way_poly_size;
00687
00688 if (index_w2<0)
00689 {
00690 Point2f w2_pt=c.evaluatePoint(time2);
00691 poly_w2=build_waypoint_poly(w2, e, w2_pt, time2, c);
00692
00693 }
00694
00695 time2-=way_poly_size;
00696
00697 float cdist = fmax(0.0,time2-time1);
00698
00699 w1.map=ops.midpoint(poly_w1.p2,poly_w1.p3);
00700 w2.map=ops.midpoint(poly_w2.p1,poly_w2.p4);
00701
00702 float edist=
00703 Euclidean::DistanceTo(w1.map,w2.map);
00704
00705
00706
00707
00708 if (cdist>Epsilon::float_value && edist>Epsilon::float_value)
00709 {
00710
00711
00712
00713 std::vector<Point2f> left_curve_pts;
00714 left_curve_pts.push_back(Point2f(poly_w1.p2.x,poly_w1.p2.y));
00715 left_curve_pts.push_back(Point2f(poly_w2.p1.x,poly_w2.p1.y));
00716
00717 SmoothCurve lc=
00718 SmoothCurve(left_curve_pts,
00719 atan2f(poly_w1.p2.y-poly_w1.p1.y,
00720 poly_w1.p2.x-poly_w1.p1.x), 1,
00721 atan2f(poly_w2.p2.y-poly_w2.p1.y,
00722 poly_w2.p2.x-poly_w2.p1.x), 1);
00723
00724 float ltime1=lc.knots[0];
00725 float ltime2=lc.knots[1];
00726
00727 std::vector<Point2f> right_curve_pts;
00728 right_curve_pts.push_back(Point2f(poly_w1.p3.x,poly_w1.p3.y));
00729 right_curve_pts.push_back(Point2f(poly_w2.p4.x,poly_w2.p4.y));
00730
00731 SmoothCurve rc=
00732 SmoothCurve(right_curve_pts,
00733 atan2f(poly_w1.p3.y-poly_w1.p4.y,
00734 poly_w1.p3.x-poly_w1.p4.x), 1,
00735 atan2f(poly_w2.p3.y-poly_w2.p4.y,
00736 poly_w2.p3.x-poly_w2.p4.x), 1);
00737
00738
00739 float rtime1=rc.knots[0];
00740 float rtime2=rc.knots[1];
00741
00742
00743 WayPointNode midpoint=w2;
00744 midpoint.lane_width=(w1.lane_width + w2.lane_width)/2.0;
00745
00746
00747 midpoint.map=ops.midpoint(w1.map,w2.map);
00748
00749 float midtime=(time1+time2)/2;
00750 float lmidtime=(ltime1+ltime2)/2;
00751 float rmidtime=(rtime1+rtime2)/2;
00752
00753 int next_polyid=poly_id_counter;
00754
00755 e.left_boundary=UNDEFINED;
00756 e.right_boundary=UNDEFINED;
00757
00758 transition=true;
00759 trans_index=index_w1;
00760
00761 MakeLanePolygon(w1, midpoint, e, time1, midtime, c, false,
00762 ltime1,lmidtime,lc,rtime1,rmidtime,rc);
00763
00764
00765
00766
00767
00768 allPolys[next_polyid].p1=poly_w1.p2;
00769 allPolys[next_polyid].p4=poly_w1.p3;
00770
00771 midpoint.id=w1.id;
00772
00773 MakeLanePolygon(midpoint, w2, e, midtime, time2, c, false,
00774 lmidtime,ltime2,lc,rmidtime,rtime2,rc);
00775
00776
00777
00778
00779 allPolys[poly_id_counter-1].p2=poly_w2.p1;
00780 allPolys[poly_id_counter-1].p3=poly_w2.p4;
00781
00782 for (uint i=next_polyid; i<allPolys.size(); i++)
00783 allPolys[i].is_transition=true;
00784 }
00785 else
00786 if (index_w1 >=0 && index_w2 >=0)
00787 {
00788 allPolys[index_w1].p2=allPolys[index_w2].p1;
00789 allPolys[index_w1].p3=allPolys[index_w2].p4;
00790 }
00791 }
00792
00797 int MapLanes::getAllLanes(art_msgs::ArtLanes *lanes)
00798 {
00799 lanes->polygons.clear();
00800
00801 for(unsigned int i = 0; i < filtPolys.size(); i++)
00802 {
00803 art_msgs::ArtQuadrilateral temp = filtPolys.at(i).GetQuad();
00804 lanes->polygons.push_back(temp);
00805 }
00806
00807 return lanes->polygons.size();
00808 }
00809
00810
00811 int MapLanes::getLanes(art_msgs::ArtLanes *lanes, MapXY here)
00812 {
00813 if (range < 0)
00814 return getAllLanes(lanes);
00815
00816 lanes->polygons.clear();
00817
00818 for(unsigned int i = 0; i < filtPolys.size(); i++)
00819 {
00820 art_msgs::ArtQuadrilateral temp = filtPolys.at(i).GetQuad();
00821 float dist = Euclidean::DistanceTo(MapXY(temp.midpoint), here);
00822
00823 if(dist <= range)
00824 {
00825 lanes->polygons.push_back(temp);
00826 allPolys[i] = poly(temp);
00827 }
00828 }
00829
00830 ROS_DEBUG_STREAM("found " << lanes->polygons.size()
00831 << " polygons within " << range
00832 << " meters of (" << here.x << "," << here.y << ")");
00833
00834 return 0;
00835 }
00836
00837 int MapLanes::getVisionLanes(art_msgs::ArtLanes *lanes, float x, float y,
00838 float heading)
00839 {
00840 if (range < 0)
00841 return getAllLanes(lanes);
00842
00843 lanes->polygons.clear();
00844
00845 int index = ops.getContainingPoly(allPolys,x,y);
00846 if (index < 0)
00847 {
00848 return 0;
00849 }
00850
00851 poly current = allPolys.at(index);
00852 for(unsigned int i = 0; i < filtPolys.size(); i++)
00853 {
00854 art_msgs::ArtQuadrilateral temp = filtPolys.at(i).GetQuad();
00855
00856 if (temp.start_way.lane != current.start_way.lane
00857 || temp.start_way.seg != current.start_way.seg
00858 || temp.is_transition
00859 || temp.contains_way)
00860 continue;
00861
00862 float angle=AngleFromXY(x,y,heading,temp.midpoint.x,temp.midpoint.y);
00863 float dist=DistFromXY(x,y,temp.midpoint.x,temp.midpoint.y);
00864
00865 if((dist <= range) && (dist>10.0) && (fabs(angle)<DEG_T_RAD*25))
00866 {
00867 lanes->polygons.push_back(temp);
00868 }
00869 }
00870
00871
00872 return 0;
00873 }
00874
00875 void MapLanes::UpdatePoly(polyUpdate upPoly, float rrX, float rrY, float rrOri)
00876 {
00877 if (upPoly.poly_id <=0 || upPoly.poly_id>=(int)filtPolys.size()) {
00878 return;
00879 }
00880 if (upPoly.distance<3.0) return;
00881 FilteredPolygon* filt=&(filtPolys.at(upPoly.poly_id));
00882 poly curr=filt->GetPolygon();
00883
00884
00885 if (upPoly.poly_id <=0 || upPoly.poly_id>=(int)filtPolys.size()) {
00886 return;
00887 }
00888
00889
00890
00891 poly prev=(filtPolys.at(upPoly.poly_id-1)).GetPolygon();
00892 poly next=(filtPolys.at(upPoly.poly_id+1)).GetPolygon();
00893
00894 if (prev.contains_way && (upPoly.point_id==0 || upPoly.point_id==3)) return;
00895
00896 if (next.contains_way && (upPoly.point_id==1 || upPoly.point_id==2)) return;
00897
00898
00899
00900 filt->UpdatePoint(upPoly.point_id,upPoly.distance,upPoly.bearing,upPoly.confidence,rrX,rrY,Normalise_PI(rrOri+PI));
00901
00902 #ifdef DEBUGMAP
00903 WritePolygonToDebugFile(upPoly.poly_id);
00904 #endif
00905
00906 int point=0;
00907 if ((upPoly.point_id==0 || upPoly.point_id==3) && curr.poly_id==prev.poly_id+1 && curr.start_way.lane==prev.start_way.lane && curr.start_way.seg==prev.start_way.seg) {
00908 FilteredPolygon* temp=&(filtPolys.at(prev.poly_id));
00909 if (upPoly.point_id==0) point=1;
00910 if (upPoly.point_id==3) point=2;
00911 temp->UpdatePoint(point,upPoly.distance,upPoly.bearing,upPoly.confidence,rrX,rrY,Normalise_PI(rrOri+PI));
00912
00913 #ifdef DEBUGMAP
00914 WritePolygonToDebugFile(prev.poly_id);
00915 #endif
00916 }
00917 if ((upPoly.point_id==1 || upPoly.point_id==2) && curr.poly_id==next.poly_id-1 && curr.start_way.lane==next.start_way.lane && curr.start_way.seg==next.start_way.seg) {
00918 FilteredPolygon* temp=&(filtPolys.at(next.poly_id));
00919 if (upPoly.point_id==1) point=0;
00920 if (upPoly.point_id==2) point=3;
00921 temp->UpdatePoint(point,upPoly.distance,upPoly.bearing,upPoly.confidence,rrX,rrY,Normalise_PI(rrOri+PI));
00922
00923 #ifdef DEBUGMAP
00924 WritePolygonToDebugFile(next.poly_id);
00925 #endif
00926 }
00927 }
00928
00929 void MapLanes::UpdateWithCurrent(int i){
00930 static gaussian g1(0.0,1.0);
00931 FilteredPolygon* filt=&(filtPolys.at(i));
00932 poly temp2 = filtPolys.at(i).GetPolygon();
00933 if (temp2.is_transition || temp2.contains_way) return;
00934
00935 float angle=AngleFromXY(rX,rY,rOri,temp2.p1.x,temp2.p1.y);
00936 float distU=DistFromXY(rX,rY,temp2.p1.x,temp2.p1.y);
00937 if (distU>5 && distU<80 && fabs(angle) < 0.2) filt->UpdatePoint(0,distU+g1.get_sample_1D(),angle,1.0,rX,rY,rOri);
00938
00939 angle=AngleFromXY(rX,rY,rOri,temp2.p2.x,temp2.p2.y);
00940 distU=DistFromXY(rX,rY,temp2.p2.x,temp2.p2.y);
00941 if (distU>5 && distU<80 && fabs(angle) < 0.2) filt->UpdatePoint(1,distU+g1.get_sample_1D(),angle,1.0,rX,rY,rOri);
00942
00943 angle=AngleFromXY(rX,rY,rOri,temp2.p3.x,temp2.p3.y);
00944 distU=DistFromXY(rX,rY,temp2.p3.x,temp2.p3.y);
00945 if (distU>5 && distU<80 && fabs(angle) < 0.2) filt->UpdatePoint(2,distU+g1.get_sample_1D(),angle,1.0,rX,rY,rOri);
00946
00947 angle=AngleFromXY(rX,rY,rOri,temp2.p4.x,temp2.p4.y);
00948 distU=DistFromXY(rX,rY,temp2.p4.x,temp2.p4.y);
00949 if (distU>5 && distU<80 && fabs(angle) < 0.2) filt->UpdatePoint(3,distU+g1.get_sample_1D(),angle,1.0,rX,rY,rOri);
00950 }
00951
00952
00953 void MapLanes::testDraw(bool with_trans)
00954 {
00955 ZonePerimeterList empty_zones;
00956 MapLanes::testDraw(with_trans, empty_zones);
00957 }
00958
00959
00960 void MapLanes::testDraw(bool with_trans, const ZonePerimeterList &zones)
00961 {
00962 float max_x = -FLT_MAX;
00963 float min_x = FLT_MAX;
00964 float max_y = -FLT_MAX;
00965 float min_y = FLT_MAX;
00966
00967 FILE* gpsFile = fopen("gps.kml","wb");
00968
00969 fprintf(gpsFile,"<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n <kml xmlns=\"http://earth.google.com/kml/2.2\">\n <Document>\n<name>Maplanes Polygon centres</name>\n<description>Path of the polygons</description>\n<Style id=\"yellowLineGreenPoly\">\n<LineStyle>\n<color>7f00ffff</color>\n<width>4</width>\n</LineStyle>\n<PolyStyle>\n<color>7f00ff00</color>\n</PolyStyle>\n</Style>\n<Placemark>\n<name>Absolute Extruded</name>\n<description>Transparent green wall with yellow outlines</description>\n<styleUrl>#yellowLineGreenPoly</styleUrl>\n<LineString>\n<extrude>1</extrude>\n<tessellate>1</tessellate>\n<altitudeMode>clampToGround</altitudeMode>\n<coordinates>\n");
00970
00971 float yOff = 7.5;
00972 float xOff = 2.9;
00973
00974 for(int i = 0; i < (int)filtPolys.size(); i++)
00975 {
00976 poly node=(filtPolys.at(i).GetPolygon());
00977 double lon,lat;
00978 UTM::UTMtoLL(cY+node.midpoint.y+yOff,cX+node.midpoint.x+xOff,
00979 "11S",lon,lat);
00980 fprintf(gpsFile,"%lf,%lf,0\n",lat,lon);
00981
00982 max_x=fmax(fmax(fmax(fmax(node.p1.x,node.p2.x),
00983 node.p3.x), node.p4.x),max_x);
00984 max_y=fmax(fmax(fmax(fmax(node.p1.y,node.p2.y),
00985 node.p3.y),node.p4.y),max_y);
00986 min_x=fmin(fmin(fmin(fmin(node.p1.x,node.p2.x),
00987 node.p3.x),node.p4.x),min_x);
00988 min_y=fmin(fmin(fmin(fmin(node.p1.y,node.p2.y),
00989 node.p3.y),node.p4.y),min_y);
00990 }
00991 fprintf(gpsFile,
00992 "</coordinates>\n</LineString>\n</Placemark>\n</Document>\n</kml>");
00993 fclose(gpsFile);
00994
00995
00996 for(uint i = 0; i < graph->nodes_size; i++)
00997 {
00998 max_x=fmax(graph->nodes[i].map.x,max_x);
00999 max_y=fmax(graph->nodes[i].map.y,max_y);
01000 min_x=fmin(graph->nodes[i].map.x,min_x);
01001 min_y=fmin(graph->nodes[i].map.y,min_y);
01002 }
01003
01004 int xsize=(int)ceil(max_x - min_x);
01005 int ysize=(int)ceil(max_y - min_y);
01006
01007 if (xsize < 240)
01008 {
01009 min_x -= (240-xsize)/2;
01010 max_x += (240-xsize)/2;
01011 xsize=240;
01012 }
01013
01014 if (ysize < 168)
01015 {
01016 min_y -= (168-ysize)/2;
01017 max_y += (168-ysize)/2;
01018 ysize=168;
01019 }
01020
01021 float ratio=3;
01022 float image_size=xsize*ysize*DEFAULT_RATIO*DEFAULT_RATIO;
01023 if (image_size > (2048.0*2048))
01024 ratio=sqrtf((2047*2047.0)/(xsize*ysize));
01025
01026 std::cerr << "World size: "<<xsize<<","<<ysize<<std::endl;
01027 std::cerr << "Image size: "<<xsize*ratio<<","<<ysize*ratio<<std::endl;
01028
01029
01030
01031 DrawLanes* edgeImage = new DrawLanes(xsize,ysize,ratio);
01032 DrawLanes* polyImage = new DrawLanes(xsize,ysize,ratio);
01033
01034
01035 for(uint i = 0; i < graph->edges_size; i++)
01036 {
01037 WayPointNode w1=graph->nodes[graph->edges[i].startnode_index];
01038 WayPointNode w2=graph->nodes[graph->edges[i].endnode_index];
01039 edgeImage->addTrace(w1.map.x-min_x, max_y-w1.map.y,
01040 w2.map.x-min_x, max_y-w2.map.y);
01041
01042 }
01043
01044 for(uint i = 0; i < graph->nodes_size; i++)
01045 {
01046 WayPointNode w1=graph->nodes[i];
01047 polyImage->addWay(w1.map.x-min_x, max_y-w1.map.y);
01048 }
01049
01050 #if 0 //TODO
01051 for(unsigned i = 0; i < zones.size(); i++)
01052 {
01053 polyImage->addZone(zones[i], min_x, max_y);
01054 }
01055 #endif
01056
01057
01058 for(int i = 0; i < (int)filtPolys.size(); i++)
01059 {
01060 poly temp = filtPolys.at(i).GetPolygon();
01061 polyImage->addPoly(temp.p1.x-min_x, temp.p2.x-min_x,
01062 temp.p3.x-min_x,temp.p4.x-min_x,
01063 max_y-temp.p1.y, max_y-temp.p2.y,
01064 max_y-temp.p3.y, max_y-temp.p4.y,
01065 temp.is_stop,
01066 temp.is_transition && !with_trans);
01067 }
01068 bool drawRobot=false;
01069 if (drawRobot) polyImage->addRobot(rX-min_x,max_y-rY);
01070
01071
01072 ROS_INFO("Writing way-point image");
01073 edgeImage->savePGM("wayImage.ppm");
01074
01075 char* temp=new char[255];
01076 sprintf(temp,"polyImage%i.ppm",writecounter);
01077 writecounter++;
01078
01079 ROS_INFO("Writing polygons image");
01080 polyImage->savePGM(temp);
01081 delete temp;
01082
01083 }
01084
01085 #ifdef DEBUGMAP
01086 void MapLanes::WritePolygonToDebugFile(int i) {
01087 poly node=(filtPolys.at(i).GetPolygon());
01088 fprintf(debugFile,"%i %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %i %i\n",
01089 i,node.p1.x,node.p1.y,node.p2.x,node.p2.y,
01090 node.p3.x,node.p3.y,node.p4.x,node.p4.y,
01091 node.midpoint.x,node.midpoint.y, node.length,
01092 node.left_boundary,node.right_boundary);
01093 }
01094 #endif
01095
01096 bool MapLanes::WriteToFile(char* fName) {
01097 FILE* f = fopen(fName,"wb");
01098 if (f==NULL) {
01099 ROS_WARN("MapLanes::WriteToFile Failed - Can't open file");
01100 return false;
01101 }
01102 int sizeAll = allPolys.size();
01103 int sizeFilt = filtPolys.size();
01104
01105 int ret=fprintf(f,"%i %i\n",sizeAll,sizeFilt);
01106 if (ret<1) {
01107 ROS_WARN("MapLanes::SaveToFile Failed - Failed size write");
01108 return false;
01109 }
01110
01111 for(int i = 0; i < sizeAll; i++)
01112 {
01113 ret=fwrite(&(allPolys.at(i)),sizeof(poly),1,f);
01114 if (ret<1) {
01115 ROS_WARN("MapLanes::WriteToFile Failed - Failed poly write");
01116 return false;
01117 }
01118 }
01119 for(int i = 0; i < sizeFilt; i++)
01120 {
01121 ret=fwrite(&(filtPolys.at(i)),sizeof(FilteredPolygon),1,f);
01122 if (ret<1) {
01123 ROS_WARN("MapLanes::WriteToFile Failed - Failed FilteredPoylgon write");
01124 return false;
01125 }
01126 }
01127 fclose(f);
01128 return true;
01129 }
01130
01131 bool MapLanes::LoadFromFile(char* fName) {
01132 FILE* f = fopen(fName,"rb");
01133
01134 if (f==NULL) {
01135 ROS_WARN("MapLanes::LoadFromFile Failed - Can't open file");
01136 return false;
01137 }
01138
01139 int sizeAll = 0;
01140 int sizeFilt = 0;
01141
01142 int ret = fscanf(f,"%i %i\n",&sizeAll,&sizeFilt);
01143
01144 if (ret < 1) {
01145 ROS_WARN("MapLanes::LoadFromFile Failed - Failed size read");
01146 allPolys.clear();
01147 filtPolys.clear();
01148 return false;
01149 }
01150 if (sizeAll < 0 || sizeFilt < 0) {
01151 ROS_WARN("MapLanes::LoadFromFile Failed - Sizes < 0");
01152 allPolys.clear();
01153 filtPolys.clear();
01154 return false;
01155 }
01156
01157
01158 fpos_t position;
01159 fgetpos (f, &position);
01160 long now=ftell(f);
01161 fseek (f, 0, SEEK_END);
01162 long size=ftell(f)-now;
01163 fsetpos(f,&position);
01164 int expected=(sizeAll*sizeof(poly)) + (sizeFilt*sizeof(FilteredPolygon));
01165 if (size!=expected) {
01166 ROS_WARN("MapLanes::LoadFromFile Failed - Incorred File Size");
01167 allPolys.clear();
01168 filtPolys.clear();
01169 return false;
01170 }
01171
01172 allPolys.clear();
01173 filtPolys.clear();
01174
01175 poly p;
01176 for(int i = 0; i < sizeAll; i++)
01177 {
01178 ret=fread(&p,sizeof(poly),1,f);
01179 if (ret<1) {
01180 ROS_WARN("MapLanes::LoadFromFile Failed - Failed poly read");
01181 allPolys.clear();
01182 filtPolys.clear();
01183 return false;
01184 }
01185 allPolys.push_back(p);
01186 }
01187 FilteredPolygon fp;
01188 for(int i = 0; i < sizeFilt; i++)
01189 {
01190 ret=fread(&fp,sizeof(FilteredPolygon),1,f);
01191 if (ret<1) {
01192 ROS_WARN("MapLanes::LoadFromFile Failed - Failed FilteredPolygon read");
01193 allPolys.clear();
01194 filtPolys.clear();
01195 return false;
01196 }
01197 filtPolys.push_back(fp);
01198 }
01199 fclose(f);
01200 return true;
01201 }