Eclipse 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-2019 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", unbuildIsWarning, operation),
102  myLookupTable(lookup),
104  for (const E* const edge : edges) {
105  myEdgeInfos.push_back(typename BASE::EdgeInfo(edge));
106  myMaxSpeed = MAX2(myMaxSpeed, edge->getSpeedLimit() * MAX2(1.0, edge->getLengthGeometryFactor()));
107  }
108  }
109 
110  AStarRouter(const std::vector<typename BASE::EdgeInfo>& edgeInfos, bool unbuildIsWarning, typename BASE::Operation operation, const std::shared_ptr<const LookupTable> lookup = 0) :
111  BASE("AStarRouter", unbuildIsWarning, operation),
112  myLookupTable(lookup),
114  for (const auto& edgeInfo : edgeInfos) {
115  myEdgeInfos.push_back(typename BASE::EdgeInfo(edgeInfo.edge));
116  myMaxSpeed = MAX2(myMaxSpeed, edgeInfo.edge->getSpeedLimit() * edgeInfo.edge->getLengthGeometryFactor());
117  }
118  }
119 
121  virtual ~AStarRouter() {}
122 
124  return new AStarRouter<E, V, BASE>(myEdgeInfos, this->myErrorMsgHandler == MsgHandler::getWarningInstance(), this->myOperation, myLookupTable);
125  }
126 
127  void init() {
128  // all EdgeInfos touched in the previous query are either in myFrontierList or myFound: clean those up
129  for (auto& edgeInfo : myFrontierList) {
130  edgeInfo->reset();
131  }
132  myFrontierList.clear();
133  for (auto& edgeInfo : myFound) {
134  edgeInfo->reset();
135  }
136  myFound.clear();
137  }
138 
139 
141  virtual bool compute(const E* from, const E* to, const V* const vehicle,
142  SUMOTime msTime, std::vector<const E*>& into, bool silent = false) {
143  assert(from != 0 && to != 0);
144  // check whether from and to can be used
145  if (this->isProhibited(from, vehicle)) {
146  if (!silent) {
147  this->myErrorMsgHandler->inform("Vehicle '" + Named::getIDSecure(vehicle) + "' is not allowed on source edge '" + from->getID() + "'.");
148  }
149  return false;
150  }
151  if (this->isProhibited(to, vehicle)) {
152  if (!silent) {
153  this->myErrorMsgHandler->inform("Vehicle '" + Named::getIDSecure(vehicle) + "' is not allowed on destination edge '" + to->getID() + "'.");
154  }
155  return false;
156  }
157  double length = 0.; // dummy for the via edge cost update
158  this->startQuery();
159 #ifdef ASTAR_DEBUG_QUERY
160  std::cout << "DEBUG: starting search for '" << Named::getIDSecure(vehicle) << "' speed: " << MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor()) << " time: " << STEPS2TIME(msTime) << "\n";
161 #endif
162  const SUMOVehicleClass vClass = vehicle == 0 ? SVC_IGNORING : vehicle->getVClass();
163  if (this->myBulkMode) {
164  const auto& toInfo = myEdgeInfos[to->getNumericalID()];
165  if (toInfo.visited) {
166  buildPathFrom(&toInfo, into);
167  this->endQuery(1);
168  return true;
169  }
170  } else {
171  init();
172  // add begin node
173  auto* const fromInfo = &(myEdgeInfos[from->getNumericalID()]);
174  fromInfo->effort = 0.;
175  fromInfo->heuristicEffort = 0.;
176  fromInfo->prev = nullptr;
177  fromInfo->leaveTime = STEPS2TIME(msTime);
178  myFrontierList.push_back(fromInfo);
179  }
180  // loop
181  int num_visited = 0;
182  const bool mayRevisit = myLookupTable != 0 && !myLookupTable->consistent();
183  const double speed = vehicle == nullptr ? myMaxSpeed : MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor());
184  while (!myFrontierList.empty()) {
185  num_visited += 1;
186  // use the node with the minimal length
187  auto* const minimumInfo = myFrontierList.front();
188  const E* const minEdge = minimumInfo->edge;
189  // check whether the destination node was already reached
190  if (minEdge == to) {
191  buildPathFrom(minimumInfo, into);
192  this->endQuery(num_visited);
193 #ifdef ASTAR_DEBUG_QUERY_PERF
194  std::cout << "visited " + toString(num_visited) + " edges (final path length=" + toString(into.size())
195  + " time=" + toString(recomputeCosts(into, vehicle, msTime))
196  + " edges=" + toString(into) + ")\n";
197 #endif
198 #ifdef ASTAR_DEBUG_VISITED
199  OutputDevice& dev = OutputDevice::getDevice(Named::getIDSecure(vehicle) + "_" + time2string(msTime) + "_" + from->getID() + "_" + to->getID());
200  for (const auto& i : myEdgeInfos) {
201  if (i.visited) {
202  dev << "edge:" << i.edge->getID() << "\n";
203  }
204  }
205  dev.close();
206 #endif
207  return true;
208  }
209  std::pop_heap(myFrontierList.begin(), myFrontierList.end(), myComparator);
210  myFrontierList.pop_back();
211  myFound.push_back(minimumInfo);
212  minimumInfo->visited = true;
213 #ifdef ASTAR_DEBUG_QUERY
214  std::cout << "DEBUG: hit=" << minEdge->getID()
215  << " TT=" << minimumInfo->effort
216  << " EF=" << this->getEffort(minEdge, vehicle, minimumInfo->leaveTime)
217  << " HT=" << minimumInfo->heuristicEffort
218  << " Q(TT,HT,Edge)=";
219  for (typename std::vector<EdgeInfo*>::iterator it = myFrontierList.begin(); it != myFrontierList.end(); it++) {
220  std::cout << (*it)->effort << "," << (*it)->heuristicEffort << "," << (*it)->edge->getID() << " ";
221  }
222  std::cout << "\n";
223 #endif
224  const double effortDelta = this->getEffort(minEdge, vehicle, minimumInfo->leaveTime);
225  const double leaveTime = minimumInfo->leaveTime + this->getTravelTime(minEdge, vehicle, minimumInfo->leaveTime, effortDelta);
226 
227  // admissible A* heuristic: straight line distance at maximum speed
228  const double heuristic_remaining = (myLookupTable == nullptr ? minEdge->getDistanceTo(to) / speed :
229  myLookupTable->lowerBound(minEdge, to, speed, vehicle->getChosenSpeedFactor(),
230  minEdge->getMinimumTravelTime(nullptr), to->getMinimumTravelTime(nullptr)));
231  if (heuristic_remaining == UNREACHABLE) {
232  continue;
233  }
234  const double heuristicEffort = minimumInfo->effort + effortDelta + heuristic_remaining;
235  // check all ways from the node with the minimal length
236  for (const std::pair<const E*, const E*>& follower : minEdge->getViaSuccessors(vClass)) {
237  auto* const followerInfo = &(myEdgeInfos[follower.first->getNumericalID()]);
238  // check whether it can be used
239  if (this->isProhibited(follower.first, vehicle)) {
240  continue;
241  }
242  double effort = minimumInfo->effort + effortDelta;
243  double time = leaveTime;
244  this->updateViaEdgeCost(follower.second, vehicle, time, effort, length);
245  const double oldEffort = followerInfo->effort;
246  if ((!followerInfo->visited || mayRevisit) && effort < oldEffort) {
247  followerInfo->effort = effort;
248  // if we use the effort including the via effort below we would count the via twice as shown by the ticket676 test
249  followerInfo->heuristicEffort = MIN2(heuristicEffort, followerInfo->heuristicEffort);
250  followerInfo->leaveTime = time;
251  followerInfo->prev = minimumInfo;
252 #ifdef ASTAR_DEBUG_QUERY_FOLLOWERS
253  std::cout << " follower=" << followerInfo->edge->getID()
254  << " OEF=" << (oldEffort == std::numeric_limits<double>::max() ? "inf" : toString(oldEffort))
255  << " TT=" << effort << " HR=" << heuristic_remaining << " HT=" << followerInfo->heuristicEffort << "\n";
256 #endif
257  if (oldEffort == std::numeric_limits<double>::max()) {
258  myFrontierList.push_back(followerInfo);
259  std::push_heap(myFrontierList.begin(), myFrontierList.end(), myComparator);
260  } else {
261  auto fi = std::find(myFrontierList.begin(), myFrontierList.end(), followerInfo);
262  if (fi == myFrontierList.end()) {
263  assert(mayRevisit);
264  myFrontierList.push_back(followerInfo);
265  std::push_heap(myFrontierList.begin(), myFrontierList.end(), myComparator);
266  } else {
267  std::push_heap(myFrontierList.begin(), fi + 1, myComparator);
268  }
269  }
270  }
271  }
272  }
273  this->endQuery(num_visited);
274 #ifdef ASTAR_DEBUG_QUERY_PERF
275  std::cout << "visited " + toString(num_visited) + " edges (unsuccessful path length: " + toString(into.size()) + ")\n";
276 #endif
277  if (!silent) {
278  this->myErrorMsgHandler->inform("No connection between edge '" + from->getID() + "' and edge '" + to->getID() + "' found.");
279  }
280  return false;
281  }
282 
283 public:
285  void buildPathFrom(const typename BASE::EdgeInfo* rbegin, std::vector<const E*>& edges) {
286  std::vector<const E*> tmp;
287  while (rbegin != 0) {
288  tmp.push_back(rbegin->edge);
289  rbegin = rbegin->prev;
290  }
291  std::copy(tmp.rbegin(), tmp.rend(), std::back_inserter(edges));
292  }
293 
294 protected:
296  std::vector<typename BASE::EdgeInfo> myEdgeInfos;
297 
299  std::vector<typename BASE::EdgeInfo*> myFrontierList;
301  std::vector<typename BASE::EdgeInfo*> myFound;
302 
303  EdgeInfoComparator myComparator;
304 
306  const std::shared_ptr<const LookupTable> myLookupTable;
307 
309  double myMaxSpeed;
310 };
311 
312 
313 #endif
314 
315 /****************************************************************************/
316 
static MsgHandler * getWarningInstance()
Returns the instance to add warnings to.
Definition: MsgHandler.cpp:72
void close()
Closes the device and removes it from the dictionary.
long long int SUMOTime
Definition: SUMOTime.h:35
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:110
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:285
std::vector< typename BASE::EdgeInfo > myEdgeInfos
The container of edge information.
Definition: AStarRouter.h:296
T MAX2(T a, T b)
Definition: StdDefs.h:80
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
Definition: Named.h:70
const std::shared_ptr< const LookupTable > myLookupTable
the lookup table for travel time heuristics
Definition: AStarRouter.h:306
virtual ~AStarRouter()
Destructor.
Definition: AStarRouter.h:121
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:48
std::vector< typename BASE::EdgeInfo * > myFound
list of visited Edges (for resetting)
Definition: AStarRouter.h:301
#define STEPS2TIME(x)
Definition: SUMOTime.h:57
double getTravelTime(const ROEdge *const edge, const ROVehicle *const, double)
T MIN2(T a, T b)
Definition: StdDefs.h:74
double myMaxSpeed
maximum speed in the network
Definition: AStarRouter.h:309
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:123
AbstractLookupTable< E, V > LookupTable
Definition: AStarRouter.h:80
EdgeInfoComparator myComparator
Definition: AStarRouter.h:303
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:299
virtual bool compute(const E *from, const E *to, const V *const vehicle, SUMOTime msTime, std::vector< const E *> &into, bool silent=false)
Builds the route between the given edges using the minimum travel time.
Definition: AStarRouter.h:141
Static storage of an output device and its base (abstract) implementation.
Definition: OutputDevice.h:64
#define NUMERICAL_EPS
Definition: config.h:145
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:127