road.cc
Go to the documentation of this file.
00001 /*
00002  *  Navigator road controller finite state machine
00003  *
00004  *  Copyright (C) 2007, 2010, Austin Robot Technology
00005  *  License: Modified BSD Software License Agreement
00006  *
00007  *  $Id: road.cc 1639 2011-08-11 23:39:23Z jack.oquin $
00008  */
00009 
00010 #include "navigator_internal.h"
00011 #include "Controller.h"
00012 #include "course.h"
00013 #include "obstacle.h"
00014 #include "road.h"
00015 
00016 //#include "evade.h"
00017 #include "follow_lane.h"
00018 #include "follow_safely.h"
00019 #include "halt.h"
00020 //#include "real_zone.h"
00021 #include "passing.h"
00022 #include "ntimer.h"
00023 #include "uturn.h"
00024 //#include "voronoi_zone.h"
00025 
00026 Road::Road(Navigator *navptr, int _verbose):
00027   Controller(navptr, _verbose)
00028 {
00029   passing_first=true;
00030 
00031   // initialize transition table, unused entries cause an error action
00032   for (int event = 0; event < (int) NavRoadEvent::N_events; ++event)
00033     for (int state = 0; state < (int) NavRoadState::N_states; ++state)
00034       {
00035         transtion_t *xp = &ttable[event][state];
00036         xp->action = &Road::ActionError;
00037         xp->next = (NavRoadState::state_t) state;
00038       }
00039 
00040   // valid transitions alphabetically by previous state, event
00041   
00042   Add(NavRoadEvent::Collision,          &Road::ActionToEvade,
00043       NavRoadState::Block,              NavRoadState::Evade);
00044   Add(NavRoadEvent::FollowLane,         &Road::ActionToFollow,
00045       NavRoadState::Block,              NavRoadState::Follow);
00046   Add(NavRoadEvent::None,               &Road::ActionInBlock,
00047       NavRoadState::Block,              NavRoadState::Block);
00048   Add(NavRoadEvent::Uturn,              &Road::ActionToUturn,
00049       NavRoadState::Block,              NavRoadState::Uturn);
00050   
00051 #if 0 // JOQ: I don't think this is possible
00052   Add(NavRoadEvent::Block,              &Road::ActionInEvade,
00053       NavRoadState::Evade,              NavRoadState::Evade);
00054 #endif
00055   Add(NavRoadEvent::FollowLane,         &Road::ActionEvadeToFollow,
00056       NavRoadState::Evade,              NavRoadState::Follow);
00057   Add(NavRoadEvent::None,               &Road::ActionInEvade,
00058       NavRoadState::Evade,              NavRoadState::Evade);
00059   Add(NavRoadEvent::Pass,               &Road::ActionToPass,
00060       NavRoadState::Evade,              NavRoadState::Pass);
00061   // JOQ: this looks wrong:
00062   Add(NavRoadEvent::WaitPass,           &Road::ActionInEvade,
00063       NavRoadState::Evade,              NavRoadState::Evade);
00064   
00065   Add(NavRoadEvent::Block,              &Road::ActionToBlock,
00066       NavRoadState::Follow,             NavRoadState::Block);
00067   Add(NavRoadEvent::ChangeLane,         &Road::ActionToWaitLane,
00068       NavRoadState::Follow,             NavRoadState::WaitLane);
00069   Add(NavRoadEvent::Collision,          &Road::ActionToEvade,
00070       NavRoadState::Follow,             NavRoadState::Evade);
00071   Add(NavRoadEvent::FollowLane,         &Road::ActionInFollow,
00072       NavRoadState::Follow,             NavRoadState::Follow);
00073   Add(NavRoadEvent::Merge,              &Road::ActionToWaitCross,
00074       NavRoadState::Follow,             NavRoadState::WaitCross);
00075   Add(NavRoadEvent::None,               &Road::ActionInFollow,
00076       NavRoadState::Follow,             NavRoadState::Follow);
00077   Add(NavRoadEvent::Perimeter,          &Road::ActionToZone,
00078       NavRoadState::Follow,             NavRoadState::Zone);
00079   Add(NavRoadEvent::StopLine,           &Road::ActionToWaitStop,
00080       NavRoadState::Follow,             NavRoadState::WaitStop);
00081   Add(NavRoadEvent::Uturn,              &Road::ActionToUturn,
00082       NavRoadState::Follow,             NavRoadState::Uturn);
00083   Add(NavRoadEvent::WaitPass,           &Road::ActionToWaitPass,
00084       NavRoadState::Follow,             NavRoadState::WaitPass);
00085 
00086   Add(NavRoadEvent::FollowLane,         &Road::ActionToFollow,
00087       NavRoadState::Init,               NavRoadState::Follow);
00088   Add(NavRoadEvent::Merge,              &Road::ActionToWaitCross,
00089       NavRoadState::Init,               NavRoadState::WaitCross);
00090   Add(NavRoadEvent::None,               &Road::ActionInInit,
00091       NavRoadState::Init,               NavRoadState::Init);
00092   Add(NavRoadEvent::Perimeter,          &Road::ActionToZone,
00093       NavRoadState::Init,               NavRoadState::Zone);
00094   
00095   Add(NavRoadEvent::Block,              &Road::ActionPassToBlock,
00096       NavRoadState::Pass,               NavRoadState::Block);
00097   Add(NavRoadEvent::Collision,          &Road::ActionPassToEvade,
00098       NavRoadState::Pass,               NavRoadState::Evade);
00099   Add(NavRoadEvent::FollowLane,         &Road::ActionPassToFollow,
00100       NavRoadState::Pass,               NavRoadState::Follow);
00101   Add(NavRoadEvent::None,               &Road::ActionInPass,
00102       NavRoadState::Pass,               NavRoadState::Pass);
00103   Add(NavRoadEvent::Pass,               &Road::ActionInPass,
00104       NavRoadState::Pass,               NavRoadState::Pass);
00105 
00106   Add(NavRoadEvent::FollowLane,         &Road::ActionToFollow,
00107       NavRoadState::Uturn,              NavRoadState::Follow);
00108   Add(NavRoadEvent::None,               &Road::ActionInUturn,
00109       NavRoadState::Uturn,              NavRoadState::Uturn);
00110 
00111   Add(NavRoadEvent::FollowLane,         &Road::ActionToFollow,
00112       NavRoadState::WaitCross,          NavRoadState::Follow);
00113   Add(NavRoadEvent::None,               &Road::ActionInWaitCross,
00114       NavRoadState::WaitCross,          NavRoadState::WaitCross);
00115   Add(NavRoadEvent::Merge,              &Road::ActionInWaitCross,
00116       NavRoadState::WaitCross,          NavRoadState::WaitCross);
00117 
00118   Add(NavRoadEvent::FollowLane,         &Road::ActionToFollow,
00119       NavRoadState::WaitLane,           NavRoadState::Follow);
00120   Add(NavRoadEvent::None,               &Road::ActionInWaitLane,
00121       NavRoadState::WaitLane,           NavRoadState::WaitLane);
00122   Add(NavRoadEvent::ChangeLane,         &Road::ActionInWaitLane,
00123       NavRoadState::WaitLane,           NavRoadState::WaitLane);
00124 
00125   Add(NavRoadEvent::Block,              &Road::ActionToBlock,
00126       NavRoadState::WaitPass,           NavRoadState::Block);
00127   Add(NavRoadEvent::Collision,          &Road::ActionToEvade,
00128       NavRoadState::WaitPass,           NavRoadState::Evade);
00129   Add(NavRoadEvent::FollowLane,         &Road::ActionWaitPassToFollow,
00130       NavRoadState::WaitPass,           NavRoadState::Follow);
00131   Add(NavRoadEvent::None,               &Road::ActionInWaitPass,
00132       NavRoadState::WaitPass,           NavRoadState::WaitPass);
00133   Add(NavRoadEvent::Pass,               &Road::ActionToPass,
00134       NavRoadState::WaitPass,           NavRoadState::Pass);
00135   Add(NavRoadEvent::WaitPass,           &Road::ActionInWaitPass,
00136       NavRoadState::WaitPass,           NavRoadState::WaitPass);
00137 
00138   Add(NavRoadEvent::Merge,              &Road::ActionToWaitCross,
00139       NavRoadState::WaitStop,           NavRoadState::WaitCross);
00140   Add(NavRoadEvent::None,               &Road::ActionInWaitStop,
00141       NavRoadState::WaitStop,           NavRoadState::WaitStop);
00142   Add(NavRoadEvent::StopLine,           &Road::ActionInWaitStop,
00143       NavRoadState::WaitStop,           NavRoadState::WaitStop);
00144 
00145   Add(NavRoadEvent::None,               &Road::ActionInZone,
00146       NavRoadState::Zone,               NavRoadState::Zone);
00147   Add(NavRoadEvent::Perimeter,          &Road::ActionZoneToWaitCross,
00148       NavRoadState::Zone,               NavRoadState::WaitCross);
00149 
00150   // allocate subordinate controllers
00151   //evade = new Evade(navptr, _verbose);
00152   follow_lane = new FollowLane(navptr, _verbose);
00153   follow_safely = new FollowSafely(navptr, _verbose);
00154   halt = new Halt(navptr, _verbose);
00155   passing = new Passing(navptr, _verbose);
00156   uturn = new Uturn(navptr, _verbose);
00157   //zone = new RealZone(navptr, _verbose);
00158 
00159   // allocate timers
00160   passing_timer = new NavTimer();
00161   precedence_timer = new NavTimer();
00162   roadblock_timer = new NavTimer();
00163   stop_line_timer = new NavTimer();
00164 
00165   // reset this controller only
00166   reset_me();
00167 }
00168 
00169 Road::~Road()
00170 {
00171   //delete evade;
00172   delete follow_lane;
00173   delete follow_safely;
00174   delete halt;
00175   delete passing;
00176   delete uturn;
00177   //delete zone;
00178 
00179   delete passing_timer;
00180   delete precedence_timer;
00181   delete roadblock_timer;
00182   delete stop_line_timer;
00183 };
00184 
00185 // add a transition to the table
00186 void Road::Add(NavRoadEvent::event_t event, action_t action,
00187                NavRoadState::state_t from_state,
00188                NavRoadState::state_t to_state)
00189 {
00190   transtion_t *xp = &ttable[event][from_state];
00191   xp->action = action;
00192   xp->next = to_state;
00193 }
00194 
00195 void Road::cancel_all_timers(void)
00196 {
00197   passing_timer->Cancel();
00198   precedence_timer->Cancel();
00199   roadblock_timer->Cancel();
00200   stop_line_timer->Cancel();
00201 }
00202 
00203 Controller::result_t Road::control(pilot_command_t &pcmd)
00204 {
00205   // get next event
00206   event = pending_event;
00207   pending_event = NavRoadEvent::None;
00208 
00209   // state transition table pointer
00210   transtion_t *xp = &ttable[event.Value()][state.Value()];
00211 
00212   // do state transition
00213   prev = state;
00214   state = xp->next;
00215   if (state != prev)
00216     {
00217       if (verbose)
00218         ART_MSG(4, "Navigator road state changing from %s to %s, event = %s",
00219                 prev.Name(), state.Name(), event.Name());
00220       navdata->road.state = state.Value(); // update data state message
00221     }
00222   else
00223     {
00224       if (verbose >= 4)
00225         ART_MSG(4, "Navigator road state is %s, event = %s",
00226                 state.Name(), event.Name());
00227     }
00228 
00229   // perform transition action, returning next Pilot command
00230   action_t action = xp->action;
00231   return (this->*action)(pcmd);
00232 }
00233 
00234 // reset this controller and all subordinates
00235 void Road::reset(void)
00236 {
00237   trace_reset("Road");
00238   reset_me();
00239   //evade->reset();
00240   follow_lane->reset();
00241   follow_safely->reset();
00242   halt->reset();
00243   passing->reset();
00244   uturn->reset();
00245   //zone->reset();
00246 }
00247 
00248 // reset this controller only
00249 void Road::reset_me(void)
00250 {
00251   state = NavRoadState();               // initial state
00252   event = NavRoadEvent::None;
00253   pending_event = NavRoadEvent::None;
00254   cancel_all_timers();
00255 }
00256 
00257 void Road::set_waypt_event(void)
00258 {
00259   if (ElementID(navdata->last_waypt) == ElementID(order->waypt[1].id))
00260     {
00261       // new way-point reached, be careful of the order of these tests
00262       if (order->waypt[1].is_perimeter)
00263         pending_event = NavRoadEvent::Perimeter;
00264       else if (order->waypt[1].is_stop)
00265         pending_event = NavRoadEvent::StopLine;
00266       else if (course->uturn_waypt(1))
00267         pending_event = NavRoadEvent::Uturn;
00268       else if (order->waypt[1].is_lane_change)
00269         pending_event = NavRoadEvent::ChangeLane;
00270       else if (order->waypt[1].is_exit
00271                && !course->same_lane(order->waypt[1].id,
00272                                      order->waypt[2].id)) {
00273         if (course->nqe_special(1,2))
00274           ART_MSG(3, "Not going to merge for special case");
00275         else
00276           pending_event = NavRoadEvent::Merge;
00277       }
00278     }
00279 }
00280 
00282 // state transition action methods
00284 
00285 Controller::result_t Road::ActionError(pilot_command_t &pcmd)
00286 {
00287   ART_MSG(4, "Invalid Navigator event %s in road state %s",
00288           event.Name(), prev.Name());
00289   halt->control(pcmd);
00290   return NotApplicable;
00291 }
00292 
00293 Controller::result_t Road::ActionFail(pilot_command_t &pcmd)
00294 {
00295   ART_MSG(4, "Navigator road FSM failure!");
00296   halt->control(pcmd);
00297   return NotImplemented;
00298 }
00299 
00300 // steady state actions
00301 
00302 Controller::result_t Road::ActionInBlock(pilot_command_t &pcmd)
00303 {
00304   result_t result = Blocked;
00305   if (course->new_waypts())
00306     {
00307       // Commander issued an order with new way-points
00308       // TODO: cancel timeout
00309       if (course->uturn_waypt(0))
00310         {
00311           pending_event = NavRoadEvent::Uturn;    
00312           ART_MSG(1, "Doing U-turn after road block");
00313         }
00314       else 
00315         {
00316           pending_event = NavRoadEvent::FollowLane;
00317           ART_MSG(1, "Following lane (NOT U-turn) after road block");
00318         }
00319       navdata->road_blocked = false;
00320       navdata->replan_waypt = ElementID().toMapID();
00321       halt->control(pcmd);              // do the order next cycle
00322     }
00323   else
00324     {
00325       // no new order, keep trying to execute the old one
00326       // TODO: check for timeout and call unstuck controller
00327       result = follow_lane->control(pcmd);
00328       if (result == Collision)
00329         {
00330           // either pass or drive off road immediately
00331           pending_event = NavRoadEvent::Collision;
00332         }
00333       else if (result != Blocked)
00334         {
00335           ART_MSG(1, "No longer blocked, following lane again.");
00336           pending_event = NavRoadEvent::FollowLane;
00337 
00338           // maybe generate another event, if way-point reached
00339           set_waypt_event();
00340 
00341           // no longer blocked
00342           navdata->road_blocked = false;
00343           navdata->replan_waypt = ElementID().toMapID();
00344           // TODO: cancel timeout
00345         }
00346     }
00347   return result;
00348 }  
00349 
00350 Controller::result_t Road::ActionInEvade(pilot_command_t &pcmd)
00351 {
00353   //result_t result = evade->control(pcmd);
00354   result_t result = halt->control(pcmd);
00355   if (result == Finished)
00356     {
00357       pending_event = NavRoadEvent::FollowLane;
00358       result = OK;
00359     }
00360   return result;
00361 }
00362 
00363 Controller::result_t Road::ActionInFollow(pilot_command_t &pcmd)
00364 {
00365   // cancel passing timer when moving in the Follow state
00366   if (!navdata->stopped)
00367     passing_timer->Cancel();
00368 
00369   result_t result = follow_lane->control(pcmd);
00370   if (result == Blocked)
00371     {
00372       pending_event = NavRoadEvent::WaitPass;
00373       result = OK;
00374     }
00375   else if (result == Collision)
00376     {
00377       // either pass or drive off road immediately
00378       pending_event = NavRoadEvent::Collision;
00379     }
00380   else
00381     {
00382       // generate event depending on way-point reached
00383       set_waypt_event();
00384       result = OK;
00385     }
00386   return result;
00387 }
00388 
00389 Controller::result_t Road::ActionInInit(pilot_command_t &pcmd)
00390 {
00391   if (order->waypt[0].is_perimeter || order->waypt[0].is_spot)
00392     pending_event = NavRoadEvent::Perimeter;
00393   else
00394     //pending_event = NavRoadEvent::Merge;
00395     pending_event = NavRoadEvent::FollowLane;
00396   return halt->control(pcmd);
00397 }
00398 
00399 Controller::result_t Road::ActionInPass(pilot_command_t &pcmd)
00400 {
00401   result_t result = passing->control(pcmd);
00402   if (result == Finished)
00403     {
00404       pending_event = NavRoadEvent::FollowLane;
00405       roadblock_timer->Cancel();
00406       result = OK;
00407     }
00408   else if (result == Blocked)
00409     {
00410       // don't report road blocked immediately, wait a while and see
00411       // if it stays blocked before giving up on this part of the road
00412       if (roadblock_timer->Check())
00413         {
00414           pending_event = NavRoadEvent::Block;
00415           roadblock_timer->Cancel();
00416         }
00417       else
00418         {
00419           if (passing_first)
00420             {
00421               passing_first=false;
00422               // TODO: should only reset blockage timer the first time through
00423               obstacle->blocked();
00424               roadblock_timer->Restart(config_->roadblock_delay);
00425               result = Unsafe;
00426             }
00427         }
00428     }
00429   else if (result == Collision)
00430     {
00431       // pass or drive off road immediately
00432       pending_event = NavRoadEvent::Collision;
00433       roadblock_timer->Cancel();
00434     }
00435   return result;
00436 }
00437 
00438 Controller::result_t Road::ActionInUturn(pilot_command_t &pcmd)
00439 {
00440   result_t result = uturn->control(pcmd);
00441   if (result == Finished)
00442     {
00443       pending_event = NavRoadEvent::FollowLane;
00444       result = OK;
00445     }
00446   return result;
00447 }  
00448 
00449 Controller::result_t Road::ActionInWaitCross(pilot_command_t &pcmd)
00450 {
00451   result_t result = halt->control(pcmd); // stay put this cycle
00452   // TODO: add a timeout for when the observer is not applicable?
00453   if (obstacle->observer_clear(crossing_observer))
00454     {
00455       pending_event = NavRoadEvent::FollowLane;
00456     }
00457   return result;
00458 }  
00459 
00460 Controller::result_t Road::ActionInWaitLane(pilot_command_t &pcmd)
00461 {
00462   result_t result = halt->control(pcmd); // stay put this cycle
00463   if (lane_direction == Course::Straight) // missing polygons?
00464     {
00465       ART_MSG(1, "Not waiting for lane to clear -- polygons missing.");
00466       pending_event = NavRoadEvent::FollowLane;
00467     }
00468   else
00469     {
00470       // direction known, query the relevant observer
00471       Observation::_oid_type lane_observer;
00472       if (lane_direction == Course::Left)
00473         {
00474           ART_MSG(1, "Waiting for nearest left lane to clear.");
00475           lane_observer = Observation::Adjacent_left;
00476         }
00477       else
00478         {
00479           ART_MSG(1, "Waiting for nearest right lane to clear.");
00480           lane_observer = Observation::Adjacent_right;
00481         }
00482       // TODO: add a timeout for when the observer is not applicable?
00483       if (obstacle->observer_clear(lane_observer))
00484         {
00485           pending_event = NavRoadEvent::FollowLane;
00486         }
00487     }
00488   return result;
00489 }
00490 
00491 Controller::result_t Road::ActionInWaitPass(pilot_command_t &pcmd)
00492 {
00493   // wait a while to give obstacle a chance to move
00494   if (passing_timer->Check()            // timer expired?
00495       && obstacle->passing_lane_clear()) // clear to go?
00496     {
00497       // passing lane is clear, go now
00498       pending_event = NavRoadEvent::Pass;
00499     }
00500   
00501   // continue checking whether lane is still blocked, sets
00502   // obstacle->last_obst as a side-effect
00503   result_t result = follow_safely->control(pcmd);
00504   if (result == OK) // no longer blocked?
00505     {
00506       pending_event = NavRoadEvent::FollowLane;
00507     }
00508   else if (result == Collision)
00509     {
00510       pending_event = NavRoadEvent::Collision;
00511     }
00512 
00513   // stay put for this cycle
00514   halt->control(pcmd);
00515   return result;
00516 }  
00517 
00518 Controller::result_t Road::ActionInWaitStop(pilot_command_t &pcmd)
00519 {
00520   halt->control(pcmd);                  // stay put this cycle
00521 
00522   // wait until stopped for a while, then query observer
00523   if (stop_line_timer->Check()          // initial timer expired?
00524       && obstacle->observer_clear(Observation::Intersection))
00525     {
00526       ART_MSG(1, "Our turn to cross intersection.");
00527       stop_line_timer->Cancel();
00528       precedence_timer->Cancel();
00529       pending_event = NavRoadEvent::Merge;
00530     }
00531 
00532   // restart precedence timer if number of cars remaining changed
00533   Observation obs = obstacle->observation(Observation::Intersection);
00534   if (obs.applicable && obs.nobjects != prev_nobjects)
00535     {
00536       prev_nobjects = obs.nobjects;
00537       ART_MSG(1, "Cars having precedence at this intersection: %d.",
00538               prev_nobjects);
00539       precedence_timer->Start(config_->precedence_delay);
00540     }
00541 
00542   // catch intersection precedence violation
00543   if (precedence_timer->Check())
00544     {
00545       ART_MSG(1, "Intersection precedence %.1f second timeout, go ahead.",
00546               config_->precedence_delay);
00547       stop_line_timer->Cancel();
00548       precedence_timer->Cancel();
00549       pending_event = NavRoadEvent::Merge;
00550     }
00551   return OK;
00552 }  
00553 
00554 Controller::result_t Road::ActionInZone(pilot_command_t &pcmd)
00555 {
00556   //result_t result = zone->control(pcmd);
00557   result_t result = NotApplicable;      // scaffolding
00558   if (ElementID(navdata->last_waypt) == ElementID(order->waypt[1].id)
00559       && order->waypt[1].is_perimeter)
00560     {
00561       // reached zone perimeter way-point
00562       pending_event = NavRoadEvent::Perimeter;
00563     }
00564   else if (result == NotApplicable)
00565     {
00566       pending_event = NavRoadEvent::Perimeter;
00567       result = OK;
00568     }
00569 #if 1
00570   else
00571     // restart blockage timer (no point running Escape, still in Zone)
00572     obstacle->blocked();
00573 #endif
00574 
00575   return result;
00576 }
00577 
00578 // state entry actions
00579 
00580 Controller::result_t Road::ActionEvadeToFollow(pilot_command_t &pcmd)
00581 {
00582   ART_MSG(1, "Collision evasion done, continue in original lane");
00583   course->reset();                      // JOQ: is this needed?
00584   return ActionToFollow(pcmd);
00585 }
00586 
00587 Controller::result_t Road::ActionPassToBlock(pilot_command_t &pcmd)
00588 {
00589   ART_MSG(1, "passing blocked, replan route from here");
00590   course->reset();
00591   course->plan = course->passed_lane;   // restore original plan
00592   return ActionToBlock(pcmd);
00593 }
00594 
00595 Controller::result_t Road::ActionPassToEvade(pilot_command_t &pcmd)
00596 {
00597   ART_MSG(1, "danger while passing, try to evade");
00598   course->reset();
00599   course->plan = course->passed_lane;   // restore original plan
00600   return ActionToEvade(pcmd);
00601 }
00602 
00603 Controller::result_t Road::ActionPassToFollow(pilot_command_t &pcmd)
00604 {
00605   ART_MSG(1, "passing completed, returning to original lane");
00606   course->reset();
00607   course->signal_pass_return();
00608   return ActionToFollow(pcmd);
00609 }
00610 
00611 Controller::result_t Road::ActionToBlock(pilot_command_t &pcmd)
00612 {
00613   ART_MSG(1, "Road blocked!");
00614   course->turn_signals_off();
00615   navdata->road_blocked = true;
00616   navdata->replan_waypt = course->replan_roadblock().toMapID();
00617   follow_lane->reset();                 // JOQ: is this needed?
00618   return ActionInBlock(pcmd);           // wait for commander to replan
00619 }
00620 
00621 Controller::result_t Road::ActionToEvade(pilot_command_t &pcmd)
00622 {
00623 #if 1  // don't try to pass for now, easier to unit test Evade logic
00624   // collision detected: first try to pass immediately
00625   if (course->find_passing_lane())
00626     {
00627       // found a passing lane
00628       if (obstacle->passing_lane_clear())
00629         {
00630           // passing lane is clear
00631           pending_event = NavRoadEvent::Pass;
00632           return halt->control(pcmd);   // start passing next cycle
00633         }
00634     }
00635   // no passing lane: evade collision by leaving lane to the right
00636   //evade->reset();
00637 #endif
00638   return ActionInEvade(pcmd);
00639 }
00640 
00641 Controller::result_t Road::ActionToFollow(pilot_command_t &pcmd)
00642 {
00643   // leave passing_timer running so we handle oscillations between
00644   // Follow and WaitPass states (flickering obstacles, etc.
00645 #if 0
00646   cancel_all_timers();
00647 #endif
00648   follow_lane->reset();
00649   return ActionInFollow(pcmd);
00650 }  
00651 
00652 Controller::result_t Road::ActionToPass(pilot_command_t &pcmd)
00653 {
00654   // obstacle->last_obst was set in the previous cycle, by the follow
00655   // safely controller.
00656   if (course->switch_to_passing_lane())
00657     {
00658       passing_timer->Cancel();
00659       passing->reset();
00660       return ActionInPass(pcmd);
00661     }
00662   else
00663     {
00664       // failed to switch lanes, road blocked
00665       // TODO: detect recursive blockage in course->find_passing_lane()
00666       pending_event = NavRoadEvent::Block;
00667       return halt->control(pcmd);
00668     }
00669 }
00670 
00671 Controller::result_t Road::ActionToUturn(pilot_command_t &pcmd)
00672 {
00673   ART_MSG(1, "Start U-turn.");
00674   course->turn_signal_on(true);         // signal left
00675   uturn->reset();
00676   return ActionInUturn(pcmd);
00677 }
00678 
00679 Controller::result_t Road::ActionToWaitCross(pilot_command_t &pcmd)
00680 {
00681   Course::direction_t crossing_direction = course->intersection_direction();
00682 #if 0  // always use Merge_across_all, Merge_into_nearest is broken
00683   if (crossing_direction == Course::Right)
00684     {
00685       // assume always headed for nearest right lane
00686       // TODO: how do I verify that?
00687       ART_MSG(1, "Waiting for nearest intersection lane to clear.");
00688       crossing_observer = Observation::Merge_into_nearest;
00689     }
00690   else
00691 #endif
00692     {
00693       // going Straight or Left, wait for all lanes to clear
00694       ART_MSG(1, "Waiting for all intersection lanes to clear.");
00695       crossing_observer = Observation::Merge_across_all;
00696     }
00697   course->signal_for_direction(crossing_direction);
00698   return ActionInWaitCross(pcmd);
00699 }  
00700 
00701 Controller::result_t Road::ActionToWaitLane(pilot_command_t &pcmd)
00702 {
00703   lane_direction = course->lane_change_direction();
00704   course->signal_for_direction(lane_direction);
00705   return ActionInWaitLane(pcmd);
00706 }  
00707 
00708 Controller::result_t Road::ActionToWaitPass(pilot_command_t &pcmd)
00709 {
00710   passing_first=true;
00711 
00712   // select passing lane
00713   if (course->find_passing_lane())
00714     {
00715       // success: prepare to pass
00716       course->signal_pass();
00717       passing_timer->Restart(config_->passing_delay);
00718       return ActionInWaitPass(pcmd);
00719     }
00720   else
00721     {
00722       // failed: road is blocked
00723       pending_event = NavRoadEvent::Block;
00724       halt->control(pcmd);
00725       return Blocked;
00726     }
00727 }  
00728 
00729 Controller::result_t Road::ActionToWaitStop(pilot_command_t &pcmd)
00730 {
00731   ART_MSG(1, "Waiting for intersection precedence.");
00732   course->signal_for_direction(course->intersection_direction());
00733   stop_line_timer->Start(config_->stop_line_delay);
00734   precedence_timer->Start(config_->precedence_delay);
00735   prev_nobjects = -1;
00736   return ActionInWaitStop(pcmd);
00737 }  
00738 
00739 Controller::result_t Road::ActionToZone(pilot_command_t &pcmd)
00740 {
00741   ART_MSG(1, "Entering zone");
00742   //zone->reset();
00743   return ActionInZone(pcmd);
00744 }
00745 
00746 Controller::result_t Road::ActionWaitPassToFollow(pilot_command_t &pcmd)
00747 {
00748   ART_MSG(1, "no need to pass, continue in this lane");
00749   course->reset();
00750   course->turn_signals_off();
00751   return ActionToFollow(pcmd);
00752 }
00753 
00754 Controller::result_t Road::ActionZoneToWaitCross(pilot_command_t &pcmd)
00755 {
00756   ART_MSG(1, "Leaving zone");
00757   return ActionToWaitCross(pcmd);
00758 }
 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