art_nav: art_nav: run.cc Source File

00001 /*
00002  *  Navigator run controller
00003  *
00004  *  Copyright (C) 2007, 2010, Austin Robot Technology
00005  *  License: Modified BSD Software License Agreement
00006  *
00007  *  $Id: run.cc 1856 2011-11-18 00:39:09Z jack.oquin $
00008  */
00009 
00010 #include "navigator_internal.h"
00011 #include "Controller.h"
00012 #include "course.h"
00013 #include "obstacle.h"
00014 #include "ntimer.h"
00015 #include "run.h"
00016 
00017 #include "halt.h"
00018 #include "road.h"
00019 #if NOT_PORTED_TO_ROS
00020 #include "safety.h"
00021 #include "voronoi_zone.h"
00022 #endif
00023 
00024 #include <art_nav/estimate.h>
00025 #include <art_map/ZoneOps.h>
00026 
00027 /* @brief Navigator run controller */
00028 
00029 static const char *state_name[] =
00030   {
00031     "Continue",
00032     "Escape",
00033     "Replan",
00034   };
00035 
00036 Run::Run(Navigator *navptr, int _verbose):
00037   Controller(navptr, _verbose)
00038 {
00039   halt =        new Halt(navptr, _verbose);
00040   road =        new Road(navptr, _verbose);
00041 #if NOT_PORTED_TO_ROS
00042   safety =      new Safety(navptr, _verbose);
00043   unstuck =     new VoronoiZone(navptr, _verbose);
00044 
00045   escape_timer = new NavTimer(cycle);
00046 #endif
00047   go_state = Continue;
00048 };
00049 
00050 Run::~Run()
00051 {
00052   delete halt;
00053   delete road;
00054 #if NOT_PORTED_TO_ROS
00055   delete safety;
00056   delete unstuck;
00057 
00058   delete escape_timer;
00059 #endif
00060 };
00061 
00062 // go to escape state
00063 void Run::begin_escape(void)
00064 {
00065 #if NOT_PORTED_TO_ROS
00066   unstuck->reset();
00067   blockage_pose = estimate->pos;
00068   blockage_waypt_dist =
00069     Euclidean::DistanceToWaypt(estimate->pos, order->waypt[1]);
00070   escape_timer->Start(escape_timeout_secs);
00071   set_go_state(Escape);
00072 #endif
00073 }
00074 
00087 Controller::result_t Run::control(pilot_command_t &pcmd)
00088 {
00089   // Do nothing if the order is still Run.  Wait until Commander sends
00090   // a new behavior.  Also, wait until the course controller receives
00091   // data from the maplanes driver.
00092   if (NavBehavior(order->behavior) == NavBehavior::Run
00093       || course->polygons.empty())
00094     {
00095       ROS_DEBUG_STREAM("run controller not initialized, have "
00096                        << course->polygons.size() << " polygons");
00097       // Run order in Run state is valid, but does nothing.
00098       // Do nothing until lanes data are initialized.
00099       return halt->control(pcmd);
00100     }
00101   
00102   // initialize pilot command to speed limit, straight ahead
00103   pcmd.yawRate = 0.0;
00104   pcmd.velocity = fminf(order->max_speed, config_->max_speed);
00105   
00106   // estimate current dead reckoning position based on time of current
00107   // cycle and latest odometry course and speed
00108   Estimate::control_pose(*odom, ros::Time::now(), *estimate);
00109 
00110   course->begin_run_cycle();
00111 
00112   trace("begin run controller", pcmd);
00113 
00114   result_t result;
00115 
00116   // select subordinate controller based on order behavior
00117   switch (order->behavior.value)
00118     {
00119     case NavBehavior::Go:
00120       result = go(pcmd);
00121       break;
00122 
00123     case NavBehavior::Initialize: 
00124       result = initialize(pcmd);
00125       break;
00126       
00127     default:
00128       ROS_ERROR("unsupported Navigator behavior: %s (halting)",
00129                 NavBehavior(order->behavior).Name());
00130       halt->control(pcmd);
00131       result = NotImplemented;
00132     }
00133 
00134 #if NOT_PORTED_TO_ROS
00135   if (extra_safety_check)
00136     {
00137       // make safety check for obstacle in our immediate path
00138       result_t sres = safety->control(pcmd);
00139       if (sres != OK)
00140         result = sres;
00141     }
00142 #endif
00143 
00144   if (navdata->reverse)
00145     {
00146       // modify pilot command to use reverse motion
00147       pcmd.velocity = -pcmd.velocity;
00148     }
00149 
00150   course->end_run_cycle();
00151 
00152   trace("run controller", pcmd, result);
00153 
00154   return result;
00155 };
00156 
00166 Controller::result_t Run::initialize(pilot_command_t &pcmd)
00167 {
00168   result_t result;
00169 
00170   ElementID start_way = starting_waypt();
00171   if (start_way != ElementID())
00172     {
00173       course->new_waypoint_reached(start_way);
00174       ROS_INFO("starting way-point: %s", start_way.name().str);
00175       result = OK;
00176     }
00177   else
00178     {
00179       // no starting way-point found
00180       ROS_WARN("no starting way-point found, run failed.");
00181       result = NotApplicable;
00182       course->no_waypoint_reached();
00183     }
00184   
00185   halt->control(pcmd);
00186   return result;
00187 }
00188 
00189 
00219 Controller::result_t Run::go(pilot_command_t &pcmd)
00220 {
00221   if (ElementID(order->waypt[0].id) == ElementID(order->waypt[1].id))
00222     {
00223       // halt and let Commander catch up (does not count as timeout)
00224       ROS_INFO("already reached all way-points in order, halting");
00225       return halt->control(pcmd);
00226     }
00227 
00228 #if 0 // not doing blockage timer yet
00229   // check whether car moving, set blockage timer appropriately
00230   obstacle->update_blockage_state();
00231 
00232   // increment and check blockage timer -- do once per cycle
00233   if (obstacle->blockage_timeout())
00234     {
00235       obstacle->unblocked();            // cancel timer
00236       if (escape)
00237         {
00238           ART_MSG(1, "Run controller blocked, try to escape.");
00239           begin_escape();
00240         }
00241       else
00242         ART_MSG(1, "Run controller blocked, but not trying to escape.");
00243     }
00244 #endif // no blockage timeout
00245 
00246   switch (go_state)
00247     {
00248 #if 0 // not doing Escape yet
00249     case Escape:
00250       {
00251         float blockage_distance =
00252           Euclidean::DistanceTo(blockage_pose, estimate->pos);
00253 
00254         if (!escape_timer->Check()
00255             && blockage_distance < escape_distance)
00256           {
00257             // vehicle blocked, try to escape (reach waypt[1] if close)
00258             result_t result = unstuck->control(pcmd);
00259             if (result != Finished)
00260                 return result;
00261 
00262             ART_MSG(1, "Unstuck controller returns Finished");
00263           }
00264 
00265         ART_MSG(1, "Car moved %.3fm since blocking, try to replan.",
00266                 blockage_distance);
00267         escape_timer->Cancel();
00268         last_replan = order->replan_num;
00269         set_go_state(Replan);
00270         // fall through...
00271       }
00272 #endif // no escape yet
00273 
00274     case Replan:
00275       if (order->replan_num == last_replan)
00276         {
00277           ElementID start_id = starting_waypt();
00278           navdata->replan_waypt = start_id.toMapID();
00279           if (start_id == ElementID())
00280             {
00281               ROS_WARN("Unable to replan from here, keep trying to escape.");
00282               begin_escape();
00283             }
00284           return halt->control(pcmd);
00285         }
00286 
00287       // Commander issued new replanned order
00288       navdata->replan_waypt = ElementID().toMapID();
00289       road->reset();
00290       set_go_state(Continue);
00291       // fall through...
00292 
00293     case Continue:
00294       // normal processing -- run the road state machine
00295       return road->control(pcmd);
00296 
00297     default:
00298       // not reached, only to avoid compiler warning
00299       return NotImplemented;
00300     }
00301 }
00302 
00303 // reset all subordinate controllers
00304 void Run::reset(void)
00305 {
00306   go_state = Continue;
00307 
00308   halt->reset();
00309   road->reset();
00310 
00311 #if NOT_PORTED_TO_ROS
00312   safety->reset();
00313 #endif
00314 }
00315 
00317 void Run::set_go_state(state_t newstate)
00318 {
00319   if (go_state != newstate)
00320     {
00321       ROS_DEBUG("Go behavior state changing from %s to %s",
00322                 state_name[go_state], state_name[newstate]);
00323       go_state = newstate;
00324     }
00325 }
00326 
00333 ElementID Run::starting_waypt(void)
00334 {
00335   ElementID waypt;
00336 
00337 #if NOT_PORTED_TO_ROS
00338   // first look for a containing zone
00339   segment_id_t starting_zone =
00340     ZoneOps::containing_zone(course->zones, MapXY(estimate->pos));
00341 
00342   if (verbose >= 4)
00343     ART_MSG(2, "Run::starting_waypt() containing zone is %d", starting_zone);
00344 
00345   if(starting_zone > 0) {
00346     ZonePerimeter zone = ZoneOps::get_zone_by_id(course->zones, starting_zone);
00347     
00348     waypt = (ZoneOps::starting_node_for_zone(zone)).id;
00349     
00350     if (waypt != ElementID())
00351       {
00352         ART_MSG(2, "Starting at %s in zone %d",
00353                 waypt.name().str, zone.zone_id);
00354         return waypt;
00355       }
00356   }
00357 #endif
00358 
00359   // not in a zone -- find an appropriate lane polygon
00360   int index = pops->getStartingPoly(MapPose(estimate->pose.pose),
00361                                     course->polygons,
00362                                     config_->initialize_distance,
00363                                     config_->initialize_min_angle);
00364 
00365   if (index < 0)
00366     {
00367       // no starting way-point found
00368       ROS_FATAL_STREAM("getStartingPoly() failed, returning " << index);
00369     }
00370   else
00371     {
00372       waypt = course->polygons[index].start_way;
00373       ROS_DEBUG_STREAM("starting_waypt() is " << waypt.name().str
00374                        << ", polygon " << course->polygons[index].poly_id);
00375     }
00376 
00377   return waypt;
00378 }