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 359866071 : MSLCHelper::getRoundaboutDistBonus(const MSVehicle& veh,
45 : double bonusParam,
46 : const MSVehicle::LaneQ& curr,
47 : const MSVehicle::LaneQ& neigh,
48 : const MSVehicle::LaneQ& best) {
49 359866071 : if (veh.getLaneChangeModel().isOpposite()) {
50 : return 0;
51 : }
52 359481934 : 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 359481934 : 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 356176241 : double seen = -veh.getPositionOnLane();
74 :
75 : // first check using only normal lanes
76 935026106 : for (int i = 0; i < (int)best.bestContinuations.size(); i++) {
77 719570865 : const MSLane* lane = best.bestContinuations[i];
78 719570865 : if (lane == nullptr) {
79 3539846 : lane = veh.getLane();
80 : }
81 719570865 : 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 699448406 : 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 604825533 : if (lane->getEdge().isRoundabout()) {
101 : enteredRoundabout = true;
102 54956674 : if (junction->getIncoming().size() + junction->getOutgoing().size() > 2) {
103 54760179 : roundaboutJunctionsAhead++;
104 : }
105 549868859 : } else if (enteredRoundabout) {
106 : // only check the first roundabout
107 : break;
108 : }
109 578849865 : seen += lane->getLength();
110 : }
111 : // no bonus if we want to take the next exit
112 241430909 : 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 74210557 : for (int i = 0; i < (int)best.bestContinuations.size(); i++) {
129 74085123 : MSLane* lane = best.bestContinuations[i];
130 74085123 : if (lane == nullptr) {
131 692620 : continue;
132 : }
133 73392503 : if (lane->getEdge().isRoundabout()) {
134 : enteredRoundabout = true;
135 31985634 : } else if (enteredRoundabout) {
136 : // only check the first roundabout
137 : break;
138 : }
139 : MSLane* via = nullptr;
140 56870047 : if (prevNormal != nullptr) {
141 123852577 : for (MSLink* link : prevNormal->getLinkCont()) {
142 83630420 : if (link->getLane() == lane) {
143 : via = link->getViaLane();
144 : }
145 : }
146 : }
147 56870047 : if (enteredRoundabout) {
148 41406869 : distanceInRoundabout += lane->getLength();
149 41406869 : if (via != nullptr) {
150 39748425 : distanceInRoundabout += via->getLength();
151 : }
152 : }
153 : // discount vehicles that are upstream from ego
154 56870047 : const double upstreamDiscount = &lane->getEdge() == &veh.getLane()->getEdge()
155 56870047 : ? (lane->getLength() - veh.getPositionOnLane()) / lane->getLength() : 1;
156 : prevNormal = lane;
157 56870047 : 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 56870047 : if (via != nullptr) {
164 40087940 : 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 56870047 : if (i < (int)inner.bestContinuations.size()) {
172 56870047 : MSLane* innerLane = inner.bestContinuations[i];
173 56870047 : 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 56870047 : if (prevInner != nullptr) {
180 88494942 : for (MSLink* link : prevInner->getLinkCont()) {
181 48272785 : if (link->getLane() == innerLane && link->getViaLane() != nullptr) {
182 40087940 : 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 16647890 : 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 16647884 : const double bonus = roundaboutJunctionsAhead * 7.5;
221 16647884 : 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 16647884 : 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 16647884 : 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 16647884 : return result;
239 : }
240 :
241 :
242 : bool
243 11702733 : 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 11702733 : if (blocker != nullptr && (blocker->getLaneChangeModel().getOwnState() & lcaCounter) != 0) {
254 : // is there enough space in front of us for the blocker?
255 519972 : const double required = blocker->getVehicleType().getLengthWithGap() + veh.getVehicleType().getMinGap();
256 519972 : const double potential = leftSpace - veh.getCarFollowModel().brakeGap(
257 519972 : veh.getSpeed(), veh.getCarFollowModel().getMaxDecel(), 0);
258 519972 : if (required <= potential) {
259 : // save at least his length in myLeadingBlockerLength
260 491175 : 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 81342 : const double required2 = veh.getVehicleType().getLengthWithGap() + blocker->getVehicleType().getMinGap();
276 81342 : const double foeLeftSpace = leftSpace - veh.getPositionOnLane() + blocker->getPositionOnLane() - POSITION_EPS;
277 81342 : 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 81342 : if (!canReserve && !reliefConnection) {
294 41754 : const int blockerState = blocker->getLaneChangeModel().getOwnState();
295 41754 : if ((blockerState & LCA_STRATEGIC) != 0
296 : && (blockerState & LCA_URGENT) != 0) {
297 : // reserve anyway and try to avoid deadlock with emergency deceleration
298 38611 : 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 81342 : return canReserve;
307 : }
308 : }
309 : return true;
310 : }
311 :
312 :
313 : bool
314 208729 : MSLCHelper::canSaveBlockerLength(const MSVehicle& veh, double requested, double leftSpace) {
315 208729 : 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 208729 : return potential >= requested;
322 : }
323 :
324 :
325 : bool
326 15438120 : MSLCHelper::divergentRoute(const MSVehicle& v1, const MSVehicle& v2) {
327 : // a sufficient, but not necessary condition for divergence
328 15672737 : return (v1.getLane()->isInternal() && v2.getLane()->isInternal()
329 158253 : && v1.getLane()->getEdge().getFromJunction() == v2.getLane()->getEdge().getFromJunction()
330 15584814 : && &v1.getLane()->getEdge() != &v2.getLane()->getEdge());
331 : }
332 :
333 :
334 : double
335 2200471 : 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 2200471 : const double nextGap = currentGap + SPEED2DIST(leaderPlannedSpeed - follower.getSpeed());
340 2200471 : double sGap = follower.getCarFollowModel().getSecureGap(&follower, &leader, follower.getSpeed(), leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
341 2200471 : if (nextGap >= sGap) {
342 : // follower may still accelerate
343 1966775 : const double nextGapMin = currentGap + SPEED2DIST(leaderPlannedSpeed - follower.getCarFollowModel().maxNextSpeed(follower.getSpeed(), &follower));
344 1966775 : const double vSafe = follower.getCarFollowModel().followSpeed(
345 1966775 : &follower, follower.getSpeed(), nextGapMin, leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
346 1966775 : 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 233696 : const double vSafe = follower.getCarFollowModel().followSpeed(
350 233696 : &follower, follower.getSpeed(), nextGap, leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
351 : // avoid emergency deceleration
352 233696 : return MAX2(vSafe, follower.getCarFollowModel().minNextSpeed(follower.getSpeed(), &follower));
353 : }
354 : }
355 :
356 :
357 : bool
358 133680667 : MSLCHelper::isBidiLeader(const MSVehicle* leader, const std::vector<MSLane*>& cont) {
359 133680667 : if (leader == nullptr) {
360 : return false;
361 : }
362 125810427 : const MSLane* lane1 = leader->getLane()->getNormalSuccessorLane()->getBidiLane();
363 125810427 : const MSLane* lane2 = leader->getLane()->getNormalPredecessorLane()->getBidiLane();
364 125810427 : if (lane1 == nullptr && lane2 == nullptr) {
365 : return false;
366 : }
367 2366313 : bool result = std::find(cont.begin(), cont.end(), lane1) != cont.end();
368 2366313 : if (!result && lane1 != lane2 && lane2 != nullptr) {
369 528644 : result = std::find(cont.begin(), cont.end(), lane2) != cont.end();
370 : }
371 : return result;
372 : }
373 :
374 :
375 : bool
376 49761 : MSLCHelper::isBidiFollower(const MSVehicle* ego, const MSVehicle* follower) {
377 49761 : if (follower == nullptr) {
378 : return false;
379 : }
380 : bool result = false;
381 38121 : const MSLane* lane1 = follower->getLane()->getNormalSuccessorLane()->getBidiLane();
382 38121 : const MSLane* lane2 = follower->getLane()->getNormalPredecessorLane()->getBidiLane();
383 38121 : const ConstMSEdgeVector& route = ego->getRoute().getEdges();
384 38121 : if (lane1 != nullptr) {
385 37402 : result = std::find(route.begin(), route.end(), &lane1->getEdge()) != route.end();
386 : }
387 38121 : if (!result && lane1 != lane2 && lane2 != nullptr) {
388 4562 : result = std::find(route.begin(), route.end(), &lane2->getEdge()) != route.end();
389 : }
390 : return result;
391 : }
392 :
393 :
394 : bool
395 8445211 : MSLCHelper::unwillingToHelp(const MSVehicle& ego, double plannedSpeed, const MSVehicle& nv) {
396 8445211 : 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 7169104 : 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 6968107 : const double nvLaneMax = nv.getLane()->getVehicleMaxSpeed(&nv);
408 6968107 : 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 : /****************************************************************************/
|
