SUMO - Simulation of Urban MObility
AStarRouter.h
Go to the documentation of this file.
1 /****************************************************************************/
9 // A* Algorithm using euclidean distance heuristic.
10 // Based on DijkstraRouterTT. For routing by effort a novel heuristic would be needed.
11 /****************************************************************************/
12 // SUMO, Simulation of Urban MObility; see http://sumo.dlr.de/
13 // Copyright (C) 2012-2017 DLR (http://www.dlr.de/) and contributors
14 /****************************************************************************/
15 //
16 // This file is part of SUMO.
17 // SUMO is free software: you can redistribute it and/or modify
18 // it under the terms of the GNU General Public License as published by
19 // the Free Software Foundation, either version 3 of the License, or
20 // (at your option) any later version.
21 //
22 /****************************************************************************/
23 #ifndef AStarRouter_h
24 #define AStarRouter_h
25 
26 
27 // ===========================================================================
28 // included modules
29 // ===========================================================================
30 #ifdef _MSC_VER
31 #include <windows_config.h>
32 #else
33 #include <config.h>
34 #endif
35 
36 #include <cassert>
37 #include <string>
38 #include <functional>
39 #include <vector>
40 #include <set>
41 #include <limits>
42 #include <algorithm>
43 #include <iterator>
44 #include <map>
46 #include <utils/common/StdDefs.h>
47 #include <utils/common/ToString.h>
49 #include "SUMOAbstractRouter.h"
50 
51 //#define ASTAR_DEBUG_QUERY
52 //#define ASTAR_DEBUG_QUERY_PERF
53 //#define ASTAR_DEBUG_VISITED
54 
55 // ===========================================================================
56 // class definitions
57 // ===========================================================================
73 template<class E, class V, class PF>
74 class AStarRouter : public SUMOAbstractRouter<E, V>, public PF {
75 
76 public:
77  typedef double(* Operation)(const E* const, const V* const, double);
78  typedef std::vector<std::vector<double> > LookupTable;
79 
85  class EdgeInfo {
86  public:
88  EdgeInfo(const E* e) :
89  edge(e),
90  traveltime(std::numeric_limits<double>::max()),
91  heuristicTime(std::numeric_limits<double>::max()),
92  prev(0),
93  visited(false) {
94  }
95 
97  const E* edge;
98 
100  double traveltime;
101 
104 
107 
109  bool visited;
110 
111  inline void reset() {
112  // heuristicTime is set before adding to the frontier, thus no reset is needed
113  traveltime = std::numeric_limits<double>::max();
114  visited = false;
115  }
116 
117  };
118 
124  public:
126  bool operator()(const EdgeInfo* nod1, const EdgeInfo* nod2) const {
127  if (nod1->heuristicTime == nod2->heuristicTime) {
128  return nod1->edge->getNumericalID() > nod2->edge->getNumericalID();
129  }
130  return nod1->heuristicTime > nod2->heuristicTime;
131  }
132  };
133 
135  AStarRouter(const std::vector<E*>& edges, bool unbuildIsWarning, Operation operation, const LookupTable* const lookup = 0):
136  SUMOAbstractRouter<E, V>(operation, "AStarRouter"),
137  myErrorMsgHandler(unbuildIsWarning ? MsgHandler::getWarningInstance() : MsgHandler::getErrorInstance()),
138  myLookupTable(lookup),
140  for (typename std::vector<E*>::const_iterator i = edges.begin(); i != edges.end(); ++i) {
141  myEdgeInfos.push_back(EdgeInfo(*i));
142  myMaxSpeed = MAX2(myMaxSpeed, (*i)->getSpeedLimit() * MAX2(1.0, (*i)->getLengthGeometryFactor()));
143  }
144  }
145 
146  AStarRouter(const std::vector<EdgeInfo>& edgeInfos, bool unbuildIsWarning, Operation operation, const LookupTable* const lookup = 0):
147  SUMOAbstractRouter<E, V>(operation, "AStarRouter"),
148  myErrorMsgHandler(unbuildIsWarning ? MsgHandler::getWarningInstance() : MsgHandler::getErrorInstance()),
149  myLookupTable(lookup),
151  for (typename std::vector<EdgeInfo>::const_iterator i = edgeInfos.begin(); i != edgeInfos.end(); ++i) {
152  myEdgeInfos.push_back(EdgeInfo(i->edge));
153  myMaxSpeed = MAX2(myMaxSpeed, i->edge->getSpeedLimit() * i->edge->getLengthGeometryFactor());
154  }
155  }
156 
158  virtual ~AStarRouter() {}
159 
162  }
163 
164  static LookupTable* createLookupTable(const std::string& filename, const int size) {
165  LookupTable* const result = new LookupTable();
166  BinaryInputDevice dev(filename);
167  for (int i = 0; i < size; i++) {
168  for (int j = 0; j < size; j++) {
169  double val;
170  dev >> val;
171  (*result)[i].push_back(val);
172  }
173  }
174  return result;
175  }
176 
177  void init() {
178  // all EdgeInfos touched in the previous query are either in myFrontierList or myFound: clean those up
179  for (typename std::vector<EdgeInfo*>::iterator i = myFrontierList.begin(); i != myFrontierList.end(); i++) {
180  (*i)->reset();
181  }
182  myFrontierList.clear();
183  for (typename std::vector<EdgeInfo*>::iterator i = myFound.begin(); i != myFound.end(); i++) {
184  (*i)->reset();
185  }
186  myFound.clear();
187  }
188 
189 
191  virtual bool compute(const E* from, const E* to, const V* const vehicle,
192  SUMOTime msTime, std::vector<const E*>& into) {
193  assert(from != 0 && to != 0);
194  // check whether from and to can be used
195  if (PF::operator()(from, vehicle)) {
196  myErrorMsgHandler->inform("Vehicle '" + vehicle->getID() + "' is not allowed on source edge '" + from->getID() + "'.");
197  return false;
198  }
199  if (PF::operator()(to, vehicle)) {
200  myErrorMsgHandler->inform("Vehicle '" + vehicle->getID() + "' is not allowed on destination edge '" + to->getID() + "'.");
201  return false;
202  }
203  this->startQuery();
204 #ifdef ASTAR_DEBUG_QUERY
205  std::cout << "DEBUG: starting search for '" << vehicle->getID() << "' speed: " << MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor()) << " time: " << STEPS2TIME(msTime) << "\n";
206 #endif
207  const SUMOVehicleClass vClass = vehicle == 0 ? SVC_IGNORING : vehicle->getVClass();
208  const double time = STEPS2TIME(msTime);
209  if (this->myBulkMode) {
210  const EdgeInfo& toInfo = myEdgeInfos[to->getNumericalID()];
211  if (toInfo.visited) {
212  buildPathFrom(&toInfo, into);
213  this->endQuery(1);
214  return true;
215  }
216  } else {
217  init();
218  // add begin node
219  EdgeInfo* const fromInfo = &(myEdgeInfos[from->getNumericalID()]);
220  fromInfo->traveltime = 0;
221  fromInfo->prev = 0;
222  myFrontierList.push_back(fromInfo);
223  }
224  // loop
225  int num_visited = 0;
226  const double speed = MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor());
227  while (!myFrontierList.empty()) {
228  num_visited += 1;
229  // use the node with the minimal length
230  EdgeInfo* const minimumInfo = myFrontierList.front();
231  const E* const minEdge = minimumInfo->edge;
232  // check whether the destination node was already reached
233  if (minEdge == to) {
234  buildPathFrom(minimumInfo, into);
235  this->endQuery(num_visited);
236 #ifdef ASTAR_DEBUG_QUERY_PERF
237  std::cout << "visited " + toString(num_visited) + " edges (final path length=" + toString(into.size())
238  + " time=" + toString(recomputeCosts(into, vehicle, msTime))
239  + " edges=" + toString(into) + ")\n";
240 #endif
241 #ifdef ASTAR_DEBUG_VISITED
242  OutputDevice& dev = OutputDevice::getDevice(vehicle->getID() + "_" + time2string(msTime) + "_" + from->getID() + "_" + to->getID());
243  for (typename std::vector<EdgeInfo>::const_iterator i = myEdgeInfos.begin(); i != myEdgeInfos.end(); ++i) {
244  if (i->visited) {
245  dev << "edge:" << i->edge->getID() << "\n";
246  }
247  }
248  dev.close();
249 #endif
250  return true;
251  }
252  pop_heap(myFrontierList.begin(), myFrontierList.end(), myComparator);
253  myFrontierList.pop_back();
254  myFound.push_back(minimumInfo);
255  minimumInfo->visited = true;
256 #ifdef ASTAR_DEBUG_QUERY
257  std::cout << "DEBUG: hit '" << minEdge->getID() << "' TT: " << minimumInfo->traveltime << " E: " << this->getEffort(minEdge, vehicle, time + minimumInfo->traveltime) << " Q: ";
258  for (typename std::vector<EdgeInfo*>::iterator it = myFrontierList.begin(); it != myFrontierList.end(); it++) {
259  std::cout << (*it)->traveltime << "," << (*it)->edge->getID() << " ";
260  }
261  std::cout << "\n";
262 #endif
263  const double traveltime = minimumInfo->traveltime + this->getEffort(minEdge, vehicle, time + minimumInfo->traveltime);
264  // admissible A* heuristic: straight line distance at maximum speed
265  const double heuristic_remaining = myLookupTable == 0 ? minEdge->getDistanceTo(to) / speed : (*myLookupTable)[minEdge->getNumericalID()][to->getNumericalID()] / vehicle->getChosenSpeedFactor();
266  // check all ways from the node with the minimal length
267  const std::vector<E*>& successors = minEdge->getSuccessors(vClass);
268  for (typename std::vector<E*>::const_iterator it = successors.begin(); it != successors.end(); ++it) {
269  const E* const follower = *it;
270  EdgeInfo* const followerInfo = &(myEdgeInfos[follower->getNumericalID()]);
271  // check whether it can be used
272  if (PF::operator()(follower, vehicle)) {
273  continue;
274  }
275  const double oldEffort = followerInfo->traveltime;
276  if (!followerInfo->visited && traveltime < oldEffort) {
277  followerInfo->traveltime = traveltime;
278  followerInfo->heuristicTime = traveltime + heuristic_remaining;
279  /* the code below results in fewer edges being looked up but is more costly due to the effort
280  calculations. Overall it resulted in a slowdown in the Berlin tests but could be made configurable someday.
281  followerInfo->heuristicTime = traveltime;
282  if (follower != to) {
283  if (myLookupTable == 0) {
284  // admissible A* heuristic: straight line distance at maximum speed
285  followerInfo->heuristicTime += this->getEffort(follower, vehicle, time + traveltime) + follower->getDistanceTo(to) / speed;
286  } else {
287  followerInfo->heuristicTime += this->getEffort(follower, vehicle, time + traveltime) + (*myLookupTable)[follower->getNumericalID()][to->getNumericalID()] / vehicle->getChosenSpeedFactor();
288  }
289  }*/
290 #ifdef ASTAR_DEBUG_QUERY
291  //std::cout << " follower=" << followerInfo->edge->getID() << " oldEffort=" << oldEffort << " rem=" << heuristic_remaining << " tt=" << traveltime << " ht=" << followerInfo->heuristicTime << "\n";
292 #endif
293  followerInfo->prev = minimumInfo;
294  if (oldEffort == std::numeric_limits<double>::max()) {
295  myFrontierList.push_back(followerInfo);
296  push_heap(myFrontierList.begin(), myFrontierList.end(), myComparator);
297  } else {
298  push_heap(myFrontierList.begin(),
299  find(myFrontierList.begin(), myFrontierList.end(), followerInfo) + 1,
300  myComparator);
301  }
302  }
303  }
304  }
305  this->endQuery(num_visited);
306 #ifdef ASTAR_DEBUG_QUERY_PERF
307  std::cout << "visited " + toString(num_visited) + " edges (unsuccesful path length: " + toString(into.size()) + ")\n";
308 #endif
309  myErrorMsgHandler->inform("No connection between edge '" + from->getID() + "' and edge '" + to->getID() + "' found.");
310  return false;
311  }
312 
313 
314  double recomputeCosts(const std::vector<const E*>& edges, const V* const v, SUMOTime msTime) const {
315  const double time = STEPS2TIME(msTime);
316  double costs = 0;
317  for (typename std::vector<const E*>::const_iterator i = edges.begin(); i != edges.end(); ++i) {
318  if (PF::operator()(*i, v)) {
319  return -1;
320  }
321  costs += this->getEffort(*i, v, time + costs);
322  }
323  return costs;
324  }
325 
326 public:
328  void buildPathFrom(const EdgeInfo* rbegin, std::vector<const E*>& edges) {
329  std::vector<const E*> tmp;
330  while (rbegin != 0) {
331  tmp.push_back(rbegin->edge);
332  rbegin = rbegin->prev;
333  }
334  std::copy(tmp.rbegin(), tmp.rend(), std::back_inserter(edges));
335  }
336 
337 protected:
339  std::vector<EdgeInfo> myEdgeInfos;
340 
342  std::vector<EdgeInfo*> myFrontierList;
344  std::vector<EdgeInfo*> myFound;
345 
346  EdgeInfoComparator myComparator;
347 
350 
352  const LookupTable* const myLookupTable;
353 
355  double myMaxSpeed;
356 };
357 
358 
359 #endif
360 
361 /****************************************************************************/
362 
static MsgHandler * getWarningInstance()
Returns the instance to add warnings to.
Definition: MsgHandler.cpp:67
double getEffort(const E *const e, const V *const v, double t) const
void close()
Closes the device and removes it from the dictionary.
static LookupTable * createLookupTable(const std::string &filename, const int size)
Definition: AStarRouter.h:164
AStarRouter(const std::vector< EdgeInfo > &edgeInfos, bool unbuildIsWarning, Operation operation, const LookupTable *const lookup=0)
Definition: AStarRouter.h:146
bool visited
The previous edge.
Definition: AStarRouter.h:109
std::vector< EdgeInfo > myEdgeInfos
The container of edge information.
Definition: AStarRouter.h:339
SUMOVehicleClass
Definition of vehicle classes to differ between different lane usage and authority types...
std::vector< EdgeInfo * > myFrontierList
A container for reusage of the min edge heap.
Definition: AStarRouter.h:342
virtual bool compute(const E *from, const E *to, const V *const vehicle, SUMOTime msTime, std::vector< const E *> &into)
Builds the route between the given edges using the minimum travel time.
Definition: AStarRouter.h:191
EdgeInfoComparator myComparator
Definition: AStarRouter.h:346
std::string time2string(SUMOTime t)
Definition: SUMOTime.cpp:60
MsgHandler *const myErrorMsgHandler
the handler for routing errors
Definition: AStarRouter.h:349
Computes the shortest path through a network using the A* algorithm.
Definition: AStarRouter.h:74
AStarRouter(const std::vector< E *> &edges, bool unbuildIsWarning, Operation operation, const LookupTable *const lookup=0)
Constructor.
Definition: AStarRouter.h:135
T MAX2(T a, T b)
Definition: StdDefs.h:70
double myMaxSpeed
maximum speed in the network
Definition: AStarRouter.h:355
double heuristicTime
Estimated time to reach the edge (traveltime + lower bound on remaining time)
Definition: AStarRouter.h:103
void init()
Definition: AStarRouter.h:177
double(* Operation)(const E *const, const V *const, double)
Definition: AStarRouter.h:77
EdgeInfo * prev
The previous edge.
Definition: AStarRouter.h:106
#define max(a, b)
Definition: polyfonts.c:65
virtual SUMOAbstractRouter< E, V > * clone()
Definition: AStarRouter.h:160
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:56
EdgeInfo(const E *e)
Constructor.
Definition: AStarRouter.h:88
virtual ~AStarRouter()
Destructor.
Definition: AStarRouter.h:158
bool myBulkMode
whether we are currently operating several route queries in a bulk
#define STEPS2TIME(x)
Definition: SUMOTime.h:65
T MIN2(T a, T b)
Definition: StdDefs.h:64
void buildPathFrom(const EdgeInfo *rbegin, std::vector< const E *> &edges)
Builds the path from marked edges.
Definition: AStarRouter.h:328
Operation myOperation
The object&#39;s operation to perform.
double traveltime
Effort to reach the edge.
Definition: AStarRouter.h:100
bool operator()(const EdgeInfo *nod1, const EdgeInfo *nod2) const
Comparing method.
Definition: AStarRouter.h:126
double recomputeCosts(const std::vector< const E *> &edges, const V *const v, SUMOTime msTime) const
Definition: AStarRouter.h:314
static OutputDevice & getDevice(const std::string &name)
Returns the described OutputDevice.
void inform(std::string msg, bool addType=true)
adds a new error to the list
Definition: MsgHandler.cpp:85
Static storage of an output device and its base (abstract) implementation.
Definition: OutputDevice.h:71
void endQuery(int visits)
long long int SUMOTime
Definition: TraCIDefs.h:52
#define NUMERICAL_EPS
Definition: config.h:151
const LookupTable *const myLookupTable
the lookup table for travel time heuristics
Definition: AStarRouter.h:352
std::vector< std::vector< double > > LookupTable
Definition: AStarRouter.h:78
std::vector< EdgeInfo * > myFound
list of visited Edges (for resetting)
Definition: AStarRouter.h:344
Encapsulates binary reading operations on a file.
vehicles ignoring classes
const E * edge
The current edge.
Definition: AStarRouter.h:97