GraphSearch.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2007, Mickey Ristroph
00003  *
00004  *  License: Modified BSD Software License Agreement
00005  *
00006  *  $Id: GraphSearch.cc 425 2010-08-19 23:55:40Z jack.oquin $
00007  */
00008 
00009 #include <art_nav/GraphSearch.h>
00010 #include <art_map/euclidean_distance.h>
00011 #include <float.h>
00012 
00013 namespace GraphSearch {
00014   WayPointNodeList edge_list_to_node_list(const Graph& graph,
00015                                           WayPointEdgeList& edges) {
00016     WayPointNodeList nodes;
00017     WayPointEdgeList::iterator i = edges.begin();
00018 
00019     if(i != edges.end()) {
00020       WayPointNode *start = graph.get_node_by_index(i->startnode_index);
00021       if(start != NULL)
00022         nodes.push_back(*start);
00023     }
00024     
00025     while(i != edges.end()) {
00026       WayPointNode *end = graph.get_node_by_index(i->endnode_index);
00027       if(end != NULL)
00028         nodes.push_back(*end);
00029       i++;
00030     }
00031 
00032     return nodes;
00033   }
00034 
00035   
00036   // A possible path includes the estimated total cost, the actual cost so far,
00037   // and a list of edges to follow
00038   
00039   typedef std::pair<std::pair<double,double>,WayPointEdgeList> PossiblePath;
00040 
00041   void print_edge_list(WayPointEdgeList& edges, const Graph& graph) {
00042     std::cout<<graph.get_node_by_index(edges.begin()->startnode_index)->id.name().str;
00043     for(WayPointEdgeList::iterator i = edges.begin(); i != edges.end(); i++) {
00044       std::cout<<" -> "<<graph.get_node_by_index(i->endnode_index)->id.name().str;
00045     }
00046     std::cout<<std::endl;
00047   }
00048 
00049   void print_possible_path(PossiblePath& pp, const Graph& graph) {
00050     std::cout<<"Estimated: "<<pp.first.first<<"So far: "<<pp.first.second<<std::endl;
00051     print_edge_list(pp.second, graph);
00052   }
00053 
00054   class PossiblePathComparision
00055   {
00056   public:
00057     PossiblePathComparision() {}
00058     bool operator() (const PossiblePath& lhs, const PossiblePath& rhs) const {
00059       return (lhs.first.first > rhs.first.first);
00060     }
00061   };
00062   typedef std::priority_queue<PossiblePath,std::vector<PossiblePath>,PossiblePathComparision> PathPriorityQueue;
00063   
00064   double time_between_nodes(const WayPointNode& start,
00065                             const WayPointNode& end,
00066                             float speedlimit) {
00067     float distance = 0;
00068     if(start.is_perimeter || end.is_perimeter ||
00069        start.is_spot || end.is_spot)
00070       distance = Infinite::distance;
00071     else
00072       distance = Euclidean::DistanceTo(start.map,end.map);
00073     
00074     float time = distance/speedlimit;
00075 
00076     // penalize for stopping
00077     if ((start.id.seg != end.id.seg) ||
00078         (start.id.lane != end.id.lane) ||
00079         (end.id.pt != start.id.pt+1))
00080       time+=10.0;
00081 
00082     return time;
00083 
00084   }
00085 
00086   double time_along_edge(const Graph& graph, const WayPointEdge& edge, float speedlimit) {
00087     WayPointNode* start = graph.get_node_by_index(edge.startnode_index);
00088     WayPointNode* end = graph.get_node_by_index(edge.endnode_index);
00089 
00090     // XXX: This means that the Graph is broken
00091     if(start == NULL || end == NULL) {
00092       std::cerr<<"ERROR: Graph edges have node indexes that don't exist!\n";
00093       return Infinite::distance;
00094     }
00095     
00096     if(start == NULL || end == NULL)
00097       return FLT_MAX;
00098     
00099     float distance = 0;
00100     
00101     if (start->is_perimeter || end->is_perimeter ||
00102         start->is_spot || end->is_spot)
00103       distance = Infinite::distance;
00104     else
00105       distance=edge.distance;
00106     
00107     float speed=fmin(edge.speed_max, speedlimit);
00108 
00109     float time;
00110     if (Epsilon::equal(speed,0.0))
00111       time=Infinite::distance;
00112     else
00113       time=distance/speed;
00114 
00115     // penalize for stopping
00116     if ((start->id.seg != end->id.seg) ||
00117         (start->id.lane != end->id.lane) ||
00118         (end->id.pt != start->id.pt+1))
00119       time+=10.0;
00120     
00121     return time;
00122   }
00123 
00124   double heuristic(const Graph& graph,
00125                    const waypt_index_t start_id,
00126                    const waypt_index_t goal_id,
00127                    float speedlimit)
00128   {
00129     //printf("START: %d END: %d\n", start_id, goal_id);
00130     WayPointNode* start = graph.get_node_by_index(start_id);
00131     WayPointNode* end = graph.get_node_by_index(goal_id);
00132     // XXX: This means that the Graph is broken
00133     if(start == NULL || end == NULL) {
00134       std::cerr<<"ERROR: Graph edges have node indexes that don't exist!\n";
00135       return FLT_MAX;
00136     }
00137     return time_between_nodes(*start, *end, speedlimit);
00138   }
00139 
00140   double cost(const Graph& graph,
00141               const WayPointEdge& edge,
00142               float speedlimit) {
00143 
00144     return time_along_edge(graph, edge, speedlimit);
00145   }
00146 
00147 
00148   void add_to_queue(const Graph& graph,
00149                     PathPriorityQueue* q,
00150                     waypt_index_t from_index,
00151                     PossiblePath& old_possible_path,
00152                     const waypt_index_t goal_id,
00153                     float speedlimit,
00154                     int prev_start_index) {
00155     WayPointEdgeList edges;
00156     WayPointNode *from_node = graph.get_node_by_index(from_index);
00157     WayPointNode *prev_node = graph.get_node_by_index(prev_start_index);
00158 
00159     if(from_node == NULL) {
00160       std::cerr<<"ERROR: From index ("<<from_index<<") doesn't exist in graph!!\n";
00161       return;      
00162     }
00163 
00164     edges = graph.edges_from(from_index);
00165     
00166     for(WayPointEdgeList::iterator i = edges.begin(); i != edges.end(); i++) {
00167       WayPointNode *next_node = graph.get_node_by_index(i->endnode_index);
00168 
00169       if(next_node == NULL) {
00170         std::cerr<<"ERROR: Next index ("<<i->endnode_index
00171                  <<") doesn't exist in graph!!\n";
00172         return;      
00173       }
00174 
00175       // Don't go into a zone and right back out just to turn around.
00176       if (prev_node!=NULL && 
00177           prev_node->id.lane != 0 && 
00178           from_node->id.lane == 0 &&
00179           next_node->id.lane != 0)
00180         if (!prev_node->is_spot &&
00181             !next_node->is_spot)
00182         continue;
00183       if (!i->blocked)
00184         {
00185           //printf("Considering follow edge from %d to %d\n",
00186           //     i->startnode_index, i->endnode_index);
00187           WayPointEdgeList new_path(old_possible_path.second);
00188           new_path.push_back(*i);
00189           PossiblePath pp;
00190           
00191           // Actual cost so far
00192           pp.first.second = old_possible_path.first.second + cost(graph, *i, speedlimit);       
00193           // Estimated total cost
00194           pp.first.first = pp.first.second + heuristic(graph,
00195                                                        i->endnode_index,
00196                                                        goal_id, speedlimit);
00197           
00198           
00199           pp.second = new_path;
00200           //std::cout<<"ADDING POSSIBLE PATH: "<<std::endl;
00201           //print_possible_path(pp, graph);
00202           q->push(pp);
00203         }
00204     }
00205   }
00206   
00207   WayPointEdgeList astar_search(const Graph& graph,
00208                                 waypt_index_t start_id,
00209                                 waypt_index_t goal_id,
00210                                 float speedlimit) {
00211 
00212     std::map<waypt_index_t,bool> closed;
00213     closed[start_id] = true;
00214     PathPriorityQueue* q = new PathPriorityQueue(PossiblePathComparision());
00215     PossiblePath empty;
00216     empty.first.first = empty.first.second = 0;
00217     WayPointEdgeList empty_list;
00218 
00219     if (start_id==goal_id) {
00220       delete q;
00221       return empty_list;
00222     }
00223 
00224     // If checkpoint is parking spot, match on segment to get path
00225     // into zone
00226     WayPointNode *goal_node = graph.get_node_by_index(goal_id);
00227 
00228     if(goal_node == NULL) {
00229       std::cerr<<"ERROR: Goal index ("<<goal_id<<") doesn't exist in graph!!\n";
00230       delete q;
00231       return empty_list;      
00232     }
00233 
00234     // Seed the search....
00235     empty.second = empty_list;
00236     add_to_queue(graph, q, start_id, empty, goal_id, speedlimit, -1);
00237     
00238     // Main searching loop
00239     while(!q->empty()) {
00240       PossiblePath path = q->top();
00241       q->pop();
00242       WayPointEdge final_edge = path.second.back();
00243       if(closed[final_edge.endnode_index])
00244         continue;
00245 
00246       WayPointNode *final_node = graph.get_node_by_index(final_edge.endnode_index);
00247       if(final_node == NULL) {
00248         std::cerr<<"ERROR: Final index ("<<final_edge.endnode_index
00249                  <<") doesn't exist in graph!!\n";
00250         delete q;
00251         return empty_list;      
00252       }
00253 
00254       if(final_edge.endnode_index == goal_id) {
00255         delete q;
00256         return path.second;
00257       }
00258       closed[final_edge.endnode_index] = true;
00259       add_to_queue(graph, q, final_edge.endnode_index, path, goal_id, speedlimit, final_edge.startnode_index);
00260     }
00261     delete q;
00262     return empty_list;
00263   }
00264 
00265 };


art_nav
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:08:43