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 /****************************************************************************/
16 // A* Algorithm using euclidean distance heuristic.
17 // Based on DijkstraRouter. For routing by effort a novel heuristic would be needed.
18 /****************************************************************************/
19 #ifndef AStarRouter_h
20 #define AStarRouter_h
21 
22 
23 // ===========================================================================
24 // included modules
25 // ===========================================================================
26 #include <config.h>
27 
28 #include <cassert>
29 #include <string>
30 #include <functional>
31 #include <vector>
32 #include <set>
33 #include <limits>
34 #include <algorithm>
35 #include <iterator>
36 #include <map>
37 #include <iostream>
38 #include <memory>
42 #include <utils/common/StdDefs.h>
43 #include <utils/common/ToString.h>
46 #include "AStarLookupTable.h"
47 #include "SUMOAbstractRouter.h"
48 
49 #define UNREACHABLE (std::numeric_limits<double>::max() / 1000.0)
50 
51 //#define ASTAR_DEBUG_QUERY
52 //#define ASTAR_DEBUG_QUERY_FOLLOWERS
53 //#define ASTAR_DEBUG_QUERY_PERF
54 //#define ASTAR_DEBUG_VISITED
55 //#define ASTAR_DEBUG_LOOKUPTABLE
56 //#define ASTAR_DEBUG_LOOKUPTABLE_FROM "disabled"
57 //#define ASTAR_DEBUG_UNREACHABLE
58 
59 // ===========================================================================
60 // class definitions
61 // ===========================================================================
76 template<class E, class V>
77 class AStarRouter : public SUMOAbstractRouter<E, V> {
78 public:
82 
88  public:
90  bool operator()(const typename SUMOAbstractRouter<E, V>::EdgeInfo* nod1, const typename SUMOAbstractRouter<E, V>::EdgeInfo* nod2) const {
91  if (nod1->heuristicEffort == nod2->heuristicEffort) {
92  return nod1->edge->getNumericalID() > nod2->edge->getNumericalID();
93  }
94  return nod1->heuristicEffort > nod2->heuristicEffort;
95  }
96  };
97 
99  AStarRouter(const std::vector<E*>& edges, bool unbuildIsWarning, typename SUMOAbstractRouter<E, V>::Operation operation, const std::shared_ptr<const LookupTable> lookup = nullptr,
100  const bool havePermissions = false, const bool haveRestrictions = false) :
101  SUMOAbstractRouter<E, V>("AStarRouter", unbuildIsWarning, operation, nullptr, havePermissions, haveRestrictions),
102  myLookupTable(lookup),
104  for (const E* const edge : edges) {
105  myEdgeInfos.push_back(typename SUMOAbstractRouter<E, V>::EdgeInfo(edge));
106  myMaxSpeed = MAX2(myMaxSpeed, edge->getSpeedLimit() * MAX2(1.0, edge->getLengthGeometryFactor()));
107  }
108  }
109 
110  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,
111  const bool havePermissions = false, const bool haveRestrictions = false) :
112  SUMOAbstractRouter<E, V>("AStarRouter", unbuildIsWarning, operation, nullptr, havePermissions, haveRestrictions),
113  myLookupTable(lookup),
115  for (const auto& edgeInfo : edgeInfos) {
116  myEdgeInfos.push_back(typename SUMOAbstractRouter<E, V>::EdgeInfo(edgeInfo.edge));
117  myMaxSpeed = MAX2(myMaxSpeed, edgeInfo.edge->getSpeedLimit() * edgeInfo.edge->getLengthGeometryFactor());
118  }
119  }
120 
122  virtual ~AStarRouter() {}
123 
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  std::cout << "DEBUG: starting search for '" << Named::getIDSecure(vehicle) << "' speed: " << MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor()) << " time: " << STEPS2TIME(msTime) << "\n";
162 #endif
163  const SUMOVehicleClass vClass = vehicle == 0 ? SVC_IGNORING : vehicle->getVClass();
164  if (this->myBulkMode) {
165  const auto& toInfo = myEdgeInfos[to->getNumericalID()];
166  if (toInfo.visited) {
167  buildPathFrom(&toInfo, into);
168  this->endQuery(1);
169  return true;
170  }
171  } else {
172  init();
173  // add begin node
174  auto* const fromInfo = &(myEdgeInfos[from->getNumericalID()]);
175  fromInfo->effort = 0.;
176  fromInfo->heuristicEffort = 0.;
177  fromInfo->prev = nullptr;
178  fromInfo->leaveTime = STEPS2TIME(msTime);
179  myFrontierList.push_back(fromInfo);
180  }
181  // loop
182  int num_visited = 0;
183  const bool mayRevisit = myLookupTable != 0 && !myLookupTable->consistent();
184  const double speed = vehicle == nullptr ? myMaxSpeed : MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor());
185  while (!myFrontierList.empty()) {
186  num_visited += 1;
187  // use the node with the minimal length
188  auto* const minimumInfo = myFrontierList.front();
189  const E* const minEdge = minimumInfo->edge;
190  // check whether the destination node was already reached
191  if (minEdge == to) {
192  buildPathFrom(minimumInfo, into);
193  this->endQuery(num_visited);
194 #ifdef ASTAR_DEBUG_QUERY_PERF
195  std::cout << "visited " + toString(num_visited) + " edges (final path length=" + toString(into.size())
196  + " time=" + toString(recomputeCosts(into, vehicle, msTime))
197  + " edges=" + toString(into) + ")\n";
198 #endif
199 #ifdef ASTAR_DEBUG_VISITED
200  OutputDevice& dev = OutputDevice::getDevice(Named::getIDSecure(vehicle) + "_" + time2string(msTime) + "_" + from->getID() + "_" + to->getID());
201  for (const auto& i : myEdgeInfos) {
202  if (i.visited) {
203  dev << "edge:" << i.edge->getID() << "\n";
204  }
205  }
206  dev.close();
207 #endif
208  return true;
209  }
210  std::pop_heap(myFrontierList.begin(), myFrontierList.end(), myComparator);
211  myFrontierList.pop_back();
212  myFound.push_back(minimumInfo);
213  minimumInfo->visited = true;
214 #ifdef ASTAR_DEBUG_QUERY
215  std::cout << "DEBUG: hit=" << minEdge->getID()
216  << " TT=" << minimumInfo->effort
217  << " EF=" << this->getEffort(minEdge, vehicle, minimumInfo->leaveTime)
218  << " HT=" << minimumInfo->heuristicEffort
219  << " Q(TT,HT,Edge)=";
220  for (typename std::vector<EdgeInfo*>::iterator it = myFrontierList.begin(); it != myFrontierList.end(); it++) {
221  std::cout << (*it)->effort << "," << (*it)->heuristicEffort << "," << (*it)->edge->getID() << " ";
222  }
223  std::cout << "\n";
224 #endif
225  const double effortDelta = this->getEffort(minEdge, vehicle, minimumInfo->leaveTime);
226  const double leaveTime = minimumInfo->leaveTime + this->getTravelTime(minEdge, vehicle, minimumInfo->leaveTime, effortDelta);
227 
228  // admissible A* heuristic: straight line distance at maximum speed
229  const double heuristic_remaining = (myLookupTable == nullptr ? minEdge->getDistanceTo(to) / speed :
230  myLookupTable->lowerBound(minEdge, to, speed, vehicle->getChosenSpeedFactor(),
231  minEdge->getMinimumTravelTime(nullptr), to->getMinimumTravelTime(nullptr)));
232  if (heuristic_remaining == UNREACHABLE) {
233  continue;
234  }
235  const double heuristicEffort = minimumInfo->effort + effortDelta + heuristic_remaining;
236  // check all ways from the node with the minimal length
237  for (const std::pair<const E*, const E*>& follower : minEdge->getViaSuccessors(vClass)) {
238  auto* const followerInfo = &(myEdgeInfos[follower.first->getNumericalID()]);
239  // check whether it can be used
240  if (followerInfo->prohibited || this->isProhibited(follower.first, vehicle)) {
241  continue;
242  }
243  double effort = minimumInfo->effort + effortDelta;
244  double time = leaveTime;
245  this->updateViaEdgeCost(follower.second, vehicle, time, effort, length);
246  const double oldEffort = followerInfo->effort;
247  if ((!followerInfo->visited || mayRevisit) && effort < oldEffort) {
248  followerInfo->effort = effort;
249  // if we use the effort including the via effort below we would count the via twice as shown by the ticket676 test
250  followerInfo->heuristicEffort = MIN2(heuristicEffort, followerInfo->heuristicEffort);
251  followerInfo->leaveTime = time;
252  followerInfo->prev = minimumInfo;
253 #ifdef ASTAR_DEBUG_QUERY_FOLLOWERS
254  std::cout << " follower=" << followerInfo->edge->getID()
255  << " OEF=" << (oldEffort == std::numeric_limits<double>::max() ? "inf" : toString(oldEffort))
256  << " TT=" << effort << " HR=" << heuristic_remaining << " HT=" << followerInfo->heuristicEffort << "\n";
257 #endif
258  if (oldEffort == std::numeric_limits<double>::max()) {
259  myFrontierList.push_back(followerInfo);
260  std::push_heap(myFrontierList.begin(), myFrontierList.end(), myComparator);
261  } else {
262  auto fi = std::find(myFrontierList.begin(), myFrontierList.end(), followerInfo);
263  if (fi == myFrontierList.end()) {
264  assert(mayRevisit);
265  myFrontierList.push_back(followerInfo);
266  std::push_heap(myFrontierList.begin(), myFrontierList.end(), myComparator);
267  } else {
268  std::push_heap(myFrontierList.begin(), fi + 1, myComparator);
269  }
270  }
271  }
272  }
273  }
274  this->endQuery(num_visited);
275 #ifdef ASTAR_DEBUG_QUERY_PERF
276  std::cout << "visited " + toString(num_visited) + " edges (unsuccessful path length: " + toString(into.size()) + ")\n";
277 #endif
278  if (!silent) {
279  this->myErrorMsgHandler->informf("No connection between edge '%' and edge '%' found.", from->getID(), to->getID());
280  }
281  return false;
282  }
283 
284 
285  void prohibit(const std::vector<E*>& toProhibit) {
286  for (E* const edge : this->myProhibited) {
287  myEdgeInfos[edge->getNumericalID()].prohibited = false;
288  }
289  for (E* const edge : toProhibit) {
290  myEdgeInfos[edge->getNumericalID()].prohibited = true;
291  }
292  this->myProhibited = toProhibit;
293  }
294 
295 
297  void buildPathFrom(const typename SUMOAbstractRouter<E, V>::EdgeInfo* rbegin, std::vector<const E*>& edges) {
298  std::vector<const E*> tmp;
299  while (rbegin != 0) {
300  tmp.push_back(rbegin->edge);
301  rbegin = rbegin->prev;
302  }
303  std::copy(tmp.rbegin(), tmp.rend(), std::back_inserter(edges));
304  }
305 
306 protected:
308  std::vector<typename SUMOAbstractRouter<E, V>::EdgeInfo> myEdgeInfos;
309 
311  std::vector<typename SUMOAbstractRouter<E, V>::EdgeInfo*> myFrontierList;
313  std::vector<typename SUMOAbstractRouter<E, V>::EdgeInfo*> myFound;
314 
316 
318  const std::shared_ptr<const LookupTable> myLookupTable;
319 
321  double myMaxSpeed;
322 };
323 
324 
325 #endif
326 
327 /****************************************************************************/
328 
SUMOVehicleClass
SUMOVehicleClass
Definition of vehicle classes to differ between different lane usage and authority types.
Definition: SUMOVehicleClass.h:133
ToString.h
AStarRouter::AStarRouter
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:99
MIN2
T MIN2(T a, T b)
Definition: StdDefs.h:73
OutputDevice
Static storage of an output device and its base (abstract) implementation.
Definition: OutputDevice.h:63
NUMERICAL_EPS
#define NUMERICAL_EPS
Definition: config.h:148
MsgHandler.h
AStarRouter::AStarRouter
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:110
SUMOAbstractRouter::getEffort
double getEffort(const E *const e, const V *const v, double t) const
Definition: SUMOAbstractRouter.h:216
MsgHandler::inform
virtual void inform(std::string msg, bool addType=true)
adds a new error to the list
Definition: MsgHandler.cpp:118
SUMOTime
long long int SUMOTime
Definition: SUMOTime.h:34
AStarRouter::FLT
FullLookupTable< E, V > FLT
Definition: AStarRouter.h:80
AStarRouter::EdgeInfoComparator::operator()
bool operator()(const typename SUMOAbstractRouter< E, V >::EdgeInfo *nod1, const typename SUMOAbstractRouter< E, V >::EdgeInfo *nod2) const
Comparing method.
Definition: AStarRouter.h:90
SUMOAbstractRouter::myProhibited
std::vector< E * > myProhibited
Definition: SUMOAbstractRouter.h:253
AStarRouter::myFound
std::vector< typename SUMOAbstractRouter< E, V >::EdgeInfo * > myFound
list of visited Edges (for resetting)
Definition: AStarRouter.h:313
SUMOAbstractRouter::startQuery
void startQuery()
Definition: SUMOAbstractRouter.h:220
OutputDevice::close
void close()
Closes the device and removes it from the dictionary.
Definition: OutputDevice.cpp:207
SUMOAbstractRouter::myOperation
Operation myOperation
The object's operation to perform.
Definition: SUMOAbstractRouter.h:239
AStarRouter::myEdgeInfos
std::vector< typename SUMOAbstractRouter< E, V >::EdgeInfo > myEdgeInfos
The container of edge information.
Definition: AStarRouter.h:308
MAX2
T MAX2(T a, T b)
Definition: StdDefs.h:79
BinaryInputDevice.h
LandmarkLookupTable
Computes the shortest path through a network using the A* algorithm.
Definition: AStarLookupTable.h:100
AStarRouter::myComparator
EdgeInfoComparator myComparator
Definition: AStarRouter.h:315
SUMOAbstractRouter::EdgeInfo::heuristicEffort
double heuristicEffort
Estimated effort to reach the edge (effort + lower bound on remaining effort)
Definition: SUMOAbstractRouter.h:69
MsgHandler::getWarningInstance
static MsgHandler * getWarningInstance()
Returns the instance to add warnings to.
Definition: MsgHandler.cpp:68
AStarRouter::LMLT
LandmarkLookupTable< E, V > LMLT
Definition: AStarRouter.h:81
AStarRouter::myLookupTable
const std::shared_ptr< const LookupTable > myLookupTable
the lookup table for travel time heuristics
Definition: AStarRouter.h:318
AStarRouter::LookupTable
AbstractLookupTable< E, V > LookupTable
Definition: AStarRouter.h:79
STEPS2TIME
#define STEPS2TIME(x)
Definition: SUMOTime.h:56
SUMOAbstractRouter::isProhibited
bool isProhibited(const E *const edge, const V *const vehicle) const
Definition: SUMOAbstractRouter.h:159
OutputDevice.h
AStarRouter::myMaxSpeed
double myMaxSpeed
maximum speed in the network
Definition: AStarRouter.h:321
time2string
std::string time2string(SUMOTime t)
Definition: SUMOTime.cpp:67
AStarRouter::compute
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
SUMOAbstractRouter::EdgeInfo
Definition: SUMOAbstractRouter.h:53
AStarLookupTable.h
SUMOAbstractRouter::EdgeInfo::prev
const EdgeInfo * prev
The previous edge.
Definition: SUMOAbstractRouter.h:75
AStarRouter::EdgeInfoComparator
Definition: AStarRouter.h:87
AStarRouter::clone
virtual SUMOAbstractRouter< E, V > * clone()
Definition: AStarRouter.h:124
SUMOAbstractRouter::EdgeInfo::edge
const E *const edge
The current edge.
Definition: SUMOAbstractRouter.h:62
SUMOAbstractRouter::myBulkMode
bool myBulkMode
whether we are currently operating several route queries in a bulk
Definition: SUMOAbstractRouter.h:245
SUMOAbstractRouter::getTravelTime
double getTravelTime(const E *const e, const V *const v, const double t, const double effort) const
Definition: SUMOAbstractRouter.h:165
SUMOAbstractRouter
Definition: SUMOAbstractRouter.h:46
SUMOAbstractRouter::endQuery
void endQuery(int visits)
Definition: SUMOAbstractRouter.h:225
toString
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:47
FullLookupTable
Definition: AStarLookupTable.h:69
StringUtils.h
OutputDevice::getDevice
static OutputDevice & getDevice(const std::string &name)
Returns the described OutputDevice.
Definition: OutputDevice.cpp:54
SUMOAbstractRouter::myErrorMsgHandler
MsgHandler *const myErrorMsgHandler
the handler for routing errors
Definition: SUMOAbstractRouter.h:236
AbstractLookupTable
Definition: AStarLookupTable.h:58
SUMOAbstractRouter::recomputeCosts
double recomputeCosts(const std::vector< const E * > &edges, const V *const v, SUMOTime msTime, double *lengthp=nullptr) const
Definition: SUMOAbstractRouter.h:195
SUMOAbstractRouter::updateViaEdgeCost
void updateViaEdgeCost(const E *viaEdge, const V *const v, double &time, double &effort, double &length) const
Definition: SUMOAbstractRouter.h:169
AStarRouter::init
void init()
Definition: AStarRouter.h:128
AStarRouter
Computes the shortest path through a network using the A* algorithm.
Definition: AStarRouter.h:77
AStarRouter::~AStarRouter
virtual ~AStarRouter()
Destructor.
Definition: AStarRouter.h:122
AStarRouter::prohibit
void prohibit(const std::vector< E * > &toProhibit)
Definition: AStarRouter.h:285
MsgHandler::informf
void informf(const std::string &format, T value, Targs... Fargs)
adds a new formatted message
Definition: MsgHandler.h:115
config.h
Named::getIDSecure
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:69
StringTokenizer.h
StdDefs.h
AStarRouter::myFrontierList
std::vector< typename SUMOAbstractRouter< E, V >::EdgeInfo * > myFrontierList
A container for reusage of the min edge heap.
Definition: AStarRouter.h:311
SVC_IGNORING
vehicles ignoring classes
Definition: SUMOVehicleClass.h:135
SUMOAbstractRouter.h
AStarRouter::buildPathFrom
void buildPathFrom(const typename SUMOAbstractRouter< E, V >::EdgeInfo *rbegin, std::vector< const E * > &edges)
Builds the path from marked edges.
Definition: AStarRouter.h:297
UNREACHABLE
#define UNREACHABLE
Definition: AStarRouter.h:49