Eclipse SUMO - Simulation of Urban MObility
MSLCHelper.cpp
Go to the documentation of this file.
1/****************************************************************************/
2// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3// Copyright (C) 2013-2023 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/****************************************************************************/
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>
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
42double
44 double bonusParam,
45 const MSVehicle::LaneQ& curr,
46 const MSVehicle::LaneQ& neigh,
47 const MSVehicle::LaneQ& best) {
48 if (veh.getLaneChangeModel().isOpposite()) {
49 return 0;
50 }
51 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 double seen = -veh.getPositionOnLane();
69
70 // first check using only normal lanes
71 for (int i = 0; i < (int)best.bestContinuations.size(); i++) {
72 const MSLane* lane = best.bestContinuations[i];
73 if (lane == nullptr) {
74 lane = veh.getLane();
75 }
76 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 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 if (lane->getEdge().isRoundabout()) {
96 enteredRoundabout = true;
97 if (junction->getIncoming().size() + junction->getOutgoing().size() > 2) {
98 roundaboutJunctionsAhead++;
99 }
100 } else if (enteredRoundabout) {
101 // only check the first roundabout
102 break;
103 }
104 seen += lane->getLength();
105 }
106 // no bonus if we want to take the next exit
107 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 for (int i = 0; i < (int)best.bestContinuations.size(); i++) {
120 MSLane* lane = best.bestContinuations[i];
121 if (lane == nullptr) {
122 continue;
123 }
124 if (lane->getEdge().isRoundabout()) {
125 enteredRoundabout = true;
126 } else if (enteredRoundabout) {
127 // only check the first roundabout
128 break;
129 }
130 MSLane* via = nullptr;
131 if (prevNormal != nullptr) {
132 for (MSLink* link : prevNormal->getLinkCont()) {
133 if (link->getLane() == lane) {
134 via = link->getViaLane();
135 }
136 }
137 }
138 if (enteredRoundabout) {
139 distanceInRoundabout += lane->getLength();
140 if (via != nullptr) {
141 distanceInRoundabout += via->getLength();
142 }
143 }
144 // discount vehicles that are upstream from ego
145 const double upstreamDiscount = &lane->getEdge() == &veh.getLane()->getEdge()
146 ? (lane->getLength() - veh.getPositionOnLane()) / lane->getLength() : 1;
147 prevNormal = lane;
148 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 if (via != nullptr) {
155 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 if (i < (int)inner.bestContinuations.size()) {
163 MSLane* innerLane = inner.bestContinuations[i];
164 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 if (prevInner != nullptr) {
171 for (MSLink* link : prevInner->getLinkCont()) {
172 if (link->getLane() == innerLane && link->getViaLane() != nullptr) {
173 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 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 const double bonus = roundaboutJunctionsAhead * 7.5;
203 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 const double jamFactor = MAX2(0.0, relativeJam);
206 const double result = distanceInRoundabout * jamFactor * bonusParam * 9; // the 9 is abitrary and only there for backward compatibility
207#ifdef DEBUG_WANTS_CHANGE
208 if (debugVehicle) {
209 std::cout << " relativeJam=" << relativeJam
210 << " jamFactor=" << jamFactor
211 << " distanceBonus=" << result
212 << "\n";
213 }
214#endif
215 return result;
216}
217
218
219bool
220MSLCHelper::updateBlockerLength(const MSVehicle& veh, MSVehicle* blocker, int lcaCounter, double leftSpace, bool reliefConnection, double& leadingBlockerLength) {
221#ifdef DEBUG_SAVE_BLOCKER_LENGTH
222 if (DEBUG_COND) {
223 std::cout << SIMTIME
224 << " veh=" << veh.getID()
225 << " saveBlockerLength blocker=" << Named::getIDSecure(blocker)
226 << " bState=" << (blocker == 0 ? "None" : toString((LaneChangeAction)blocker->getLaneChangeModel().getOwnState()))
227 << "\n";
228 }
229#endif
230 if (blocker != nullptr && (blocker->getLaneChangeModel().getOwnState() & lcaCounter) != 0) {
231 // is there enough space in front of us for the blocker?
232 const double potential = leftSpace - veh.getCarFollowModel().brakeGap(
233 veh.getSpeed(), veh.getCarFollowModel().getMaxDecel(), 0);
234 if (blocker->getVehicleType().getLengthWithGap() <= potential) {
235 // save at least his length in myLeadingBlockerLength
236 leadingBlockerLength = MAX2(blocker->getVehicleType().getLengthWithGap(), leadingBlockerLength);
237#ifdef DEBUG_SAVE_BLOCKER_LENGTH
238 if (DEBUG_COND) {
239 std::cout << SIMTIME
240 << " veh=" << veh.getID()
241 << " blocker=" << Named::getIDSecure(blocker)
242 << " saving myLeadingBlockerLength=" << leadingBlockerLength
243 << "\n";
244 }
245#endif
246 } else {
247 // we cannot save enough space for the blocker. It needs to save
248 // space for ego instead
249 const bool canReserve = blocker->getLaneChangeModel().saveBlockerLength(veh.getVehicleType().getLengthWithGap(), leftSpace);
250 //reliefConnection ? std::numeric_limits<double>::max() : leftSpace);
251#ifdef DEBUG_SAVE_BLOCKER_LENGTH
252 if (DEBUG_COND) {
253 std::cout << SIMTIME
254 << " veh=" << veh.getID()
255 << " blocker=" << Named::getIDSecure(blocker)
256 << " cannot save space=" << blocker->getVehicleType().getLengthWithGap()
257 << " potential=" << potential
258 << " myReserved=" << leadingBlockerLength
259 << " canReserve=" << canReserve
260 << " reliefConnection=" << reliefConnection
261 << "\n";
262 }
263#endif
264 if (!canReserve && !reliefConnection) {
265 const int blockerState = blocker->getLaneChangeModel().getOwnState();
266 if ((blockerState & LCA_STRATEGIC) != 0
267 && (blockerState & LCA_URGENT) != 0) {
268 // reserve anyway and try to avoid deadlock with emergency deceleration
269 leadingBlockerLength = MAX2(blocker->getVehicleType().getLengthWithGap(), leadingBlockerLength);
270#ifdef DEBUG_SAVE_BLOCKER_LENGTH
271 if (DEBUG_COND) {
272 std::cout << " reserving anyway to avoid deadlock (will cause emergency braking)\n";
273 }
274#endif
275 }
276 }
277 return canReserve;
278 }
279 }
280 return true;
281}
282
283
284bool
285MSLCHelper::canSaveBlockerLength(const MSVehicle& veh, double requested, double leftSpace) {
286 const double potential = leftSpace - veh.getCarFollowModel().brakeGap(veh.getSpeed(), veh.getCarFollowModel().getMaxDecel(), veh.getActionStepLengthSecs());
287#ifdef DEBUG_SAVE_BLOCKER_LENGTH
288 if (DEBUG_COND) {
289 std::cout << SIMTIME << " canSaveBlockerLength veh=" << veh.getID() << " requested=" << requested << " leftSpace=" << leftSpace << " potential=" << potential << "\n";
290 }
291#endif
292 return potential >= requested;
293}
294
295
296bool
298 // a sufficient, but not necessary condition for divergence
299 return (v1.getLane()->isInternal() && v2.getLane()->isInternal()
301 && &v1.getLane()->getEdge() != &v2.getLane()->getEdge());
302}
303
304
305double
306MSLCHelper::getSpeedPreservingSecureGap(const MSVehicle& leader, const MSVehicle& follower, double currentGap, double leaderPlannedSpeed) {
307 // whatever speed the follower choses in the next step, it will change both
308 // the secureGap and the required followSpeed.
309 // Let's assume the leader maintains speed
310 const double nextGap = currentGap + SPEED2DIST(leaderPlannedSpeed - follower.getSpeed());
311 double sGap = follower.getCarFollowModel().getSecureGap(&follower, &leader, follower.getSpeed(), leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
312 if (nextGap >= sGap) {
313 // follower may still accelerate
314 const double nextGapMin = currentGap + SPEED2DIST(leaderPlannedSpeed - follower.getCarFollowModel().maxNextSpeed(follower.getSpeed(), &follower));
315 const double vSafe = follower.getCarFollowModel().followSpeed(
316 &follower, follower.getSpeed(), nextGapMin, leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
317 return MAX2(vSafe, follower.getSpeed());
318 } else {
319 // follower must brake. The following brakes conservatively since the actual gap will be lower due to braking.
320 const double vSafe = follower.getCarFollowModel().followSpeed(
321 &follower, follower.getSpeed(), nextGap, leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
322 // avoid emergency deceleration
323 return MAX2(vSafe, follower.getCarFollowModel().minNextSpeed(follower.getSpeed(), &follower));
324 }
325}
326
327
328bool
329MSLCHelper::isBidiLeader(const MSVehicle* leader, const std::vector<MSLane*>& cont) {
330 if (leader == nullptr) {
331 return false;
332 }
333 const MSLane* lane1 = leader->getLane()->getNormalSuccessorLane()->getBidiLane();
334 const MSLane* lane2 = leader->getLane()->getNormalPredecessorLane()->getBidiLane();
335 if (lane1 == nullptr && lane2 == nullptr) {
336 return false;
337 }
338 bool result = std::find(cont.begin(), cont.end(), lane1) != cont.end();
339 if (!result && lane1 != lane2 && lane2 != nullptr) {
340 result = std::find(cont.begin(), cont.end(), lane2) != cont.end();
341 }
342 return result;
343}
344
345
346bool
347MSLCHelper::isBidiFollower(const MSVehicle* ego, const MSVehicle* follower) {
348 if (follower == nullptr) {
349 return false;
350 }
351 bool result = false;
352 const MSLane* lane1 = follower->getLane()->getNormalSuccessorLane()->getBidiLane();
353 const MSLane* lane2 = follower->getLane()->getNormalPredecessorLane()->getBidiLane();
354 const ConstMSEdgeVector& route = ego->getRoute().getEdges();
355 if (lane1 != nullptr) {
356 result = std::find(route.begin(), route.end(), &lane1->getEdge()) != route.end();
357 }
358 if (!result && lane1 != lane2 && lane2 != nullptr) {
359 result = std::find(route.begin(), route.end(), &lane2->getEdge()) != route.end();
360 }
361 return result;
362}
363
364/****************************************************************************/
std::vector< const MSEdge * > ConstMSEdgeVector
Definition: MSEdge.h:74
#define DEBUG_COND
Definition: MSLCHelper.cpp:34
#define SPEED2DIST(x)
Definition: SUMOTime.h:45
#define SIMTIME
Definition: SUMOTime.h:62
LaneChangeAction
The state of a vehicle's lane-change behavior.
@ LCA_URGENT
The action is urgent (to be defined by lc-model)
@ LCA_STRATEGIC
The action is needed to follow the route (navigational lc)
T MAX2(T a, T b)
Definition: StdDefs.h:82
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:46
virtual bool saveBlockerLength(double, double)
reserve space at the end of the lane to avoid dead locks
virtual bool debugVehicle() const
whether the current vehicles shall be debugged
const MSRoute & getRoute() const
Returns the current route.
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
virtual double maxNextSpeed(double speed, const MSVehicle *const veh) const
Returns the maximum speed given the current speed.
Definition: MSCFModel.cpp:292
virtual double minNextSpeed(double speed, const MSVehicle *const veh=0) const
Returns the minimum speed given the current speed (depends on the numerical update scheme and its ste...
Definition: MSCFModel.cpp:298
virtual double getSecureGap(const MSVehicle *const veh, const MSVehicle *const, const double speed, const double leaderSpeed, const double leaderMaxDecel) const
Returns the minimum gap to reserve if the leader is braking at maximum (>=0)
Definition: MSCFModel.cpp:166
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver's reaction time tau (i....
Definition: MSCFModel.h:380
double getMaxDecel() const
Get the vehicle type's maximal comfortable deceleration [m/s^2].
Definition: MSCFModel.h:264
virtual double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0, const CalcReason usage=CalcReason::CURRENT) const =0
Computes the vehicle's follow speed (no dawdling)
const MSJunction * getToJunction() const
Definition: MSEdge.h:415
const MSJunction * getFromJunction() const
Definition: MSEdge.h:411
bool isRoundabout() const
Definition: MSEdge.h:694
The base class for an intersection.
Definition: MSJunction.h:58
const ConstMSEdgeVector & getOutgoing() const
Definition: MSJunction.h:116
const ConstMSEdgeVector & getIncoming() const
Definition: MSJunction.h:110
static bool isBidiFollower(const MSVehicle *ego, const MSVehicle *follower)
Definition: MSLCHelper.cpp:347
static bool canSaveBlockerLength(const MSVehicle &veh, double requested, double leftSpace)
Definition: MSLCHelper.cpp:285
static double getSpeedPreservingSecureGap(const MSVehicle &leader, const MSVehicle &follower, double currentGap, double leaderPlannedSpeed)
Definition: MSLCHelper.cpp:306
static double getRoundaboutDistBonus(const MSVehicle &veh, double bonusParam, const MSVehicle::LaneQ &curr, const MSVehicle::LaneQ &neigh, const MSVehicle::LaneQ &best)
Definition: MSLCHelper.cpp:43
static bool isBidiLeader(const MSVehicle *leader, const std::vector< MSLane * > &cont)
Definition: MSLCHelper.cpp:329
static bool updateBlockerLength(const MSVehicle &veh, MSVehicle *blocker, int lcaCounter, double leftSpace, bool reliefConnection, double &leadingBlockerLength)
Definition: MSLCHelper.cpp:220
static bool divergentRoute(const MSVehicle &v1, const MSVehicle &v2)
return whether the vehicles are on the same junction but on divergent paths
Definition: MSLCHelper.cpp:297
Representation of a lane in the micro simulation.
Definition: MSLane.h:84
const MSLane * getNormalSuccessorLane() const
get normal lane following this internal lane, for normal lanes, the lane itself is returned
Definition: MSLane.cpp:3034
double getBruttoVehLenSum() const
Returns the sum of lengths of vehicles, including their minGaps, which were on the lane during the la...
Definition: MSLane.h:1131
double getLength() const
Returns the lane's length.
Definition: MSLane.h:593
int getIndex() const
Returns the lane's index.
Definition: MSLane.h:629
bool isInternal() const
Definition: MSLane.cpp:2456
MSLane * getBidiLane() const
retrieve bidirectional lane or nullptr
Definition: MSLane.cpp:4425
MSEdge & getEdge() const
Returns the lane's edge.
Definition: MSLane.h:745
const MSLane * getNormalPredecessorLane() const
get normal lane leading to this internal lane, for normal lanes, the lane itself is returned
Definition: MSLane.cpp:3024
const std::vector< MSLink * > & getLinkCont() const
returns the container with all links !!!
Definition: MSLane.h:707
const ConstMSEdgeVector & getEdges() const
Definition: MSRoute.h:124
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:77
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:5620
double getActionStepLengthSecs() const
Returns the vehicle's action step length in secs, i.e. the interval between two action points.
Definition: MSVehicle.h:536
const MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:584
double getSpeed() const
Returns the vehicle's current speed.
Definition: MSVehicle.h:493
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
Definition: MSVehicle.h:978
double getPositionOnLane() const
Get the vehicle's position along the lane.
Definition: MSVehicle.h:377
double getLengthWithGap() const
Get vehicle's length including the minimum gap [m].
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
Definition: Named.h:67
const std::string & getID() const
Returns the id.
Definition: Named.h:74
A structure representing the best lanes for continuing the current route starting at 'lane'.
Definition: MSVehicle.h:869
std::vector< MSLane * > bestContinuations
Definition: MSVehicle.h:889
MSLane * lane
The described lane.
Definition: MSVehicle.h:871