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-2020 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials are made available under the
5 // terms of the Eclipse Public License 2.0 which is available at
6 // https://www.eclipse.org/legal/epl-2.0/
7 // This Source Code may also be made available under the following Secondary
8 // Licenses when the conditions for such availability set forth in the Eclipse
9 // Public License 2.0 are satisfied: GNU General Public License, version 2
10 // or later which is available at
11 // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 /****************************************************************************/
20 // A* Algorithm using euclidean distance heuristic.
21 // Based on DijkstraRouter. For routing by effort a novel heuristic would be needed.
22 /****************************************************************************/
23 #pragma once
24 #include <config.h>
25 
26 #include <cassert>
27 #include <string>
28 #include <functional>
29 #include <vector>
30 #include <set>
31 #include <limits>
32 #include <algorithm>
33 #include <iterator>
34 #include <map>
35 #include <iostream>
36 #include <memory>
40 #include <utils/common/StdDefs.h>
41 #include <utils/common/ToString.h>
43 #include "AStarLookupTable.h"
44 #include "SUMOAbstractRouter.h"
45 
46 #define UNREACHABLE (std::numeric_limits<double>::max() / 1000.0)
47 
48 //#define ASTAR_DEBUG_QUERY
49 //#define ASTAR_DEBUG_QUERY_FOLLOWERS
50 //#define ASTAR_DEBUG_QUERY_PERF
51 //#define ASTAR_DEBUG_VISITED
52 //#define ASTAR_DEBUG_COND (vehicle->getID() == "")
53 //#define ASTAR_DEBUG_COND (true)
54 //#define ASTAR_DEBUG_LOOKUPTABLE
55 //#define ASTAR_DEBUG_LOOKUPTABLE_FROM "disabled"
56 //#define ASTAR_DEBUG_UNREACHABLE
57 
58 // ===========================================================================
59 // class definitions
60 // ===========================================================================
75 template<class E, class V>
76 class AStarRouter : public SUMOAbstractRouter<E, V> {
77 public:
81 
87  public:
89  bool operator()(const typename SUMOAbstractRouter<E, V>::EdgeInfo* nod1, const typename SUMOAbstractRouter<E, V>::EdgeInfo* nod2) const {
90  if (nod1->heuristicEffort == nod2->heuristicEffort) {
91  return nod1->edge->getNumericalID() > nod2->edge->getNumericalID();
92  }
93  return nod1->heuristicEffort > nod2->heuristicEffort;
94  }
95  };
96 
98  AStarRouter(const std::vector<E*>& edges, bool unbuildIsWarning, typename SUMOAbstractRouter<E, V>::Operation operation, const std::shared_ptr<const LookupTable> lookup = nullptr,
99  const bool havePermissions = false, const bool haveRestrictions = false) :
100  SUMOAbstractRouter<E, V>("AStarRouter", unbuildIsWarning, operation, nullptr, havePermissions, haveRestrictions),
101  myLookupTable(lookup),
102  myMaxSpeed(NUMERICAL_EPS) {
103  for (const E* const edge : edges) {
104  myEdgeInfos.push_back(typename SUMOAbstractRouter<E, V>::EdgeInfo(edge));
105  myMaxSpeed = MAX2(myMaxSpeed, edge->getSpeedLimit() * MAX2(1.0, edge->getLengthGeometryFactor()));
106  }
107  }
108 
109  AStarRouter(const std::vector<typename SUMOAbstractRouter<E, V>::EdgeInfo>& edgeInfos, bool unbuildIsWarning, typename SUMOAbstractRouter<E, V>::Operation operation, const std::shared_ptr<const LookupTable> lookup = nullptr,
110  const bool havePermissions = false, const bool haveRestrictions = false) :
111  SUMOAbstractRouter<E, V>("AStarRouter", unbuildIsWarning, operation, nullptr, havePermissions, haveRestrictions),
112  myLookupTable(lookup),
113  myMaxSpeed(NUMERICAL_EPS) {
114  for (const auto& edgeInfo : edgeInfos) {
115  myEdgeInfos.push_back(typename SUMOAbstractRouter<E, V>::EdgeInfo(edgeInfo.edge));
116  myMaxSpeed = MAX2(myMaxSpeed, edgeInfo.edge->getSpeedLimit() * edgeInfo.edge->getLengthGeometryFactor());
117  }
118  }
119 
121  virtual ~AStarRouter() {}
122 
126  }
127 
128  void init() {
129  // all EdgeInfos touched in the previous query are either in myFrontierList or myFound: clean those up
130  for (auto& edgeInfo : myFrontierList) {
131  edgeInfo->reset();
132  }
133  myFrontierList.clear();
134  for (auto& edgeInfo : myFound) {
135  edgeInfo->reset();
136  }
137  myFound.clear();
138  }
139 
140 
142  bool compute(const E* from, const E* to, const V* const vehicle,
143  SUMOTime msTime, std::vector<const E*>& into, bool silent = false) {
144  assert(from != nullptr && to != nullptr);
145  // check whether from and to can be used
146  if (myEdgeInfos[from->getNumericalID()].prohibited || this->isProhibited(from, vehicle)) {
147  if (!silent) {
148  this->myErrorMsgHandler->inform("Vehicle '" + Named::getIDSecure(vehicle) + "' is not allowed on source edge '" + from->getID() + "'.");
149  }
150  return false;
151  }
152  if (myEdgeInfos[to->getNumericalID()].prohibited || this->isProhibited(to, vehicle)) {
153  if (!silent) {
154  this->myErrorMsgHandler->inform("Vehicle '" + Named::getIDSecure(vehicle) + "' is not allowed on destination edge '" + to->getID() + "'.");
155  }
156  return false;
157  }
158  double length = 0.; // dummy for the via edge cost update
159  this->startQuery();
160 #ifdef ASTAR_DEBUG_QUERY
161  if (ASTAR_DEBUG_COND) {
162  std::cout << "DEBUG: starting search for '" << Named::getIDSecure(vehicle) << "' speed: " << MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor()) << " time: " << STEPS2TIME(msTime) << "\n";
163  }
164 #endif
165 
166  const SUMOVehicleClass vClass = vehicle == 0 ? SVC_IGNORING : vehicle->getVClass();
167  if (this->myBulkMode) {
168  const auto& toInfo = myEdgeInfos[to->getNumericalID()];
169  if (toInfo.visited) {
170  buildPathFrom(&toInfo, into);
171  this->endQuery(1);
172  return true;
173  }
174  } else {
175  init();
176  // add begin node
177  auto* const fromInfo = &(myEdgeInfos[from->getNumericalID()]);
178  fromInfo->effort = 0.;
179  fromInfo->heuristicEffort = 0.;
180  fromInfo->prev = nullptr;
181  fromInfo->leaveTime = STEPS2TIME(msTime);
182  myFrontierList.push_back(fromInfo);
183  }
184  // loop
185  int num_visited = 0;
186  const bool mayRevisit = myLookupTable != 0 && !myLookupTable->consistent();
187  const double speed = vehicle == nullptr ? myMaxSpeed : MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor());
188  while (!myFrontierList.empty()) {
189  num_visited += 1;
190  // use the node with the minimal length
191  auto* const minimumInfo = myFrontierList.front();
192  const E* const minEdge = minimumInfo->edge;
193  // check whether the destination node was already reached
194  if (minEdge == to) {
195  buildPathFrom(minimumInfo, into);
196  this->endQuery(num_visited);
197 #ifdef ASTAR_DEBUG_QUERY_PERF
198  if (ASTAR_DEBUG_COND) {
199  std::cout << "visited " + toString(num_visited) + " edges (final path length=" + toString(into.size())
200  + " time=" + toString(this->recomputeCosts(into, vehicle, msTime))
201  + " edges=" + toString(into) + ")\n";
202  }
203 #endif
204 #ifdef ASTAR_DEBUG_VISITED
205  if (ASTAR_DEBUG_COND) {
207  for (const auto& i : myEdgeInfos) {
208  if (i.visited) {
209  dev << "edge:" << i.edge->getID() << "\n";
210  }
211  }
212  dev.close();
213  }
214 #endif
215  return true;
216  }
217  std::pop_heap(myFrontierList.begin(), myFrontierList.end(), myComparator);
218  myFrontierList.pop_back();
219  myFound.push_back(minimumInfo);
220  minimumInfo->visited = true;
221 #ifdef ASTAR_DEBUG_QUERY
222  if (ASTAR_DEBUG_COND) {
223  std::cout << "DEBUG: hit=" << minEdge->getID()
224  << " TT=" << minimumInfo->effort
225  << " EF=" << this->getEffort(minEdge, vehicle, minimumInfo->leaveTime)
226  << " HT=" << minimumInfo->heuristicEffort
227  << " Q(TT,HT,Edge)=";
228  for (auto edgeInfo : myFrontierList) {
229  std::cout << edgeInfo->effort << "," << edgeInfo->heuristicEffort << "," << edgeInfo->edge->getID() << " ";
230  }
231  std::cout << "\n";
232  }
233 #endif
234  const double effortDelta = this->getEffort(minEdge, vehicle, minimumInfo->leaveTime);
235  const double leaveTime = minimumInfo->leaveTime + this->getTravelTime(minEdge, vehicle, minimumInfo->leaveTime, effortDelta);
236 
237  // admissible A* heuristic: straight line distance at maximum speed
238  const double heuristic_remaining = (myLookupTable == nullptr ? minEdge->getDistanceTo(to) / speed :
239  myLookupTable->lowerBound(minEdge, to, speed, vehicle->getChosenSpeedFactor(),
240  minEdge->getMinimumTravelTime(nullptr), to->getMinimumTravelTime(nullptr)));
241  if (heuristic_remaining == UNREACHABLE) {
242  continue;
243  }
244  const double heuristicEffort = minimumInfo->effort + effortDelta + heuristic_remaining;
245  // check all ways from the node with the minimal length
246  for (const std::pair<const E*, const E*>& follower : minEdge->getViaSuccessors(vClass)) {
247  auto* const followerInfo = &(myEdgeInfos[follower.first->getNumericalID()]);
248  // check whether it can be used
249  if (followerInfo->prohibited || this->isProhibited(follower.first, vehicle)) {
250  continue;
251  }
252  double effort = minimumInfo->effort + effortDelta;
253  double time = leaveTime;
254  this->updateViaEdgeCost(follower.second, vehicle, time, effort, length);
255  const double oldEffort = followerInfo->effort;
256  if ((!followerInfo->visited || mayRevisit) && effort < oldEffort) {
257  followerInfo->effort = effort;
258  // if we use the effort including the via effort below we would count the via twice as shown by the ticket676 test
259  followerInfo->heuristicEffort = MIN2(heuristicEffort, followerInfo->heuristicEffort);
260  followerInfo->leaveTime = time;
261  followerInfo->prev = minimumInfo;
262 #ifdef ASTAR_DEBUG_QUERY_FOLLOWERS
263  if (ASTAR_DEBUG_COND) {
264  std::cout << " follower=" << followerInfo->edge->getID()
265  << " OEF=" << (oldEffort == std::numeric_limits<double>::max() ? "inf" : toString(oldEffort))
266  << " TT=" << effort << " HR=" << heuristic_remaining << " HT=" << followerInfo->heuristicEffort << "\n";
267  }
268 #endif
269  if (oldEffort == std::numeric_limits<double>::max()) {
270  myFrontierList.push_back(followerInfo);
271  std::push_heap(myFrontierList.begin(), myFrontierList.end(), myComparator);
272  } else {
273  auto fi = std::find(myFrontierList.begin(), myFrontierList.end(), followerInfo);
274  if (fi == myFrontierList.end()) {
275  assert(mayRevisit);
276  myFrontierList.push_back(followerInfo);
277  std::push_heap(myFrontierList.begin(), myFrontierList.end(), myComparator);
278  } else {
279  std::push_heap(myFrontierList.begin(), fi + 1, myComparator);
280  }
281  }
282  }
283  }
284  }
285  this->endQuery(num_visited);
286 #ifdef ASTAR_DEBUG_QUERY_PERF
287  if (ASTAR_DEBUG_COND) {
288  std::cout << "visited " + toString(num_visited) + " edges (unsuccessful path length: " + toString(into.size()) + ")\n";
289  }
290 #endif
291  if (!silent) {
292  this->myErrorMsgHandler->informf("No connection between edge '%' and edge '%' found.", from->getID(), to->getID());
293  }
294  return false;
295  }
296 
297 
298  void prohibit(const std::vector<E*>& toProhibit) {
299  for (E* const edge : this->myProhibited) {
300  myEdgeInfos[edge->getNumericalID()].prohibited = false;
301  }
302  for (E* const edge : toProhibit) {
303  myEdgeInfos[edge->getNumericalID()].prohibited = true;
304  }
305  this->myProhibited = toProhibit;
306  }
307 
308 
310  void buildPathFrom(const typename SUMOAbstractRouter<E, V>::EdgeInfo* rbegin, std::vector<const E*>& edges) {
311  std::vector<const E*> tmp;
312  while (rbegin != 0) {
313  tmp.push_back(rbegin->edge);
314  rbegin = rbegin->prev;
315  }
316  std::copy(tmp.rbegin(), tmp.rend(), std::back_inserter(edges));
317  }
318 
319 protected:
321  std::vector<typename SUMOAbstractRouter<E, V>::EdgeInfo> myEdgeInfos;
322 
324  std::vector<typename SUMOAbstractRouter<E, V>::EdgeInfo*> myFrontierList;
326  std::vector<typename SUMOAbstractRouter<E, V>::EdgeInfo*> myFound;
327 
329 
331  const std::shared_ptr<const LookupTable> myLookupTable;
332 
334  double myMaxSpeed;
335 };
#define UNREACHABLE
Definition: AStarRouter.h:46
#define STEPS2TIME(x)
Definition: SUMOTime.h:53
long long int SUMOTime
Definition: SUMOTime.h:31
SUMOVehicleClass
Definition of vehicle classes to differ between different lane usage and authority types.
@ SVC_IGNORING
vehicles ignoring classes
T MIN2(T a, T b)
Definition: StdDefs.h:73
T MAX2(T a, T b)
Definition: StdDefs.h:79
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:44
bool operator()(const typename SUMOAbstractRouter< E, V >::EdgeInfo *nod1, const typename SUMOAbstractRouter< E, V >::EdgeInfo *nod2) const
Comparing method.
Definition: AStarRouter.h:89
Computes the shortest path through a network using the A* algorithm.
Definition: AStarRouter.h:76
double myMaxSpeed
maximum speed in the network
Definition: AStarRouter.h:334
std::vector< typename SUMOAbstractRouter< E, V >::EdgeInfo * > myFrontierList
A container for reusage of the min edge heap.
Definition: AStarRouter.h:324
void buildPathFrom(const typename SUMOAbstractRouter< E, V >::EdgeInfo *rbegin, std::vector< const E * > &edges)
Builds the path from marked edges.
Definition: AStarRouter.h:310
virtual SUMOAbstractRouter< E, V > * clone()
Definition: AStarRouter.h:123
void prohibit(const std::vector< E * > &toProhibit)
Definition: AStarRouter.h:298
EdgeInfoComparator myComparator
Definition: AStarRouter.h:328
FullLookupTable< E, V > FLT
Definition: AStarRouter.h:79
virtual ~AStarRouter()
Destructor.
Definition: AStarRouter.h:121
std::vector< typename SUMOAbstractRouter< E, V >::EdgeInfo * > myFound
list of visited Edges (for resetting)
Definition: AStarRouter.h:326
void init()
Definition: AStarRouter.h:128
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:142
AStarRouter(const std::vector< E * > &edges, bool unbuildIsWarning, typename SUMOAbstractRouter< E, V >::Operation operation, const std::shared_ptr< const LookupTable > lookup=nullptr, const bool havePermissions=false, const bool haveRestrictions=false)
Constructor.
Definition: AStarRouter.h:98
std::vector< typename SUMOAbstractRouter< E, V >::EdgeInfo > myEdgeInfos
The container of edge information.
Definition: AStarRouter.h:321
AStarRouter(const std::vector< typename SUMOAbstractRouter< E, V >::EdgeInfo > &edgeInfos, bool unbuildIsWarning, typename SUMOAbstractRouter< E, V >::Operation operation, const std::shared_ptr< const LookupTable > lookup=nullptr, const bool havePermissions=false, const bool haveRestrictions=false)
Definition: AStarRouter.h:109
LandmarkLookupTable< E, V > LMLT
Definition: AStarRouter.h:80
AbstractLookupTable< E, V > LookupTable
Definition: AStarRouter.h:78
const std::shared_ptr< const LookupTable > myLookupTable
the lookup table for travel time heuristics
Definition: AStarRouter.h:331
Computes the shortest path through a network using the A* algorithm.
virtual void inform(std::string msg, bool addType=true)
adds a new error to the list
Definition: MsgHandler.cpp:117
static MsgHandler * getWarningInstance()
Returns the instance to add warnings to.
Definition: MsgHandler.cpp:67
void informf(const std::string &format, T value, Targs... Fargs)
adds a new formatted message
Definition: MsgHandler.h:113
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:66
Static storage of an output device and its base (abstract) implementation.
Definition: OutputDevice.h:60
void close()
Closes the device and removes it from the dictionary.
static OutputDevice & getDevice(const std::string &name)
Returns the described OutputDevice.
const E *const edge
The current edge.
const EdgeInfo * prev
The previous edge.
double heuristicEffort
Estimated effort to reach the edge (effort + lower bound on remaining effort)
MsgHandler *const myErrorMsgHandler
the handler for routing errors
std::vector< E * > myProhibited
const bool myHavePermissions
whether edge permissions need to be considered
bool myBulkMode
whether we are currently operating several route queries in a bulk
double recomputeCosts(const std::vector< const E * > &edges, const V *const v, SUMOTime msTime, double *lengthp=nullptr) const
Operation myOperation
The object's operation to perform.
double getTravelTime(const E *const e, const V *const v, const double t, const double effort) const
double getEffort(const E *const e, const V *const v, double t) const
void updateViaEdgeCost(const E *viaEdge, const V *const v, double &time, double &effort, double &length) const
const bool myHaveRestrictions
whether edge restrictions need to be considered
void endQuery(int visits)