Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2025 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 : /****************************************************************************/
14 : /// @file MSStoppingPlaceRerouter.cpp
15 : /// @author Mirko Barthauer
16 : /// @date Mon, 17 June 2024
17 : ///
18 : // The StoppingPlaceRerouter provides an interface to structure the rerouting
19 : // to the best StoppingPlace according to the evaluation components and
20 : // associated weights.
21 : /****************************************************************************/
22 : #include <utils/vehicle/SUMOVehicle.h>
23 : #include <microsim/MSEdge.h>
24 : #include <microsim/MSGlobals.h>
25 : #include <microsim/MSLane.h>
26 : #include <microsim/MSRoute.h>
27 : #include <microsim/MSParkingArea.h>
28 : #include <microsim/MSStoppingPlace.h>
29 : #include <microsim/MSVehicleType.h>
30 : #include <microsim/trigger/MSChargingStation.h>
31 : #include "MSStoppingPlaceRerouter.h"
32 :
33 : //#define DEBUG_STOPPINGPLACE
34 : #define DEBUGCOND (veh.isSelected())
35 : //#define DEBUGCOND (true)
36 :
37 :
38 : ///@brief Constructor
39 3959 : MSStoppingPlaceRerouter::MSStoppingPlaceRerouter(std::string paramPrefix, bool checkValidity, StoppingPlaceParamMap_t addEvalParams, StoppingPlaceParamSwitchMap_t addInvertParams) :
40 3959 : myParamPrefix(paramPrefix), myCheckValidity(checkValidity) {
41 35631 : myEvalParams = { {"probability", 0.}, {"capacity", 0.}, {"timefrom", 0.}, {"timeto", 0.}, {"distancefrom", 0.}, {"distanceto", 1.}, {"absfreespace", 0.}, {"relfreespace", 0.}, };
42 35631 : myInvertParams = { {"probability", false}, { "capacity", true }, { "timefrom", false }, { "timeto", false }, { "distancefrom", false }, { "distanceto", false }, { "absfreespace", true }, { "relfreespace", true } };
43 4135 : for (auto param : addEvalParams) {
44 176 : myEvalParams[param.first] = param.second;
45 352 : myInvertParams[param.first] = (addInvertParams.count(param.first) > 0) ? addInvertParams[param.first] : false;
46 : }
47 35807 : for (auto param : myEvalParams) {
48 31848 : myNormParams.insert({param.first, param.first != "probability"});
49 : }
50 4135 : }
51 :
52 : MSStoppingPlace*
53 81896 : MSStoppingPlaceRerouter::rerouteStoppingPlace(MSStoppingPlace* destStoppingPlace, const std::vector<StoppingPlaceVisible>& stoppingPlaceCandidates, const std::vector<double>& probs, SUMOVehicle& veh, bool& newDestination, ConstMSEdgeVector& newRoute, StoppingPlaceParamMap_t& scores,
54 : const Prohibitions& closedEdges, const int insertStopIndex, const bool keepCurrentStop) {
55 : // Reroute destination from initial stopping place to an alternative stopping place
56 : // if the following conditions are met:
57 : // - next stop target is a stopping place of the right type
58 : // - target is included in the current alternative set
59 : // - target is visibly full
60 : // Any stopping places that are visibly full at the current location are
61 : // committed to the stopping place memory corresponding to their type
62 :
63 81896 : MSStoppingPlace* nearStoppingPlace = nullptr;
64 :
65 : // get vehicle params
66 : bool destVisible = false;
67 81896 : if (destStoppingPlace != nullptr) {
68 81786 : destVisible = (&destStoppingPlace->getLane().getEdge() == veh.getEdge());
69 : // if the vehicle is on the destination stop edge it is always visible
70 181003 : for (auto stoppingPlace : stoppingPlaceCandidates) {
71 119020 : if (stoppingPlace.first == destStoppingPlace && stoppingPlace.second) {
72 : destVisible = true;
73 : break;
74 : }
75 : }
76 : }
77 81896 : const MSRoute& route = veh.getRoute();
78 :
79 81896 : MSStoppingPlace* onTheWay = nullptr;
80 81896 : const int stopAnywhere = (int)getWeight(veh, "anywhere", -1);
81 81896 : const bool ignoreDest = getWeight(veh, "ignoreDest", destStoppingPlace != nullptr ? 0 : 1) != 0;
82 : // check whether we are ready to accept any free stopping place along the
83 : // way to our destination
84 81896 : if (stopAnywhere < 0 || stopAnywhere > getNumberStoppingPlaceReroutes(veh)) {
85 81861 : if (!destVisible) {
86 : // cannot determine destination occupancy, only register visibly full
87 119838 : for (const StoppingPlaceVisible& stoppingPlace : stoppingPlaceCandidates) {
88 70487 : if (stoppingPlace.second && getLastStepStoppingPlaceOccupancy(stoppingPlace.first) >= getStoppingPlaceCapacity(stoppingPlace.first)) {
89 6344 : rememberStoppingPlaceScore(veh, stoppingPlace.first, "occupied");
90 6344 : rememberBlockedStoppingPlace(veh, stoppingPlace.first, &stoppingPlace.first->getLane().getEdge() == veh.getEdge());
91 : }
92 : }
93 : #ifdef DEBUG_STOPPINGPLACE
94 : if (DEBUGCOND) {
95 : //std::cout << SIMTIME << " veh=" << veh.getID() << " dest=" << ((destStoppingPlace == nullptr)? "null" : destStoppingPlace->getID()) << " stopAnywhere=" << stopAnywhere << " reroutes=" << getNumberStoppingPlaceReroutes(veh) << " stay on original route\n";
96 : }
97 : #endif
98 : }
99 : } else {
100 : double bestDist = std::numeric_limits<double>::max();
101 35 : const double brakeGap = veh.getBrakeGap(true);
102 200 : for (const StoppingPlaceVisible& item : stoppingPlaceCandidates) {
103 165 : if (item.second) {
104 70 : if (&item.first->getLane().getEdge() == veh.getEdge()
105 70 : && getLastStepStoppingPlaceOccupancy(item.first) < getStoppingPlaceCapacity(item.first)) {
106 25 : const double distToStart = item.first->getBeginLanePosition() - veh.getPositionOnLane();
107 25 : const double distToEnd = item.first->getEndLanePosition() - veh.getPositionOnLane();
108 25 : if (distToEnd > brakeGap) {
109 40 : rememberStoppingPlaceScore(veh, item.first, "dist=" + toString(distToStart));
110 20 : if (distToStart < bestDist) {
111 : bestDist = distToStart;
112 20 : onTheWay = item.first;
113 : }
114 : } else {
115 10 : rememberStoppingPlaceScore(veh, item.first, "tooClose");
116 : }
117 : }
118 : }
119 : }
120 : #ifdef DEBUG_STOPPINGPLACE
121 : if (DEBUGCOND) {
122 : std::cout << SIMTIME << " veh=" << veh.getID()
123 : << " dest=" << ((destStoppingPlace == nullptr) ? "null" : destStoppingPlace->getID()) << " stopAnywhere=" << stopAnywhere << " reroutes=" << getNumberStoppingPlaceReroutes(veh) << " alongTheWay=" << Named::getIDSecure(onTheWay) << "\n";
124 : }
125 : #endif
126 : }
127 81896 : if (!ignoreDest && !destVisible && onTheWay == nullptr) {
128 : return nullptr;
129 : }
130 :
131 32666 : if (ignoreDest || getLastStepStoppingPlaceOccupancy(destStoppingPlace) >= getStoppingPlaceCapacity(destStoppingPlace) || onTheWay != nullptr) {
132 : // if the current route ends at the stopping place, the new route will
133 : // also end at the new stopping place
134 38206 : newDestination = (destStoppingPlace != nullptr && &destStoppingPlace->getLane().getEdge() == route.getLastEdge()
135 3875 : && veh.getArrivalPos() >= destStoppingPlace->getBeginLanePosition()
136 3875 : && veh.getArrivalPos() <= destStoppingPlace->getEndLanePosition()
137 19916 : && veh.getStops().size() == 1);
138 :
139 : #ifdef DEBUG_STOPPINGPLACE
140 : if (DEBUGCOND) {
141 : std::cout << SIMTIME << " veh=" << veh.getID()
142 : << " newDest=" << newDestination
143 : << " onTheWay=" << Named::getIDSecure(onTheWay)
144 : << "\n";
145 : }
146 : #endif
147 : std::map<MSStoppingPlace*, ConstMSEdgeVector> newRoutes;
148 : std::map<MSStoppingPlace*, ConstMSEdgeVector> stopApproaches;
149 19158 : StoppingPlaceParamMap_t weights = collectWeights(veh); // add option to patch values for interdependent values
150 : StoppingPlaceParamMap_t maxValues;
151 172618 : for (auto param : weights) {
152 153460 : maxValues[param.first] = 0.;
153 : }
154 :
155 : // a map stores elegible stopping places
156 : StoppingPlaceMap_t stoppingPlaces;
157 19158 : SUMOAbstractRouter<MSEdge, SUMOVehicle>& router = getRouter(veh, closedEdges);
158 19158 : const double brakeGap = veh.getBrakeGap(true);
159 :
160 19158 : if (onTheWay != nullptr) {
161 : // compute new route
162 20 : if (newDestination) {
163 0 : newRoute.push_back(veh.getEdge());
164 : } else {
165 20 : bool valid = evaluateDestination(veh, brakeGap, newDestination, onTheWay, getLastStepStoppingPlaceOccupancy(onTheWay), 1, router, stoppingPlaces, newRoutes, stopApproaches, maxValues, scores, insertStopIndex, keepCurrentStop);
166 20 : if (!valid) {
167 0 : WRITE_WARNINGF(TL("Stopping place '%' along the way cannot be used by vehicle '%' for unknown reason"), onTheWay->getID(), veh.getID());
168 0 : return nullptr;
169 : }
170 20 : newRoute = newRoutes[onTheWay];
171 : }
172 20 : return onTheWay;
173 : }
174 : int numAlternatives = 0;
175 : std::vector<std::tuple<SUMOTime, MSStoppingPlace*, int>> blockedTimes;
176 19138 : resetStoppingPlaceScores(veh);
177 :
178 19138 : if (destStoppingPlace != nullptr) {
179 19028 : rememberStoppingPlaceScore(veh, destStoppingPlace, "occupied");
180 19028 : rememberBlockedStoppingPlace(veh, destStoppingPlace, &destStoppingPlace->getLane().getEdge() == veh.getEdge());
181 : }
182 19138 : const SUMOTime stoppingPlaceMemory = TIME2STEPS(getWeight(veh, "memory", 600));
183 19138 : const double stoppingPlaceFrustration = getWeight(veh, "frustration", 100);
184 19138 : const double stoppingPlaceKnowledge = getWeight(veh, "knowledge", 0);
185 :
186 89258 : for (int i = 0; i < (int)stoppingPlaceCandidates.size(); ++i) {
187 : // alternative occupancy is randomized (but never full) if invisible
188 : // current destination must be visible at this point
189 70120 : if (!useStoppingPlace(stoppingPlaceCandidates[i].first)) {
190 0 : continue;
191 : }
192 70120 : const bool visible = stoppingPlaceCandidates[i].second || (stoppingPlaceCandidates[i].first == destStoppingPlace && destVisible);
193 70120 : double occupancy = getStoppingPlaceOccupancy(stoppingPlaceCandidates[i].first);
194 70120 : if (!visible && (stoppingPlaceKnowledge == 0 || stoppingPlaceKnowledge < RandHelper::rand(veh.getRNG()))) {
195 39309 : double capacity = getStoppingPlaceCapacity(stoppingPlaceCandidates[i].first);
196 39309 : const double minOccupancy = MIN2(capacity - NUMERICAL_EPS, (getNumberStoppingPlaceReroutes(veh) * capacity / stoppingPlaceFrustration));
197 : occupancy = RandHelper::rand(minOccupancy, capacity);
198 : // previously visited?
199 39309 : SUMOTime blockedTime = sawBlockedStoppingPlace(veh, stoppingPlaceCandidates[i].first, false);
200 39309 : if (blockedTime >= 0 && SIMSTEP - blockedTime < stoppingPlaceMemory) {
201 : // assume it's still occupied
202 : occupancy = capacity;
203 21218 : blockedTimes.push_back(std::make_tuple(blockedTime, stoppingPlaceCandidates[i].first, i));
204 : #ifdef DEBUG_STOPPINGPLACE
205 : if (DEBUGCOND) {
206 : std::cout << " altStoppingPlace=" << stoppingPlaceCandidates[i].first->getID() << " was blocked at " << time2string(blockedTime) << "\n";
207 : }
208 : #endif
209 : }
210 : }
211 70120 : if (occupancy < getStoppingPlaceCapacity(stoppingPlaceCandidates[i].first)) {
212 19951 : if (evaluateDestination(veh, brakeGap, newDestination, stoppingPlaceCandidates[i].first, occupancy, probs[i], router, stoppingPlaces, newRoutes, stopApproaches, maxValues, scores, insertStopIndex, keepCurrentStop)) {
213 19589 : numAlternatives++;
214 : }
215 50169 : } else if (visible) {
216 : // might only be visible now (i.e. because it's on the other
217 : // side of the street), so we should remember this for later.
218 28735 : rememberStoppingPlaceScore(veh, stoppingPlaceCandidates[i].first, "occupied");
219 28735 : rememberBlockedStoppingPlace(veh, stoppingPlaceCandidates[i].first, &stoppingPlaceCandidates[i].first->getLane().getEdge() == veh.getEdge());
220 : }
221 : }
222 :
223 19138 : if (numAlternatives == 0) {
224 : // use parkingArea with lowest blockedTime
225 11869 : std::sort(blockedTimes.begin(), blockedTimes.end(),
226 16192 : [](std::tuple<SUMOTime, MSStoppingPlace*, int> const & t1, std::tuple<SUMOTime, MSStoppingPlace*, int> const & t2) {
227 16192 : if (std::get<0>(t1) < std::get<0>(t2)) {
228 : return true;
229 : }
230 9441 : if (std::get<0>(t1) == std::get<0>(t2)) {
231 199 : if (std::get<1>(t1)->getID() < std::get<1>(t2)->getID()) {
232 : return true;
233 : }
234 0 : if (std::get<1>(t1)->getID() == std::get<1>(t2)->getID()) {
235 0 : return std::get<2>(t1) < std::get<2>(t2);
236 : }
237 : }
238 : return false;
239 : }
240 : );
241 11869 : for (auto item : blockedTimes) {
242 : MSStoppingPlace* sp = std::get<1>(item);
243 4839 : double prob = probs[std::get<2>(item)];
244 : // all stopping places are occupied. We have no good basis for
245 : // prefering one or the other based on estimated occupancy
246 4839 : double occupancy = RandHelper::rand(getStoppingPlaceCapacity(sp));
247 4839 : if (evaluateDestination(veh, brakeGap, newDestination, sp, occupancy, prob, router, stoppingPlaces, newRoutes, stopApproaches, maxValues, scores, insertStopIndex, keepCurrentStop)) {
248 : #ifdef DEBUG_STOPPINGPLACE
249 : if (DEBUGCOND) {
250 : std::cout << " altStoppingPlace=" << sp->getID() << " targeting occupied stopping place based on blockTime " << STEPS2TIME(std::get<0>(item)) << " among " << blockedTimes.size() << " alternatives\n";
251 : }
252 : #endif
253 : numAlternatives = 1;
254 : break;
255 : }
256 : //std::cout << " candidate=" << item.second->getID() << " observed=" << time2string(item.first) << "\n";
257 : }
258 11869 : if (numAlternatives == 0) {
259 : // take any random target but prefer one that hasn't been visited yet
260 : std::vector<std::pair<SUMOTime, MSStoppingPlace*>> candidates;
261 23727 : for (const StoppingPlaceVisible& stoppingPlaceCandidate : stoppingPlaceCandidates) {
262 16697 : if (stoppingPlaceCandidate.first == destStoppingPlace) {
263 : continue;
264 : }
265 12518 : SUMOTime dummy = sawBlockedStoppingPlace(veh, stoppingPlaceCandidate.first, true);
266 12518 : if (dummy < 0) {
267 : // randomize among the unvisited
268 801 : dummy = -RandHelper::rand(1000000);
269 : }
270 12518 : candidates.push_back(std::make_pair(dummy, stoppingPlaceCandidate.first));
271 : }
272 7030 : std::sort(candidates.begin(), candidates.end(),
273 : [](std::tuple<SUMOTime, MSStoppingPlace*> const & t1, std::tuple<SUMOTime, MSStoppingPlace*> const & t2) {
274 12636 : return std::get<0>(t1) < std::get<0>(t2) || (std::get<0>(t1) == std::get<0>(t2) && std::get<1>(t1)->getID() < std::get<1>(t2)->getID());
275 : }
276 : );
277 8282 : for (auto item : candidates) {
278 7427 : if (evaluateDestination(veh, brakeGap, newDestination, item.second, 0, 1, router, stoppingPlaces, newRoutes, stopApproaches, maxValues, scores, insertStopIndex, keepCurrentStop)) {
279 : #ifdef DEBUG_STOPPINGPLACE
280 : if (DEBUGCOND) {
281 : std::cout << " altStoppingPlace=" << item.second->getID() << " targeting occupied stopping place (based on pure randomness) among " << candidates.size() << " alternatives\n";
282 : }
283 : #endif
284 : numAlternatives = 1;
285 : break;
286 : }
287 : }
288 7030 : }
289 : }
290 38276 : getRouter(veh); // reset closed edges
291 :
292 : #ifdef DEBUG_STOPPINGPLACE
293 : if (DEBUGCOND) {
294 : std::cout << " maxValues=" << joinToString(maxValues, " ", ":") << "\n";
295 : }
296 : #endif
297 :
298 : // minimum cost to get the parking area
299 : double minStoppingPlaceCost = 0.0;
300 :
301 49741 : for (StoppingPlaceMap_t::iterator it = stoppingPlaces.begin(); it != stoppingPlaces.end(); ++it) {
302 : // get the parking values
303 : StoppingPlaceParamMap_t stoppingPlaceValues = it->second;
304 :
305 41173 : if (weights["probability"] > 0. && maxValues["probability"] > 0.) {
306 : // random search should not drive past a usable parking area
307 : bool dominated = false;
308 10570 : double endPos = it->first->getEndLanePosition();
309 10570 : const ConstMSEdgeVector& to1 = stopApproaches[it->first];
310 : assert(to1.size() > 0);
311 33069 : for (auto altSp : stoppingPlaces) {
312 24376 : if (altSp.first == it->first) {
313 : continue;
314 : }
315 15503 : const ConstMSEdgeVector& to2 = stopApproaches[altSp.first];
316 : assert(to2.size() > 0);
317 15503 : if (to1.size() > to2.size()) {
318 6380 : if (std::equal(to2.begin(), to2.end(), to1.begin())) {
319 : // other target lies on the route to the current candidate
320 : dominated = true;
321 : //std::cout << SIMTIME << " rrP veh=" << veh.getID() << " full=" << destParkArea->getID() << " cand=" << it->first->getID() << " onTheWay=" << altPa.first->getID() << "\n";
322 : break;
323 : }
324 9123 : } else if (to1 == to2 && endPos > altSp.first->getEndLanePosition()) {
325 : // other target is on the same edge but ahead of the current candidate
326 : dominated = true;
327 : //std::cout << SIMTIME << " rrP veh=" << veh.getID() << " full=" << destParkArea->getID() << " cand=" << it->first->getID() << " sameEdge=" << altPa.first->getID() << "\n";
328 : break;
329 : }
330 : }
331 : double prob = 0;
332 : if (!dominated) {
333 8693 : prob = RandHelper::rand(stoppingPlaceValues["probability"], veh.getRNG());
334 8693 : stoppingPlaceValues["probability"] = 1.0 - prob / maxValues["probability"];
335 : } else {
336 : // worst probability score
337 1877 : stoppingPlaceValues["probability"] = 1.0;
338 : }
339 : } else {
340 : // value takes no effect due to weight=0
341 20033 : stoppingPlaceValues["probability"] = 0;
342 : }
343 :
344 : // get the parking area cost
345 30603 : double stoppingPlaceCost = getTargetValue(stoppingPlaceValues, maxValues, weights, myNormParams, myInvertParams);
346 30603 : rememberStoppingPlaceScore(veh, it->first, toString(stoppingPlaceCost));
347 :
348 : // get the parking area with minimum cost
349 30603 : if (nearStoppingPlace == nullptr || stoppingPlaceCost < minStoppingPlaceCost) {
350 : minStoppingPlaceCost = stoppingPlaceCost;
351 22138 : nearStoppingPlace = it->first;
352 22138 : newRoute = newRoutes[nearStoppingPlace];
353 : }
354 : #ifdef DEBUG_STOPPINGPLACE
355 : if (DEBUGCOND) {
356 : std::cout << " altStoppingPlace=" << it->first->getID() << " score=" << stoppingPlaceCost << " vals=" << joinToString(stoppingPlaceValues, " ", ":") << "\n";
357 : }
358 : #endif
359 : }
360 : // expose the scores of the best solution
361 19138 : if (nearStoppingPlace != nullptr) {
362 164685 : for (auto component : stoppingPlaces[nearStoppingPlace]) {
363 146402 : scores[component.first] = component.second;
364 : }
365 : }
366 19138 : setNumberStoppingPlaceReroutes(veh, getNumberStoppingPlaceReroutes(veh) + 1);
367 19138 : } else {
368 : #ifdef DEBUG_STOPPINGPLACE
369 : if (DEBUGCOND) {
370 : std::cout << SIMTIME << " veh=" << veh.getID() << " dest=" << destStoppingPlace->getID() << " sufficient space\n";
371 : }
372 : #endif
373 : }
374 :
375 : #ifdef DEBUG_STOPPINGPLACE
376 : if (DEBUGCOND) {
377 : std::cout << " stoppingPlaceResult=" << Named::getIDSecure(nearStoppingPlace) << "\n";
378 : }
379 : #endif
380 32646 : return nearStoppingPlace;
381 : }
382 :
383 :
384 : bool
385 32237 : MSStoppingPlaceRerouter::evaluateDestination(SUMOVehicle& veh, double brakeGap, bool newDestination, MSStoppingPlace* alternative,
386 : double occupancy, double prob, SUMOAbstractRouter<MSEdge, SUMOVehicle>& router, StoppingPlaceMap_t& stoppingPlaces,
387 : std::map<MSStoppingPlace*, ConstMSEdgeVector>& newRoutes, std::map<MSStoppingPlace*, ConstMSEdgeVector>& stoppingPlaceApproaches,
388 : StoppingPlaceParamMap_t& maxValues, StoppingPlaceParamMap_t& addInput, const int insertStopIndex, const bool keepCurrentStop) {
389 :
390 : // a map stores the stopping place values
391 : StoppingPlaceParamMap_t stoppingPlaceValues;
392 32237 : const SUMOTime now = SIMSTEP;
393 :
394 32237 : const MSRoute& route = veh.getRoute();
395 32237 : const RGBColor& c = route.getColor();
396 32237 : const MSEdge* stoppingPlaceEdge = &(alternative->getLane().getEdge());
397 :
398 32237 : const bool includeInternalLengths = MSGlobals::gUsingInternalLanes && MSNet::getInstance()->hasInternalLinks();
399 :
400 : // Compute the route from the current edge to the stopping place edge
401 : ConstMSEdgeVector edgesToStop;
402 : ConstMSEdgeVector edgesUpstream;
403 32237 : const double targetPos = alternative->getLastFreePos(veh);
404 32237 : MSRouteIterator rerouteOriginIt = determineRerouteOrigin(veh, insertStopIndex);
405 32237 : double posOnLane = veh.getPositionOnLane();
406 32237 : if (insertStopIndex > 0) {
407 : posOnLane = 0.;
408 : // determine preceding edges
409 0 : for (MSRouteIterator it = veh.getCurrentRouteEdge(); it != rerouteOriginIt; ++it) {
410 : if (it != rerouteOriginIt) {
411 0 : edgesUpstream.push_back(*it);
412 : }
413 : }
414 : }
415 32237 : const MSEdge* rerouteOrigin = *rerouteOriginIt;
416 32237 : router.compute(rerouteOrigin, posOnLane, stoppingPlaceEdge, targetPos, &veh, now, edgesToStop, true);
417 32237 : if (edgesToStop.size() > 0) {
418 : // Compute the route from the stopping place edge to the end of the route
419 31760 : if (insertStopIndex == 0 && rerouteOrigin != veh.getEdge()) {
420 5 : edgesToStop.insert(edgesToStop.begin(), veh.getEdge());
421 : }
422 : // prepend preceding edges
423 : std::reverse(edgesUpstream.begin(), edgesUpstream.end());
424 31760 : for (auto edge : edgesUpstream) {
425 0 : edgesToStop.insert(edgesToStop.begin(), edge);
426 : }
427 : ConstMSEdgeVector edgesFromStop;
428 31760 : stoppingPlaceApproaches[alternative] = edgesToStop;
429 :
430 31760 : const MSEdge* nextDestination = route.getLastEdge();
431 31760 : double nextPos = veh.getArrivalPos();
432 31760 : int nextDestinationIndex = route.size() - 1;
433 31760 : if (!newDestination) {
434 30390 : std::vector<std::pair<int, double> > stopIndices = veh.getStopIndices();
435 30390 : int nextDestStopIndex = 1 + insertStopIndex;
436 30390 : if (!keepCurrentStop) {
437 0 : nextDestStopIndex++;
438 : }
439 30390 : if ((int)stopIndices.size() > nextDestStopIndex) {
440 304 : nextDestinationIndex = stopIndices[nextDestStopIndex].first;
441 304 : nextDestination = route.getEdges()[nextDestinationIndex];
442 304 : nextPos = stopIndices[nextDestStopIndex].second;
443 : }
444 30390 : router.compute(stoppingPlaceEdge, targetPos, nextDestination, nextPos, &veh, now, edgesFromStop, true);
445 30390 : }
446 31760 : if (edgesFromStop.size() > 0 || newDestination) {
447 31760 : stoppingPlaceValues["probability"] = prob;
448 31760 : if (stoppingPlaceValues["probability"] > maxValues["probability"]) {
449 18547 : maxValues["probability"] = stoppingPlaceValues["probability"];
450 : }
451 31760 : stoppingPlaceValues["capacity"] = getStoppingPlaceCapacity(alternative);
452 31760 : stoppingPlaceValues["absfreespace"] = stoppingPlaceValues["capacity"] - occupancy;
453 : // if capacity = 0 then absfreespace and relfreespace are also 0
454 63424 : stoppingPlaceValues["relfreespace"] = stoppingPlaceValues["absfreespace"] / MAX2(1.0, stoppingPlaceValues["capacity"]);
455 31760 : MSRoute routeToPark(route.getID() + "!to" + myParamPrefix + "#1", edgesToStop, false,
456 63520 : &c == &RGBColor::DEFAULT_COLOR ? nullptr : new RGBColor(c), route.getStops());
457 :
458 : // The distance from the current edge to the new parking area
459 31760 : double toPos = alternative->getBeginLanePosition();
460 31760 : if (&alternative->getLane().getEdge() == veh.getEdge()) {
461 7315 : toPos = MAX2(veh.getPositionOnLane(), toPos);
462 : }
463 31760 : stoppingPlaceValues["distanceto"] = routeToPark.getDistanceBetween(veh.getPositionOnLane(), toPos,
464 31760 : routeToPark.begin(), routeToPark.end() - 1, includeInternalLengths);
465 :
466 31760 : if (stoppingPlaceValues["distanceto"] == std::numeric_limits<double>::max()) {
467 0 : WRITE_WARNINGF(TL("Invalid distance computation for vehicle '%' to stopping place '%' at time=%."),
468 : veh.getID(), alternative->getID(), time2string(now));
469 : }
470 31760 : const double endPos = getStoppingPlaceOccupancy(alternative) == getStoppingPlaceCapacity(alternative)
471 31760 : ? alternative->getLastFreePos(veh, veh.getPositionOnLane() + brakeGap)
472 10259 : : alternative->getEndLanePosition();
473 31760 : const double distToEnd = stoppingPlaceValues["distanceto"] - toPos + endPos;
474 :
475 31760 : if (distToEnd < brakeGap) {
476 1000 : rememberStoppingPlaceScore(veh, alternative, "tooClose");
477 1000 : return false;
478 : }
479 :
480 : // The time to reach the new stopping place
481 30760 : const double correctionLastEdge = ((alternative->getLane().getLength() - alternative->getEndLanePosition()) / alternative->getLane().getVehicleMaxSpeed(&veh));
482 30760 : const double correctionFirstEdge = veh.getPositionOnLane() / edgesToStop.front()->getVehicleMaxSpeed(&veh);
483 :
484 30760 : stoppingPlaceValues["timeto"] = router.recomputeCosts(edgesToStop, &veh, SIMSTEP) - correctionLastEdge - correctionFirstEdge;
485 30760 : ConstMSEdgeVector newEdges = edgesToStop;
486 30760 : if (newDestination) {
487 1370 : stoppingPlaceValues["distancefrom"] = 0;
488 1370 : stoppingPlaceValues["timefrom"] = 0;
489 : } else {
490 29390 : MSRoute routeFromPark(route.getID() + "!from" + myParamPrefix + "#1", edgesFromStop, false,
491 58780 : &c == &RGBColor::DEFAULT_COLOR ? nullptr : new RGBColor(c), route.getStops());
492 : // The distance from the new parking area to the end of the route
493 29390 : stoppingPlaceValues["distancefrom"] = routeFromPark.getDistanceBetween(alternative->getBeginLanePosition(), routeFromPark.getLastEdge()->getLength(),
494 29390 : routeFromPark.begin(), routeFromPark.end() - 1, includeInternalLengths);
495 29390 : if (stoppingPlaceValues["distancefrom"] == std::numeric_limits<double>::max()) {
496 0 : WRITE_WARNINGF(TL("Invalid distance computation for vehicle '%' from stopping place '%' at time=%."),
497 : veh.getID(), alternative->getID(), time2string(SIMSTEP));
498 : }
499 : // The time to reach this area
500 29390 : stoppingPlaceValues["timefrom"] = router.recomputeCosts(edgesFromStop, &veh, SIMSTEP) - (alternative->getEndLanePosition() / alternative->getLane().getSpeedLimit());
501 29390 : newEdges.insert(newEdges.end(), edgesFromStop.begin() + 1, edgesFromStop.end());
502 29390 : newEdges.insert(newEdges.end(), route.begin() + nextDestinationIndex + 1, route.end());
503 29390 : }
504 :
505 : // add some additional/custom target function components
506 30760 : if (!evaluateCustomComponents(veh, brakeGap, newDestination, alternative, occupancy, prob, router, stoppingPlaceValues, stoppingPlaceApproaches[alternative], newEdges, maxValues, addInput)) {
507 : return false;
508 : }
509 30760 : if (!myCheckValidity || validComponentValues(stoppingPlaceValues)) {
510 30623 : updateMaxValues(stoppingPlaceValues, maxValues);
511 30623 : stoppingPlaces[alternative] = stoppingPlaceValues;
512 30623 : newRoutes[alternative] = newEdges;
513 : return true;
514 : } else {
515 : return false;
516 : }
517 31760 : } else {
518 0 : rememberStoppingPlaceScore(veh, alternative, "unreachable");
519 : }
520 31760 : } else {
521 954 : rememberStoppingPlaceScore(veh, alternative, "unreachable");
522 : }
523 : // unreachable
524 : return false;
525 32237 : }
526 :
527 :
528 : bool
529 30346 : MSStoppingPlaceRerouter::evaluateCustomComponents(SUMOVehicle& /*veh*/, double /*brakeGap*/, bool /*newDestination*/,
530 : MSStoppingPlace* /*alternative*/, double /*occupancy*/, double /*prob*/, SUMOAbstractRouter<MSEdge, SUMOVehicle>& /*router*/,
531 : StoppingPlaceParamMap_t& /*stoppingPlaceValues*/, ConstMSEdgeVector& /*newRoute*/, ConstMSEdgeVector& /*stoppingPlaceApproach*/,
532 : StoppingPlaceParamMap_t& /*maxValues*/, StoppingPlaceParamMap_t& /*addInput*/) {
533 30346 : return true;
534 : }
535 :
536 :
537 : bool
538 0 : MSStoppingPlaceRerouter::validComponentValues(StoppingPlaceParamMap_t& /* stoppingPlaceValues */) {
539 0 : return true;
540 : }
541 :
542 :
543 : bool
544 69767 : MSStoppingPlaceRerouter::useStoppingPlace(MSStoppingPlace* /* stoppingPlace */) {
545 69767 : return true;
546 : }
547 :
548 :
549 : SUMOAbstractRouter<MSEdge, SUMOVehicle>&
550 38100 : MSStoppingPlaceRerouter::getRouter(SUMOVehicle& veh, const Prohibitions& prohibited) {
551 38100 : return MSNet::getInstance()->getRouterTT(veh.getRNGIndex(), prohibited);
552 : }
553 :
554 :
555 : MSStoppingPlaceRerouter::StoppingPlaceParamMap_t
556 19158 : MSStoppingPlaceRerouter::collectWeights(SUMOVehicle& veh) {
557 : MSStoppingPlaceRerouter::StoppingPlaceParamMap_t result;
558 19158 : myEvalParams["distanceto"] = getWeight(veh, "distance.weight", myEvalParams["distanceto"]);
559 172618 : for (auto evalParam : myEvalParams) {
560 153460 : result[evalParam.first] = getWeight(veh, evalParam.first + ".weight", evalParam.second);
561 : }
562 19158 : result["probability"] = getWeight(veh, "probability.weight", 0.);
563 19158 : return result;
564 : }
565 :
566 :
567 : double
568 432316 : MSStoppingPlaceRerouter::getWeight(SUMOVehicle& veh, const std::string param, const double defaultWeight, const bool warn) {
569 : // get custom vehicle parameter
570 432316 : const std::string key = myParamPrefix + "." + param;
571 432316 : if (veh.getParameter().hasParameter(key)) {
572 : try {
573 76 : return StringUtils::toDouble(veh.getParameter().getParameter(key, "-1"));
574 0 : } catch (...) {
575 0 : WRITE_WARNINGF(TL("Invalid value '%' for vehicle parameter '%'"), veh.getParameter().getParameter(key, "-1"), key);
576 0 : }
577 : } else {
578 : // get custom vType parameter
579 432278 : if (veh.getVehicleType().getParameter().hasParameter(key)) {
580 : try {
581 32296 : return StringUtils::toDouble(veh.getVehicleType().getParameter().getParameter(key, "-1"));
582 0 : } catch (...) {
583 0 : WRITE_WARNINGF(TL("Invalid value '%' for vType parameter '%'"), veh.getVehicleType().getParameter().getParameter(key, "-1"), key);
584 0 : }
585 : }
586 : }
587 416130 : if (warn) {
588 0 : WRITE_MESSAGEF("Vehicle '%' does not supply vehicle parameter '%'. Using default of %\n", veh.getID(), key, toString(defaultWeight));
589 : }
590 416130 : return defaultWeight;
591 : }
592 :
593 :
594 : void
595 30623 : MSStoppingPlaceRerouter::updateMaxValues(StoppingPlaceParamMap_t& stoppingPlaceValues, StoppingPlaceParamMap_t& maxValues) {
596 276161 : for (auto it = maxValues.begin(); it != maxValues.end(); ++it) {
597 245538 : if (stoppingPlaceValues[it->first] > it->second) {
598 158101 : it->second = stoppingPlaceValues[it->first];
599 : }
600 : }
601 30623 : }
602 :
603 :
604 : double
605 30603 : MSStoppingPlaceRerouter::getTargetValue(const StoppingPlaceParamMap_t& absValues, const StoppingPlaceParamMap_t& maxValues, const StoppingPlaceParamMap_t& weights, const StoppingPlaceParamSwitchMap_t& norm, const StoppingPlaceParamSwitchMap_t& invert) {
606 : double cost = 0.;
607 275981 : for (StoppingPlaceParamMap_t::const_iterator sc = absValues.begin(); sc != absValues.end(); ++sc) {
608 245378 : double weight = weights.at(sc->first);
609 245378 : double val = sc->second;
610 245378 : if (norm.at(sc->first) && maxValues.at(sc->first) > 0.) {
611 211584 : val /= maxValues.at(sc->first);
612 : }
613 245378 : cost += (invert.at(sc->first)) ? weight * (1. - val) : weight * val;
614 : }
615 30603 : return cost;
616 : }
617 :
618 :
619 : /****************************************************************************/
|
