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 351176547 : MSLCHelper::getRoundaboutDistBonus(const MSVehicle& veh,
44 : double bonusParam,
45 : const MSVehicle::LaneQ& curr,
46 : const MSVehicle::LaneQ& neigh,
47 : const MSVehicle::LaneQ& best) {
48 351176547 : if (veh.getLaneChangeModel().isOpposite()) {
49 : return 0;
50 : }
51 350786604 : 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 350786604 : 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 348019210 : double seen = -veh.getPositionOnLane();
73 :
74 : // first check using only normal lanes
75 906644715 : for (int i = 0; i < (int)best.bestContinuations.size(); i++) {
76 695134485 : const MSLane* lane = best.bestContinuations[i];
77 695134485 : if (lane == nullptr) {
78 3343088 : lane = veh.getLane();
79 : }
80 695134485 : 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 678439294 : 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 584477998 : if (lane->getEdge().isRoundabout()) {
100 : enteredRoundabout = true;
101 54164581 : if (junction->getIncoming().size() + junction->getOutgoing().size() > 2) {
102 53968091 : roundaboutJunctionsAhead++;
103 : }
104 530313417 : } else if (enteredRoundabout) {
105 : // only check the first roundabout
106 : break;
107 : }
108 558625505 : seen += lane->getLength();
109 : }
110 : // no bonus if we want to take the next exit
111 237362723 : 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 73048352 : for (int i = 0; i < (int)best.bestContinuations.size(); i++) {
128 72966176 : MSLane* lane = best.bestContinuations[i];
129 72966176 : if (lane == nullptr) {
130 668926 : continue;
131 : }
132 72297250 : if (lane->getEdge().isRoundabout()) {
133 : enteredRoundabout = true;
134 31572179 : } else if (enteredRoundabout) {
135 : // only check the first roundabout
136 : break;
137 : }
138 : MSLane* via = nullptr;
139 55959086 : if (prevNormal != nullptr) {
140 121698418 : for (MSLink* link : prevNormal->getLinkCont()) {
141 82159672 : if (link->getLane() == lane) {
142 : via = link->getViaLane();
143 : }
144 : }
145 : }
146 55959086 : if (enteredRoundabout) {
147 40725071 : distanceInRoundabout += lane->getLength();
148 40725071 : if (via != nullptr) {
149 39109045 : distanceInRoundabout += via->getLength();
150 : }
151 : }
152 : // discount vehicles that are upstream from ego
153 55959086 : const double upstreamDiscount = &lane->getEdge() == &veh.getLane()->getEdge()
154 55959086 : ? (lane->getLength() - veh.getPositionOnLane()) / lane->getLength() : 1;
155 : prevNormal = lane;
156 55959086 : 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 55959086 : if (via != nullptr) {
163 39404539 : 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 55959086 : if (i < (int)inner.bestContinuations.size()) {
171 55959086 : MSLane* innerLane = inner.bestContinuations[i];
172 55959086 : 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 55959086 : if (prevInner != nullptr) {
179 86841329 : for (MSLink* link : prevInner->getLinkCont()) {
180 47302583 : if (link->getLane() == innerLane && link->getViaLane() != nullptr) {
181 39404539 : 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 16420340 : 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 16420334 : const double bonus = roundaboutJunctionsAhead * 7.5;
220 16420334 : 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 16420334 : 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 16420334 : 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 16420334 : return result;
238 : }
239 :
240 :
241 : bool
242 10893897 : 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 10893897 : if (blocker != nullptr && (blocker->getLaneChangeModel().getOwnState() & lcaCounter) != 0) {
253 : // is there enough space in front of us for the blocker?
254 453710 : const double potential = leftSpace - veh.getCarFollowModel().brakeGap(
255 453710 : veh.getSpeed(), veh.getCarFollowModel().getMaxDecel(), 0);
256 453710 : if (blocker->getVehicleType().getLengthWithGap() <= potential) {
257 : // save at least his length in myLeadingBlockerLength
258 842736 : leadingBlockerLength = MAX2(blocker->getVehicleType().getLengthWithGap(), leadingBlockerLength);
259 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
260 : if (DEBUG_COND) {
261 : std::cout << SIMTIME
262 : << " veh=" << veh.getID()
263 : << " blocker=" << Named::getIDSecure(blocker)
264 : << " saving myLeadingBlockerLength=" << leadingBlockerLength
265 : << "\n";
266 : }
267 : #endif
268 : } else {
269 : // we cannot save enough space for the blocker. It needs to save
270 : // space for ego instead
271 32342 : const bool canReserve = blocker->getLaneChangeModel().saveBlockerLength(veh.getVehicleType().getLengthWithGap(), leftSpace);
272 : //reliefConnection ? std::numeric_limits<double>::max() : leftSpace);
273 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
274 : if (DEBUG_COND) {
275 : std::cout << SIMTIME
276 : << " veh=" << veh.getID()
277 : << " blocker=" << Named::getIDSecure(blocker)
278 : << " cannot save space=" << blocker->getVehicleType().getLengthWithGap()
279 : << " potential=" << potential
280 : << " myReserved=" << leadingBlockerLength
281 : << " canReserve=" << canReserve
282 : << " reliefConnection=" << reliefConnection
283 : << "\n";
284 : }
285 : #endif
286 32342 : if (!canReserve && !reliefConnection) {
287 4171 : const int blockerState = blocker->getLaneChangeModel().getOwnState();
288 4171 : if ((blockerState & LCA_STRATEGIC) != 0
289 : && (blockerState & LCA_URGENT) != 0) {
290 : // reserve anyway and try to avoid deadlock with emergency deceleration
291 5372 : leadingBlockerLength = MAX2(blocker->getVehicleType().getLengthWithGap(), leadingBlockerLength);
292 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
293 : if (DEBUG_COND) {
294 : std::cout << " reserving anyway to avoid deadlock (will cause emergency braking)\n";
295 : }
296 : #endif
297 : }
298 : }
299 32342 : return canReserve;
300 : }
301 : }
302 : return true;
303 : }
304 :
305 :
306 : bool
307 203544 : MSLCHelper::canSaveBlockerLength(const MSVehicle& veh, double requested, double leftSpace) {
308 203544 : const double potential = leftSpace - veh.getCarFollowModel().brakeGap(veh.getSpeed(), veh.getCarFollowModel().getMaxDecel(), veh.getActionStepLengthSecs());
309 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
310 : if (DEBUG_COND) {
311 : std::cout << SIMTIME << " canSaveBlockerLength veh=" << veh.getID() << " requested=" << requested << " leftSpace=" << leftSpace << " potential=" << potential << "\n";
312 : }
313 : #endif
314 203544 : return potential >= requested;
315 : }
316 :
317 :
318 : bool
319 13321286 : MSLCHelper::divergentRoute(const MSVehicle& v1, const MSVehicle& v2) {
320 : // a sufficient, but not necessary condition for divergence
321 13530502 : return (v1.getLane()->isInternal() && v2.getLane()->isInternal()
322 155584 : && v1.getLane()->getEdge().getFromJunction() == v2.getLane()->getEdge().getFromJunction()
323 13465804 : && &v1.getLane()->getEdge() != &v2.getLane()->getEdge());
324 : }
325 :
326 :
327 : double
328 1856841 : MSLCHelper::getSpeedPreservingSecureGap(const MSVehicle& leader, const MSVehicle& follower, double currentGap, double leaderPlannedSpeed) {
329 : // whatever speed the follower choses in the next step, it will change both
330 : // the secureGap and the required followSpeed.
331 : // Let's assume the leader maintains speed
332 1856841 : const double nextGap = currentGap + SPEED2DIST(leaderPlannedSpeed - follower.getSpeed());
333 1856841 : double sGap = follower.getCarFollowModel().getSecureGap(&follower, &leader, follower.getSpeed(), leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
334 1856841 : if (nextGap >= sGap) {
335 : // follower may still accelerate
336 1665402 : const double nextGapMin = currentGap + SPEED2DIST(leaderPlannedSpeed - follower.getCarFollowModel().maxNextSpeed(follower.getSpeed(), &follower));
337 1665402 : const double vSafe = follower.getCarFollowModel().followSpeed(
338 1665402 : &follower, follower.getSpeed(), nextGapMin, leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
339 1665402 : return MAX2(vSafe, follower.getSpeed());
340 : } else {
341 : // follower must brake. The following brakes conservatively since the actual gap will be lower due to braking.
342 191439 : const double vSafe = follower.getCarFollowModel().followSpeed(
343 191439 : &follower, follower.getSpeed(), nextGap, leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
344 : // avoid emergency deceleration
345 191439 : return MAX2(vSafe, follower.getCarFollowModel().minNextSpeed(follower.getSpeed(), &follower));
346 : }
347 : }
348 :
349 :
350 : bool
351 656631 : MSLCHelper::isBidiLeader(const MSVehicle* leader, const std::vector<MSLane*>& cont) {
352 656631 : if (leader == nullptr) {
353 : return false;
354 : }
355 426927 : const MSLane* lane1 = leader->getLane()->getNormalSuccessorLane()->getBidiLane();
356 426927 : const MSLane* lane2 = leader->getLane()->getNormalPredecessorLane()->getBidiLane();
357 426927 : if (lane1 == nullptr && lane2 == nullptr) {
358 : return false;
359 : }
360 418221 : bool result = std::find(cont.begin(), cont.end(), lane1) != cont.end();
361 418221 : if (!result && lane1 != lane2 && lane2 != nullptr) {
362 71850 : result = std::find(cont.begin(), cont.end(), lane2) != cont.end();
363 : }
364 : return result;
365 : }
366 :
367 :
368 : bool
369 25080 : MSLCHelper::isBidiFollower(const MSVehicle* ego, const MSVehicle* follower) {
370 25080 : if (follower == nullptr) {
371 : return false;
372 : }
373 : bool result = false;
374 19737 : const MSLane* lane1 = follower->getLane()->getNormalSuccessorLane()->getBidiLane();
375 19737 : const MSLane* lane2 = follower->getLane()->getNormalPredecessorLane()->getBidiLane();
376 19737 : const ConstMSEdgeVector& route = ego->getRoute().getEdges();
377 19737 : if (lane1 != nullptr) {
378 19359 : result = std::find(route.begin(), route.end(), &lane1->getEdge()) != route.end();
379 : }
380 19737 : if (!result && lane1 != lane2 && lane2 != nullptr) {
381 2382 : result = std::find(route.begin(), route.end(), &lane2->getEdge()) != route.end();
382 : }
383 : return result;
384 : }
385 :
386 : /****************************************************************************/
|
