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