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 296301277 : MSLCHelper::getRoundaboutDistBonus(const MSVehicle& veh,
44 : double bonusParam,
45 : const MSVehicle::LaneQ& curr,
46 : const MSVehicle::LaneQ& neigh,
47 : const MSVehicle::LaneQ& best) {
48 296301277 : if (veh.getLaneChangeModel().isOpposite()) {
49 : return 0;
50 : }
51 295913681 : 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 295913681 : 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 293555224 : double seen = -veh.getPositionOnLane();
73 :
74 : // first check using only normal lanes
75 768681371 : for (int i = 0; i < (int)best.bestContinuations.size(); i++) {
76 603828836 : const MSLane* lane = best.bestContinuations[i];
77 603828836 : if (lane == nullptr) {
78 3068532 : lane = veh.getLane();
79 : }
80 603828836 : 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 588580091 : 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 500802241 : if (lane->getEdge().isRoundabout()) {
100 : enteredRoundabout = true;
101 53655049 : if (junction->getIncoming().size() + junction->getOutgoing().size() > 2) {
102 53430611 : roundaboutJunctionsAhead++;
103 : }
104 447147192 : } else if (enteredRoundabout) {
105 : // only check the first roundabout
106 : break;
107 : }
108 475126147 : seen += lane->getLength();
109 : }
110 : // no bonus if we want to take the next exit
111 190528629 : if (roundaboutJunctionsAhead < 2) {
112 : return 0;
113 : }
114 :
115 : // compute bonus value based on jamming and exact distances (taking into
116 : // account internal lanes)
117 : double occupancyOuter = 0;
118 : double occupancyInner = 0;
119 : double distanceInRoundabout = 0;
120 : MSLane* prevNormal = nullptr;
121 : MSLane* prevInner = nullptr;
122 : enteredRoundabout = false;
123 72218227 : for (int i = 0; i < (int)best.bestContinuations.size(); i++) {
124 72061508 : MSLane* lane = best.bestContinuations[i];
125 72061508 : if (lane == nullptr) {
126 629402 : continue;
127 : }
128 71432106 : if (lane->getEdge().isRoundabout()) {
129 : enteredRoundabout = true;
130 31192597 : } else if (enteredRoundabout) {
131 : // only check the first roundabout
132 : break;
133 : }
134 : MSLane* via = nullptr;
135 55246658 : if (prevNormal != nullptr) {
136 120387609 : for (MSLink* link : prevNormal->getLinkCont()) {
137 81483118 : if (link->getLane() == lane) {
138 : via = link->getViaLane();
139 : }
140 : }
141 : }
142 55246658 : if (enteredRoundabout) {
143 40239509 : distanceInRoundabout += lane->getLength();
144 40239509 : if (via != nullptr) {
145 38743523 : distanceInRoundabout += via->getLength();
146 : }
147 : }
148 : // discount vehicles that are upstream from ego
149 55246658 : const double upstreamDiscount = &lane->getEdge() == &veh.getLane()->getEdge()
150 55246658 : ? (lane->getLength() - veh.getPositionOnLane()) / lane->getLength() : 1;
151 : prevNormal = lane;
152 55246658 : occupancyOuter += upstreamDiscount * lane->getBruttoVehLenSum();
153 : #ifdef DEBUG_WANTS_CHANGE
154 : if (debugVehicle) {
155 : std::cout << " lane=" << lane->getID() << " occ=" << lane->getBruttoVehLenSum() << " discount=" << upstreamDiscount << " outer=" << occupancyOuter << "\n";
156 : }
157 : #endif
158 55246658 : if (via != nullptr) {
159 38840362 : occupancyOuter += via->getBruttoVehLenSum();
160 : #ifdef DEBUG_WANTS_CHANGE
161 : if (debugVehicle) {
162 : std::cout << " via=" << via->getID() << " occ=" << via->getBruttoVehLenSum() << " outer=" << occupancyOuter << "\n";
163 : }
164 : #endif
165 : }
166 55246658 : if (i < (int)inner.bestContinuations.size()) {
167 55246658 : MSLane* innerLane = inner.bestContinuations[i];
168 55246658 : occupancyInner += upstreamDiscount * innerLane->getBruttoVehLenSum();
169 : #ifdef DEBUG_WANTS_CHANGE
170 : if (debugVehicle) {
171 : std::cout << " inner=" << innerLane->getID() << " occ=" << innerLane->getBruttoVehLenSum() << " discount=" << upstreamDiscount << " inner=" << occupancyInner << "\n";
172 : }
173 : #endif
174 55246658 : if (prevInner != nullptr) {
175 85531314 : for (MSLink* link : prevInner->getLinkCont()) {
176 46626823 : if (link->getLane() == innerLane && link->getViaLane() != nullptr) {
177 38840362 : occupancyInner += link->getViaLane()->getBruttoVehLenSum();
178 : #ifdef DEBUG_WANTS_CHANGE
179 : if (debugVehicle) {
180 : std::cout << " innerVia=" << link->getViaLane()->getID() << " occ=" << link->getViaLane()->getBruttoVehLenSum() << " inner=" << occupancyInner << "\n";
181 : }
182 : #endif
183 : }
184 : }
185 : }
186 : prevInner = innerLane;
187 : }
188 : }
189 :
190 : #ifdef DEBUG_WANTS_CHANGE
191 : if (debugVehicle) {
192 : std::cout << " distanceInRoundabout=" << distanceInRoundabout
193 : << " roundaboutJunctionsAhead=" << roundaboutJunctionsAhead
194 : << " occupancyInner=" << occupancyInner
195 : << " occupancyOuter=" << occupancyOuter
196 : << "\n";
197 : }
198 : #endif
199 :
200 : const double maxOccupancy = MAX2(occupancyInner, occupancyOuter);
201 16342167 : if (maxOccupancy == 0) {
202 : // no bonues if the roundabout is empty
203 : return 0;
204 : }
205 : // give some bonus for using the inside lane at equal occupancy
206 16195955 : const double bonus = roundaboutJunctionsAhead * 7.5;
207 16195955 : const double relativeJam = (occupancyOuter - occupancyInner + bonus) / (maxOccupancy + bonus);
208 : // no bonus if the inner lane or the left lane entering the roundabout is jammed
209 : double jamFactor = MAX2(0.0, relativeJam);
210 16195955 : if (veh.getLane()->getEdge().isRoundabout() && curr.lane->getIndex() > neigh.lane->getIndex()) {
211 : // only use jamFactor when deciding to move to the inside lane but prefer
212 : // staying inside if the distance allows it
213 : jamFactor = 1;
214 : }
215 16195955 : const double result = distanceInRoundabout * jamFactor * bonusParam * 9; // the 9 is abitrary and only there for backward compatibility
216 : #ifdef DEBUG_WANTS_CHANGE
217 : if (debugVehicle) {
218 : std::cout << " relativeJam=" << relativeJam
219 : << " jamFactor=" << jamFactor
220 : << " distanceBonus=" << result
221 : << "\n";
222 : }
223 : #endif
224 16195955 : return result;
225 : }
226 :
227 :
228 : bool
229 9435187 : MSLCHelper::updateBlockerLength(const MSVehicle& veh, MSVehicle* blocker, int lcaCounter, double leftSpace, bool reliefConnection, double& leadingBlockerLength) {
230 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
231 : if (DEBUG_COND) {
232 : std::cout << SIMTIME
233 : << " veh=" << veh.getID()
234 : << " saveBlockerLength blocker=" << Named::getIDSecure(blocker)
235 : << " bState=" << (blocker == 0 ? "None" : toString((LaneChangeAction)blocker->getLaneChangeModel().getOwnState()))
236 : << "\n";
237 : }
238 : #endif
239 9435187 : if (blocker != nullptr && (blocker->getLaneChangeModel().getOwnState() & lcaCounter) != 0) {
240 : // is there enough space in front of us for the blocker?
241 318351 : const double potential = leftSpace - veh.getCarFollowModel().brakeGap(
242 318351 : veh.getSpeed(), veh.getCarFollowModel().getMaxDecel(), 0);
243 318351 : if (blocker->getVehicleType().getLengthWithGap() <= potential) {
244 : // save at least his length in myLeadingBlockerLength
245 584248 : leadingBlockerLength = MAX2(blocker->getVehicleType().getLengthWithGap(), leadingBlockerLength);
246 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
247 : if (DEBUG_COND) {
248 : std::cout << SIMTIME
249 : << " veh=" << veh.getID()
250 : << " blocker=" << Named::getIDSecure(blocker)
251 : << " saving myLeadingBlockerLength=" << leadingBlockerLength
252 : << "\n";
253 : }
254 : #endif
255 : } else {
256 : // we cannot save enough space for the blocker. It needs to save
257 : // space for ego instead
258 26227 : const bool canReserve = blocker->getLaneChangeModel().saveBlockerLength(veh.getVehicleType().getLengthWithGap(), leftSpace);
259 : //reliefConnection ? std::numeric_limits<double>::max() : leftSpace);
260 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
261 : if (DEBUG_COND) {
262 : std::cout << SIMTIME
263 : << " veh=" << veh.getID()
264 : << " blocker=" << Named::getIDSecure(blocker)
265 : << " cannot save space=" << blocker->getVehicleType().getLengthWithGap()
266 : << " potential=" << potential
267 : << " myReserved=" << leadingBlockerLength
268 : << " canReserve=" << canReserve
269 : << " reliefConnection=" << reliefConnection
270 : << "\n";
271 : }
272 : #endif
273 26227 : if (!canReserve && !reliefConnection) {
274 3408 : const int blockerState = blocker->getLaneChangeModel().getOwnState();
275 3408 : if ((blockerState & LCA_STRATEGIC) != 0
276 : && (blockerState & LCA_URGENT) != 0) {
277 : // reserve anyway and try to avoid deadlock with emergency deceleration
278 3962 : leadingBlockerLength = MAX2(blocker->getVehicleType().getLengthWithGap(), leadingBlockerLength);
279 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
280 : if (DEBUG_COND) {
281 : std::cout << " reserving anyway to avoid deadlock (will cause emergency braking)\n";
282 : }
283 : #endif
284 : }
285 : }
286 26227 : return canReserve;
287 : }
288 : }
289 : return true;
290 : }
291 :
292 :
293 : bool
294 195765 : MSLCHelper::canSaveBlockerLength(const MSVehicle& veh, double requested, double leftSpace) {
295 195765 : const double potential = leftSpace - veh.getCarFollowModel().brakeGap(veh.getSpeed(), veh.getCarFollowModel().getMaxDecel(), veh.getActionStepLengthSecs());
296 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
297 : if (DEBUG_COND) {
298 : std::cout << SIMTIME << " canSaveBlockerLength veh=" << veh.getID() << " requested=" << requested << " leftSpace=" << leftSpace << " potential=" << potential << "\n";
299 : }
300 : #endif
301 195765 : return potential >= requested;
302 : }
303 :
304 :
305 : bool
306 11704156 : MSLCHelper::divergentRoute(const MSVehicle& v1, const MSVehicle& v2) {
307 : // a sufficient, but not necessary condition for divergence
308 11884128 : return (v1.getLane()->isInternal() && v2.getLane()->isInternal()
309 124218 : && v1.getLane()->getEdge().getFromJunction() == v2.getLane()->getEdge().getFromJunction()
310 11823400 : && &v1.getLane()->getEdge() != &v2.getLane()->getEdge());
311 : }
312 :
313 :
314 : double
315 1685112 : MSLCHelper::getSpeedPreservingSecureGap(const MSVehicle& leader, const MSVehicle& follower, double currentGap, double leaderPlannedSpeed) {
316 : // whatever speed the follower choses in the next step, it will change both
317 : // the secureGap and the required followSpeed.
318 : // Let's assume the leader maintains speed
319 1685112 : const double nextGap = currentGap + SPEED2DIST(leaderPlannedSpeed - follower.getSpeed());
320 1685112 : double sGap = follower.getCarFollowModel().getSecureGap(&follower, &leader, follower.getSpeed(), leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
321 1685112 : if (nextGap >= sGap) {
322 : // follower may still accelerate
323 1501519 : const double nextGapMin = currentGap + SPEED2DIST(leaderPlannedSpeed - follower.getCarFollowModel().maxNextSpeed(follower.getSpeed(), &follower));
324 1501519 : const double vSafe = follower.getCarFollowModel().followSpeed(
325 1501519 : &follower, follower.getSpeed(), nextGapMin, leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
326 1501519 : return MAX2(vSafe, follower.getSpeed());
327 : } else {
328 : // follower must brake. The following brakes conservatively since the actual gap will be lower due to braking.
329 183593 : const double vSafe = follower.getCarFollowModel().followSpeed(
330 183593 : &follower, follower.getSpeed(), nextGap, leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
331 : // avoid emergency deceleration
332 183593 : return MAX2(vSafe, follower.getCarFollowModel().minNextSpeed(follower.getSpeed(), &follower));
333 : }
334 : }
335 :
336 :
337 : bool
338 673776 : MSLCHelper::isBidiLeader(const MSVehicle* leader, const std::vector<MSLane*>& cont) {
339 673776 : if (leader == nullptr) {
340 : return false;
341 : }
342 435426 : const MSLane* lane1 = leader->getLane()->getNormalSuccessorLane()->getBidiLane();
343 435426 : const MSLane* lane2 = leader->getLane()->getNormalPredecessorLane()->getBidiLane();
344 435426 : if (lane1 == nullptr && lane2 == nullptr) {
345 : return false;
346 : }
347 426936 : bool result = std::find(cont.begin(), cont.end(), lane1) != cont.end();
348 426936 : if (!result && lane1 != lane2 && lane2 != nullptr) {
349 71946 : result = std::find(cont.begin(), cont.end(), lane2) != cont.end();
350 : }
351 : return result;
352 : }
353 :
354 :
355 : bool
356 25767 : MSLCHelper::isBidiFollower(const MSVehicle* ego, const MSVehicle* follower) {
357 25767 : if (follower == nullptr) {
358 : return false;
359 : }
360 : bool result = false;
361 20346 : const MSLane* lane1 = follower->getLane()->getNormalSuccessorLane()->getBidiLane();
362 20346 : const MSLane* lane2 = follower->getLane()->getNormalPredecessorLane()->getBidiLane();
363 20346 : const ConstMSEdgeVector& route = ego->getRoute().getEdges();
364 20346 : if (lane1 != nullptr) {
365 19977 : result = std::find(route.begin(), route.end(), &lane1->getEdge()) != route.end();
366 : }
367 20346 : if (!result && lane1 != lane2 && lane2 != nullptr) {
368 2253 : result = std::find(route.begin(), route.end(), &lane2->getEdge()) != route.end();
369 : }
370 : return result;
371 : }
372 :
373 : /****************************************************************************/
|
