command.cc
Go to the documentation of this file.
00001 /*
00002  *  ART vehicle commander class definition.
00003  *
00004  *  Copyright (C) 2007, 2010, Austin Robot Technology
00005  *
00006  *  License: Modified BSD Software License Agreement
00007  *
00008  * $Id: command.cc 615 2010-09-24 16:07:50Z jack.oquin $
00009  */
00010 
00011 #include <art_nav/GraphSearch.h>
00012 #include <art_nav/NavEstopState.h>
00013 
00014 #include "command.h"
00015 #include "FSM.h"
00016 
00017 Commander::Commander(int verbosity, double limit, 
00018                      Graph* _graph, Mission* _mission,
00019                      const ZonePerimeterList& _zones) 
00020 {
00021   route = new Path();
00022   route->clear();
00023   speedlimit = limit;
00024   zones = _zones;
00025   verbose = verbosity;
00026   fsm = new CmdrFSM(this, verbose);
00027   graph = _graph;
00028   mission = _mission;
00029   blockages = new Blockages(graph, route);
00030   set_checkpoint_goals();
00031   replan_num = 0;
00032 }
00033 
00034 Commander::~Commander()
00035 {
00036   delete blockages;
00037   delete fsm;
00038   delete route;
00039 }
00040 
00041 
00042 
00048 art_msgs::Order Commander::command(const art_msgs::NavigatorState &_navstate)
00049 {
00050   navstate = &_navstate;       // save state pointer in class variable
00051 
00052   order = art_msgs::Order();             // begin with empty order
00053   //order.max_speed = 0.0;
00054   //order.min_speed = 0.0;
00055 
00056   // handle initial startup sequence
00057   if (navstate->estop.state != art_msgs::EstopState::Run)
00058     {
00059       // do nothing if Navigator not running
00060       ROS_DEBUG("Waiting for Navigator to enter Run state.");
00061       order.behavior.value = NavBehavior::None;
00062       return order;
00063     }
00064   else if (ElementID(navstate->last_waypt) == ElementID())
00065     {
00066       // initialize navigator if last_waypt not set yet
00067       ROS_DEBUG("Waiting for Navigator to find initial way-point.");
00068       order.behavior.value = NavBehavior::Initialize;
00069       return order;
00070     }
00071 
00072   // determine most urgent event, and perform state transition for it,
00073   // returning next command
00074   return fsm->control(navstate);
00075 }
00076 
00078 // Private methods
00080 
00081 // get next checkpoint, return true if any remain
00082 //
00083 // updates:
00084 //      goal
00085 //      goal2
00086 //
00087 // Called when the current checkpoint has just been reached.
00088 //
00089 // An MDF could repeat a checkpoint one or more times, but Commander
00090 // does not handle that gracefully, so the parser removes duplicates.
00091 //
00092 bool Commander::next_checkpoint(void)
00093 {
00094 
00095 
00096   // remove the former checkpoint from the head of the queue
00097   bool retval=mission->nextPoint();
00098 
00099   mission->save("mission_state");  
00100   
00101   
00102   if (!retval)                          // none left?
00103     return false;                       // done with mission
00104 
00105   set_checkpoint_goals();
00106 
00107   return true;
00108 }
00109 
00110 // prepare next Navigator order
00111 //
00112 // on entry: route contains one or more waypoints starting at current one
00113 //
00114 art_msgs::Order Commander::prepare_order(art_msgs::Behavior::_value_type behavior)
00115 {
00116   order.behavior.value = behavior;
00117   ROS_DEBUG_STREAM("order.behavior = " << NavBehavior(order.behavior).Name());
00118   ROS_DEBUG_STREAM("goal = "<<goal.id.name().str);
00119 
00120   // include next two checkpoints for monitoring purposes
00121   order.chkpt[0] = goal.toWayPoint();
00122   order.chkpt[1] = goal2.toWayPoint();
00123 
00124   for (unsigned i = 0; i < art_msgs::Order::N_WAYPTS; ++i)
00125     {
00126       WayPointEdge curr_edge;
00127       
00128       WayPointNode* next_node;
00129       
00130       if (i==0) {
00131         curr_edge=route->at(0);
00132         
00133         if (curr_edge.distance<0) {
00134           ROS_FATAL("Route is completely empty..");
00135           order.behavior.value = NavBehavior::Abort;
00136           return order;
00137         }
00138 
00139         order.min_speed=fmin(speedlimit,curr_edge.speed_min);
00140         order.max_speed=fmin(speedlimit,curr_edge.speed_max);      
00141         
00142         next_node=graph->get_node_by_index(curr_edge.startnode_index);  
00143       }
00144       else {
00145         curr_edge=route->at(i-1);
00146         
00147         if (curr_edge.distance<0) {
00148           ROS_FATAL("Route is completely empty..");
00149           order.behavior.value = NavBehavior::Abort;
00150           return order;
00151         }
00152         
00153         order.waypt[i-1].is_lane_change=curr_edge.is_implicit;
00154         
00155         next_node=graph->get_node_by_index(curr_edge.endnode_index);
00156       }
00157       
00158       if (next_node==NULL)
00159         {
00160           ROS_WARN_STREAM("plan waypt (id: " << next_node->id.name().str
00161                           << ") is not in the RNDF graph");
00162           order.behavior.value = NavBehavior::Abort;
00163           return order;
00164         }
00165      
00166       order.waypt[i] = next_node->toWayPoint();
00167 
00168       if (ElementID(order.waypt[i].id) == goal.id)
00169         {
00170           // this is a goalpoint
00171           order.waypt[i].is_goal = true;
00172           if (ElementID(order.waypt[i].id) == goal2.id) // last goalpoint?
00173             order.waypt[i].is_stop = true; // tell navigator to stop there
00174         }
00175       
00176       ROS_DEBUG_STREAM("waypt[" << i << "] = "
00177                        << ElementID(order.waypt[i].id).name().str
00178                        << " (" << order.waypt[i].mapxy.x
00179                        << ", " << order.waypt[i].mapxy.y
00180                        << ") E" << order.waypt[i].is_entry
00181                        << ", G" << order.waypt[i].is_goal
00182                        << ", P" << order.waypt[i].is_spot
00183                        << ", S" << order.waypt[i].is_stop
00184                        << ", X" << order.waypt[i].is_exit
00185                        << ", Z" << order.waypt[i].is_perimeter);
00186     }
00187   
00188   for (uint i=0; i<zones.size(); i++)
00189     if (zones[i].zone_id==order.waypt[0].id.seg &&
00190         zones[i].zone_id==order.waypt[1].id.seg)
00191       order.max_speed=fmin(speedlimit,zones[i].speed_limit);
00192 
00193   ROS_DEBUG_STREAM("Max speed = " << order.max_speed << " m/s");
00194   ROS_DEBUG_STREAM("Min speed = " << order.min_speed << " m/s");
00195 
00196   order.replan_num=replan_num;
00197   order.next_uturn=-1;
00198 
00199   for (unsigned i = 0; i < art_msgs::Order::N_WAYPTS-1; ++i)
00200     if (order.waypt[i].id.seg == order.waypt[i+1].id.seg
00201         && order.waypt[i].id.lane != order.waypt[i+1].id.lane &&
00202         !order.waypt[i].is_lane_change)
00203       {
00204         order.next_uturn=i;
00205         break;
00206       }
00207 
00208   ROS_DEBUG_STREAM("Next uturn = " << order.next_uturn);
00209   ROS_DEBUG_STREAM("Replan num = " << order.replan_num);
00210   
00211   return order;
00212 }
00213 
00214 // replan route, return true if successful
00215 bool Commander::replan_route()
00216 {
00217   replan_num++;
00218 
00219   // get current way-point node
00220   WayPointNode *current;
00221   
00222   current= graph->get_node_by_id(navstate->last_waypt);
00223   if (current == NULL)
00224     {
00225       ROS_WARN_STREAM("last_waypt "
00226                       << ElementID(navstate->last_waypt).name().str
00227                       << " is not in the RNDF graph");
00228       return false;
00229     }
00230   
00231   // We've already checked for this in FSM::current_event(), if
00232   // still true its the end of the run.
00233   if (current->index==goal.index)
00234     return true;
00235 
00236   // call A* from current to goal
00237   WayPointEdgeList edges =
00238     GraphSearch::astar_search(*graph, current->index, goal.index, speedlimit);
00239     
00240   // Edges will be empty if we are planning inside a zone
00241   if (edges.empty()) // no route?
00242     {
00243       if (!blockages->empty())
00244         {
00245           ROS_ERROR("No path found. Removing blockage and trying again");
00246           blockages->pop_oldest();
00247           replan_num--;
00248           return replan_route();
00249         }
00250       else 
00251         { 
00252           ROS_ERROR("No path found to next checkpoint");
00253           ROS_ERROR_STREAM(" Attempted to find a path between "
00254                            << current->id.name().str << " and "
00255                            << goal.id.name().str);
00256           return false;
00257         }
00258     }
00259   
00260   Path new_route;
00261 
00262   new_route.new_path(current->index,goal.index,edges);
00263   
00264   
00265   if (goal2.index != goal.index) {
00266     
00267     edges = GraphSearch::astar_search(*graph, goal.index, goal2.index,
00268                                       speedlimit);
00269     
00270     if (edges.empty())          // no route?
00271       {
00272         if (!blockages->empty())
00273           {
00274             ROS_ERROR("No path found. Removing blockage and trying again");
00275             blockages->pop_oldest();
00276             replan_num--;
00277             return replan_route();
00278           }
00279         else 
00280           { 
00281             ROS_ERROR_STREAM("No path found to next checkpoint");
00282             ROS_ERROR_STREAM("Attempted to find a path between "
00283                              << current->id.name().str << " and "
00284                              << goal.id.name().str);
00285             return false;
00286           }
00287       }
00288     
00289   
00290     new_route.append_path(goal.index, goal2.index, edges);
00291   }
00292 
00293   *route=new_route;
00294 
00295 
00296   // optionally print the entire route path
00297   if (verbose)
00298     route->print(*graph);
00299   
00300   return true;
00301 }
00302 
00303 // set immediate checkpoint goals from mission
00304 //
00305 // exit:
00306 //      goal = WayPointNode of next checkpoint
00307 //      goal2 = following checkpoint if any, immediate goal otherwise
00308 //
00309 void Commander::set_checkpoint_goals(void)
00310 {
00311   ElementID goal_id = mission->current_checkpoint_elementid();
00312   WayPointNode *goal_ptr = graph->get_node_by_id(goal_id);
00313   if (goal_ptr)
00314     {
00315       goal = *goal_ptr;
00316       if (mission->checkpoint_elementid.size() > 1)
00317         goal2 = *graph->get_node_by_id(mission->next_checkpoint_elementid());
00318       else
00319         goal2 = goal;
00320       
00321       ROS_DEBUG_STREAM("goal checkpoints: "
00322                        << goal.id.name().str << ", "
00323                        << goal2.id.name().str);
00324     }
00325   else
00326     {
00327       ROS_WARN_STREAM("bad goal checkpoint: "
00328                       << goal_id.name().str
00329                       << ", no such way-point node.");
00330     }
00331 }
 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