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-2018 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
17 // A* Algorithm using euclidean distance heuristic.
18 // Based on DijkstraRouter. For routing by effort a novel heuristic would be needed.
19 /****************************************************************************/
20 #ifndef AStarRouter_h
21 #define AStarRouter_h
22 
23 
24 // ===========================================================================
25 // included modules
26 // ===========================================================================
27 #include <config.h>
28 
29 #include <cassert>
30 #include <string>
31 #include <functional>
32 #include <vector>
33 #include <set>
34 #include <limits>
35 #include <algorithm>
36 #include <iterator>
37 #include <map>
38 #include <iostream>
39 #include <memory>
43 #include <utils/common/StdDefs.h>
44 #include <utils/common/ToString.h>
47 #include "AStarLookupTable.h"
48 #include "SUMOAbstractRouter.h"
49 
50 #define UNREACHABLE (std::numeric_limits<double>::max() / 1000.0)
51 
52 //#define ASTAR_DEBUG_QUERY
53 //#define ASTAR_DEBUG_QUERY_FOLLOWERS
54 //#define ASTAR_DEBUG_QUERY_PERF
55 //#define ASTAR_DEBUG_VISITED
56 //#define ASTAR_DEBUG_LOOKUPTABLE
57 //#define ASTAR_DEBUG_LOOKUPTABLE_FROM "disabled"
58 //#define ASTAR_DEBUG_UNREACHABLE
59 
60 // ===========================================================================
61 // class definitions
62 // ===========================================================================
77 template<class E, class V, class BASE>
78 class AStarRouter : public BASE {
79 public:
83 
89  public:
91  bool operator()(const typename BASE::EdgeInfo* nod1, const typename BASE::EdgeInfo* nod2) const {
92  if (nod1->heuristicEffort == nod2->heuristicEffort) {
93  return nod1->edge->getNumericalID() > nod2->edge->getNumericalID();
94  }
95  return nod1->heuristicEffort > nod2->heuristicEffort;
96  }
97  };
98 
100  AStarRouter(const std::vector<E*>& edges, bool unbuildIsWarning, typename BASE::Operation operation, const std::shared_ptr<const LookupTable> lookup = 0) :
101  BASE("AStarRouter", operation),
102  myErrorMsgHandler(unbuildIsWarning ? MsgHandler::getWarningInstance() : MsgHandler::getErrorInstance()),
103  myLookupTable(lookup),
105  for (typename std::vector<E*>::const_iterator i = edges.begin(); i != edges.end(); ++i) {
106  myEdgeInfos.push_back(typename BASE::EdgeInfo(*i));
107  myMaxSpeed = MAX2(myMaxSpeed, (*i)->getSpeedLimit() * MAX2(1.0, (*i)->getLengthGeometryFactor()));
108  }
109  }
110 
111  AStarRouter(const std::vector<typename BASE::EdgeInfo>& edgeInfos, bool unbuildIsWarning, typename BASE::Operation operation, const std::shared_ptr<const LookupTable> lookup = 0) :
112  BASE("AStarRouter", operation),
113  myErrorMsgHandler(unbuildIsWarning ? MsgHandler::getWarningInstance() : MsgHandler::getErrorInstance()),
114  myLookupTable(lookup),
116  for (const auto& edgeInfo : edgeInfos) {
117  myEdgeInfos.push_back(typename BASE::EdgeInfo(edgeInfo.edge));
118  myMaxSpeed = MAX2(myMaxSpeed, edgeInfo.edge->getSpeedLimit() * edgeInfo.edge->getLengthGeometryFactor());
119  }
120  }
121 
123  virtual ~AStarRouter() {}
124 
127  }
128 
129  void init() {
130  // all EdgeInfos touched in the previous query are either in myFrontierList or myFound: clean those up
131  for (auto& edgeInfo : myFrontierList) {
132  edgeInfo->reset();
133  }
134  myFrontierList.clear();
135  for (auto& edgeInfo : myFound) {
136  edgeInfo->reset();
137  }
138  myFound.clear();
139  }
140 
141 
143  virtual bool compute(const E* from, const E* to, const V* const vehicle,
144  SUMOTime msTime, std::vector<const E*>& into) {
145  assert(from != 0 && to != 0);
146  // check whether from and to can be used
147  if (this->isProhibited(from, vehicle)) {
148  myErrorMsgHandler->inform("Vehicle '" + vehicle->getID() + "' is not allowed on source edge '" + from->getID() + "'.");
149  return false;
150  }
151  if (this->isProhibited(to, vehicle)) {
152  myErrorMsgHandler->inform("Vehicle '" + vehicle->getID() + "' is not allowed on destination edge '" + to->getID() + "'.");
153  return false;
154  }
155  this->startQuery();
156 #ifdef ASTAR_DEBUG_QUERY
157  std::cout << "DEBUG: starting search for '" << vehicle->getID() << "' speed: " << MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor()) << " time: " << STEPS2TIME(msTime) << "\n";
158 #endif
159  const SUMOVehicleClass vClass = vehicle == 0 ? SVC_IGNORING : vehicle->getVClass();
160  if (this->myBulkMode) {
161  const auto& toInfo = myEdgeInfos[to->getNumericalID()];
162  if (toInfo.visited) {
163  buildPathFrom(&toInfo, into);
164  this->endQuery(1);
165  return true;
166  }
167  } else {
168  init();
169  // add begin node
170  auto* const fromInfo = &(myEdgeInfos[from->getNumericalID()]);
171  fromInfo->effort = 0.;
172  fromInfo->prev = nullptr;
173  fromInfo->leaveTime = STEPS2TIME(msTime);
174  myFrontierList.push_back(fromInfo);
175  }
176  // loop
177  int num_visited = 0;
178  const bool mayRevisit = myLookupTable != 0 && !myLookupTable->consistent();
179  const double speed = vehicle == nullptr ? myMaxSpeed : MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor());
180  while (!myFrontierList.empty()) {
181  num_visited += 1;
182  // use the node with the minimal length
183  auto* const minimumInfo = myFrontierList.front();
184  const E* const minEdge = minimumInfo->edge;
185  // check whether the destination node was already reached
186  if (minEdge == to) {
187  buildPathFrom(minimumInfo, into);
188  this->endQuery(num_visited);
189 #ifdef ASTAR_DEBUG_QUERY_PERF
190  std::cout << "visited " + toString(num_visited) + " edges (final path length=" + toString(into.size())
191  + " time=" + toString(recomputeCosts(into, vehicle, msTime))
192  + " edges=" + toString(into) + ")\n";
193 #endif
194 #ifdef ASTAR_DEBUG_VISITED
195  OutputDevice& dev = OutputDevice::getDevice(vehicle->getID() + "_" + time2string(msTime) + "_" + from->getID() + "_" + to->getID());
196  for (const auto& i : myEdgeInfos) {
197  if (i.visited) {
198  dev << "edge:" << i.edge->getID() << "\n";
199  }
200  }
201  dev.close();
202 #endif
203  return true;
204  }
205  pop_heap(myFrontierList.begin(), myFrontierList.end(), myComparator);
206  myFrontierList.pop_back();
207  myFound.push_back(minimumInfo);
208  minimumInfo->visited = true;
209  const E* viaEdge = minimumInfo->via;
210 #ifdef ASTAR_DEBUG_QUERY
211  std::cout << "DEBUG: hit=" << minEdge->getID()
212  << " TT=" << minimumInfo->traveltime
213  << " EF=" << this->getEffort(minEdge, vehicle, time + minimumInfo->traveltime)
214  << " HT=" << minimumInfo->heuristicTime
215  << " Q(TT,HT,Edge)=";
216  for (typename std::vector<EdgeInfo*>::iterator it = myFrontierList.begin(); it != myFrontierList.end(); it++) {
217  std::cout << (*it)->traveltime << "," << (*it)->heuristicTime << "," << (*it)->edge->getID() << " ";
218  }
219  std::cout << "\n";
220 #endif
221  double effort = minimumInfo->effort;
222  double leaveTime = minimumInfo->leaveTime;
223  while (viaEdge != nullptr && viaEdge->isInternal()) {
224  const double viaEffortDelta = this->getEffort(viaEdge, vehicle, leaveTime);
225  leaveTime += this->getTravelTime(viaEdge, vehicle, leaveTime, viaEffortDelta);
226  effort += viaEffortDelta;
227  viaEdge = viaEdge->getViaSuccessors().front().first;
228  }
229  const double effortDelta = this->getEffort(minEdge, vehicle, leaveTime);
230  leaveTime += this->getTravelTime(minEdge, vehicle, minimumInfo->leaveTime, effortDelta);
231  effort += effortDelta;
232 
233  // admissible A* heuristic: straight line distance at maximum speed
234  const double heuristic_remaining = (myLookupTable == nullptr ? minEdge->getDistanceTo(to) / speed :
235  myLookupTable->lowerBound(minEdge, to, speed, vehicle->getChosenSpeedFactor(), minEdge->getMinimumTravelTime(0), to->getMinimumTravelTime(0)));
236  if (heuristic_remaining == UNREACHABLE) {
237  continue;
238  }
239  // check all ways from the node with the minimal length
240  for (const std::pair<const E*, const E*>& follower : minEdge->getViaSuccessors(vClass)) {
241  auto* const followerInfo = &(myEdgeInfos[follower.first->getNumericalID()]);
242  // check whether it can be used
243  if (this->isProhibited(follower.first, vehicle)) {
244  continue;
245  }
246  const double oldEffort = followerInfo->effort;
247  if ((!followerInfo->visited || mayRevisit) && effort < oldEffort) {
248  followerInfo->effort = effort;
249  followerInfo->heuristicEffort = effort + heuristic_remaining;
250  followerInfo->leaveTime = leaveTime;
251  followerInfo->prev = minimumInfo;
252  followerInfo->via = follower.second;
253  /* the code below results in fewer edges being looked up but is more costly due to the effort
254  calculations. Overall it resulted in a slowdown in the Berlin tests but could be made configurable someday.
255  followerInfo->heuristicTime = traveltime;
256  if (follower != to) {
257  if (myLookupTable == 0) {
258  // admissible A* heuristic: straight line distance at maximum speed
259  followerInfo->heuristicTime += this->getEffort(follower, vehicle, time + traveltime) + follower->getDistanceTo(to) / speed;
260  } else {
261  followerInfo->heuristicTime += this->getEffort(follower, vehicle, time + traveltime) + (*myLookupTable)[follower->getNumericalID()][to->getNumericalID()] / vehicle->getChosenSpeedFactor();
262  }
263  }*/
264 #ifdef ASTAR_DEBUG_QUERY_FOLLOWERS
265  std::cout << " follower=" << followerInfo->edge->getID() << " OEF=" << oldEffort << " TT=" << traveltime << " HR=" << heuristic_remaining << " HT=" << followerInfo->heuristicTime << "\n";
266 #endif
267  if (oldEffort == std::numeric_limits<double>::max()) {
268  myFrontierList.push_back(followerInfo);
269  push_heap(myFrontierList.begin(), myFrontierList.end(), myComparator);
270  } else {
271  auto fi = find(myFrontierList.begin(), myFrontierList.end(), followerInfo);
272  if (fi == myFrontierList.end()) {
273  assert(mayRevisit);
274  myFrontierList.push_back(followerInfo);
275  push_heap(myFrontierList.begin(), myFrontierList.end(), myComparator);
276  } else {
277  push_heap(myFrontierList.begin(), fi + 1, myComparator);
278  }
279  }
280  }
281  }
282  }
283  this->endQuery(num_visited);
284 #ifdef ASTAR_DEBUG_QUERY_PERF
285  std::cout << "visited " + toString(num_visited) + " edges (unsuccesful path length: " + toString(into.size()) + ")\n";
286 #endif
287  myErrorMsgHandler->inform("No connection between edge '" + from->getID() + "' and edge '" + to->getID() + "' found.");
288  return false;
289  }
290 
291 public:
293  void buildPathFrom(const typename BASE::EdgeInfo* rbegin, std::vector<const E*>& edges) {
294  std::vector<const E*> tmp;
295  while (rbegin != 0) {
296  tmp.push_back(rbegin->edge);
297  rbegin = rbegin->prev;
298  }
299  std::copy(tmp.rbegin(), tmp.rend(), std::back_inserter(edges));
300  }
301 
302 protected:
304  std::vector<typename BASE::EdgeInfo> myEdgeInfos;
305 
307  std::vector<typename BASE::EdgeInfo*> myFrontierList;
309  std::vector<typename BASE::EdgeInfo*> myFound;
310 
311  EdgeInfoComparator myComparator;
312 
315 
317  const std::shared_ptr<const LookupTable> myLookupTable;
318 
320  double myMaxSpeed;
321 };
322 
323 
324 #endif
325 
326 /****************************************************************************/
327 
static MsgHandler * getWarningInstance()
Returns the instance to add warnings to.
Definition: MsgHandler.cpp:67
void close()
Closes the device and removes it from the dictionary.
long long int SUMOTime
Definition: SUMOTime.h:36
SUMOVehicleClass
Definition of vehicle classes to differ between different lane usage and authority types...
AStarRouter(const std::vector< typename BASE::EdgeInfo > &edgeInfos, bool unbuildIsWarning, typename BASE::Operation operation, const std::shared_ptr< const LookupTable > lookup=0)
Definition: AStarRouter.h:111
FullLookupTable< E, V > FLT
Definition: AStarRouter.h:81
#define UNREACHABLE
Definition: AStarRouter.h:50
std::string time2string(SUMOTime t)
Definition: SUMOTime.cpp:65
Computes the shortest path through a network using the A* algorithm.
Definition: AStarRouter.h:78
void buildPathFrom(const typename BASE::EdgeInfo *rbegin, std::vector< const E *> &edges)
Builds the path from marked edges.
Definition: AStarRouter.h:293
std::vector< typename BASE::EdgeInfo > myEdgeInfos
The container of edge information.
Definition: AStarRouter.h:304
T MAX2(T a, T b)
Definition: StdDefs.h:76
const std::shared_ptr< const LookupTable > myLookupTable
the lookup table for travel time heuristics
Definition: AStarRouter.h:317
virtual ~AStarRouter()
Destructor.
Definition: AStarRouter.h:123
MsgHandler *const myErrorMsgHandler
the handler for routing errors
Definition: AStarRouter.h:314
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:49
std::vector< typename BASE::EdgeInfo * > myFound
list of visited Edges (for resetting)
Definition: AStarRouter.h:309
#define STEPS2TIME(x)
Definition: SUMOTime.h:58
double getTravelTime(const ROEdge *const edge, const ROVehicle *const, double)
T MIN2(T a, T b)
Definition: StdDefs.h:70
double myMaxSpeed
maximum speed in the network
Definition: AStarRouter.h:320
bool operator()(const typename BASE::EdgeInfo *nod1, const typename BASE::EdgeInfo *nod2) const
Comparing method.
Definition: AStarRouter.h:91
virtual SUMOAbstractRouter< E, V > * clone()
Definition: AStarRouter.h:125
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:143
AbstractLookupTable< E, V > LookupTable
Definition: AStarRouter.h:80
EdgeInfoComparator myComparator
Definition: AStarRouter.h:311
static OutputDevice & getDevice(const std::string &name)
Returns the described OutputDevice.
AStarRouter(const std::vector< E *> &edges, bool unbuildIsWarning, typename BASE::Operation operation, const std::shared_ptr< const LookupTable > lookup=0)
Constructor.
Definition: AStarRouter.h:100
std::vector< typename BASE::EdgeInfo * > myFrontierList
A container for reusage of the min edge heap.
Definition: AStarRouter.h:307
void inform(std::string msg, bool addType=true)
adds a new error to the list
Definition: MsgHandler.cpp:113
Static storage of an output device and its base (abstract) implementation.
Definition: OutputDevice.h:64
#define NUMERICAL_EPS
Definition: config.h:148
Computes the shortest path through a network using the A* algorithm.
LandmarkLookupTable< E, V > LMLT
Definition: AStarRouter.h:82
vehicles ignoring classes
void init()
Definition: AStarRouter.h:129