Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2013-2026 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 MSLCHelper.cpp
15 : /// @author Jakob Erdmann
16 : /// @date Fri, 19.06.2020
17 : ///
18 : // Common functions for lane change models
19 : /****************************************************************************/
20 :
21 : #include <microsim/MSEdge.h>
22 : #include <microsim/MSLane.h>
23 : #include <microsim/MSLink.h>
24 : #include <microsim/MSVehicle.h>
25 : #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
26 : #include "MSLCHelper.h"
27 :
28 : // ===========================================================================
29 : // Debug flags
30 : // ===========================================================================
31 : //#define DEBUG_WANTS_CHANGE
32 : //#define DEBUG_SAVE_BLOCKER_LENGTH
33 : //#define DEBUG_UNWILLING_TO_HELP
34 :
35 : #define DEBUG_COND (veh.isSelected())
36 : //#define DEBUG_COND (true)
37 :
38 :
39 : // ===========================================================================
40 : // member method definitions
41 : // ===========================================================================
42 :
43 : double
44 363677154 : MSLCHelper::getRoundaboutDistBonus(const MSVehicle& veh,
45 : double bonusParam,
46 : const MSVehicle::LaneQ& curr,
47 : const MSVehicle::LaneQ& neigh,
48 : const MSVehicle::LaneQ& best) {
49 363677154 : if (veh.getLaneChangeModel().isOpposite()) {
50 : return 0;
51 : }
52 363291189 : const MSVehicle::LaneQ& inner = neigh.lane->getIndex() > curr.lane->getIndex() ? neigh : curr;
53 : #ifdef DEBUG_WANTS_CHANGE
54 : const bool debugVehicle = veh.getLaneChangeModel().debugVehicle();
55 : if (debugVehicle) {
56 : std::cout << SIMTIME << " veh=" << veh.getID() << " getRoundaboutDistBonus bonusParam=" << bonusParam
57 : << " curr=" << curr.lane->getID()
58 : << " neigh=" << neigh.lane->getID()
59 : << " inner=" << inner.lane->getID()
60 : << " best=" << best.lane->getID()
61 : << "\n innerCont=" << toString(inner.bestContinuations)
62 : << "\n bestCont=" << toString(best.bestContinuations)
63 : << "\n";
64 : }
65 : #endif
66 363291189 : if (neigh.lane == inner.lane && curr.bestContinuations.size() < neigh.bestContinuations.size()) {
67 : // the current lane does not continue to the roundabout and we need a strategic change first.
68 : return 0;
69 : }
70 :
71 : int roundaboutJunctionsAhead = 0;
72 : bool enteredRoundabout = false;
73 359922176 : double seen = -veh.getPositionOnLane();
74 :
75 : // first check using only normal lanes
76 943856541 : for (int i = 0; i < (int)best.bestContinuations.size(); i++) {
77 726066031 : const MSLane* lane = best.bestContinuations[i];
78 726066031 : if (lane == nullptr) {
79 3546227 : lane = veh.getLane();
80 : }
81 726066031 : if ((!enteredRoundabout || lane->getEdge().isRoundabout()) && i >= (int)inner.bestContinuations.size()) {
82 : // no bonus if we cannot continue on the inner lane until leaving the roundabout
83 : #ifdef DEBUG_WANTS_CHANGE
84 : if (debugVehicle) {
85 : std::cout << " noBonus: inner does not continue (lane=" << lane->getID() << ")\n";
86 : }
87 : #endif
88 : return 0;
89 : }
90 705014362 : if (seen > 300) {
91 : // avoid long look-ahead
92 : #ifdef DEBUG_WANTS_CHANGE
93 : if (debugVehicle) {
94 : std::cout << " noBonus: seen=" << seen << " (lane=" << lane->getID() << ")\n";
95 : }
96 : #endif
97 : return 0;
98 : }
99 : const MSJunction* junction = lane->getEdge().getToJunction();
100 609839865 : if (lane->getEdge().isRoundabout()) {
101 : enteredRoundabout = true;
102 54800611 : if (junction->getIncoming().size() + junction->getOutgoing().size() > 2) {
103 54604116 : roundaboutJunctionsAhead++;
104 : }
105 555039254 : } else if (enteredRoundabout) {
106 : // only check the first roundabout
107 : break;
108 : }
109 583934365 : seen += lane->getLength();
110 : }
111 : // no bonus if we want to take the next exit
112 243696010 : if (roundaboutJunctionsAhead < 2) {
113 : #ifdef DEBUG_WANTS_CHANGE
114 : if (debugVehicle) {
115 : std::cout << " noBonus: roundaboutJunctionsAhead=" << roundaboutJunctionsAhead << "\n";
116 : }
117 : #endif
118 : return 0;
119 : }
120 : // compute bonus value based on jamming and exact distances (taking into
121 : // account internal lanes)
122 : double occupancyOuter = 0;
123 : double occupancyInner = 0;
124 : double distanceInRoundabout = 0;
125 : MSLane* prevNormal = nullptr;
126 : MSLane* prevInner = nullptr;
127 : enteredRoundabout = false;
128 74039471 : for (int i = 0; i < (int)best.bestContinuations.size(); i++) {
129 73914037 : MSLane* lane = best.bestContinuations[i];
130 73914037 : if (lane == nullptr) {
131 688217 : continue;
132 : }
133 73225820 : if (lane->getEdge().isRoundabout()) {
134 : enteredRoundabout = true;
135 31916430 : } else if (enteredRoundabout) {
136 : // only check the first roundabout
137 : break;
138 : }
139 : MSLane* via = nullptr;
140 56745940 : if (prevNormal != nullptr) {
141 123504195 : for (MSLink* link : prevNormal->getLinkCont()) {
142 83363569 : if (link->getLane() == lane) {
143 : via = link->getViaLane();
144 : }
145 : }
146 : }
147 56745940 : if (enteredRoundabout) {
148 41309390 : distanceInRoundabout += lane->getLength();
149 41309390 : if (via != nullptr) {
150 39655494 : distanceInRoundabout += via->getLength();
151 : }
152 : }
153 : // discount vehicles that are upstream from ego
154 56745940 : const double upstreamDiscount = &lane->getEdge() == &veh.getLane()->getEdge()
155 56745940 : ? (lane->getLength() - veh.getPositionOnLane()) / lane->getLength() : 1;
156 : prevNormal = lane;
157 56745940 : occupancyOuter += upstreamDiscount * lane->getBruttoVehLenSum();
158 : #ifdef DEBUG_WANTS_CHANGE
159 : if (debugVehicle) {
160 : std::cout << " lane=" << lane->getID() << " occ=" << lane->getBruttoVehLenSum() << " discount=" << upstreamDiscount << " outer=" << occupancyOuter << "\n";
161 : }
162 : #endif
163 56745940 : if (via != nullptr) {
164 40006409 : occupancyOuter += via->getBruttoVehLenSum();
165 : #ifdef DEBUG_WANTS_CHANGE
166 : if (debugVehicle) {
167 : std::cout << " via=" << via->getID() << " occ=" << via->getBruttoVehLenSum() << " outer=" << occupancyOuter << "\n";
168 : }
169 : #endif
170 : }
171 56745940 : if (i < (int)inner.bestContinuations.size()) {
172 56745940 : MSLane* innerLane = inner.bestContinuations[i];
173 56745940 : occupancyInner += upstreamDiscount * innerLane->getBruttoVehLenSum();
174 : #ifdef DEBUG_WANTS_CHANGE
175 : if (debugVehicle) {
176 : std::cout << " inner=" << innerLane->getID() << " occ=" << innerLane->getBruttoVehLenSum() << " discount=" << upstreamDiscount << " inner=" << occupancyInner << "\n";
177 : }
178 : #endif
179 56745940 : if (prevInner != nullptr) {
180 88418646 : for (MSLink* link : prevInner->getLinkCont()) {
181 48278020 : if (link->getLane() == innerLane && link->getViaLane() != nullptr) {
182 40006409 : occupancyInner += link->getViaLane()->getBruttoVehLenSum();
183 : #ifdef DEBUG_WANTS_CHANGE
184 : if (debugVehicle) {
185 : std::cout << " innerVia=" << link->getViaLane()->getID() << " occ=" << link->getViaLane()->getBruttoVehLenSum() << " inner=" << occupancyInner << "\n";
186 : }
187 : #endif
188 : }
189 : }
190 : }
191 : prevInner = innerLane;
192 : }
193 : }
194 :
195 : #ifdef DEBUG_WANTS_CHANGE
196 : if (debugVehicle) {
197 : std::cout << " distanceInRoundabout=" << distanceInRoundabout
198 : << " roundaboutJunctionsAhead=" << roundaboutJunctionsAhead
199 : << " occupancyInner=" << occupancyInner
200 : << " occupancyOuter=" << occupancyOuter
201 : << "\n";
202 : }
203 : #endif
204 16605314 : if (abs(curr.bestLaneOffset) > 1 && enteredRoundabout) {
205 134 : const double bGap = veh.getCarFollowModel().brakeGap(veh.getSpeed() + ACCEL2SPEED(veh.getCarFollowModel().getMaxAccel()), veh.getCarFollowModel().getMaxDecel(), veh.getActionStepLengthSecs());
206 134 : const double reservation = veh.getLaneChangeModel().getExtraReservation(curr.bestLaneOffset);
207 134 : const double leftSpace = distanceInRoundabout - reservation - veh.getPositionOnLane();
208 : #ifdef DEBUG_WANTS_CHANGE
209 : if (debugVehicle) {
210 : std::cout << " bGap=" << bGap << " reserving=" << reservation << " leftover=" << (leftSpace - bGap) << "\n";
211 : }
212 : #endif
213 134 : if (bGap > leftSpace) {
214 : return 0;
215 : }
216 : }
217 :
218 : const double maxOccupancy = MAX2(occupancyInner, occupancyOuter);
219 : // give some bonus for using the inside lane at equal occupancy
220 16605308 : const double bonus = roundaboutJunctionsAhead * 7.5;
221 16605308 : const double relativeJam = (occupancyOuter - occupancyInner + bonus) / (maxOccupancy + bonus);
222 : // no bonus if the inner lane or the left lane entering the roundabout is jammed
223 : double jamFactor = MAX2(0.0, relativeJam);
224 16605308 : if (veh.getLane()->getEdge().isRoundabout() && curr.lane->getIndex() > neigh.lane->getIndex()) {
225 : // only use jamFactor when deciding to move to the inside lane but prefer
226 : // staying inside if the distance allows it
227 : jamFactor = 1;
228 : }
229 16605308 : const double result = distanceInRoundabout * jamFactor * bonusParam * 9; // the 9 is abitrary and only there for backward compatibility
230 : #ifdef DEBUG_WANTS_CHANGE
231 : if (debugVehicle) {
232 : std::cout << " relativeJam=" << relativeJam
233 : << " jamFactor=" << jamFactor
234 : << " distanceBonus=" << result
235 : << "\n";
236 : }
237 : #endif
238 16605308 : return result;
239 : }
240 :
241 :
242 : bool
243 11775993 : MSLCHelper::updateBlockerLength(const MSVehicle& veh, MSVehicle* blocker, int lcaCounter, double leftSpace, bool reliefConnection, double& leadingBlockerLength) {
244 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
245 : if (DEBUG_COND) {
246 : std::cout << SIMTIME
247 : << " veh=" << veh.getID()
248 : << " saveBlockerLength blocker=" << Named::getIDSecure(blocker)
249 : << " bState=" << (blocker == 0 ? "None" : toString((LaneChangeAction)blocker->getLaneChangeModel().getOwnState()))
250 : << "\n";
251 : }
252 : #endif
253 11775993 : if (blocker != nullptr && (blocker->getLaneChangeModel().getOwnState() & lcaCounter) != 0) {
254 : // is there enough space in front of us for the blocker?
255 521889 : const double required = blocker->getVehicleType().getLengthWithGap() + veh.getVehicleType().getMinGap();
256 521889 : const double potential = leftSpace - veh.getCarFollowModel().brakeGap(
257 521889 : veh.getSpeed(), veh.getCarFollowModel().getMaxDecel(), 0);
258 521889 : if (required <= potential) {
259 : // save at least his length in myLeadingBlockerLength
260 492740 : leadingBlockerLength = MAX2(required, leadingBlockerLength);
261 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
262 : if (DEBUG_COND) {
263 : std::cout << SIMTIME
264 : << " veh=" << veh.getID()
265 : << " required=" << required
266 : << " potential=" << potential
267 : << " blocker=" << Named::getIDSecure(blocker)
268 : << " saving myLeadingBlockerLength=" << leadingBlockerLength
269 : << "\n";
270 : }
271 : #endif
272 : } else {
273 : // we cannot save enough space for the blocker. It needs to save
274 : // space for ego instead
275 81479 : const double required2 = veh.getVehicleType().getLengthWithGap() + blocker->getVehicleType().getMinGap();
276 81479 : const double foeLeftSpace = leftSpace - veh.getPositionOnLane() + blocker->getPositionOnLane() - POSITION_EPS;
277 81479 : const bool canReserve = blocker->getLaneChangeModel().saveBlockerLength(required2, foeLeftSpace);
278 : //reliefConnection ? std::numeric_limits<double>::max() : leftSpace);
279 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
280 : if (DEBUG_COND) {
281 : std::cout << SIMTIME
282 : << " veh=" << veh.getID()
283 : << " required=" << required
284 : << " potential=" << potential
285 : << " blocker=" << Named::getIDSecure(blocker)
286 : << " required2=" << required2
287 : << " foeCanReserve=" << canReserve
288 : << " myReserved=" << leadingBlockerLength
289 : << " reliefConnection=" << reliefConnection
290 : << "\n";
291 : }
292 : #endif
293 81479 : if (!canReserve && !reliefConnection) {
294 41905 : const int blockerState = blocker->getLaneChangeModel().getOwnState();
295 41905 : if ((blockerState & LCA_STRATEGIC) != 0
296 : && (blockerState & LCA_URGENT) != 0) {
297 : // reserve anyway and try to avoid deadlock with emergency deceleration
298 38660 : leadingBlockerLength = MAX2(required, leadingBlockerLength);
299 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
300 : if (DEBUG_COND) {
301 : std::cout << " reserving anyway to avoid deadlock (will cause emergency braking)\n";
302 : }
303 : #endif
304 : }
305 : }
306 81479 : return canReserve;
307 : }
308 : }
309 : return true;
310 : }
311 :
312 :
313 : bool
314 208867 : MSLCHelper::canSaveBlockerLength(const MSVehicle& veh, double requested, double leftSpace) {
315 208867 : const double potential = leftSpace - veh.getCarFollowModel().brakeGap(veh.getSpeed(), veh.getCarFollowModel().getMaxDecel(), veh.getActionStepLengthSecs());
316 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
317 : if (DEBUG_COND) {
318 : std::cout << SIMTIME << " canSaveBlockerLength veh=" << veh.getID() << " requested=" << requested << " leftSpace=" << leftSpace << " potential=" << potential << "\n";
319 : }
320 : #endif
321 208867 : return potential >= requested;
322 : }
323 :
324 :
325 : bool
326 15579221 : MSLCHelper::divergentRoute(const MSVehicle& v1, const MSVehicle& v2) {
327 : // a sufficient, but not necessary condition for divergence
328 15812384 : return (v1.getLane()->isInternal() && v2.getLane()->isInternal()
329 157406 : && v1.getLane()->getEdge().getFromJunction() == v2.getLane()->getEdge().getFromJunction()
330 15725214 : && &v1.getLane()->getEdge() != &v2.getLane()->getEdge());
331 : }
332 :
333 :
334 : double
335 2196430 : MSLCHelper::getSpeedPreservingSecureGap(const MSVehicle& leader, const MSVehicle& follower, double currentGap, double leaderPlannedSpeed) {
336 : // whatever speed the follower choses in the next step, it will change both
337 : // the secureGap and the required followSpeed.
338 : // Let's assume the leader maintains speed
339 2196430 : const double nextGap = currentGap + SPEED2DIST(leaderPlannedSpeed - follower.getSpeed());
340 2196430 : double sGap = follower.getCarFollowModel().getSecureGap(&follower, &leader, follower.getSpeed(), leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
341 2196430 : if (nextGap >= sGap) {
342 : // follower may still accelerate
343 1963165 : const double nextGapMin = currentGap + SPEED2DIST(leaderPlannedSpeed - follower.getCarFollowModel().maxNextSpeed(follower.getSpeed(), &follower));
344 1963165 : const double vSafe = follower.getCarFollowModel().followSpeed(
345 1963165 : &follower, follower.getSpeed(), nextGapMin, leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
346 1963165 : return MAX2(vSafe, follower.getSpeed());
347 : } else {
348 : // follower must brake. The following brakes conservatively since the actual gap will be lower due to braking.
349 233265 : const double vSafe = follower.getCarFollowModel().followSpeed(
350 233265 : &follower, follower.getSpeed(), nextGap, leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
351 : // avoid emergency deceleration
352 233265 : return MAX2(vSafe, follower.getCarFollowModel().minNextSpeed(follower.getSpeed(), &follower));
353 : }
354 : }
355 :
356 :
357 : bool
358 134673506 : MSLCHelper::isBidiLeader(const MSVehicle* leader, const std::vector<MSLane*>& cont) {
359 134673506 : if (leader == nullptr) {
360 : return false;
361 : }
362 126812138 : const MSLane* lane1 = leader->getLane()->getNormalSuccessorLane()->getBidiLane();
363 126812138 : const MSLane* lane2 = leader->getLane()->getNormalPredecessorLane()->getBidiLane();
364 126812138 : if (lane1 == nullptr && lane2 == nullptr) {
365 : return false;
366 : }
367 2361450 : bool result = std::find(cont.begin(), cont.end(), lane1) != cont.end();
368 2361450 : if (!result && lane1 != lane2 && lane2 != nullptr) {
369 528284 : result = std::find(cont.begin(), cont.end(), lane2) != cont.end();
370 : }
371 : return result;
372 : }
373 :
374 :
375 : bool
376 48371 : MSLCHelper::isBidiFollower(const MSVehicle* ego, const MSVehicle* follower) {
377 48371 : if (follower == nullptr) {
378 : return false;
379 : }
380 : bool result = false;
381 37333 : const MSLane* lane1 = follower->getLane()->getNormalSuccessorLane()->getBidiLane();
382 37333 : const MSLane* lane2 = follower->getLane()->getNormalPredecessorLane()->getBidiLane();
383 37333 : const ConstMSEdgeVector& route = ego->getRoute().getEdges();
384 37333 : if (lane1 != nullptr) {
385 36646 : result = std::find(route.begin(), route.end(), &lane1->getEdge()) != route.end();
386 : }
387 37333 : if (!result && lane1 != lane2 && lane2 != nullptr) {
388 4189 : result = std::find(route.begin(), route.end(), &lane2->getEdge()) != route.end();
389 : }
390 : return result;
391 : }
392 :
393 :
394 : bool
395 8497963 : MSLCHelper::unwillingToHelp(const MSVehicle& ego, double plannedSpeed, const MSVehicle& nv) {
396 8497963 : if (nv.getLaneChangeModel().getCooperativeHelpTime() < 0 || ego.getWaitingTime() < nv.getLaneChangeModel().getCooperativeHelpTime()) {
397 : // ego vehicle has not been waiting long enough to be eligible for unconditional help
398 7201623 : if (nv.getLaneChangeModel().getCooperativeHelpThreshold() >= 0 && (nv.getSpeed() - plannedSpeed) > nv.getLaneChangeModel().getCooperativeHelpThreshold()) {
399 : // neighhbor not willing to help because ego is much slower
400 : #ifdef DEBUG_UNWILLING_TO_HELP
401 : if (DEBUG_COND) {
402 : std::cout << "\n nv=" << nv.getID() << " not willing to help because ego is much slower\n";
403 : }
404 : #endif
405 : return true;
406 : }
407 7000626 : const double nvLaneMax = nv.getLane()->getVehicleMaxSpeed(&nv);
408 7000626 : if (nv.getSpeed() / nvLaneMax < nv.getLaneChangeModel().getCooperativeMinSpeed()) {
409 : // neighbor not willing to help because it is already to slow and does not want to disturb the flow
410 : #ifdef DEBUG_UNWILLING_TO_HELP
411 : if (DEBUG_COND) {
412 : std::cout << "\n nv=" << nv.getID() << " not willing to help because it is already too slow\n";
413 : }
414 : #endif
415 : return true;
416 : }
417 : }
418 : return false;
419 : }
420 : /****************************************************************************/
|