50 #define UNREACHABLE (std::numeric_limits<double>::max() / 1000.0) 77 template<
class E,
class V,
class BASE>
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();
95 return nod1->heuristicEffort > nod2->heuristicEffort;
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),
104 for (
const E*
const edge : edges) {
105 myEdgeInfos.push_back(
typename BASE::EdgeInfo(edge));
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),
114 for (
const auto& edgeInfo : edgeInfos) {
115 myEdgeInfos.push_back(
typename BASE::EdgeInfo(edgeInfo.edge));
132 myFrontierList.clear();
133 for (
auto& edgeInfo :
myFound) {
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);
145 if (this->isProhibited(from, vehicle)) {
147 this->myErrorMsgHandler->inform(
"Vehicle '" +
Named::getIDSecure(vehicle) +
"' is not allowed on source edge '" + from->getID() +
"'.");
151 if (this->isProhibited(to, vehicle)) {
153 this->myErrorMsgHandler->inform(
"Vehicle '" +
Named::getIDSecure(vehicle) +
"' is not allowed on destination edge '" + to->getID() +
"'.");
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";
163 if (this->myBulkMode) {
164 const auto& toInfo =
myEdgeInfos[to->getNumericalID()];
165 if (toInfo.visited) {
173 auto*
const fromInfo = &(
myEdgeInfos[from->getNumericalID()]);
174 fromInfo->effort = 0.;
175 fromInfo->heuristicEffort = 0.;
176 fromInfo->prev =
nullptr;
183 const double speed = vehicle ==
nullptr ?
myMaxSpeed :
MIN2(vehicle->getMaxSpeed(),
myMaxSpeed * vehicle->getChosenSpeedFactor());
188 const E*
const minEdge = minimumInfo->edge;
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";
198 #ifdef ASTAR_DEBUG_VISITED 202 dev <<
"edge:" << i.edge->getID() <<
"\n";
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)=";
220 std::cout << (*it)->effort <<
"," << (*it)->heuristicEffort <<
"," << (*it)->edge->getID() <<
" ";
224 const double effortDelta = this->getEffort(minEdge, vehicle, minimumInfo->leaveTime);
225 const double leaveTime = minimumInfo->leaveTime + this->
getTravelTime(minEdge, vehicle, minimumInfo->leaveTime, effortDelta);
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)));
234 const double heuristicEffort = minimumInfo->effort + effortDelta + heuristic_remaining;
236 for (
const std::pair<const E*, const E*>& follower : minEdge->getViaSuccessors(vClass)) {
237 auto*
const followerInfo = &(
myEdgeInfos[follower.first->getNumericalID()]);
239 if (this->isProhibited(follower.first, vehicle)) {
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;
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";
257 if (oldEffort == std::numeric_limits<double>::max()) {
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";
278 this->myErrorMsgHandler->inform(
"No connection between edge '" + from->getID() +
"' and edge '" + to->getID() +
"' found.");
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;
291 std::copy(tmp.rbegin(), tmp.rend(), std::back_inserter(edges));
301 std::vector<typename BASE::EdgeInfo*>
myFound;
static MsgHandler * getWarningInstance()
Returns the instance to add warnings to.
void close()
Closes the device and removes it from the dictionary.
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)
FullLookupTable< E, V > FLT
std::string time2string(SUMOTime t)
Computes the shortest path through a network using the A* algorithm.
void buildPathFrom(const typename BASE::EdgeInfo *rbegin, std::vector< const E *> &edges)
Builds the path from marked edges.
std::vector< typename BASE::EdgeInfo > myEdgeInfos
The container of edge information.
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
const std::shared_ptr< const LookupTable > myLookupTable
the lookup table for travel time heuristics
virtual ~AStarRouter()
Destructor.
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
std::vector< typename BASE::EdgeInfo * > myFound
list of visited Edges (for resetting)
double getTravelTime(const ROEdge *const edge, const ROVehicle *const, double)
double myMaxSpeed
maximum speed in the network
bool operator()(const typename BASE::EdgeInfo *nod1, const typename BASE::EdgeInfo *nod2) const
Comparing method.
virtual SUMOAbstractRouter< E, V > * clone()
AbstractLookupTable< E, V > LookupTable
EdgeInfoComparator myComparator
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.
std::vector< typename BASE::EdgeInfo * > myFrontierList
A container for reusage of the min edge heap.
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.
Static storage of an output device and its base (abstract) implementation.
Computes the shortest path through a network using the A* algorithm.
LandmarkLookupTable< E, V > LMLT
vehicles ignoring classes