Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2013-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 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 :
34 : #define DEBUG_COND (veh.isSelected())
35 : //#define DEBUG_COND (true)
36 :
37 :
38 : // ===========================================================================
39 : // member method definitions
40 : // ===========================================================================
41 :
42 : double
43 349299967 : MSLCHelper::getRoundaboutDistBonus(const MSVehicle& veh,
44 : double bonusParam,
45 : const MSVehicle::LaneQ& curr,
46 : const MSVehicle::LaneQ& neigh,
47 : const MSVehicle::LaneQ& best) {
48 349299967 : if (veh.getLaneChangeModel().isOpposite()) {
49 : return 0;
50 : }
51 348914572 : const MSVehicle::LaneQ& inner = neigh.lane->getIndex() > curr.lane->getIndex() ? neigh : curr;
52 : #ifdef DEBUG_WANTS_CHANGE
53 : const bool debugVehicle = veh.getLaneChangeModel().debugVehicle();
54 : if (debugVehicle) {
55 : std::cout << SIMTIME << " veh=" << veh.getID() << " getRoundaboutDistBonus bonusParam=" << bonusParam
56 : << " curr=" << curr.lane->getID()
57 : << " neigh=" << neigh.lane->getID()
58 : << " inner=" << inner.lane->getID()
59 : << " best=" << best.lane->getID()
60 : << "\n innerCont=" << toString(inner.bestContinuations)
61 : << "\n bestCont=" << toString(best.bestContinuations)
62 : << "\n";
63 : }
64 : #endif
65 348914572 : if (neigh.lane == inner.lane && curr.bestContinuations.size() < neigh.bestContinuations.size()) {
66 : // the current lane does not continue to the roundabout and we need a strategic change first.
67 : return 0;
68 : }
69 :
70 : int roundaboutJunctionsAhead = 0;
71 : bool enteredRoundabout = false;
72 346187013 : double seen = -veh.getPositionOnLane();
73 :
74 : // first check using only normal lanes
75 904576449 : for (int i = 0; i < (int)best.bestContinuations.size(); i++) {
76 692908980 : const MSLane* lane = best.bestContinuations[i];
77 692908980 : if (lane == nullptr) {
78 3335176 : lane = veh.getLane();
79 : }
80 692908980 : if ((!enteredRoundabout || lane->getEdge().isRoundabout()) && i >= (int)inner.bestContinuations.size()) {
81 : // no bonus if we cannot continue on the inner lane until leaving the roundabout
82 : #ifdef DEBUG_WANTS_CHANGE
83 : if (debugVehicle) {
84 : std::cout << " noBonus: inner does not continue (lane=" << lane->getID() << ")\n";
85 : }
86 : #endif
87 : return 0;
88 : }
89 676195163 : if (seen > 300) {
90 : // avoid long look-ahead
91 : #ifdef DEBUG_WANTS_CHANGE
92 : if (debugVehicle) {
93 : std::cout << " noBonus: seen=" << seen << " (lane=" << lane->getID() << ")\n";
94 : }
95 : #endif
96 : return 0;
97 : }
98 : const MSJunction* junction = lane->getEdge().getToJunction();
99 584252776 : if (lane->getEdge().isRoundabout()) {
100 : enteredRoundabout = true;
101 54639338 : if (junction->getIncoming().size() + junction->getOutgoing().size() > 2) {
102 54442848 : roundaboutJunctionsAhead++;
103 : }
104 529613438 : } else if (enteredRoundabout) {
105 : // only check the first roundabout
106 : break;
107 : }
108 558389436 : seen += lane->getLength();
109 : }
110 : // no bonus if we want to take the next exit
111 237530809 : if (roundaboutJunctionsAhead < 2) {
112 : #ifdef DEBUG_WANTS_CHANGE
113 : if (debugVehicle) {
114 : std::cout << " noBonus: roundaboutJunctionsAhead=" << roundaboutJunctionsAhead << "\n";
115 : }
116 : #endif
117 : return 0;
118 : }
119 : // compute bonus value based on jamming and exact distances (taking into
120 : // account internal lanes)
121 : double occupancyOuter = 0;
122 : double occupancyInner = 0;
123 : double distanceInRoundabout = 0;
124 : MSLane* prevNormal = nullptr;
125 : MSLane* prevInner = nullptr;
126 : enteredRoundabout = false;
127 73685261 : for (int i = 0; i < (int)best.bestContinuations.size(); i++) {
128 73552492 : MSLane* lane = best.bestContinuations[i];
129 73552492 : if (lane == nullptr) {
130 666536 : continue;
131 : }
132 72885956 : if (lane->getEdge().isRoundabout()) {
133 : enteredRoundabout = true;
134 31743027 : } else if (enteredRoundabout) {
135 : // only check the first roundabout
136 : break;
137 : }
138 : MSLane* via = nullptr;
139 56487574 : if (prevNormal != nullptr) {
140 122947707 : for (MSLink* link : prevNormal->getLinkCont()) {
141 82991284 : if (link->getLane() == lane) {
142 : via = link->getViaLane();
143 : }
144 : }
145 : }
146 56487574 : if (enteredRoundabout) {
147 41142929 : distanceInRoundabout += lane->getLength();
148 41142929 : if (via != nullptr) {
149 39537390 : distanceInRoundabout += via->getLength();
150 : }
151 : }
152 : // discount vehicles that are upstream from ego
153 56487574 : const double upstreamDiscount = &lane->getEdge() == &veh.getLane()->getEdge()
154 56487574 : ? (lane->getLength() - veh.getPositionOnLane()) / lane->getLength() : 1;
155 : prevNormal = lane;
156 56487574 : occupancyOuter += upstreamDiscount * lane->getBruttoVehLenSum();
157 : #ifdef DEBUG_WANTS_CHANGE
158 : if (debugVehicle) {
159 : std::cout << " lane=" << lane->getID() << " occ=" << lane->getBruttoVehLenSum() << " discount=" << upstreamDiscount << " outer=" << occupancyOuter << "\n";
160 : }
161 : #endif
162 56487574 : if (via != nullptr) {
163 39822216 : occupancyOuter += via->getBruttoVehLenSum();
164 : #ifdef DEBUG_WANTS_CHANGE
165 : if (debugVehicle) {
166 : std::cout << " via=" << via->getID() << " occ=" << via->getBruttoVehLenSum() << " outer=" << occupancyOuter << "\n";
167 : }
168 : #endif
169 : }
170 56487574 : if (i < (int)inner.bestContinuations.size()) {
171 56487574 : MSLane* innerLane = inner.bestContinuations[i];
172 56487574 : occupancyInner += upstreamDiscount * innerLane->getBruttoVehLenSum();
173 : #ifdef DEBUG_WANTS_CHANGE
174 : if (debugVehicle) {
175 : std::cout << " inner=" << innerLane->getID() << " occ=" << innerLane->getBruttoVehLenSum() << " discount=" << upstreamDiscount << " inner=" << occupancyInner << "\n";
176 : }
177 : #endif
178 56487574 : if (prevInner != nullptr) {
179 87855830 : for (MSLink* link : prevInner->getLinkCont()) {
180 47899407 : if (link->getLane() == innerLane && link->getViaLane() != nullptr) {
181 39822216 : occupancyInner += link->getViaLane()->getBruttoVehLenSum();
182 : #ifdef DEBUG_WANTS_CHANGE
183 : if (debugVehicle) {
184 : std::cout << " innerVia=" << link->getViaLane()->getID() << " occ=" << link->getViaLane()->getBruttoVehLenSum() << " inner=" << occupancyInner << "\n";
185 : }
186 : #endif
187 : }
188 : }
189 : }
190 : prevInner = innerLane;
191 : }
192 : }
193 :
194 : #ifdef DEBUG_WANTS_CHANGE
195 : if (debugVehicle) {
196 : std::cout << " distanceInRoundabout=" << distanceInRoundabout
197 : << " roundaboutJunctionsAhead=" << roundaboutJunctionsAhead
198 : << " occupancyInner=" << occupancyInner
199 : << " occupancyOuter=" << occupancyOuter
200 : << "\n";
201 : }
202 : #endif
203 16531151 : if (abs(curr.bestLaneOffset) > 1 && enteredRoundabout) {
204 134 : const double bGap = veh.getCarFollowModel().brakeGap(veh.getSpeed() + ACCEL2SPEED(veh.getCarFollowModel().getMaxAccel()), veh.getCarFollowModel().getMaxDecel(), veh.getActionStepLengthSecs());
205 134 : const double reservation = veh.getLaneChangeModel().getExtraReservation(curr.bestLaneOffset);
206 134 : const double leftSpace = distanceInRoundabout - reservation - veh.getPositionOnLane();
207 : #ifdef DEBUG_WANTS_CHANGE
208 : if (debugVehicle) {
209 : std::cout << " bGap=" << bGap << " reserving=" << reservation << " leftover=" << (leftSpace - bGap) << "\n";
210 : }
211 : #endif
212 134 : if (bGap > leftSpace) {
213 : return 0;
214 : }
215 : }
216 :
217 : const double maxOccupancy = MAX2(occupancyInner, occupancyOuter);
218 : // give some bonus for using the inside lane at equal occupancy
219 16531145 : const double bonus = roundaboutJunctionsAhead * 7.5;
220 16531145 : const double relativeJam = (occupancyOuter - occupancyInner + bonus) / (maxOccupancy + bonus);
221 : // no bonus if the inner lane or the left lane entering the roundabout is jammed
222 : double jamFactor = MAX2(0.0, relativeJam);
223 16531145 : if (veh.getLane()->getEdge().isRoundabout() && curr.lane->getIndex() > neigh.lane->getIndex()) {
224 : // only use jamFactor when deciding to move to the inside lane but prefer
225 : // staying inside if the distance allows it
226 : jamFactor = 1;
227 : }
228 16531145 : const double result = distanceInRoundabout * jamFactor * bonusParam * 9; // the 9 is abitrary and only there for backward compatibility
229 : #ifdef DEBUG_WANTS_CHANGE
230 : if (debugVehicle) {
231 : std::cout << " relativeJam=" << relativeJam
232 : << " jamFactor=" << jamFactor
233 : << " distanceBonus=" << result
234 : << "\n";
235 : }
236 : #endif
237 16531145 : return result;
238 : }
239 :
240 :
241 : bool
242 10838520 : MSLCHelper::updateBlockerLength(const MSVehicle& veh, MSVehicle* blocker, int lcaCounter, double leftSpace, bool reliefConnection, double& leadingBlockerLength) {
243 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
244 : if (DEBUG_COND) {
245 : std::cout << SIMTIME
246 : << " veh=" << veh.getID()
247 : << " saveBlockerLength blocker=" << Named::getIDSecure(blocker)
248 : << " bState=" << (blocker == 0 ? "None" : toString((LaneChangeAction)blocker->getLaneChangeModel().getOwnState()))
249 : << "\n";
250 : }
251 : #endif
252 10838520 : if (blocker != nullptr && (blocker->getLaneChangeModel().getOwnState() & lcaCounter) != 0) {
253 : // is there enough space in front of us for the blocker?
254 457685 : const double required = blocker->getVehicleType().getLengthWithGap() + veh.getVehicleType().getMinGap();
255 457685 : const double potential = leftSpace - veh.getCarFollowModel().brakeGap(
256 457685 : veh.getSpeed(), veh.getCarFollowModel().getMaxDecel(), 0);
257 457685 : if (required <= potential) {
258 : // save at least his length in myLeadingBlockerLength
259 474164 : leadingBlockerLength = MAX2(required, leadingBlockerLength);
260 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
261 : if (DEBUG_COND) {
262 : std::cout << SIMTIME
263 : << " veh=" << veh.getID()
264 : << " required=" << required
265 : << " potential=" << potential
266 : << " blocker=" << Named::getIDSecure(blocker)
267 : << " saving myLeadingBlockerLength=" << leadingBlockerLength
268 : << "\n";
269 : }
270 : #endif
271 : } else {
272 : // we cannot save enough space for the blocker. It needs to save
273 : // space for ego instead
274 38233 : const double required2 = veh.getVehicleType().getLengthWithGap() + blocker->getVehicleType().getMinGap();
275 38233 : const double foeLeftSpace = leftSpace - veh.getPositionOnLane() + blocker->getPositionOnLane() - POSITION_EPS;
276 38233 : const bool canReserve = blocker->getLaneChangeModel().saveBlockerLength(required2, foeLeftSpace);
277 : //reliefConnection ? std::numeric_limits<double>::max() : leftSpace);
278 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
279 : if (DEBUG_COND) {
280 : std::cout << SIMTIME
281 : << " veh=" << veh.getID()
282 : << " required=" << required
283 : << " potential=" << potential
284 : << " blocker=" << Named::getIDSecure(blocker)
285 : << " required2=" << required2
286 : << " foeCanReserve=" << canReserve
287 : << " myReserved=" << leadingBlockerLength
288 : << " reliefConnection=" << reliefConnection
289 : << "\n";
290 : }
291 : #endif
292 38233 : if (!canReserve && !reliefConnection) {
293 10579 : const int blockerState = blocker->getLaneChangeModel().getOwnState();
294 10579 : if ((blockerState & LCA_STRATEGIC) != 0
295 : && (blockerState & LCA_URGENT) != 0) {
296 : // reserve anyway and try to avoid deadlock with emergency deceleration
297 8666 : leadingBlockerLength = MAX2(required, leadingBlockerLength);
298 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
299 : if (DEBUG_COND) {
300 : std::cout << " reserving anyway to avoid deadlock (will cause emergency braking)\n";
301 : }
302 : #endif
303 : }
304 : }
305 38233 : return canReserve;
306 : }
307 : }
308 : return true;
309 : }
310 :
311 :
312 : bool
313 165625 : MSLCHelper::canSaveBlockerLength(const MSVehicle& veh, double requested, double leftSpace) {
314 165625 : const double potential = leftSpace - veh.getCarFollowModel().brakeGap(veh.getSpeed(), veh.getCarFollowModel().getMaxDecel(), veh.getActionStepLengthSecs());
315 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
316 : if (DEBUG_COND) {
317 : std::cout << SIMTIME << " canSaveBlockerLength veh=" << veh.getID() << " requested=" << requested << " leftSpace=" << leftSpace << " potential=" << potential << "\n";
318 : }
319 : #endif
320 165625 : return potential >= requested;
321 : }
322 :
323 :
324 : bool
325 13315557 : MSLCHelper::divergentRoute(const MSVehicle& v1, const MSVehicle& v2) {
326 : // a sufficient, but not necessary condition for divergence
327 13509045 : return (v1.getLane()->isInternal() && v2.getLane()->isInternal()
328 140043 : && v1.getLane()->getEdge().getFromJunction() == v2.getLane()->getEdge().getFromJunction()
329 13444888 : && &v1.getLane()->getEdge() != &v2.getLane()->getEdge());
330 : }
331 :
332 :
333 : double
334 1902281 : MSLCHelper::getSpeedPreservingSecureGap(const MSVehicle& leader, const MSVehicle& follower, double currentGap, double leaderPlannedSpeed) {
335 : // whatever speed the follower choses in the next step, it will change both
336 : // the secureGap and the required followSpeed.
337 : // Let's assume the leader maintains speed
338 1902281 : const double nextGap = currentGap + SPEED2DIST(leaderPlannedSpeed - follower.getSpeed());
339 1902281 : double sGap = follower.getCarFollowModel().getSecureGap(&follower, &leader, follower.getSpeed(), leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
340 1902281 : if (nextGap >= sGap) {
341 : // follower may still accelerate
342 1701709 : const double nextGapMin = currentGap + SPEED2DIST(leaderPlannedSpeed - follower.getCarFollowModel().maxNextSpeed(follower.getSpeed(), &follower));
343 1701709 : const double vSafe = follower.getCarFollowModel().followSpeed(
344 1701709 : &follower, follower.getSpeed(), nextGapMin, leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
345 1701709 : return MAX2(vSafe, follower.getSpeed());
346 : } else {
347 : // follower must brake. The following brakes conservatively since the actual gap will be lower due to braking.
348 200572 : const double vSafe = follower.getCarFollowModel().followSpeed(
349 200572 : &follower, follower.getSpeed(), nextGap, leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
350 : // avoid emergency deceleration
351 200572 : return MAX2(vSafe, follower.getCarFollowModel().minNextSpeed(follower.getSpeed(), &follower));
352 : }
353 : }
354 :
355 :
356 : bool
357 659811 : MSLCHelper::isBidiLeader(const MSVehicle* leader, const std::vector<MSLane*>& cont) {
358 659811 : if (leader == nullptr) {
359 : return false;
360 : }
361 432120 : const MSLane* lane1 = leader->getLane()->getNormalSuccessorLane()->getBidiLane();
362 432120 : const MSLane* lane2 = leader->getLane()->getNormalPredecessorLane()->getBidiLane();
363 432120 : if (lane1 == nullptr && lane2 == nullptr) {
364 : return false;
365 : }
366 423696 : bool result = std::find(cont.begin(), cont.end(), lane1) != cont.end();
367 423696 : if (!result && lane1 != lane2 && lane2 != nullptr) {
368 71226 : result = std::find(cont.begin(), cont.end(), lane2) != cont.end();
369 : }
370 : return result;
371 : }
372 :
373 :
374 : bool
375 24906 : MSLCHelper::isBidiFollower(const MSVehicle* ego, const MSVehicle* follower) {
376 24906 : if (follower == nullptr) {
377 : return false;
378 : }
379 : bool result = false;
380 19434 : const MSLane* lane1 = follower->getLane()->getNormalSuccessorLane()->getBidiLane();
381 19434 : const MSLane* lane2 = follower->getLane()->getNormalPredecessorLane()->getBidiLane();
382 19434 : const ConstMSEdgeVector& route = ego->getRoute().getEdges();
383 19434 : if (lane1 != nullptr) {
384 18924 : result = std::find(route.begin(), route.end(), &lane1->getEdge()) != route.end();
385 : }
386 19434 : if (!result && lane1 != lane2 && lane2 != nullptr) {
387 2508 : result = std::find(route.begin(), route.end(), &lane2->getEdge()) != route.end();
388 : }
389 : return result;
390 : }
391 :
392 : /****************************************************************************/
|
