Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2013-2024 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 290360380 : MSLCHelper::getRoundaboutDistBonus(const MSVehicle& veh,
44 : double bonusParam,
45 : const MSVehicle::LaneQ& curr,
46 : const MSVehicle::LaneQ& neigh,
47 : const MSVehicle::LaneQ& best) {
48 290360380 : if (veh.getLaneChangeModel().isOpposite()) {
49 : return 0;
50 : }
51 289972723 : 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 :
66 : int roundaboutJunctionsAhead = 0;
67 : bool enteredRoundabout = false;
68 289972723 : double seen = -veh.getPositionOnLane();
69 :
70 : // first check using only normal lanes
71 764504797 : for (int i = 0; i < (int)best.bestContinuations.size(); i++) {
72 604326398 : const MSLane* lane = best.bestContinuations[i];
73 604326398 : if (lane == nullptr) {
74 3053885 : lane = veh.getLane();
75 : }
76 604326398 : if ((!enteredRoundabout || lane->getEdge().isRoundabout()) && i >= (int)inner.bestContinuations.size()) {
77 : // no bonus if we cannot continue on the inner lane until leaving the roundabout
78 : #ifdef DEBUG_WANTS_CHANGE
79 : if (debugVehicle) {
80 : std::cout << " noBonus: inner does not continue (lane=" << lane->getID() << ")\n";
81 : }
82 : #endif
83 : return 0;
84 : }
85 589076412 : if (seen > 300) {
86 : // avoid long look-ahead
87 : #ifdef DEBUG_WANTS_CHANGE
88 : if (debugVehicle) {
89 : std::cout << " noBonus: seen=" << seen << " (lane=" << lane->getID() << ")\n";
90 : }
91 : #endif
92 : return 0;
93 : }
94 : const MSJunction* junction = lane->getEdge().getToJunction();
95 500492685 : if (lane->getEdge().isRoundabout()) {
96 : enteredRoundabout = true;
97 53940645 : if (junction->getIncoming().size() + junction->getOutgoing().size() > 2) {
98 53716207 : roundaboutJunctionsAhead++;
99 : }
100 446552040 : } else if (enteredRoundabout) {
101 : // only check the first roundabout
102 : break;
103 : }
104 474532074 : seen += lane->getLength();
105 : }
106 : // no bonus if we want to take the next exit
107 186139010 : if (roundaboutJunctionsAhead < 2) {
108 : return 0;
109 : }
110 :
111 : // compute bonus value based on jamming and exact distances (taking into
112 : // account internal lanes)
113 : double occupancyOuter = 0;
114 : double occupancyInner = 0;
115 : double distanceInRoundabout = 0;
116 : MSLane* prevNormal = nullptr;
117 : MSLane* prevInner = nullptr;
118 : enteredRoundabout = false;
119 72082720 : for (int i = 0; i < (int)best.bestContinuations.size(); i++) {
120 71926024 : MSLane* lane = best.bestContinuations[i];
121 71926024 : if (lane == nullptr) {
122 626609 : continue;
123 : }
124 71299415 : if (lane->getEdge().isRoundabout()) {
125 : enteredRoundabout = true;
126 31078220 : } else if (enteredRoundabout) {
127 : // only check the first roundabout
128 : break;
129 : }
130 : MSLane* via = nullptr;
131 55146984 : if (prevNormal != nullptr) {
132 120261699 : for (MSLink* link : prevNormal->getLinkCont()) {
133 81423842 : if (link->getLane() == lane) {
134 : via = link->getViaLane();
135 : }
136 : }
137 : }
138 55146984 : if (enteredRoundabout) {
139 40221195 : distanceInRoundabout += lane->getLength();
140 40221195 : if (via != nullptr) {
141 38714036 : distanceInRoundabout += via->getLength();
142 : }
143 : }
144 : // discount vehicles that are upstream from ego
145 55146984 : const double upstreamDiscount = &lane->getEdge() == &veh.getLane()->getEdge()
146 55146984 : ? (lane->getLength() - veh.getPositionOnLane()) / lane->getLength() : 1;
147 : prevNormal = lane;
148 55146984 : occupancyOuter += upstreamDiscount * lane->getBruttoVehLenSum();
149 : #ifdef DEBUG_WANTS_CHANGE
150 : if (debugVehicle) {
151 : std::cout << " lane=" << lane->getID() << " occ=" << lane->getBruttoVehLenSum() << " discount=" << upstreamDiscount << " outer=" << occupancyOuter << "\n";
152 : }
153 : #endif
154 55146984 : if (via != nullptr) {
155 38773728 : occupancyOuter += via->getBruttoVehLenSum();
156 : #ifdef DEBUG_WANTS_CHANGE
157 : if (debugVehicle) {
158 : std::cout << " via=" << via->getID() << " occ=" << via->getBruttoVehLenSum() << " outer=" << occupancyOuter << "\n";
159 : }
160 : #endif
161 : }
162 55146984 : if (i < (int)inner.bestContinuations.size()) {
163 55146984 : MSLane* innerLane = inner.bestContinuations[i];
164 55146984 : occupancyInner += upstreamDiscount * innerLane->getBruttoVehLenSum();
165 : #ifdef DEBUG_WANTS_CHANGE
166 : if (debugVehicle) {
167 : std::cout << " inner=" << innerLane->getID() << " occ=" << innerLane->getBruttoVehLenSum() << " discount=" << upstreamDiscount << " inner=" << occupancyInner << "\n";
168 : }
169 : #endif
170 55146984 : if (prevInner != nullptr) {
171 85386304 : for (MSLink* link : prevInner->getLinkCont()) {
172 46548447 : if (link->getLane() == innerLane && link->getViaLane() != nullptr) {
173 38773728 : occupancyInner += link->getViaLane()->getBruttoVehLenSum();
174 : #ifdef DEBUG_WANTS_CHANGE
175 : if (debugVehicle) {
176 : std::cout << " innerVia=" << link->getViaLane()->getID() << " occ=" << link->getViaLane()->getBruttoVehLenSum() << " inner=" << occupancyInner << "\n";
177 : }
178 : #endif
179 : }
180 : }
181 : }
182 : prevInner = innerLane;
183 : }
184 : }
185 :
186 : #ifdef DEBUG_WANTS_CHANGE
187 : if (debugVehicle) {
188 : std::cout << " distanceInRoundabout=" << distanceInRoundabout
189 : << " roundaboutJunctionsAhead=" << roundaboutJunctionsAhead
190 : << " occupancyInner=" << occupancyInner
191 : << " occupancyOuter=" << occupancyOuter
192 : << "\n";
193 : }
194 : #endif
195 :
196 : const double maxOccupancy = MAX2(occupancyInner, occupancyOuter);
197 16309127 : if (maxOccupancy == 0) {
198 : // no bonues if the roundabout is empty
199 : return 0;
200 : }
201 : // give some bonus for using the inside lane at equal occupancy
202 16162695 : const double bonus = roundaboutJunctionsAhead * 7.5;
203 16162695 : const double relativeJam = (occupancyOuter - occupancyInner + bonus) / (maxOccupancy + bonus);
204 : // no bonus if the inner lane or the left lane entering the roundabout is jammed
205 : double jamFactor = MAX2(0.0, relativeJam);
206 16162695 : if (veh.getLane()->getEdge().isRoundabout() && curr.lane->getIndex() > neigh.lane->getIndex()) {
207 : // only use jamFactor when deciding to move to the inside lane but prefer
208 : // staying inside if the distance allows it
209 : jamFactor = 1;
210 : }
211 16162695 : const double result = distanceInRoundabout * jamFactor * bonusParam * 9; // the 9 is abitrary and only there for backward compatibility
212 : #ifdef DEBUG_WANTS_CHANGE
213 : if (debugVehicle) {
214 : std::cout << " relativeJam=" << relativeJam
215 : << " jamFactor=" << jamFactor
216 : << " distanceBonus=" << result
217 : << "\n";
218 : }
219 : #endif
220 16162695 : return result;
221 : }
222 :
223 :
224 : bool
225 9839593 : MSLCHelper::updateBlockerLength(const MSVehicle& veh, MSVehicle* blocker, int lcaCounter, double leftSpace, bool reliefConnection, double& leadingBlockerLength) {
226 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
227 : if (DEBUG_COND) {
228 : std::cout << SIMTIME
229 : << " veh=" << veh.getID()
230 : << " saveBlockerLength blocker=" << Named::getIDSecure(blocker)
231 : << " bState=" << (blocker == 0 ? "None" : toString((LaneChangeAction)blocker->getLaneChangeModel().getOwnState()))
232 : << "\n";
233 : }
234 : #endif
235 9839593 : if (blocker != nullptr && (blocker->getLaneChangeModel().getOwnState() & lcaCounter) != 0) {
236 : // is there enough space in front of us for the blocker?
237 320147 : const double potential = leftSpace - veh.getCarFollowModel().brakeGap(
238 320147 : veh.getSpeed(), veh.getCarFollowModel().getMaxDecel(), 0);
239 320147 : if (blocker->getVehicleType().getLengthWithGap() <= potential) {
240 : // save at least his length in myLeadingBlockerLength
241 587712 : leadingBlockerLength = MAX2(blocker->getVehicleType().getLengthWithGap(), leadingBlockerLength);
242 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
243 : if (DEBUG_COND) {
244 : std::cout << SIMTIME
245 : << " veh=" << veh.getID()
246 : << " blocker=" << Named::getIDSecure(blocker)
247 : << " saving myLeadingBlockerLength=" << leadingBlockerLength
248 : << "\n";
249 : }
250 : #endif
251 : } else {
252 : // we cannot save enough space for the blocker. It needs to save
253 : // space for ego instead
254 26291 : const bool canReserve = blocker->getLaneChangeModel().saveBlockerLength(veh.getVehicleType().getLengthWithGap(), leftSpace);
255 : //reliefConnection ? std::numeric_limits<double>::max() : leftSpace);
256 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
257 : if (DEBUG_COND) {
258 : std::cout << SIMTIME
259 : << " veh=" << veh.getID()
260 : << " blocker=" << Named::getIDSecure(blocker)
261 : << " cannot save space=" << blocker->getVehicleType().getLengthWithGap()
262 : << " potential=" << potential
263 : << " myReserved=" << leadingBlockerLength
264 : << " canReserve=" << canReserve
265 : << " reliefConnection=" << reliefConnection
266 : << "\n";
267 : }
268 : #endif
269 26291 : if (!canReserve && !reliefConnection) {
270 3514 : const int blockerState = blocker->getLaneChangeModel().getOwnState();
271 3514 : if ((blockerState & LCA_STRATEGIC) != 0
272 : && (blockerState & LCA_URGENT) != 0) {
273 : // reserve anyway and try to avoid deadlock with emergency deceleration
274 4130 : leadingBlockerLength = MAX2(blocker->getVehicleType().getLengthWithGap(), leadingBlockerLength);
275 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
276 : if (DEBUG_COND) {
277 : std::cout << " reserving anyway to avoid deadlock (will cause emergency braking)\n";
278 : }
279 : #endif
280 : }
281 : }
282 26291 : return canReserve;
283 : }
284 : }
285 : return true;
286 : }
287 :
288 :
289 : bool
290 195834 : MSLCHelper::canSaveBlockerLength(const MSVehicle& veh, double requested, double leftSpace) {
291 195834 : const double potential = leftSpace - veh.getCarFollowModel().brakeGap(veh.getSpeed(), veh.getCarFollowModel().getMaxDecel(), veh.getActionStepLengthSecs());
292 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
293 : if (DEBUG_COND) {
294 : std::cout << SIMTIME << " canSaveBlockerLength veh=" << veh.getID() << " requested=" << requested << " leftSpace=" << leftSpace << " potential=" << potential << "\n";
295 : }
296 : #endif
297 195834 : return potential >= requested;
298 : }
299 :
300 :
301 : bool
302 11670662 : MSLCHelper::divergentRoute(const MSVehicle& v1, const MSVehicle& v2) {
303 : // a sufficient, but not necessary condition for divergence
304 11825769 : return (v1.getLane()->isInternal() && v2.getLane()->isInternal()
305 98380 : && v1.getLane()->getEdge().getFromJunction() == v2.getLane()->getEdge().getFromJunction()
306 11763986 : && &v1.getLane()->getEdge() != &v2.getLane()->getEdge());
307 : }
308 :
309 :
310 : double
311 1570794 : MSLCHelper::getSpeedPreservingSecureGap(const MSVehicle& leader, const MSVehicle& follower, double currentGap, double leaderPlannedSpeed) {
312 : // whatever speed the follower choses in the next step, it will change both
313 : // the secureGap and the required followSpeed.
314 : // Let's assume the leader maintains speed
315 1570794 : const double nextGap = currentGap + SPEED2DIST(leaderPlannedSpeed - follower.getSpeed());
316 1570794 : double sGap = follower.getCarFollowModel().getSecureGap(&follower, &leader, follower.getSpeed(), leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
317 1570794 : if (nextGap >= sGap) {
318 : // follower may still accelerate
319 1397622 : const double nextGapMin = currentGap + SPEED2DIST(leaderPlannedSpeed - follower.getCarFollowModel().maxNextSpeed(follower.getSpeed(), &follower));
320 1397622 : const double vSafe = follower.getCarFollowModel().followSpeed(
321 1397622 : &follower, follower.getSpeed(), nextGapMin, leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
322 1397622 : return MAX2(vSafe, follower.getSpeed());
323 : } else {
324 : // follower must brake. The following brakes conservatively since the actual gap will be lower due to braking.
325 173172 : const double vSafe = follower.getCarFollowModel().followSpeed(
326 173172 : &follower, follower.getSpeed(), nextGap, leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
327 : // avoid emergency deceleration
328 173172 : return MAX2(vSafe, follower.getCarFollowModel().minNextSpeed(follower.getSpeed(), &follower));
329 : }
330 : }
331 :
332 :
333 : bool
334 665538 : MSLCHelper::isBidiLeader(const MSVehicle* leader, const std::vector<MSLane*>& cont) {
335 665538 : if (leader == nullptr) {
336 : return false;
337 : }
338 425076 : const MSLane* lane1 = leader->getLane()->getNormalSuccessorLane()->getBidiLane();
339 425076 : const MSLane* lane2 = leader->getLane()->getNormalPredecessorLane()->getBidiLane();
340 425076 : if (lane1 == nullptr && lane2 == nullptr) {
341 : return false;
342 : }
343 416766 : bool result = std::find(cont.begin(), cont.end(), lane1) != cont.end();
344 416766 : if (!result && lane1 != lane2 && lane2 != nullptr) {
345 76191 : result = std::find(cont.begin(), cont.end(), lane2) != cont.end();
346 : }
347 : return result;
348 : }
349 :
350 :
351 : bool
352 24954 : MSLCHelper::isBidiFollower(const MSVehicle* ego, const MSVehicle* follower) {
353 24954 : if (follower == nullptr) {
354 : return false;
355 : }
356 : bool result = false;
357 19992 : const MSLane* lane1 = follower->getLane()->getNormalSuccessorLane()->getBidiLane();
358 19992 : const MSLane* lane2 = follower->getLane()->getNormalPredecessorLane()->getBidiLane();
359 19992 : const ConstMSEdgeVector& route = ego->getRoute().getEdges();
360 19992 : if (lane1 != nullptr) {
361 19524 : result = std::find(route.begin(), route.end(), &lane1->getEdge()) != route.end();
362 : }
363 19992 : if (!result && lane1 != lane2 && lane2 != nullptr) {
364 2277 : result = std::find(route.begin(), route.end(), &lane2->getEdge()) != route.end();
365 : }
366 : return result;
367 : }
368 :
369 : /****************************************************************************/
|
