$search
00001 /* 00002 * Navigator U-turn controller 00003 * 00004 * Copyright (C) 2007, 2010, Austin Robot Technology 00005 * 00006 * License: Modified BSD Software License Agreement 00007 * 00008 * $Id: uturn.cc 1613 2011-08-06 20:48:58Z jack.oquin $ 00009 */ 00010 00011 #include <angles/angles.h> 00012 00013 #include "navigator_internal.h" 00014 #include "Controller.h" 00015 #include "course.h" 00016 #include "obstacle.h" 00017 #include "uturn.h" 00018 //#include "safety.h" 00019 #include "stop.h" 00020 00021 #include <art_map/coordinates.h> 00022 using namespace Coordinates; 00023 #include <art_map/rotate_translate_transform.h> 00024 #include <art_msgs/ArtVehicle.h> 00025 using art_msgs::ArtVehicle; 00026 00027 // PFB: TODO: get all left lanes and take first one going other 00028 // direction. 00029 00030 // PFB TODO: Pad end of polygons until we get 15 meters on both side. 00031 // Then define "inside" or "outside" of lanes for points on robot we 00032 // care about (tires, bumper, etc.): if line segment connecting point 00033 // to closest point on right intersects left, then outside (and vice 00034 // versa). 00035 00036 // TODO Cont'd: If inside use first intersection point on circle. If 00037 // outside, use second. Only turn M_PI forward or backwards (u-turn 00038 // is theorectically M_PI turnaround (this *may* cause one more 00039 // backup, but prevents over shooting in other cases). 00040 00041 static const char *state_name[] = 00042 { 00043 "Backward", 00044 "Forward", 00045 "Wait", 00046 }; 00047 00048 Uturn::Uturn(Navigator *navptr, int _verbose): 00049 Controller(navptr, _verbose) 00050 { 00051 ROS_INFO("vehicle turn radius %.3fm, front, rear wheel radii %.3fm, %.3f", 00052 ArtVehicle::turn_radius, 00053 ArtVehicle::front_outer_wheel_turn_radius, 00054 ArtVehicle::rear_outer_wheel_turn_radius); 00055 //safety = new Safety(navptr, _verbose); 00056 stop = new Stop(navptr, _verbose); 00057 reset_me(); 00058 } 00059 00060 Uturn::~Uturn() 00061 { 00062 //delete safety; 00063 delete stop; 00064 } 00065 00066 // returns true if circle and line segment intersect 00067 // 00068 // exit: sets first meeting point, if they do 00069 // 00070 // In 2D geometry, a circle and a line intersect at zero, one or two 00071 // points. Those are the zeroes of a real-valued quadratic equation, 00072 // so their number is determined by its discriminant. Since p1 and p2 00073 // describe a line segment, only solutions between the two are of 00074 // interest. P1 is the first of the two in the direction of the lane. 00075 // 00076 // Let D(p,q) represent the Euclidean distance from point p to q. Let 00077 // L(p1,p2) be the line segment from p1 to p2, the set of all points q 00078 // such that D(p1,q) + D(q,p2) = D(p1,p2). Let C(c,r) be the circle 00079 // of radius r around center point c, the set of all points q such 00080 // that D(c,q) = r. Transforming all these points from MapXY into a 00081 // frame of reference with origin at the center of the circle 00082 // simplifies the equations for points (x,y) belonging to the line 00083 // through L and the circle C: 00084 // 00085 // y = s*x + k (line L) 00086 // y^2 + x^2 = r^2 (circle C) 00087 // 00088 // The slope (s) of the line is (p2.y-p1.y)/(p2.x-p1.x), and its 00089 // intercept (k) is p1.y-(s*p1.x). 00090 // 00091 // Beware the special case of L being vertical: s would be infinite 00092 // and x is a constant. This requires special handling. So does the 00093 // special case of p1 = p2, where the line segment is a point. 00094 // 00095 // In all other cases, intersection points must satisfy both 00096 // equations, so substitute the first equation for y in the second: 00097 // 00098 // (s*x + k)^2 + x^2 = r^2 00099 // 00100 // Grouping this into a quadratic equation on x, we get: 00101 // 00102 // (s^2+1)*x^2 + (2*s*k)*x + k^2 - r^2 = 0 00103 // 00104 // The canonical form is a*x^2 + b*x + c = 0, so we have: 00105 // 00106 // a = s^2 + 1 00107 // b = 2*s*k 00108 // c = k^2 - r^2 00109 // 00110 // The discriminant, D, of this equation (b^2-4*a*c) is: 00111 // 00112 // D = (2*s*k)^2 - 4*(s^2+1)*(k^2-r^2) 00113 // D = 4 * (s^2*r^2 + r^2 - k^2)) 00114 // 00115 // When D > 0 the equation has two roots (intersection points), when D 00116 // is 0 there is one, and D < 0 means there are none. As noted above, 00117 // any roots outside the line segment L(p1,p2) do not count and must be 00118 // ignored. 00119 // 00120 bool Uturn::circle_and_line_intersect(MapXY c, float r, 00121 MapXY point1, MapXY point2, 00122 MapXY &meetpt) 00123 { 00124 bool intersects = false; 00125 bool have_roots = false; 00126 MapXY root1, root2; // roots relative to center 00127 00128 // transformed coordinates relative to center of circle: 00129 MapXY p1 = point1 - c; 00130 MapXY p2 = point2 - c; 00131 00132 // Check for the special case of L being vertical. 00133 if (Epsilon::equal(p1.x, p2.x)) 00134 { 00135 if (Epsilon::equal(p1.y, p2.y)) 00136 { 00137 // the two points are the same 00138 if (verbose >= 4) 00139 ART_MSG(8, "line segment is a point"); 00140 if (Epsilon::equal(r*r, p1.x*p1.x + p1.y*p1.y)) 00141 { 00142 have_roots = true; 00143 root1 = p1; 00144 root2 = p1; 00145 } 00146 } 00147 else 00148 { 00149 // For a vertical line, x is a constant. 00150 if (verbose >= 4) 00151 ART_MSG(8, "line segment is vertical"); 00152 00153 root1.x = root2.x = p1.x; 00154 00155 // Compute y^2 from the formula for the circle. 00156 float y2 = r*r - root1.x*root1.x; 00157 if (y2 >= 0.0) 00158 { 00159 have_roots = true; 00160 root1.y = sqrtf(y2); 00161 root2.y = -root1.y; 00162 } 00163 } 00164 } 00165 else 00166 { 00167 // normal quadratic equation: 00168 float s = (p2.y - p1.y) / (p2.x - p1.x); 00169 float k = p1.y - s*p1.x; 00170 float discriminant = 4.0 * (s*s * r*r + r*r - k*k); 00171 00172 if (verbose >= 6) 00173 ART_MSG(8, "discriminant of circle and line y = (%.3f * x + %.3f)" 00174 " is %.3f", s, k, discriminant); 00175 00176 // does the equation have any roots? 00177 if (discriminant >= 0.0) 00178 { 00179 // compute quadratic roots 00180 float a = s*s + 1.0f; 00181 float b = 2.0f * s * k; 00182 float c = k*k - r*r; 00183 00184 if (verbose >= 6) 00185 ART_MSG(8, "y = (%.3f * x^2 + %.3f * x + %.3f)" 00186 " has at least one root", a, b, c); 00187 00188 if (discriminant >= Epsilon::float_value 00189 && fabsf(b) >= Epsilon::float_value) 00190 { 00191 // Compute roots of the quadratic using the floating 00192 // point implementation trick in the Wikipedia 00193 // "Quadratic Equation" article. It ensures that the 00194 // quantities added are of the same sign, avoiding 00195 // catastrophic cancellation. The root2 computation 00196 // uses the fact that the product of the roots is c/a. 00197 float t = (-(1.0f/2.0f) * (b + sign(b) * sqrtf(discriminant))); 00198 root1.x = t/a; 00199 root2.x = c/t; 00200 } 00201 else 00202 { 00203 // apply algebraic formula directly 00204 root1.x = (-b - sqrtf(discriminant)) / 2 * a; 00205 root2.x = (-b + sqrtf(discriminant)) / 2 * a; 00206 } 00207 00208 // compute the y coordinates from the equation for the line 00209 root1.y = s*root1.x + k; 00210 root2.y = s*root2.x + k; 00211 have_roots = true; 00212 } 00213 } 00214 00215 if (have_roots) 00216 { 00217 if (verbose >= 6) 00218 ART_MSG(8, "circle intersects (%.3f,%.3f), (%.3f,%.3f) at (%.3f,%.3f)" 00219 " and (%.3f,%.3f)", 00220 p1.x, p1.y, p2.x, p2.y, 00221 root1.x, root1.y, root2.x, root2.y); 00222 00223 // see if the roots are contained in the line segment 00224 bool r1_valid = Euclidean::point_in_line_segment(root1, p1, p2); 00225 bool r2_valid = Euclidean::point_in_line_segment(root2, p1, p2); 00226 00227 intersects = (r1_valid || r2_valid); 00228 00229 // return the appropriate meeting point 00230 if (r1_valid && r2_valid) 00231 { 00232 // see which of root1 and root2 comes first in the lane 00233 if (Euclidean::DistanceTo(root1, p1) 00234 <= Euclidean::DistanceTo(root2, p1)) 00235 meetpt = root1 + c; 00236 else 00237 meetpt = root2 + c; 00238 } 00239 else if (r1_valid) 00240 { 00241 meetpt = root1 + c; 00242 } 00243 else if (r2_valid) 00244 { 00245 meetpt = root2 + c; 00246 } 00247 } 00248 00249 if (intersects) 00250 { 00251 if (verbose >= 4) 00252 ART_MSG(8, "U-turn intersects (%.3f,%.3f), (%.3f,%.3f) at (%.3f,%.3f)", 00253 point1.x, point1.y, point2.x, point2.y, 00254 meetpt.x, meetpt.y); 00255 } 00256 else if (verbose >= 5) 00257 ART_MSG(8, "U-turn does not intersect (%.3f,%.3f), (%.3f,%.3f)", 00258 point1.x, point1.y, point2.x, point2.y); 00259 00260 return intersects; 00261 } 00262 00263 // perform U-turn 00264 // 00265 // All these calculations are done using the absolute value of the 00266 // speed. The run controller will convert them later if the 00267 // navdata->reverse flag gets set. 00268 // 00269 // entry: 00270 // navdata->reverse indicates current direction of travel 00271 // exit: 00272 // navdata->reverse indicates desired direction of travel 00273 // result: 00274 // OK, if able to continue or waiting for obstacle 00275 // Finished, if U-turn completed 00276 // NotApplicable, if no U-turn in order or no polygons available 00277 // safety controller results, if applicable 00278 // 00279 Controller::result_t Uturn::control(pilot_command_t &pcmd) 00280 { 00281 // This controller never marks a way-point officially reached. 00282 // Don't want run controller to get upset about that. 00283 course->no_waypoint_reached(); 00284 00285 result_t result = OK; 00286 if (do_init) 00287 { 00288 result = initialize(); 00289 if (result == NotApplicable) 00290 return result; 00291 } 00292 00293 if (verbose >= 2) 00294 { 00295 // car may not be at a U-turn way-point. 00296 ART_MSG(5, "U-turn from near waypoint %s to %s, state is %s", 00297 uturn_exit.start_way.name().str, 00298 uturn_entry.start_way.name().str, 00299 state_name[state]); 00300 } 00301 00302 // set Pilot command for slow speed, hard left turn 00303 pcmd.velocity = fminf(pcmd.velocity, config_->uturn_speed); 00304 pcmd.yawRate = config_->uturn_yaw_rate; 00305 00306 // The car's heading monotonically approaches that of the goal 00307 // way-point from right to left. Due to lane heading differences, 00308 // the remaining angle could be a little more than 180 degrees. 00309 // Convert a normalized angle to the right rear into a larger left 00310 // turn, ranging from -90 to 270 degrees. When it goes negative, we 00311 // have turned too far. 00312 00313 00314 float remaining_angle = normalize(goal_heading 00315 - MapPose(estimate->pose.pose).yaw); 00316 if (remaining_angle < -HALFPI) 00317 remaining_angle += TWOPI; 00318 00319 float desired_arc_length = remaining_angle * ArtVehicle::turn_radius; 00320 ROS_DEBUG("%.3f radians (%.f degrees) remain for U-turn, %.3fm arc", 00321 remaining_angle, angles::to_degrees(remaining_angle), 00322 desired_arc_length); 00323 00324 switch (state) 00325 { 00326 case Wait: 00327 // wait until left lane is clear. 00328 if (obstacle->observer_clear(Observation::Adjacent_left)) 00329 { 00330 if (verbose) 00331 ART_MSG(1, "U-turn entry lane clear"); 00332 set_state(Forward); 00333 } 00334 else 00335 { 00336 // TODO: check a timer. What if it expires? Return Blocked? 00337 if (verbose) 00338 ART_MSG(2, "U-turn entry lane blocked"); 00339 } 00340 pcmd.velocity = 0.0; // wait for lane to clear 00341 break; 00342 00343 case Forward: 00344 { 00345 if (outside_lanes_front()) // gone too far forward? 00346 { 00347 if (navdata->stopped) 00348 { 00349 set_state(Backward); 00350 navdata->reverse = true; 00351 } 00352 pcmd.velocity = 0.0; 00353 break; 00354 } 00355 00356 float arc_length = estimate_uturn_distance(true, desired_arc_length); 00357 #if 0 00358 if (remaining_angle <= config_->uturn_stop_heading 00359 || arc_length > desired_arc_length) 00360 #else // Pat's way (Pat want to make sure we are catching this on 00361 // both sides, so normalize the heading): 00362 if (fabs(Coordinates::normalize(remaining_angle)) <= config_->uturn_stop_heading) 00363 #endif 00364 { 00365 // The car has a clear path to the goal heading or the 00366 // remaining angle is close enough for follow_lane to 00367 // handle. Since the car is still moving, it's good to 00368 // finish a little early to avoid turning too far. 00369 result = Finished; 00370 break; 00371 } 00372 00373 if (stop->control(pcmd, arc_length, 00374 config_->uturn_threshold, config_->uturn_speed) == Finished) 00375 { 00376 set_state(Backward); 00377 navdata->reverse = true; 00378 } 00379 } 00380 break; 00381 00382 case Backward: 00383 if (outside_lanes_rear() // gone too far backward? 00384 || (!outside_lanes_front() 00385 && (estimate_uturn_distance(true, desired_arc_length) 00386 > desired_arc_length))) 00387 { 00388 // The car has either gone too far backwards or there is a 00389 // clear path to goal heading going forward. Stop and go 00390 // forward again. 00391 if (navdata->stopped) 00392 { 00393 set_state(Forward); 00394 navdata->reverse = false; 00395 } 00396 pcmd.velocity = 0.0; 00397 } 00398 else // continue backing 00399 { 00400 // estimate distance to lane boundary 00401 float arc_length = estimate_uturn_distance(false, 00402 desired_arc_length); 00403 00404 // continue backing 00405 if (stop->control(pcmd, arc_length, config_->uturn_threshold) == Finished) 00406 { 00407 // this is far enough 00408 set_state(Forward); 00409 navdata->reverse = false; 00410 } 00411 } 00412 break; 00413 }; 00414 00415 #if 0 // not using safety controller yet 00416 // must always run this on the final command 00417 result_t sresult = safety->control(pcmd); 00418 if (result != Finished) 00419 result = sresult; // return safety result 00420 00421 // TODO: implement obstacle evasion if Unsafe or Blocked 00422 if (sresult == Blocked) 00423 switch (state) { 00424 case Forward: 00425 set_state(Backward); 00426 navdata->reverse = true; 00427 break; 00428 case Backward: 00429 set_state(Forward); 00430 navdata->reverse = false; 00431 break; 00432 default: 00433 break; 00434 } 00435 #endif // not using safety controller yet 00436 00437 trace("uturn controller", pcmd, result); 00438 00439 return result; 00440 }; 00441 00442 float Uturn::calculate_arc_length(bool forward, 00443 const MapXY& center, 00444 float safety_radius, 00445 const MapXY& p1, 00446 const MapXY& p2) 00447 { 00448 MapXY curb_point; // point where path meets curb 00449 00450 float arc_length=Infinite::distance; 00451 00452 // TODO: (1) Return both intersection points, it is possible 00453 // (though unlikely) for the second one to have a shorter arc 00454 // length. (2) Compute the safety radii of both outside wheels, 00455 // and use the shorter arc distance of the two. 00456 00457 // compute intersection of circle with polygon, set curb_point 00458 if (circle_and_line_intersect(center, safety_radius, p1, p2, curb_point)) 00459 { 00460 // bearings of car and meeting point from center of circle 00461 float curb_bearing = bearing(center, curb_point); 00462 float car_bearing; 00463 if (forward) 00464 { 00465 // going forward, use the front axle's bearing 00466 Polar front_axle(0.0, ArtVehicle::wheelbase); 00467 car_bearing = 00468 bearing(center, 00469 Polar_to_MapXY(front_axle, 00470 MapPose(estimate->pose.pose))); 00471 } 00472 else 00473 { 00474 // in reverse, use the rear axle's bearing 00475 car_bearing = bearing(center, MapXY(estimate->pose.pose.position)); 00476 } 00477 00478 // determine angle from car to meeting point 00479 // (an arc to the left is non-negative) 00480 float arc = curb_bearing - car_bearing; 00481 while (arc < 0.0) 00482 arc += TWOPI; 00483 00484 // distance from car bumper to meeting point around the circle 00485 arc_length = arc * ArtVehicle::turn_radius; 00486 00487 if (verbose >= 5) 00488 ART_MSG(5, "U-turn arc length %.3fm, bearing radians [%.3f,%.3f]", 00489 arc_length, car_bearing, curb_bearing); 00490 } 00491 return arc_length; 00492 } 00493 00494 // returns distance to where estimated path intersects a curb 00495 // 00496 // This estimate assumes the car follows a circular path which may 00497 // intersect the lane boundary at one or two points in each polygon, 00498 // and determines the first intersection point along that path. 00499 // 00500 float Uturn::estimate_uturn_distance(bool forward, float desired_arc_length) 00501 { 00502 // compute center point of circle 00503 MapXY center; // center of curvature 00504 float safety_radius; 00505 if (forward) 00506 { 00507 // circle to left of the car 00508 center = Polar_to_MapXY(Polar(HALFPI, ArtVehicle::turn_radius), 00509 MapPose(estimate->pose.pose)); 00510 safety_radius = ArtVehicle::front_outer_wheel_turn_radius; 00511 } 00512 else 00513 { 00514 // circle to right of the car 00515 center = Polar_to_MapXY(Polar(-HALFPI, ArtVehicle::turn_radius), 00516 MapPose(estimate->pose.pose)); 00517 safety_radius = ArtVehicle::rear_outer_wheel_turn_radius; 00518 } 00519 00520 if (verbose >= 2) 00521 ART_MSG(5, "U-turn circle center going %s is (%.3f,%.3f)", 00522 (forward? "forward": "backward"), center.x, center.y); 00523 00524 float min_arc_length = Infinite::distance; 00525 00526 // check lane polygon right boundaries in the direction of lane heading 00527 for (unsigned pidx = 0; pidx < uturn_polys.size(); ++pidx) 00528 { 00529 MapXY p1 = uturn_polys.at(pidx).p4; // bottom right 00530 MapXY p2 = uturn_polys.at(pidx).p3; // top right 00531 00532 float arc_length = 00533 calculate_arc_length(forward,center,safety_radius,p1,p2); 00534 min_arc_length = fminf(arc_length, min_arc_length); 00535 } 00536 00537 if (min_arc_length < desired_arc_length) 00538 { 00539 if (verbose >= 4) 00540 ART_MSG(5, "U-turn %s arc intersects in %.3fm", 00541 (forward? "forward": "backward"), min_arc_length); 00542 return min_arc_length; 00543 } 00544 00545 int index=pops->getClosestPoly(uturn_polys,uturn_entry.midpoint); 00546 if (index >= 0) 00547 { 00548 MapXY p1 = uturn_polys.at(index).p4; // bottom right 00549 MapXY p2 = uturn_polys.at(index).p3; // top right 00550 00551 MapXY mid=pops->midpoint(p1,p2); 00552 posetype origin(0,0,0); 00553 posetype origin_abs(mid.x,mid.y,bearing(p1,p2)); 00554 00555 posetype far(300,0,0); 00556 00557 rotate_translate_transform long_trans; 00558 long_trans.find_transform(origin, origin_abs); 00559 posetype far_abs=long_trans.apply_transform(far); 00560 far.x*=-1; 00561 posetype nfar_abs=long_trans.apply_transform(far); 00562 00563 p2.x=far_abs.x; 00564 p2.y=far_abs.y; 00565 p1.x=nfar_abs.x; 00566 p1.y=nfar_abs.y; 00567 00568 00569 float arc_length = 00570 calculate_arc_length(forward,center,safety_radius,p1,p2); 00571 min_arc_length = fminf(arc_length, min_arc_length); 00572 } 00573 00574 index=pops->getClosestPoly(uturn_polys,uturn_exit.midpoint); 00575 if (index >= 0) 00576 { 00577 MapXY p1 = uturn_polys.at(index).p4; // bottom right 00578 MapXY p2 = uturn_polys.at(index).p3; // top right 00579 00580 MapXY mid=pops->midpoint(p1,p2); 00581 posetype origin(0,0,0); 00582 posetype origin_abs(mid.x,mid.y,bearing(p1,p2)); 00583 00584 posetype far(300,0,0); 00585 00586 rotate_translate_transform long_trans; 00587 long_trans.find_transform(origin, origin_abs); 00588 posetype far_abs=long_trans.apply_transform(far); 00589 far.x*=-1; 00590 posetype nfar_abs=long_trans.apply_transform(far); 00591 00592 p2.x=far_abs.x; 00593 p2.y=far_abs.y; 00594 p1.x=nfar_abs.x; 00595 p1.y=nfar_abs.y; 00596 00597 float arc_length = 00598 calculate_arc_length(forward,center,safety_radius,p1,p2); 00599 min_arc_length = fminf(arc_length, min_arc_length); 00600 } 00601 00602 if (min_arc_length < Infinite::distance) 00603 { 00604 if (verbose >= 4) 00605 ART_MSG(5, "U-turn %s arc intersects in %.3fm", 00606 (forward? "forward": "backward"), min_arc_length); 00607 } 00608 else 00609 { 00610 if (verbose >= 5) 00611 ART_MSG(2, "U-turn %s arc does not intersect curb locally", 00612 (forward? "forward": "backward")); 00613 } 00614 00615 return min_arc_length; 00616 } 00617 00618 // initialize U-turn operation 00619 Controller::result_t Uturn::initialize(void) 00620 { 00621 poly_list_t current_lane_polys, left_lane_polys; 00622 00623 00624 pops->AddLanePolys(course->polygons, current_lane_polys, 00625 order->waypt[0].id); 00626 00627 int uturn_exit_index = 00628 pops->getClosestPoly(current_lane_polys, 00629 MapPose(estimate->pose.pose)); 00630 00631 int uturn_entry_index=-1; 00632 00633 if (uturn_exit_index >= 0) 00634 { 00635 MapXY exit_pose; 00636 exit_pose = current_lane_polys.at(uturn_exit_index).midpoint; 00637 pops->AddLanePolys(course->polygons, left_lane_polys, 00638 order->waypt[1].id); 00639 uturn_entry_index = pops->getClosestPoly(left_lane_polys, exit_pose); 00640 } 00641 00642 if (uturn_exit_index < 0 || uturn_entry_index < 0) 00643 { 00644 ROS_ERROR("no U-turn according to polygons: %d %d", 00645 uturn_exit_index, uturn_entry_index); 00646 return NotApplicable; 00647 } 00648 00649 do_init = false; 00650 00651 uturn_exit = current_lane_polys.at(uturn_exit_index); 00652 uturn_entry = left_lane_polys.at(uturn_entry_index); 00653 00654 for (uint i=0; i<current_lane_polys.size();i++) 00655 if (Euclidean::DistanceTo(current_lane_polys[i].midpoint, 00656 uturn_exit.midpoint) <= 15.0) 00657 uturn_polys.push_back(current_lane_polys[i]); 00658 00659 for (uint i=0; i<left_lane_polys.size();i++) 00660 if (Euclidean::DistanceTo(left_lane_polys[i].midpoint, 00661 uturn_entry.midpoint) <= 15.0) 00662 uturn_polys.push_back(left_lane_polys[i]); 00663 00664 00665 // TODO: make sure all four wheels are inside the uturn_polys. 00666 00667 course->log("U-turn", uturn_polys); 00668 00669 if (uturn_polys.empty()) 00670 return NotApplicable; 00671 00672 // get polygon index of uturn_entry, the goal way-point 00673 int p_index = pops->getContainingPoly(uturn_polys, uturn_entry.midpoint); 00674 if (p_index < 0) 00675 { 00676 ART_ERROR("no polygon for U-turn entry waypoint."); 00677 return NotApplicable; 00678 } 00679 00680 // save heading of the goal polygon 00681 goal_heading = pops->PolyHeading(uturn_polys.at(p_index)); 00682 00683 return OK; 00684 } 00685 00686 // return true when either front wheel is outside the U-turn lanes. 00687 bool Uturn::outside_lanes_front(void) 00688 { 00689 if (point_outside_lanes(wheel_location(ArtVehicle::front_right_wheel_px, 00690 ArtVehicle::front_right_wheel_py))) 00691 { 00692 if (verbose >= 3) 00693 ART_MSG(6, "right front wheel is outside the U-turn lanes"); 00694 return true; 00695 } 00696 00697 if (point_outside_lanes(wheel_location(ArtVehicle::front_left_wheel_px, 00698 ArtVehicle::front_left_wheel_py))) 00699 { 00700 if (verbose >= 3) 00701 ART_MSG(6, "left front wheel is outside the U-turn lanes"); 00702 return true; 00703 } 00704 00705 return false; 00706 } 00707 00708 // return true when either rear wheel is outside the U-turn lanes. 00709 bool Uturn::outside_lanes_rear(void) 00710 { 00711 if (point_outside_lanes(wheel_location(ArtVehicle::rear_right_wheel_px, 00712 ArtVehicle::rear_right_wheel_py))) 00713 { 00714 if (verbose >= 3) 00715 ART_MSG(6, "right rear wheel is outside the U-turn lanes"); 00716 return true; 00717 } 00718 00719 if (point_outside_lanes(wheel_location(ArtVehicle::rear_left_wheel_px, 00720 ArtVehicle::rear_left_wheel_py))) 00721 { 00722 if (verbose >= 3) 00723 ART_MSG(6, "left rear wheel is outside the U-turn lanes"); 00724 return true; 00725 } 00726 00727 return false; 00728 } 00729 00730 // return true if point is outside the U-turn lanes 00731 bool Uturn::point_outside_lanes(MapXY point) 00732 { 00733 int pindex = pops->getClosestPoly(uturn_polys, point); 00734 if (pindex < 0) // no polygon found? 00735 return true; // must be outside lane 00736 00737 poly nearest = uturn_polys.at(pindex); 00738 00739 // get the bearing of the point from right side of polygon 00740 float point_bearing = normalize(bearing(nearest.p4, point) 00741 - bearing(nearest.p4, nearest.p3)); 00742 00743 if (point_bearing >= 0.0) 00744 return false; // point is left of right curb 00745 00746 if (verbose >= 4) 00747 ART_MSG(7, "point (%.3f %.3f) is outside nearest polygon, %d", 00748 point.x, point.y, nearest.poly_id); 00749 00750 return true; 00751 } 00752 00753 // reset all subordinate controllers 00754 void Uturn::reset(void) 00755 { 00756 trace_reset("Uturn"); 00757 reset_me(); 00758 //safety->reset(); 00759 stop->reset(); 00760 } 00761 00762 // reset controller 00763 void Uturn::reset_me(void) 00764 { 00765 navdata->reverse = false; 00766 state = Wait; 00767 do_init = true; 00768 uturn_polys.clear(); 00769 } 00770 00771 // set new state 00772 void Uturn::set_state(state_t newstate) 00773 { 00774 if (state != newstate) 00775 { 00776 if (verbose) 00777 ART_MSG(4, "U-turn state changing from %s to %s", 00778 state_name[state], state_name[newstate]); 00779 state = newstate; 00780 } 00781 } 00782 00783 // return MapXY position of egocentric point 00784 MapXY Uturn::wheel_location(float x, float y) 00785 { 00786 posetype vehicle_relative(0, 0, 0); 00787 posetype wheel_relative(x, y, 0); 00788 MapPose pose2d(estimate->pose.pose); 00789 posetype vehicle_map(pose2d.map.x, pose2d.map.y, pose2d.yaw); 00790 rotate_translate_transform trans; 00791 trans.find_transform(vehicle_relative, vehicle_map); 00792 posetype wheel_map = trans.apply_transform(wheel_relative); 00793 return MapXY(wheel_map.x, wheel_map.y); 00794 }