follow_lane.cc
Go to the documentation of this file.
00001 /*
00002  *  Navigator follow lane controller
00003  *
00004  *  Copyright (C) 2007, 2010, Austin Robot Technology
00005  *
00006  *  License: Modified BSD Software License Agreement
00007  *
00008  *  $Id: follow_lane.cc 1876 2011-11-26 23:48:08Z jack.oquin $
00009  */
00010 
00011 #include "navigator_internal.h"
00012 #include "Controller.h"
00013 #include "course.h"
00014 #include "obstacle.h"
00015 #include "follow_lane.h"
00016 
00017 #include "avoid.h"
00018 #include "follow_safely.h"
00019 #include "slow_for_curves.h"
00020 #include "stop_area.h"
00021 #include "stop_line.h"
00022 
00023 typedef struct
00024 {
00025   const char *name;
00026   bool  do_stop;
00027 } way_table_t;
00028   
00029 static const way_table_t way_table[] =
00030   {
00031     {"Lane", false},
00032     {"LaneChange", true},
00033     {"Merge", true},
00034     {"Stop", true},
00035     {"Uturn", true},
00036     {"Zone", false},    // set to true so stop_line marks WPT reached
00037     {"ZoneExit", true}, // set to true so stop_line marks WPT reached
00038   };
00039 
00040 // return upcoming significant way-point type
00041 //
00042 // exit: sets stop_point, depending on way-point type
00043 //
00044 FollowLane::way_type_t
00045 FollowLane::approaching_waypoint_type(WayPointNode &stop_point)
00046 {
00047   way_type_t wtype = Lane;
00048   unsigned i = 0;
00049   while (++i < Order::N_WAYPTS)
00050     {
00051       // only look in the goal lane
00052       if (!course->same_lane(order->waypt[1].id, order->waypt[i].id))
00053         break;
00054       if (order->waypt[i].is_perimeter &&
00055           order->waypt[i].is_exit)
00056         {
00057           wtype = ZoneExit;
00058           break;
00059         }
00060       else if (order->waypt[i].is_perimeter)
00061         {
00062           wtype = Zone;
00063           break;
00064         }
00065       else if (order->waypt[i].is_stop)
00066         {
00067           wtype = Stop;
00068           break;
00069         }
00070       else if (course->uturn_waypt(i))
00071         {
00072           wtype = Uturn;
00073           break;
00074         }
00075       else if (order->waypt[i].is_lane_change)
00076         {
00077           wtype = LaneChange;
00078           break;
00079         }
00080       else if (order->waypt[i].is_exit
00081                && i+1 < Order::N_WAYPTS
00082                && order->waypt[i+1].is_entry
00083                && !course->same_lane(order->waypt[i].id,
00084                                      order->waypt[i+1].id))
00085         {
00086           if (course->nqe_special(i,i+1))
00087             ART_MSG(3, "Not going to merge for special case");
00088           else
00089             {
00090               // This is a transition between a pair of exit and entry
00091               // way-points in different lanes (or from end to start of a
00092               // single lane).  Treat that as a merge operation: stop and
00093               // wait until it is clear to go.
00094               
00095               // TODO: ignore this if it is not really an intersection.
00096               // That could theoretically be determined when there are no
00097               // transition polygons from that exit to any other
00098               // way-point.  Hard to do here, and only medium priority.
00099               wtype = Merge;
00100               break;
00101             }
00102         }
00103     }
00104 
00105   if (way_table[wtype].do_stop)
00106     stop_point = order->waypt[i];
00107   else
00108     stop_point.id = ElementID();
00109 
00110   return wtype;
00111 }
00112 
00113 FollowLane::FollowLane(Navigator *navptr, int _verbose):
00114   Controller(navptr, _verbose)
00115 {
00116   //avoid = new Avoid(navptr, _verbose);
00117   follow_safely = new FollowSafely(navptr, _verbose);
00118   slow_for_curves = new SlowForCurves(navptr, _verbose);
00119   stop_area =   new StopArea(navptr, _verbose);
00120   stop_line =   new StopLine(navptr, _verbose);
00121   reset_me();
00122 };
00123 
00124 FollowLane::~FollowLane()
00125 {
00126   //delete avoid;
00127   delete follow_safely;
00128   delete slow_for_curves;
00129   delete stop_area;
00130   delete stop_line;
00131 };
00132 
00133 // follow lane in the normal direction
00134 //
00135 // result:
00136 //      OK if able to continue or queued within safety area;
00137 //      Finished, if stop, intersection or U-turn way-point reached;
00138 //      Blocked, if lane blocked and outside safety area;
00139 //      safety controller results other than Blocked;
00140 //
00141 Controller::result_t FollowLane::control(pilot_command_t &pcmd)
00142 {
00143   if (order->waypt[1].is_perimeter)
00144     pcmd.velocity=fminf(pcmd.velocity,1.0); //Make this config
00145 
00146 
00147 #if 0 // not doing avoid right now
00148   pilot_command_t incmd = pcmd;         // copy of original input
00149 #endif // not doing avoid right now
00150 
00151   // set approaching way-point type
00152   way_type_t wtype = approaching_waypoint_type(course->stop_waypt);
00153 
00154   // A safety area includes an intersection and the last 30m of any
00155   // lane leading up to a stop line.
00156   bool in_intersection = (!course->same_lane(order->waypt[0].id,
00157                                              order->waypt[1].id)
00158                           || order->waypt[0].is_stop);
00159 
00160   // the first time through there is no course->plan yet
00161   if (course->in_lane(MapPose(estimate->pose.pose))) // in travel lane?
00162     {
00163       if (in_intersection)
00164         {
00165           ROS_DEBUG("Cross intersection to waypoint %s (%s ahead)",
00166                     ElementID(order->waypt[1].id).name().str,
00167                     way_table[wtype].name);
00168           // TODO: make .cfg option
00169           //      pcmd.velocity = fminf(pcmd.velocity, 3.0);
00170         }
00171       else
00172         {
00173           ROS_DEBUG("Follow lane to waypoint %s (%s ahead)",
00174                     ElementID(order->waypt[1].id).name().str,
00175                     way_table[wtype].name);
00176       
00177           // if signalling while joining a lane, turn signals off now
00178           course->turn_signals_off();
00179         }
00180 
00181       // fill in path to the next way-points
00182       course->find_travel_lane(false);  // not rejoining the lane
00183     }
00184   else
00185     {
00186       // set a path to join the travel lane
00187       ROS_DEBUG("Go to lane containing waypoint %s (%s ahead)",
00188                 ElementID(order->waypt[1].id).name().str,
00189                 way_table[wtype].name);
00190 
00191       // fill in path to the next way-points
00192       course->find_travel_lane(true);   // rejoin lane
00193     }
00194 
00195   // slow down if no plan
00196   if (course->plan.empty())
00197     nav->reduce_speed_with_min(pcmd, config_->lost_speed);
00198 
00199   bool in_safety_area = (in_intersection
00200                          || stop_area->control(pcmd) == OK);
00201 
00202   // adjust speed to maintain a safe following distance in the lane
00203   result_t result = follow_safely->control(pcmd);
00204 
00205   // reduce speed if approaching sharp turn.
00206   slow_for_curves->control(pcmd);
00207 
00208   if (in_safety_area)
00209     {
00210       // Do not return Blocked or Collision in safety area, because no
00211       // passing is allowed.
00212       if (result == Blocked)
00213         {
00214           // reset blockage timer while queued within stop area
00215           if (!in_intersection)
00216             {
00217 // #ifdef NQE
00218 //            if (order->waypt[1].id.seg!=4)
00219 //              obstacle->blocked();
00220 // #else
00221               obstacle->blocked();
00222 // #endif
00223             }
00224           
00225           result = Unsafe;
00226         }
00227       else if (result == Collision)
00228         result = Unsafe;
00229     }
00230   
00231   if (way_table[wtype].do_stop)         // stopping point in order?
00232     {
00233       result_t stop_result = stop_line->control(pcmd);
00234       if (stop_result == Finished)      // stopping point reached?
00235         result = stop_result;           // ignore follow_safely result
00236     }
00237 
00238   // set heading to desired course
00239   course->desired_heading(pcmd);
00240 
00241   // check if way-point reached, ignoring stop lines and U-turns
00242   course->lane_waypoint_reached();
00243 
00244 #if 0 // not doing avoid right now
00245   if (result == OK)                     // still moving, no Collision?
00246     {
00247       result = avoid->control(pcmd, incmd);
00248       if (result == Blocked)
00249         result = Unsafe;                // return Blocked only for passing
00250     }
00251 #endif // not doing avoid right now
00252 
00253   trace("follow_lane controller", pcmd, result);
00254 
00255   return result;
00256 };
00257 
00258 // reset all subordinate controllers
00259 void FollowLane::reset(void)
00260 {
00261   trace_reset("FollowLane");
00262   reset_me();
00263   //avoid->reset();
00264   follow_safely->reset();
00265   slow_for_curves->reset();
00266   stop_area->reset();
00267   stop_line->reset();
00268 }
00269 
00270 // reset this controller only
00271 void FollowLane::reset_me(void)
00272 {
00273   navdata->reverse = false;
00274 }
 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