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", operation),
105 for (
typename std::vector<E*>::const_iterator i = edges.begin(); i != edges.end(); ++i) {
106 myEdgeInfos.push_back(
typename BASE::EdgeInfo(*i));
111 AStarRouter(
const std::vector<typename BASE::EdgeInfo>& edgeInfos,
bool unbuildIsWarning,
typename BASE::Operation operation,
const std::shared_ptr<const LookupTable> lookup = 0) :
112 BASE(
"AStarRouter", operation),
116 for (
const auto& edgeInfo : edgeInfos) {
117 myEdgeInfos.push_back(
typename BASE::EdgeInfo(edgeInfo.edge));
134 myFrontierList.clear();
135 for (
auto& edgeInfo :
myFound) {
143 virtual bool compute(
const E* from,
const E* to,
const V*
const vehicle,
144 SUMOTime msTime, std::vector<const E*>& into) {
145 assert(from != 0 && to != 0);
147 if (this->isProhibited(from, vehicle)) {
148 myErrorMsgHandler->
inform(
"Vehicle '" + vehicle->getID() +
"' is not allowed on source edge '" + from->getID() +
"'.");
151 if (this->isProhibited(to, vehicle)) {
152 myErrorMsgHandler->
inform(
"Vehicle '" + vehicle->getID() +
"' is not allowed on destination edge '" + to->getID() +
"'.");
156 #ifdef ASTAR_DEBUG_QUERY 157 std::cout <<
"DEBUG: starting search for '" << vehicle->getID() <<
"' speed: " <<
MIN2(vehicle->getMaxSpeed(),
myMaxSpeed * vehicle->getChosenSpeedFactor()) <<
" time: " <<
STEPS2TIME(msTime) <<
"\n";
160 if (this->myBulkMode) {
161 const auto& toInfo =
myEdgeInfos[to->getNumericalID()];
162 if (toInfo.visited) {
170 auto*
const fromInfo = &(
myEdgeInfos[from->getNumericalID()]);
171 fromInfo->effort = 0.;
172 fromInfo->prev =
nullptr;
179 const double speed = vehicle ==
nullptr ?
myMaxSpeed :
MIN2(vehicle->getMaxSpeed(),
myMaxSpeed * vehicle->getChosenSpeedFactor());
184 const E*
const minEdge = minimumInfo->edge;
188 this->endQuery(num_visited);
189 #ifdef ASTAR_DEBUG_QUERY_PERF 190 std::cout <<
"visited " +
toString(num_visited) +
" edges (final path length=" +
toString(into.size())
191 +
" time=" +
toString(recomputeCosts(into, vehicle, msTime))
192 +
" edges=" +
toString(into) +
")\n";
194 #ifdef ASTAR_DEBUG_VISITED 198 dev <<
"edge:" << i.edge->getID() <<
"\n";
207 myFound.push_back(minimumInfo);
208 minimumInfo->visited =
true;
209 const E* viaEdge = minimumInfo->via;
210 #ifdef ASTAR_DEBUG_QUERY 211 std::cout <<
"DEBUG: hit=" << minEdge->getID()
212 <<
" TT=" << minimumInfo->traveltime
213 <<
" EF=" << this->getEffort(minEdge, vehicle, time + minimumInfo->traveltime)
214 <<
" HT=" << minimumInfo->heuristicTime
215 <<
" Q(TT,HT,Edge)=";
217 std::cout << (*it)->traveltime <<
"," << (*it)->heuristicTime <<
"," << (*it)->edge->getID() <<
" ";
221 double effort = minimumInfo->effort;
222 double leaveTime = minimumInfo->leaveTime;
223 while (viaEdge !=
nullptr && viaEdge->isInternal()) {
224 const double viaEffortDelta = this->getEffort(viaEdge, vehicle, leaveTime);
225 leaveTime += this->
getTravelTime(viaEdge, vehicle, leaveTime, viaEffortDelta);
226 effort += viaEffortDelta;
227 viaEdge = viaEdge->getViaSuccessors().front().first;
229 const double effortDelta = this->getEffort(minEdge, vehicle, leaveTime);
230 leaveTime += this->
getTravelTime(minEdge, vehicle, minimumInfo->leaveTime, effortDelta);
231 effort += effortDelta;
234 const double heuristic_remaining = (
myLookupTable ==
nullptr ? minEdge->getDistanceTo(to) / speed :
235 myLookupTable->lowerBound(minEdge, to, speed, vehicle->getChosenSpeedFactor(), minEdge->getMinimumTravelTime(0), to->getMinimumTravelTime(0)));
240 for (
const std::pair<const E*, const E*>& follower : minEdge->getViaSuccessors(vClass)) {
241 auto*
const followerInfo = &(
myEdgeInfos[follower.first->getNumericalID()]);
243 if (this->isProhibited(follower.first, vehicle)) {
246 const double oldEffort = followerInfo->effort;
247 if ((!followerInfo->visited || mayRevisit) && effort < oldEffort) {
248 followerInfo->effort = effort;
249 followerInfo->heuristicEffort = effort + heuristic_remaining;
250 followerInfo->leaveTime = leaveTime;
251 followerInfo->prev = minimumInfo;
252 followerInfo->via = follower.second;
264 #ifdef ASTAR_DEBUG_QUERY_FOLLOWERS 265 std::cout <<
" follower=" << followerInfo->edge->getID() <<
" OEF=" << oldEffort <<
" TT=" << traveltime <<
" HR=" << heuristic_remaining <<
" HT=" << followerInfo->heuristicTime <<
"\n";
267 if (oldEffort == std::numeric_limits<double>::max()) {
283 this->endQuery(num_visited);
284 #ifdef ASTAR_DEBUG_QUERY_PERF 285 std::cout <<
"visited " +
toString(num_visited) +
" edges (unsuccesful path length: " +
toString(into.size()) +
")\n";
287 myErrorMsgHandler->
inform(
"No connection between edge '" + from->getID() +
"' and edge '" + to->getID() +
"' found.");
293 void buildPathFrom(
const typename BASE::EdgeInfo* rbegin, std::vector<const E*>& edges) {
294 std::vector<const E*> tmp;
295 while (rbegin != 0) {
296 tmp.push_back(rbegin->edge);
297 rbegin = rbegin->prev;
299 std::copy(tmp.rbegin(), tmp.rend(), std::back_inserter(edges));
309 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.
const std::shared_ptr< const LookupTable > myLookupTable
the lookup table for travel time heuristics
virtual ~AStarRouter()
Destructor.
MsgHandler *const myErrorMsgHandler
the handler for routing errors
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()
virtual bool compute(const E *from, const E *to, const V *const vehicle, SUMOTime msTime, std::vector< const E *> &into)
Builds the route between the given edges using the minimum travel time.
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.
void inform(std::string msg, bool addType=true)
adds a new error to the list
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