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-2022 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  this->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  this->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 
129  bool compute(const E* from, const E* to, const V* const vehicle,
130  SUMOTime msTime, std::vector<const E*>& into, bool silent = false) {
131  assert(from != nullptr && to != nullptr);
132  // check whether from and to can be used
133  if (this->myEdgeInfos[from->getNumericalID()].prohibited || this->isProhibited(from, vehicle)) {
134  if (!silent) {
135  this->myErrorMsgHandler->inform("Vehicle '" + Named::getIDSecure(vehicle) + "' is not allowed on source edge '" + from->getID() + "'.");
136  }
137  return false;
138  }
139  if (this->myEdgeInfos[to->getNumericalID()].prohibited || this->isProhibited(to, vehicle)) {
140  if (!silent) {
141  this->myErrorMsgHandler->inform("Vehicle '" + Named::getIDSecure(vehicle) + "' is not allowed on destination edge '" + to->getID() + "'.");
142  }
143  return false;
144  }
145  double length = 0.; // dummy for the via edge cost update
146  this->startQuery();
147 #ifdef ASTAR_DEBUG_QUERY
148  if (ASTAR_DEBUG_COND) {
149  std::cout << "DEBUG: starting search for '" << Named::getIDSecure(vehicle) << "' speed: " << MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor()) << " time: " << STEPS2TIME(msTime) << "\n";
150  }
151 #endif
152 
153  const SUMOVehicleClass vClass = vehicle == 0 ? SVC_IGNORING : vehicle->getVClass();
154  if (this->myBulkMode && !this->myAmClean) {
155  const auto& toInfo = this->myEdgeInfos[to->getNumericalID()];
156  if (toInfo.visited) {
157  this->buildPathFrom(&toInfo, into);
158  this->endQuery(1);
159  return true;
160  }
161  } else {
162  this->init(from->getNumericalID(), msTime);
163  this->myAmClean = false;
164  }
165  // loop
166  int num_visited = 0;
167  const bool mayRevisit = myLookupTable != 0 && !myLookupTable->consistent();
168  const double speed = vehicle == nullptr ? myMaxSpeed : MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor());
169  while (!this->myFrontierList.empty()) {
170  num_visited += 1;
171  // use the node with the minimal length
172  auto* const minimumInfo = this->myFrontierList.front();
173  const E* const minEdge = minimumInfo->edge;
174  // check whether the destination node was already reached
175  if (minEdge == to) {
176  this->buildPathFrom(minimumInfo, into);
177  this->endQuery(num_visited);
178 #ifdef ASTAR_DEBUG_QUERY_PERF
179  if (ASTAR_DEBUG_COND) {
180  std::cout << "visited " + toString(num_visited) + " edges (final path length=" + toString(into.size())
181  + " time=" + toString(this->recomputeCosts(into, vehicle, msTime))
182  + " edges=" + toString(into) + ")\n";
183  }
184 #endif
185 #ifdef ASTAR_DEBUG_VISITED
186  if (ASTAR_DEBUG_COND) {
188  for (const auto& i : myEdgeInfos) {
189  if (i.visited) {
190  dev << "edge:" << i.edge->getID() << "\n";
191  }
192  }
193  dev.close();
194  }
195 #endif
196  return true;
197  }
198  std::pop_heap(this->myFrontierList.begin(), this->myFrontierList.end(), myComparator);
199  this->myFrontierList.pop_back();
200  this->myFound.push_back(minimumInfo);
201  minimumInfo->visited = true;
202 #ifdef ASTAR_DEBUG_QUERY
203  if (ASTAR_DEBUG_COND) {
204  std::cout << "DEBUG: hit=" << minEdge->getID()
205  << " TT=" << minimumInfo->effort
206  << " EF=" << this->getEffort(minEdge, vehicle, minimumInfo->leaveTime)
207  << " HT=" << minimumInfo->heuristicEffort
208  << " Q(TT,HT,Edge)=";
209  for (const auto& edgeInfo : myFrontierList) {
210  std::cout << edgeInfo->effort << "," << edgeInfo->heuristicEffort << "," << edgeInfo->edge->getID() << " ";
211  }
212  std::cout << "\n";
213  }
214 #endif
215  const double effortDelta = this->getEffort(minEdge, vehicle, minimumInfo->leaveTime);
216  const double leaveTime = minimumInfo->leaveTime + this->getTravelTime(minEdge, vehicle, minimumInfo->leaveTime, effortDelta);
217 
218  // admissible A* heuristic: straight line distance at maximum speed
219  const double heuristic_remaining = (myLookupTable == nullptr ? minEdge->getDistanceTo(to) / speed :
220  myLookupTable->lowerBound(minEdge, to, speed, vehicle->getChosenSpeedFactor(),
221  minEdge->getMinimumTravelTime(nullptr), to->getMinimumTravelTime(nullptr)));
222  if (heuristic_remaining == UNREACHABLE) {
223  continue;
224  }
225  const double heuristicEffort = minimumInfo->effort + effortDelta + heuristic_remaining;
226  // check all ways from the node with the minimal length
227  for (const std::pair<const E*, const E*>& follower : minEdge->getViaSuccessors(vClass)) {
228  auto& followerInfo = this->myEdgeInfos[follower.first->getNumericalID()];
229  // check whether it can be used
230  if (followerInfo.prohibited || this->isProhibited(follower.first, vehicle)) {
231  continue;
232  }
233  double effort = minimumInfo->effort + effortDelta;
234  double time = leaveTime;
235  this->updateViaEdgeCost(follower.second, vehicle, time, effort, length);
236  const double oldEffort = followerInfo.effort;
237  if ((!followerInfo.visited || mayRevisit) && effort < oldEffort) {
238  followerInfo.effort = effort;
239  // if we use the effort including the via effort below we would count the via twice as shown by the ticket676 test
240  followerInfo.heuristicEffort = MIN2(heuristicEffort, followerInfo.heuristicEffort);
241  followerInfo.leaveTime = time;
242  followerInfo.prev = minimumInfo;
243 #ifdef ASTAR_DEBUG_QUERY_FOLLOWERS
244  if (ASTAR_DEBUG_COND) {
245  std::cout << " follower=" << followerInfo.edge->getID()
246  << " OEF=" << (oldEffort == std::numeric_limits<double>::max() ? "inf" : toString(oldEffort))
247  << " TT=" << effort << " HR=" << heuristic_remaining << " HT=" << followerInfo.heuristicEffort << "\n";
248  }
249 #endif
250  if (oldEffort == std::numeric_limits<double>::max()) {
251  this->myFrontierList.push_back(&followerInfo);
252  std::push_heap(this->myFrontierList.begin(), this->myFrontierList.end(), myComparator);
253  } else {
254  auto fi = std::find(this->myFrontierList.begin(), this->myFrontierList.end(), &followerInfo);
255  if (fi == this->myFrontierList.end()) {
256  assert(mayRevisit);
257  this->myFrontierList.push_back(&followerInfo);
258  std::push_heap(this->myFrontierList.begin(), this->myFrontierList.end(), myComparator);
259  } else {
260  std::push_heap(this->myFrontierList.begin(), fi + 1, myComparator);
261  }
262  }
263  }
264  }
265  }
266  this->endQuery(num_visited);
267 #ifdef ASTAR_DEBUG_QUERY_PERF
268  if (ASTAR_DEBUG_COND) {
269  std::cout << "visited " + toString(num_visited) + " edges (unsuccessful path length: " + toString(into.size()) + ")\n";
270  }
271 #endif
272  if (!silent) {
273  this->myErrorMsgHandler->informf("No connection between edge '%' and edge '%' found.", from->getID(), to->getID());
274  }
275  return false;
276  }
277 
278 
279 protected:
281 
283  const std::shared_ptr<const LookupTable> myLookupTable;
284 
286  double myMaxSpeed;
287 };
#define UNREACHABLE
Definition: AStarRouter.h:46
long long int SUMOTime
Definition: GUI.h:35
#define STEPS2TIME(x)
Definition: SUMOTime.h:54
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:71
T MAX2(T a, T b)
Definition: StdDefs.h:77
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:46
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:286
virtual SUMOAbstractRouter< E, V > * clone()
Definition: AStarRouter.h:123
EdgeInfoComparator myComparator
Definition: AStarRouter.h:280
FullLookupTable< E, V > FLT
Definition: AStarRouter.h:79
virtual ~AStarRouter()
Destructor.
Definition: AStarRouter.h:121
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:129
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
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:283
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:116
static MsgHandler * getWarningInstance()
Returns the instance to add warnings to.
Definition: MsgHandler.cpp:66
void informf(const std::string &format, T value, Targs... Fargs)
adds a new formatted message
Definition: MsgHandler.h:120
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:67
Static storage of an output device and its base (abstract) implementation.
Definition: OutputDevice.h:61
void close()
Closes the device and removes it from the dictionary.
static OutputDevice & getDevice(const std::string &name, bool usePrefix=true)
Returns the described OutputDevice.
const E *const edge
The current edge.
double heuristicEffort
Estimated effort to reach the edge (effort + lower bound on remaining effort)
MsgHandler *const myErrorMsgHandler
the handler for routing errors
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
std::vector< typename SUMOAbstractRouter< E, V >::EdgeInfo > myEdgeInfos
The container of edge information.
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
void init(const int edgeID, const SUMOTime msTime)
bool myAmClean
whether we are already initialized
const bool myHaveRestrictions
whether edge restrictions need to be considered
void buildPathFrom(const typename SUMOAbstractRouter< E, V >::EdgeInfo *rbegin, std::vector< const E * > &edges)
Builds the path from marked edges.
void endQuery(int visits)
std::vector< typename SUMOAbstractRouter< E, V >::EdgeInfo * > myFrontierList
A container for reusage of the min edge heap.
std::vector< typename SUMOAbstractRouter< E, V >::EdgeInfo * > myFound
list of visited Edges (for resetting)