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 /****************************************************************************/ 10 /// @file AStarRouter.h 11 /// @author Jakob Erdmann 12 /// @author Daniel Krajzewicz 13 /// @author Michael Behrisch 14 /// @date January 2012 15 /// @version $Id$ 16 /// 17 // A* Algorithm using euclidean distance heuristic. 18 // Based on DijkstraRouter. For routing by effort a novel heuristic would be needed. 19 /****************************************************************************/ 20 #ifndef AStarRouter_h 21 #define AStarRouter_h 22 23 24 // =========================================================================== 25 // included modules 26 // =========================================================================== 27 #include <config.h> 28 29 #include <cassert> 30 #include <string> 31 #include <functional> 32 #include <vector> 33 #include <set> 34 #include <limits> 35 #include <algorithm> 36 #include <iterator> 37 #include <map> 38 #include <iostream> 39 #include <memory> 40 #include <utils/common/MsgHandler.h> 41 #include <utils/common/StringTokenizer.h> 42 #include <utils/common/StringUtils.h> 43 #include <utils/common/StdDefs.h> 44 #include <utils/common/ToString.h> 45 #include <utils/iodevices/BinaryInputDevice.h> 46 #include <utils/iodevices/OutputDevice.h> 47 #include "AStarLookupTable.h" 48 #include "SUMOAbstractRouter.h" 49 50 #define UNREACHABLE (std::numeric_limits<double>::max() / 1000.0) 51 52 //#define ASTAR_DEBUG_QUERY 53 //#define ASTAR_DEBUG_QUERY_FOLLOWERS 54 //#define ASTAR_DEBUG_QUERY_PERF 55 //#define ASTAR_DEBUG_VISITED 56 //#define ASTAR_DEBUG_LOOKUPTABLE 57 //#define ASTAR_DEBUG_LOOKUPTABLE_FROM "disabled" 58 //#define ASTAR_DEBUG_UNREACHABLE 59 60 // =========================================================================== 61 // class definitions 62 // =========================================================================== 63 /** 64 * @class AStarRouter 65 * @brief Computes the shortest path through a network using the A* algorithm. 66 * 67 * The template parameters are: 68 * @param E The edge class to use (MSEdge/ROEdge) 69 * @param V The vehicle class to use (MSVehicle/ROVehicle) 70 * @param BASE The base class to use (SUMOAbstractRouterPermissions/SUMOAbstractRouter) 71 * 72 * The router is edge-based. It must know the number of edges for internal reasons 73 * and whether a missing connection between two given edges (unbuild route) shall 74 * be reported as an error or as a warning. 75 * 76 */ 77 template<class E, class V, class BASE> 78 class AStarRouter : public BASE { 79 public: 80 typedef AbstractLookupTable<E, V> LookupTable; 81 typedef FullLookupTable<E, V> FLT; 82 typedef LandmarkLookupTable<E, V> LMLT; 83 84 /** 85 * @class EdgeInfoComparator 86 * Class to compare (and so sort) nodes by their effort 87 */ 88 class EdgeInfoComparator { 89 public: 90 /// Comparing method operator()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(); 94 } 95 return nod1->heuristicEffort > nod2->heuristicEffort; 96 } 97 }; 98 99 /// Constructor 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), 102 myLookupTable(lookup), 103 myMaxSpeed(NUMERICAL_EPS) { 104 for (const E* const edge : edges) { 105 myEdgeInfos.push_back(typename BASE::EdgeInfo(edge)); 106 myMaxSpeed = MAX2(myMaxSpeed, edge->getSpeedLimit() * MAX2(1.0, edge->getLengthGeometryFactor())); 107 } 108 } 109 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), 112 myLookupTable(lookup), 113 myMaxSpeed(NUMERICAL_EPS) { 114 for (const auto& edgeInfo : edgeInfos) { 115 myEdgeInfos.push_back(typename BASE::EdgeInfo(edgeInfo.edge)); 116 myMaxSpeed = MAX2(myMaxSpeed, edgeInfo.edge->getSpeedLimit() * edgeInfo.edge->getLengthGeometryFactor()); 117 } 118 } 119 120 /// Destructor ~AStarRouter()121 virtual ~AStarRouter() {} 122 clone()123 virtual SUMOAbstractRouter<E, V>* clone() { 124 return new AStarRouter<E, V, BASE>(myEdgeInfos, this->myErrorMsgHandler == MsgHandler::getWarningInstance(), this->myOperation, myLookupTable); 125 } 126 init()127 void init() { 128 // all EdgeInfos touched in the previous query are either in myFrontierList or myFound: clean those up 129 for (auto& edgeInfo : myFrontierList) { 130 edgeInfo->reset(); 131 } 132 myFrontierList.clear(); 133 for (auto& edgeInfo : myFound) { 134 edgeInfo->reset(); 135 } 136 myFound.clear(); 137 } 138 139 140 /** @brief Builds the route between the given edges using the minimum travel time */ 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); 144 // check whether from and to can be used 145 if (this->isProhibited(from, vehicle)) { 146 if (!silent) { 147 this->myErrorMsgHandler->inform("Vehicle '" + Named::getIDSecure(vehicle) + "' is not allowed on source edge '" + from->getID() + "'."); 148 } 149 return false; 150 } 151 if (this->isProhibited(to, vehicle)) { 152 if (!silent) { 153 this->myErrorMsgHandler->inform("Vehicle '" + Named::getIDSecure(vehicle) + "' is not allowed on destination edge '" + to->getID() + "'."); 154 } 155 return false; 156 } 157 double length = 0.; // dummy for the via edge cost update 158 this->startQuery(); 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"; 161 #endif 162 const SUMOVehicleClass vClass = vehicle == 0 ? SVC_IGNORING : vehicle->getVClass(); 163 if (this->myBulkMode) { 164 const auto& toInfo = myEdgeInfos[to->getNumericalID()]; 165 if (toInfo.visited) { 166 buildPathFrom(&toInfo, into); 167 this->endQuery(1); 168 return true; 169 } 170 } else { 171 init(); 172 // add begin node 173 auto* const fromInfo = &(myEdgeInfos[from->getNumericalID()]); 174 fromInfo->effort = 0.; 175 fromInfo->heuristicEffort = 0.; 176 fromInfo->prev = nullptr; 177 fromInfo->leaveTime = STEPS2TIME(msTime); 178 myFrontierList.push_back(fromInfo); 179 } 180 // loop 181 int num_visited = 0; 182 const bool mayRevisit = myLookupTable != 0 && !myLookupTable->consistent(); 183 const double speed = vehicle == nullptr ? myMaxSpeed : MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor()); 184 while (!myFrontierList.empty()) { 185 num_visited += 1; 186 // use the node with the minimal length 187 auto* const minimumInfo = myFrontierList.front(); 188 const E* const minEdge = minimumInfo->edge; 189 // check whether the destination node was already reached 190 if (minEdge == to) { 191 buildPathFrom(minimumInfo, into); 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"; 197 #endif 198 #ifdef ASTAR_DEBUG_VISITED 199 OutputDevice& dev = OutputDevice::getDevice(Named::getIDSecure(vehicle) + "_" + time2string(msTime) + "_" + from->getID() + "_" + to->getID()); 200 for (const auto& i : myEdgeInfos) { 201 if (i.visited) { 202 dev << "edge:" << i.edge->getID() << "\n"; 203 } 204 } 205 dev.close(); 206 #endif 207 return true; 208 } 209 std::pop_heap(myFrontierList.begin(), myFrontierList.end(), myComparator); 210 myFrontierList.pop_back(); 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)="; 219 for (typename std::vector<EdgeInfo*>::iterator it = myFrontierList.begin(); it != myFrontierList.end(); it++) { 220 std::cout << (*it)->effort << "," << (*it)->heuristicEffort << "," << (*it)->edge->getID() << " "; 221 } 222 std::cout << "\n"; 223 #endif 224 const double effortDelta = this->getEffort(minEdge, vehicle, minimumInfo->leaveTime); 225 const double leaveTime = minimumInfo->leaveTime + this->getTravelTime(minEdge, vehicle, minimumInfo->leaveTime, effortDelta); 226 227 // admissible A* heuristic: straight line distance at maximum speed 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))); 231 if (heuristic_remaining == UNREACHABLE) { 232 continue; 233 } 234 const double heuristicEffort = minimumInfo->effort + effortDelta + heuristic_remaining; 235 // check all ways from the node with the minimal length 236 for (const std::pair<const E*, const E*>& follower : minEdge->getViaSuccessors(vClass)) { 237 auto* const followerInfo = &(myEdgeInfos[follower.first->getNumericalID()]); 238 // check whether it can be used 239 if (this->isProhibited(follower.first, vehicle)) { 240 continue; 241 } 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; 248 // if we use the effort including the via effort below we would count the via twice as shown by the ticket676 test 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"; 256 #endif 257 if (oldEffort == std::numeric_limits<double>::max()) { 258 myFrontierList.push_back(followerInfo); 259 std::push_heap(myFrontierList.begin(), myFrontierList.end(), myComparator); 260 } else { 261 auto fi = std::find(myFrontierList.begin(), myFrontierList.end(), followerInfo); 262 if (fi == myFrontierList.end()) { 263 assert(mayRevisit); 264 myFrontierList.push_back(followerInfo); 265 std::push_heap(myFrontierList.begin(), myFrontierList.end(), myComparator); 266 } else { 267 std::push_heap(myFrontierList.begin(), fi + 1, myComparator); 268 } 269 } 270 } 271 } 272 } 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"; 276 #endif 277 if (!silent) { 278 this->myErrorMsgHandler->inform("No connection between edge '" + from->getID() + "' and edge '" + to->getID() + "' found."); 279 } 280 return false; 281 } 282 283 public: 284 /// Builds the path from marked edges buildPathFrom(const typename BASE::EdgeInfo * rbegin,std::vector<const E * > & edges)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; 290 } 291 std::copy(tmp.rbegin(), tmp.rend(), std::back_inserter(edges)); 292 } 293 294 protected: 295 /// The container of edge information 296 std::vector<typename BASE::EdgeInfo> myEdgeInfos; 297 298 /// A container for reusage of the min edge heap 299 std::vector<typename BASE::EdgeInfo*> myFrontierList; 300 /// @brief list of visited Edges (for resetting) 301 std::vector<typename BASE::EdgeInfo*> myFound; 302 303 EdgeInfoComparator myComparator; 304 305 /// @brief the lookup table for travel time heuristics 306 const std::shared_ptr<const LookupTable> myLookupTable; 307 308 /// @brief maximum speed in the network 309 double myMaxSpeed; 310 }; 311 312 313 #endif 314 315 /****************************************************************************/ 316 317