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 MSLCM_SL2015.h
15 : /// @author Jakob Erdmann
16 : /// @author Leonhard Luecken
17 : /// @date Tue, 06.10.2015
18 : ///
19 : // A lane change model for heterogeneous traffic (based on sub-lanes)
20 : /****************************************************************************/
21 : #pragma once
22 : #include <config.h>
23 :
24 : #include "MSAbstractLaneChangeModel.h"
25 : #include <vector>
26 :
27 :
28 : // ===========================================================================
29 : // class definitions
30 : // ===========================================================================
31 : /**
32 : * @class MSLCM_SL2015
33 : * @brief A lane change model developed by J. Erdmann
34 : */
35 : class MSLCM_SL2015 : public MSAbstractLaneChangeModel {
36 : public:
37 :
38 : MSLCM_SL2015(MSVehicle& v);
39 :
40 : virtual ~MSLCM_SL2015();
41 :
42 : /// @brief Returns the model's id
43 0 : LaneChangeModel getModelID() const override {
44 0 : return LaneChangeModel::SL2015;
45 : }
46 :
47 : /// @brief init cached parameters derived directly from model parameters
48 : void initDerivedParameters();
49 :
50 : /** @brief Called to examine whether the vehicle wants to change
51 : * with the given laneOffset (using the sublane model)
52 : * This method gets the information about the surrounding vehicles
53 : * and whether another lane may be more preferable
54 : *
55 : * TODO better documentation. Refs #2
56 : * A.o.: When is this called (as a wantsChange() exists as well!? What's the difference?)
57 : * */
58 : int wantsChangeSublane(int laneOffset,
59 : LaneChangeAction alternatives,
60 : const MSLeaderDistanceInfo& leaders,
61 : const MSLeaderDistanceInfo& followers,
62 : const MSLeaderDistanceInfo& blockers,
63 : const MSLeaderDistanceInfo& neighLeaders,
64 : const MSLeaderDistanceInfo& neighFollowers,
65 : const MSLeaderDistanceInfo& neighBlockers,
66 : const MSLane& neighLane,
67 : const std::vector<MSVehicle::LaneQ>& preb,
68 : MSVehicle** lastBlocked,
69 : MSVehicle** firstBlocked,
70 : double& latDist, double& maneuverDist, int& blocked) override;
71 :
72 : /** @brief Called to examine whether the vehicle wants to change
73 : * using the given laneOffset (this is a wrapper around wantsChangeSublane). XXX: no, it wraps _wantsChangeSublane
74 : * This method gets the information about the surrounding vehicles
75 : * and whether another lane may be more preferable
76 : *
77 : * TODO: better documentation. Refs #2
78 : * */
79 : int wantsChange(
80 : int laneOffset,
81 : MSAbstractLaneChangeModel::MSLCMessager& msgPass, int blocked,
82 : const std::pair<MSVehicle*, double>& leader,
83 : const std::pair<MSVehicle*, double>& follower,
84 : const std::pair<MSVehicle*, double>& neighLead,
85 : const std::pair<MSVehicle*, double>& neighFollow,
86 : const MSLane& neighLane,
87 : const std::vector<MSVehicle::LaneQ>& preb,
88 : MSVehicle** lastBlocked,
89 : MSVehicle** firstBlocked) override;
90 :
91 : void* inform(void* info, MSVehicle* sender) override;
92 :
93 : /** @brief Called to adapt the speed in order to allow a lane change.
94 : * It uses information on LC-related desired speed-changes from
95 : * the call to wantsChange() at the end of the previous simulation step
96 : *
97 : * @param min The minimum resulting speed
98 : * @param wanted The aspired speed of the car following model
99 : * @param max The maximum resulting speed
100 : * @param cfModel The model used
101 : * @return the new speed of the vehicle as proposed by the lane changer
102 : */
103 : double patchSpeed(const double min, const double wanted, const double max,
104 : const MSCFModel& cfModel) override;
105 :
106 : void changed() override;
107 :
108 : void resetState() override;
109 :
110 : double getSafetyFactor() const override;
111 :
112 : double getOppositeSafetyFactor() const override;
113 :
114 : void prepareStep() override;
115 :
116 : /// @brief whether the current vehicles shall be debugged
117 : bool debugVehicle() const override;
118 :
119 : void setOwnState(const int state) override;
120 :
121 : /// @brief Updates the value of safe lateral distances (mySafeLatDistLeft and mySafeLatDistRight)
122 : /// during maneuver continuation in non-action steps.
123 : virtual void updateSafeLatDist(const double travelledLatDist) override;
124 :
125 : /// @brief try to retrieve the given parameter from this device. Throw exception for unsupported key
126 : std::string getParameter(const std::string& key) const override;
127 :
128 : /// @brief try to set the given parameter for this laneChangeModel. Throw exception for unsupported key
129 : void setParameter(const std::string& key, const std::string& value) override;
130 :
131 : /// @brief decides the next lateral speed depending on the remaining lane change distance to be covered
132 : /// and updates maneuverDist according to lateral safety constraints.
133 : double computeSpeedLat(double latDist, double& maneuverDist, bool urgent) const override;
134 :
135 : LatAlignmentDefinition getDesiredAlignment() const override;
136 :
137 : protected:
138 : /** helper function which contains the actual logic */
139 : double _patchSpeed(double min, const double wanted, double max,
140 : const MSCFModel& cfModel);
141 :
142 : /// @brief helper function for doing the actual work
143 : int _wantsChangeSublane(
144 : int laneOffset,
145 : LaneChangeAction alternatives,
146 : const MSLeaderDistanceInfo& leaders,
147 : const MSLeaderDistanceInfo& followers,
148 : const MSLeaderDistanceInfo& blockers,
149 : const MSLeaderDistanceInfo& neighLeaders,
150 : const MSLeaderDistanceInfo& neighFollowers,
151 : const MSLeaderDistanceInfo& neighBlockers,
152 : const MSLane& neighLane,
153 : const std::vector<MSVehicle::LaneQ>& preb,
154 : MSVehicle** lastBlocked,
155 : MSVehicle** firstBlocked,
156 : double& latDist, double& maneuverDist, int& blocked);
157 :
158 :
159 : /* @brief decide whether we will overtake or follow blocking leaders
160 : * and inform them accordingly (see informLeader)
161 : * If we decide to follow, myVSafes will be extended
162 : * returns the planned speed if following or -1 if overtaking */
163 : double informLeaders(int blocked, int dir,
164 : const std::vector<CLeaderDist>& blockers,
165 : double remainingSeconds);
166 :
167 : /// @brief call informFollower for multiple followers
168 : void informFollowers(int blocked, int dir,
169 : const std::vector<CLeaderDist>& blockers,
170 : double remainingSeconds,
171 : double plannedSpeed);
172 :
173 : /* @brief decide whether we will overtake or follow a blocking leader
174 : * and inform it accordingly
175 : * If we decide to follow, myVSafes will be extended
176 : * returns the planned speed if following or -1 if overtaking */
177 : double informLeader(int blocked, int dir,
178 : const CLeaderDist& neighLead,
179 : double remainingSeconds);
180 :
181 : /// @brief decide whether we will try cut in before the follower or allow to be overtaken
182 : void informFollower(int blocked, int dir,
183 : const CLeaderDist& neighFollow,
184 : double remainingSeconds,
185 : double plannedSpeed);
186 :
187 :
188 : /// @brief compute useful slowdowns for blocked vehicles
189 : int slowDownForBlocked(MSVehicle** blocked, int state);
190 :
191 : /// @brief reserve space at the end of the lane to avoid dead locks
192 : bool saveBlockerLength(double length, double foeLeftSpace) override;
193 :
194 : /// @brief whether the ego vehicle is driving outside edgebounds
195 : bool outsideEdge() const;
196 :
197 : inline bool amBlockingLeader() {
198 : return (myOwnState & LCA_AMBLOCKINGLEADER) != 0;
199 : }
200 : inline bool amBlockingFollower() {
201 : return (myOwnState & LCA_AMBLOCKINGFOLLOWER) != 0;
202 : }
203 : inline bool amBlockingFollowerNB() {
204 : return (myOwnState & LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0;
205 : }
206 : inline bool amBlockingFollowerPlusNB() {
207 21468780 : return (myOwnState & (LCA_AMBLOCKINGFOLLOWER | LCA_AMBLOCKINGFOLLOWER_DONTBRAKE)) != 0;
208 : }
209 : inline bool currentDistDisallows(double dist, int laneOffset, double lookForwardDist) {
210 18814981 : return dist / (abs(laneOffset)) < lookForwardDist;
211 : }
212 : inline bool currentDistAllows(double dist, int laneOffset, double lookForwardDist) {
213 364 : return dist / abs(laneOffset) > lookForwardDist;
214 : }
215 :
216 :
217 : /// @brief information regarding save velocity (unused) and state flags of the ego vehicle
218 : typedef std::pair<double, int> Info;
219 :
220 : /// @brief update expected speeds for each sublane of the current edge
221 : void updateExpectedSublaneSpeeds(const MSLeaderDistanceInfo& ahead, int sublaneOffset, int laneIndex) override;
222 :
223 : /// @brief decide in which direction to move in case both directions are desirable
224 : StateAndDist decideDirection(StateAndDist sd1, StateAndDist sd2) const override;
225 :
226 : /// @brief return the most important change reason
227 : static int lowest_bit(int changeReason);
228 :
229 : protected:
230 :
231 : /// @brief send a speed recommendation to the given vehicle
232 : void msg(const CLeaderDist& cld, double speed, int state);
233 :
234 : /// @brief compute shift so that prevSublane + shift = newSublane
235 : int computeSublaneShift(const MSEdge* prevEdge, const MSEdge* curEdge);
236 :
237 : /// @brief get the longest vehicle in the given info
238 : CLeaderDist getLongest(const MSLeaderDistanceInfo& ldi) const;
239 :
240 : bool tieBrakeLeader(const MSVehicle* veh) const;
241 :
242 : /// @brief get the slowest vehicle in the given info
243 : static CLeaderDist getSlowest(const MSLeaderDistanceInfo& ldi);
244 :
245 : /// @brief restrict latDist to permissible speed and determine blocking state depending on that distance
246 : int checkBlocking(const MSLane& neighLane, double& latDist, double maneuverDist, int laneOffset,
247 : const MSLeaderDistanceInfo& leaders,
248 : const MSLeaderDistanceInfo& followers,
249 : const MSLeaderDistanceInfo& blockers,
250 : const MSLeaderDistanceInfo& neighLeaders,
251 : const MSLeaderDistanceInfo& neighFollowers,
252 : const MSLeaderDistanceInfo& neighBlockers,
253 : std::vector<CLeaderDist>* collectLeadBlockers = 0,
254 : std::vector<CLeaderDist>* collectFollowBlockers = 0,
255 : bool keepLatGapManeuver = false,
256 : double gapFactor = 0,
257 : int* retBlockedFully = 0);
258 :
259 : /// @brief check whether any of the vehicles overlaps with ego
260 : int checkBlockingVehicles(const MSVehicle* ego, const MSLeaderDistanceInfo& vehicles,
261 : int laneOffset, double latDist, double foeOffset, bool leaders,
262 : double& safeLatGapRight, double& safeLatGapLeft,
263 : std::vector<CLeaderDist>* collectBlockers = 0) const;
264 :
265 : /// @brief return whether the given intervals overlap
266 : static bool overlap(double right, double left, double right2, double left2);
267 :
268 : /// @brief compute lane change action from desired lateral distance
269 : static LaneChangeAction getLCA(int state, double latDist);
270 :
271 : /// @brief compute strategic lane change actions
272 : /// TODO: Better documentation, refs #2
273 : int checkStrategicChange(int ret,
274 : const MSLane& neighLane,
275 : int laneOffset,
276 : const MSLeaderDistanceInfo& leaders,
277 : const MSLeaderDistanceInfo& neighLeaders,
278 : const MSVehicle::LaneQ& curr,
279 : const MSVehicle::LaneQ& neigh,
280 : const MSVehicle::LaneQ& best,
281 : int bestLaneOffset,
282 : bool changeToBest,
283 : double& currentDist,
284 : double neighDist,
285 : double laDist,
286 : double roundaboutBonus,
287 : double latLaneDist,
288 : bool checkOpposite,
289 : double& latDist
290 : );
291 :
292 :
293 : bool mustOvertakeStopped(bool checkCurrent, const MSLane& neighLane, const MSLeaderDistanceInfo& leaders, const MSLeaderDistanceInfo& neighLead,
294 : double posOnLane, double neighDist, bool right, double latLaneDist, double& currentDist, double& latDist);
295 :
296 : /// @brief check whether lateral gap requirements are met override the current maneuver if necessary
297 : int keepLatGap(int state,
298 : const MSLeaderDistanceInfo& leaders,
299 : const MSLeaderDistanceInfo& followers,
300 : const MSLeaderDistanceInfo& blockers,
301 : const MSLeaderDistanceInfo& neighLeaders,
302 : const MSLeaderDistanceInfo& neighFollowers,
303 : const MSLeaderDistanceInfo& neighBlockers,
304 : const MSLane& neighLane,
305 : int laneOffset,
306 : double& latDist,
307 : double& maneuverDist,
308 : int& blocked);
309 :
310 :
311 : /// @brief check remaining lateral gaps for the given foe vehicles and optionally update minimum lateral gaps
312 : void updateGaps(const MSLeaderDistanceInfo& others, double foeOffset, double oldCenter, double gapFactor,
313 : double& surplusGapRight, double& surplusGapLeft, bool saveMinGap = false, double netOverlap = 0,
314 : double latDist = 0,
315 : std::vector<CLeaderDist>* collectBlockers = 0);
316 :
317 : /// @brief compute the gap factor for the given state
318 : double computeGapFactor(int state) const;
319 :
320 : /// @brief return the width of this vehicle (padded for numerical stability)
321 : double getWidth() const;
322 :
323 : /// @brief find leaders/followers that are already in a car-following relationship with ego
324 : void updateCFRelated(const MSLeaderDistanceInfo& vehicles, double foeOffset, bool leaders);
325 :
326 : /// @brief return the current sublane width (and return a sensible value when running without sublanes)
327 : double getSublaneWidth() {
328 : return MSGlobals::gLateralResolution > 0 ? MSGlobals::gLateralResolution : myVehicle.getLane()->getWidth();
329 : }
330 :
331 : /// @brief commit to lane change maneuver potentially overriding safe speed
332 : void commitManoeuvre(int blocked, int blockedFully,
333 : const MSLeaderDistanceInfo& leaders,
334 : const MSLeaderDistanceInfo& neighLeaders,
335 : const MSLane& neighLane,
336 : double maneuverDist);
337 :
338 : /// @brief compute speed when committing to an urgent change that is safe in regard to leading vehicles
339 : double commitFollowSpeed(double speed, double latDist, double secondsToLeaveLane, const MSLeaderDistanceInfo& leaders, double foeOffset) const;
340 :
341 : /// @brief check whether the sublane continues on the next lane
342 : bool sublaneEnds(int i, const MSLane* next, double shift);
343 :
344 : /// @brief estimate average speed over mySpeedGainLookahead time
345 : double forecastAverageSpeed(double vSafe, double vMax, double gap, double vLeader) const;
346 :
347 : /// @brief compute speedGain when moving by the given amount
348 : double computeSpeedGain(double latDistSublane, double defaultNextSpeed) const;
349 :
350 : /// @brief get lateral position of this vehicle
351 : double getPosLat();
352 :
353 : /// @brief get lateral drift for the current step
354 : double getLateralDrift();
355 :
356 : /// @brief return current edge width optionally extended by opposite direction lane width
357 : double getLeftBorder(bool checkOpposite = true) const;
358 :
359 : /// @brief return vehicle position relative to the current edge (extend by another virtual lane for opposite-direction driving)
360 : double getVehicleCenter() const;
361 :
362 : /// @brief return the right offset of the neighboring lane relative to the current edge
363 : double getNeighRight(const MSLane& neighLane) const;
364 :
365 : /* @brief check whether vehicle speed is appropriate for the intended maneuver distance
366 : * (rather than doing an orthogonal slide) */
367 : bool preventSliding(double maneuverDist) const;
368 :
369 : /// @brief check against thresholds
370 : inline bool wantsKeepRight(double keepRightProb) const;
371 :
372 : /// @brief check whether lane is an upcoming bidi lane
373 : bool isBidi(const MSLane* lane) const;
374 :
375 : /// @brief avoid unsafe lateral speed (overruling lcAccelLat)
376 : double emergencySpeedLat(double speedLat) const;
377 :
378 : protected:
379 : /// @brief a value for tracking the probability that a change to the right is beneficial
380 : double mySpeedGainProbabilityRight;
381 : /// @brief a value for tracking the probability that a change to the left is beneficial
382 : double mySpeedGainProbabilityLeft;
383 :
384 : /* @brief a value for tracking the probability of following the/"Rechtsfahrgebot"
385 : * A larger negative value indicates higher probability for moving to the
386 : * right (as in mySpeedGainProbability) */
387 : double myKeepRightProbability;
388 :
389 : double myLeadingBlockerLength;
390 : double myLeftSpace;
391 :
392 : /*@brief the speed to use when computing the look-ahead distance for
393 : * determining urgency of strategic lane changes */
394 : double myLookAheadSpeed;
395 :
396 : /// @brief expected travel speeds on all sublanes on the current edge(!)
397 : std::vector<double> myExpectedSublaneSpeeds;
398 :
399 : /// @brief expected travel speeds on all sublanes on the current edge(!)
400 : const MSEdge* myLastEdge;
401 :
402 : /// @brief flag to prevent speed adaptation by slowing down
403 : bool myDontBrake;
404 :
405 : /// @brief whether the current lane changing maneuver can be finished in a single step
406 : bool myCanChangeFully;
407 :
408 : /// @brief the lateral distance the vehicle can safely move in the currently considered direction
409 : double mySafeLatDistRight;
410 : double mySafeLatDistLeft;
411 :
412 : /// @brief set of vehicles that are in a car-following relationship with ego (leader of followers)
413 : std::set<const MSVehicle*> myCFRelated;
414 : bool myCFRelatedReady;
415 :
416 : /// @name user configurable model parameters (can be changed via TraCI)
417 : //@{
418 : double myStrategicParam;
419 : double myCooperativeParam;
420 : double mySpeedGainParam;
421 : double myKeepRightParam;
422 : double myOppositeParam;
423 : double mySublaneParam;
424 : // @brief minimum lateral gap
425 : double myMinGapLat;
426 : // @brief willingness to encroach on other vehicles laterally (pushing them around)
427 : double myPushy;
428 : // @brief dynamic component of willingness for longitudinal gap reduction
429 : double myImpatience;
430 : double myMinImpatience;
431 : // @brief time to reach maximum impatience in seconds
432 : double myTimeToImpatience;
433 : // @brief lateral acceleration
434 : double myAccelLat;
435 : // @brief distance to turn at which alignment should be adjusted to the turn direction
436 : double myTurnAlignmentDist;
437 : // @brief the factor by which the lookahead distance to the left differs from the lookahead to the right
438 : double myLookaheadLeft;
439 : // @brief the factor by which the speedGain-threshold for the leftdiffers from the threshold for the right
440 : double mySpeedGainRight;
441 : // @brief lane discipline factor
442 : double myLaneDiscipline;
443 : // @brief lookahead for speedGain in seconds
444 : double mySpeedGainLookahead;
445 : // @brief the minimum time to spent driving without lane change after a speed-gain change
446 : double mySpeedGainRemainTime;
447 : // @brief bonus factor staying on the inside of multi-lane roundabout
448 : double myRoundaboutBonus;
449 : // @brief factor for cooperative speed adjustment
450 : double myCooperativeSpeed;
451 : // time for unrestricted driving on the right to accept keepRight change
452 : double myKeepRightAcceptanceTime;
453 : // @brief speed difference factor for overtaking the leader on the neighbor lane before changing to that lane
454 : double myOvertakeDeltaSpeedFactor;
455 : //@}
456 :
457 : /// @name derived parameters
458 : //@{
459 : // @brief threshold value for changing to the right
460 : double myChangeProbThresholdRight;
461 : // @brief threshold value for changing to the left
462 : double myChangeProbThresholdLeft;
463 : // @brief threshold value for accepting speed loss to achieve desired sublane alignment
464 : double mySpeedLossProbThreshold;
465 :
466 : // @brief state of lane keeping imperfection
467 : double mySigmaState;
468 : //@}
469 :
470 : };
|
