uturn.cc
Go to the documentation of this file.
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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


art_nav
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Tue Sep 24 2013 10:42:11