SUMO - Simulation of Urban MObility
AStarRouter.h
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2012-2017 German Aerospace Center (DLR) and others.
4 /****************************************************************************/
5 //
6 // This program and the accompanying materials
7 // are made available under the terms of the Eclipse Public License v2.0
8 // which accompanies this distribution, and is available at
9 // http://www.eclipse.org/legal/epl-v20.html
10 //
11 /****************************************************************************/
19 // A* Algorithm using euclidean distance heuristic.
20 // Based on DijkstraRouter. For routing by effort a novel heuristic would be needed.
21 /****************************************************************************/
22 #ifndef AStarRouter_h
23 #define AStarRouter_h
24 
25 
26 // ===========================================================================
27 // included modules
28 // ===========================================================================
29 #ifdef _MSC_VER
30 #include <windows_config.h>
31 #else
32 #include <config.h>
33 #endif
34 
35 #include <cassert>
36 #include <string>
37 #include <functional>
38 #include <vector>
39 #include <set>
40 #include <limits>
41 #include <algorithm>
42 #include <iterator>
43 #include <map>
44 #include <iostream>
48 #include <utils/common/StdDefs.h>
49 #include <utils/common/ToString.h>
52 #include "AStarLookupTable.h"
53 #include "SUMOAbstractRouter.h"
54 
55 #define UNREACHABLE (std::numeric_limits<double>::max() / 1000.0)
56 
57 //#define ASTAR_DEBUG_QUERY
58 //#define ASTAR_DEBUG_QUERY_FOLLOWERS
59 //#define ASTAR_DEBUG_QUERY_PERF
60 //#define ASTAR_DEBUG_VISITED
61 //#define ASTAR_DEBUG_LOOKUPTABLE
62 //#define ASTAR_DEBUG_LOOKUPTABLE_FROM "disabled"
63 //#define ASTAR_DEBUG_UNREACHABLE
64 
65 // ===========================================================================
66 // class definitions
67 // ===========================================================================
83 template<class E, class V, class PF>
84 class AStarRouter : public SUMOAbstractRouter<E, V>, public PF {
85 
86 public:
90  typedef double(* Operation)(const E* const, const V* const, double);
91 
92 
93 
99  class EdgeInfo {
100  public:
102  EdgeInfo(const E* e) :
103  edge(e),
104  traveltime(std::numeric_limits<double>::max()),
105  heuristicTime(std::numeric_limits<double>::max()),
106  prev(0),
107  visited(false) {
108  }
109 
111  const E* edge;
112 
114  double traveltime;
115 
118 
121 
123  bool visited;
124 
125  inline void reset() {
126  // heuristicTime is set before adding to the frontier, thus no reset is needed
127  traveltime = std::numeric_limits<double>::max();
128  visited = false;
129  }
130 
131  };
132 
138  public:
140  bool operator()(const EdgeInfo* nod1, const EdgeInfo* nod2) const {
141  if (nod1->heuristicTime == nod2->heuristicTime) {
142  return nod1->edge->getNumericalID() > nod2->edge->getNumericalID();
143  }
144  return nod1->heuristicTime > nod2->heuristicTime;
145  }
146  };
147 
149  AStarRouter(const std::vector<E*>& edges, bool unbuildIsWarning, Operation operation, const LookupTable* const lookup = 0):
150  SUMOAbstractRouter<E, V>(operation, "AStarRouter"),
151  myErrorMsgHandler(unbuildIsWarning ? MsgHandler::getWarningInstance() : MsgHandler::getErrorInstance()),
152  myLookupTable(lookup),
154  for (typename std::vector<E*>::const_iterator i = edges.begin(); i != edges.end(); ++i) {
155  myEdgeInfos.push_back(EdgeInfo(*i));
156  myMaxSpeed = MAX2(myMaxSpeed, (*i)->getSpeedLimit() * MAX2(1.0, (*i)->getLengthGeometryFactor()));
157  }
158  }
159 
160  AStarRouter(const std::vector<EdgeInfo>& edgeInfos, bool unbuildIsWarning, Operation operation, const LookupTable* const lookup = 0):
161  SUMOAbstractRouter<E, V>(operation, "AStarRouter"),
162  myErrorMsgHandler(unbuildIsWarning ? MsgHandler::getWarningInstance() : MsgHandler::getErrorInstance()),
163  myLookupTable(lookup),
165  for (typename std::vector<EdgeInfo>::const_iterator i = edgeInfos.begin(); i != edgeInfos.end(); ++i) {
166  myEdgeInfos.push_back(EdgeInfo(i->edge));
167  myMaxSpeed = MAX2(myMaxSpeed, i->edge->getSpeedLimit() * i->edge->getLengthGeometryFactor());
168  }
169  }
170 
172  virtual ~AStarRouter() {}
173 
176  }
177 
178  void init() {
179  // all EdgeInfos touched in the previous query are either in myFrontierList or myFound: clean those up
180  for (typename std::vector<EdgeInfo*>::iterator i = myFrontierList.begin(); i != myFrontierList.end(); i++) {
181  (*i)->reset();
182  }
183  myFrontierList.clear();
184  for (typename std::vector<EdgeInfo*>::iterator i = myFound.begin(); i != myFound.end(); i++) {
185  (*i)->reset();
186  }
187  myFound.clear();
188  }
189 
190 
192  virtual bool compute(const E* from, const E* to, const V* const vehicle,
193  SUMOTime msTime, std::vector<const E*>& into) {
194  assert(from != 0 && to != 0);
195  // check whether from and to can be used
196  if (PF::operator()(from, vehicle)) {
197  myErrorMsgHandler->inform("Vehicle '" + vehicle->getID() + "' is not allowed on source edge '" + from->getID() + "'.");
198  return false;
199  }
200  if (PF::operator()(to, vehicle)) {
201  myErrorMsgHandler->inform("Vehicle '" + vehicle->getID() + "' is not allowed on destination edge '" + to->getID() + "'.");
202  return false;
203  }
204  this->startQuery();
205 #ifdef ASTAR_DEBUG_QUERY
206  std::cout << "DEBUG: starting search for '" << vehicle->getID() << "' speed: " << MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor()) << " time: " << STEPS2TIME(msTime) << "\n";
207 #endif
208  const SUMOVehicleClass vClass = vehicle == 0 ? SVC_IGNORING : vehicle->getVClass();
209  const double time = STEPS2TIME(msTime);
210  if (this->myBulkMode) {
211  const EdgeInfo& toInfo = myEdgeInfos[to->getNumericalID()];
212  if (toInfo.visited) {
213  buildPathFrom(&toInfo, into);
214  this->endQuery(1);
215  return true;
216  }
217  } else {
218  init();
219  // add begin node
220  EdgeInfo* const fromInfo = &(myEdgeInfos[from->getNumericalID()]);
221  fromInfo->traveltime = 0;
222  fromInfo->prev = 0;
223  myFrontierList.push_back(fromInfo);
224  }
225  // loop
226  int num_visited = 0;
227  const bool mayRevisit = myLookupTable != 0 && !myLookupTable->consistent();
228  const double speed = MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor());
229  while (!myFrontierList.empty()) {
230  num_visited += 1;
231  // use the node with the minimal length
232  EdgeInfo* const minimumInfo = myFrontierList.front();
233  const E* const minEdge = minimumInfo->edge;
234  // check whether the destination node was already reached
235  if (minEdge == to) {
236  buildPathFrom(minimumInfo, into);
237  this->endQuery(num_visited);
238 #ifdef ASTAR_DEBUG_QUERY_PERF
239  std::cout << "visited " + toString(num_visited) + " edges (final path length=" + toString(into.size())
240  + " time=" + toString(recomputeCosts(into, vehicle, msTime))
241  + " edges=" + toString(into) + ")\n";
242 #endif
243 #ifdef ASTAR_DEBUG_VISITED
244  OutputDevice& dev = OutputDevice::getDevice(vehicle->getID() + "_" + time2string(msTime) + "_" + from->getID() + "_" + to->getID());
245  for (typename std::vector<EdgeInfo>::const_iterator i = myEdgeInfos.begin(); i != myEdgeInfos.end(); ++i) {
246  if (i->visited) {
247  dev << "edge:" << i->edge->getID() << "\n";
248  }
249  }
250  dev.close();
251 #endif
252  return true;
253  }
254  pop_heap(myFrontierList.begin(), myFrontierList.end(), myComparator);
255  myFrontierList.pop_back();
256  myFound.push_back(minimumInfo);
257  minimumInfo->visited = true;
258 #ifdef ASTAR_DEBUG_QUERY
259  std::cout << "DEBUG: hit=" << minEdge->getID()
260  << " TT=" << minimumInfo->traveltime
261  << " EF=" << this->getEffort(minEdge, vehicle, time + minimumInfo->traveltime)
262  << " HT=" << minimumInfo->heuristicTime
263  << " Q(TT,HT,Edge)=";
264  for (typename std::vector<EdgeInfo*>::iterator it = myFrontierList.begin(); it != myFrontierList.end(); it++) {
265  std::cout << (*it)->traveltime << "," << (*it)->heuristicTime << "," << (*it)->edge->getID() << " ";
266  }
267  std::cout << "\n";
268 #endif
269  const double traveltime = minimumInfo->traveltime + this->getEffort(minEdge, vehicle, time + minimumInfo->traveltime);
270  // admissible A* heuristic: straight line distance at maximum speed
271  const double heuristic_remaining = (myLookupTable == 0 ? minEdge->getDistanceTo(to) / speed :
272  myLookupTable->lowerBound(minEdge, to, speed, vehicle->getChosenSpeedFactor(), minEdge->getMinimumTravelTime(0), to->getMinimumTravelTime(0)));
273  if (heuristic_remaining == UNREACHABLE) {
274  continue;
275  }
276  // check all ways from the node with the minimal length
277  const std::vector<E*>& successors = minEdge->getSuccessors(vClass);
278  for (typename std::vector<E*>::const_iterator it = successors.begin(); it != successors.end(); ++it) {
279  const E* const follower = *it;
280  EdgeInfo* const followerInfo = &(myEdgeInfos[follower->getNumericalID()]);
281  // check whether it can be used
282  if (PF::operator()(follower, vehicle)) {
283  continue;
284  }
285  const double oldEffort = followerInfo->traveltime;
286  if ((!followerInfo->visited || mayRevisit) && traveltime < oldEffort) {
287  followerInfo->traveltime = traveltime;
288  followerInfo->heuristicTime = traveltime + heuristic_remaining;
289  /* the code below results in fewer edges being looked up but is more costly due to the effort
290  calculations. Overall it resulted in a slowdown in the Berlin tests but could be made configurable someday.
291  followerInfo->heuristicTime = traveltime;
292  if (follower != to) {
293  if (myLookupTable == 0) {
294  // admissible A* heuristic: straight line distance at maximum speed
295  followerInfo->heuristicTime += this->getEffort(follower, vehicle, time + traveltime) + follower->getDistanceTo(to) / speed;
296  } else {
297  followerInfo->heuristicTime += this->getEffort(follower, vehicle, time + traveltime) + (*myLookupTable)[follower->getNumericalID()][to->getNumericalID()] / vehicle->getChosenSpeedFactor();
298  }
299  }*/
300 #ifdef ASTAR_DEBUG_QUERY_FOLLOWERS
301  std::cout << " follower=" << followerInfo->edge->getID() << " OEF=" << oldEffort << " TT=" << traveltime << " HR=" << heuristic_remaining << " HT=" << followerInfo->heuristicTime << "\n";
302 #endif
303  followerInfo->prev = minimumInfo;
304  if (oldEffort == std::numeric_limits<double>::max()) {
305  myFrontierList.push_back(followerInfo);
306  push_heap(myFrontierList.begin(), myFrontierList.end(), myComparator);
307  } else {
308  typename std::vector<EdgeInfo*>::iterator fi = find(myFrontierList.begin(), myFrontierList.end(), followerInfo);
309  if (fi == myFrontierList.end()) {
310  assert(mayRevisit);
311  myFrontierList.push_back(followerInfo);
312  push_heap(myFrontierList.begin(), myFrontierList.end(), myComparator);
313  } else {
314  push_heap(myFrontierList.begin(), fi + 1, myComparator);
315  }
316  }
317  }
318  }
319  }
320  this->endQuery(num_visited);
321 #ifdef ASTAR_DEBUG_QUERY_PERF
322  std::cout << "visited " + toString(num_visited) + " edges (unsuccesful path length: " + toString(into.size()) + ")\n";
323 #endif
324  myErrorMsgHandler->inform("No connection between edge '" + from->getID() + "' and edge '" + to->getID() + "' found.");
325  return false;
326  }
327 
328 
329  double recomputeCosts(const std::vector<const E*>& edges, const V* const v, SUMOTime msTime) const {
330  const double time = STEPS2TIME(msTime);
331  double costs = 0;
332  for (typename std::vector<const E*>::const_iterator i = edges.begin(); i != edges.end(); ++i) {
333  if (PF::operator()(*i, v)) {
334  return -1;
335  }
336  costs += this->getEffort(*i, v, time + costs);
337  }
338  return costs;
339  }
340 
341 public:
343  void buildPathFrom(const EdgeInfo* rbegin, std::vector<const E*>& edges) {
344  std::vector<const E*> tmp;
345  while (rbegin != 0) {
346  tmp.push_back(rbegin->edge);
347  rbegin = rbegin->prev;
348  }
349  std::copy(tmp.rbegin(), tmp.rend(), std::back_inserter(edges));
350  }
351 
352 protected:
354  std::vector<EdgeInfo> myEdgeInfos;
355 
357  std::vector<EdgeInfo*> myFrontierList;
359  std::vector<EdgeInfo*> myFound;
360 
361  EdgeInfoComparator myComparator;
362 
365 
367  const LookupTable* const myLookupTable;
368 
370  double myMaxSpeed;
371 };
372 
373 
374 #endif
375 
376 /****************************************************************************/
377 
static MsgHandler * getWarningInstance()
Returns the instance to add warnings to.
Definition: MsgHandler.cpp:66
double getEffort(const E *const e, const V *const v, double t) const
void close()
Closes the device and removes it from the dictionary.
AStarRouter(const std::vector< EdgeInfo > &edgeInfos, bool unbuildIsWarning, Operation operation, const LookupTable *const lookup=0)
Definition: AStarRouter.h:160
bool visited
The previous edge.
Definition: AStarRouter.h:123
std::vector< EdgeInfo > myEdgeInfos
The container of edge information.
Definition: AStarRouter.h:354
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:357
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:192
EdgeInfoComparator myComparator
Definition: AStarRouter.h:361
#define UNREACHABLE
Definition: AStarRouter.h:55
std::string time2string(SUMOTime t)
Definition: SUMOTime.cpp:59
MsgHandler *const myErrorMsgHandler
the handler for routing errors
Definition: AStarRouter.h:364
Computes the shortest path through a network using the A* algorithm.
Definition: AStarRouter.h:84
AStarRouter(const std::vector< E *> &edges, bool unbuildIsWarning, Operation operation, const LookupTable *const lookup=0)
Constructor.
Definition: AStarRouter.h:149
T MAX2(T a, T b)
Definition: StdDefs.h:73
double myMaxSpeed
maximum speed in the network
Definition: AStarRouter.h:370
double heuristicTime
Estimated time to reach the edge (traveltime + lower bound on remaining time)
Definition: AStarRouter.h:117
void init()
Definition: AStarRouter.h:178
double(* Operation)(const E *const, const V *const, double)
Definition: AStarRouter.h:90
EdgeInfo * prev
The previous edge.
Definition: AStarRouter.h:120
virtual SUMOAbstractRouter< E, V > * clone()
Definition: AStarRouter.h:174
FullLookupTable< E, V > FLT
Definition: AStarRouter.h:88
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:55
EdgeInfo(const E *e)
Constructor.
Definition: AStarRouter.h:102
virtual ~AStarRouter()
Destructor.
Definition: AStarRouter.h:172
bool myBulkMode
whether we are currently operating several route queries in a bulk
#define STEPS2TIME(x)
Definition: SUMOTime.h:64
T MIN2(T a, T b)
Definition: StdDefs.h:67
void buildPathFrom(const EdgeInfo *rbegin, std::vector< const E *> &edges)
Builds the path from marked edges.
Definition: AStarRouter.h:343
Operation myOperation
The object&#39;s operation to perform.
LandmarkLookupTable< E, V > LMLT
Definition: AStarRouter.h:89
virtual bool consistent() const =0
whether the heuristic ist consistent (found nodes are always visited on the shortest path the first t...
double traveltime
Effort to reach the edge.
Definition: AStarRouter.h:114
bool operator()(const EdgeInfo *nod1, const EdgeInfo *nod2) const
Comparing method.
Definition: AStarRouter.h:140
virtual double lowerBound(const E *from, const E *to, double speed, double speedFactor, double fromEffort, double toEffort) const =0
provide a lower bound on the distance between from and to (excluding traveltime of both edges) ...
double recomputeCosts(const std::vector< const E *> &edges, const V *const v, SUMOTime msTime) const
Definition: AStarRouter.h:329
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:84
Static storage of an output device and its base (abstract) implementation.
Definition: OutputDevice.h:70
void endQuery(int visits)
long long int SUMOTime
Definition: TraCIDefs.h:51
#define NUMERICAL_EPS
Definition: config.h:151
const LookupTable *const myLookupTable
the lookup table for travel time heuristics
Definition: AStarRouter.h:367
Computes the shortest path through a network using the A* algorithm.
std::vector< EdgeInfo * > myFound
list of visited Edges (for resetting)
Definition: AStarRouter.h:359
AbstractLookupTable< E, V > LookupTable
Definition: AStarRouter.h:87
vehicles ignoring classes
const E * edge
The current edge.
Definition: AStarRouter.h:111