Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-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_LC2013.cpp
15 : /// @author Daniel Krajzewicz
16 : /// @author Jakob Erdmann
17 : /// @author Friedemann Wesner
18 : /// @author Sascha Krieg
19 : /// @author Michael Behrisch
20 : /// @author Laura Bieker
21 : /// @author Leonhard Luecken
22 : /// @date Fri, 08.10.2013
23 : ///
24 : // A lane change model developed by J. Erdmann
25 : // based on the model of D. Krajzewicz developed between 2004 and 2011 (MSLCM_DK2004)
26 : /****************************************************************************/
27 : #include <config.h>
28 :
29 : #include <iostream>
30 : #include <utils/common/RandHelper.h>
31 : #include <utils/common/StringUtils.h>
32 : #include <microsim/transportables/MSPModel.h>
33 : #include <microsim/transportables/MSTransportableControl.h>
34 : #include <microsim/MSEdge.h>
35 : #include <microsim/MSLane.h>
36 : #include <microsim/MSLink.h>
37 : #include <microsim/MSDriverState.h>
38 : #include <microsim/MSNet.h>
39 : #include <microsim/MSStop.h>
40 : #include "MSLCHelper.h"
41 : #include "MSLCM_LC2013.h"
42 :
43 :
44 : // ===========================================================================
45 : // variable definitions
46 : // ===========================================================================
47 : #define MAGIC_OFFSET 1.
48 : #define LOOK_FORWARD 10.
49 :
50 : #define JAM_FACTOR 1.
51 :
52 : #define LCA_RIGHT_IMPATIENCE -1.
53 : #define CUT_IN_LEFT_SPEED_THRESHOLD 27.
54 :
55 : #define LOOK_AHEAD_MIN_SPEED 0.0
56 : #define LOOK_AHEAD_SPEED_MEMORY 0.9
57 :
58 : #define HELP_DECEL_FACTOR 1.0
59 :
60 : #define HELP_OVERTAKE (10.0 / 3.6)
61 : #define MIN_FALLBEHIND (7.0 / 3.6)
62 :
63 : #define RELGAIN_NORMALIZATION_MIN_SPEED 10.0
64 : #define URGENCY 2.0
65 : #define OPPOSITE_URGENCY 5.0
66 :
67 : #define KEEP_RIGHT_TIME 5.0 // the number of seconds after which a vehicle should move to the right lane
68 :
69 : #define KEEP_RIGHT_HEADWAY 2.0
70 : #define MAX_ONRAMP_LENGTH 200.
71 : #define TURN_LANE_DIST 200.0 // the distance at which a lane leading elsewhere is considered to be a turn-lane that must be avoided
72 :
73 : #define LC_RESOLUTION_SPEED_LAT 0.5 // the lateral speed (in m/s) for a standing vehicle which was unable to finish a continuous LC in time (in case mySpeedLatStanding==0), see #3771
74 :
75 : #define REACT_TO_STOPPED_DISTANCE 100
76 : #define BLOCKER_IS_BLOCKED_TIME_THRESHOLD 5 // the time after which a blocking neighbor is treated similar to a stopped vehicle
77 :
78 : // ===========================================================================
79 : // debug defines
80 : // ===========================================================================
81 : //#define DEBUG_CONSTRUCTOR
82 : //#define DEBUG_PATCH_SPEED
83 : //#define DEBUG_INFORMED
84 : //#define DEBUG_INFORMER
85 : //#define DEBUG_WANTS_CHANGE
86 : //#define DEBUG_SLOW_DOWN
87 : //#define DEBUG_COOPERATE
88 : //#define DEBUG_SAVE_BLOCKER_LENGTH
89 :
90 : //#define DEBUG_COND (myVehicle.getID() == "disabled")
91 : #define DEBUG_COND (myVehicle.isSelected())
92 : //#define DEBUG_COND (true)
93 :
94 : // ===========================================================================
95 : // member method definitions
96 : // ===========================================================================
97 3571874 : MSLCM_LC2013::MSLCM_LC2013(MSVehicle& v) :
98 : MSAbstractLaneChangeModel(v, LaneChangeModel::LC2013),
99 3571874 : mySpeedGainProbability(0),
100 3571874 : myKeepRightProbability(0),
101 3571874 : myLeadingBlockerLength(0),
102 3571874 : myLeftSpace(0),
103 3571874 : myLookAheadSpeed(LOOK_AHEAD_MIN_SPEED),
104 3571874 : myStrategicParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_STRATEGIC_PARAM, 1)),
105 3571874 : myCooperativeParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_PARAM, 1)),
106 3571874 : mySpeedGainParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAIN_PARAM, 1)),
107 3571874 : myKeepRightParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_KEEPRIGHT_PARAM, 1)),
108 3571874 : myOppositeParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_OPPOSITE_PARAM, 1)),
109 3571874 : myLookaheadLeft(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_LOOKAHEADLEFT, 2.0)),
110 3571874 : mySpeedGainRight(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAINRIGHT, 0.1)),
111 3571874 : myAssertive(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_ASSERTIVE, 1)),
112 3571874 : mySpeedGainLookahead(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD, 0)),
113 3571874 : mySpeedGainRemainTime(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAIN_REMAIN_TIME, 20)),
114 3571874 : mySpeedGainUrgency(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAIN_URGENCY, 50)),
115 3571874 : myRoundaboutBonus(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_ROUNDABOUT, myCooperativeParam)),
116 3571874 : myCooperativeSpeed(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_SPEED, myCooperativeParam)),
117 3571874 : myKeepRightAcceptanceTime(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_KEEPRIGHT_ACCEPTANCE_TIME, -1)),
118 3571874 : myOvertakeDeltaSpeedFactor(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_OVERTAKE_DELTASPEED_FACTOR, 0)),
119 7143748 : myExperimentalParam1(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_EXPERIMENTAL1, 0)) {
120 3571874 : initDerivedParameters();
121 : #ifdef DEBUG_CONSTRUCTOR
122 : if (DEBUG_COND) {
123 : std::cout << SIMTIME
124 : << " create lcModel veh=" << myVehicle.getID()
125 : << " lcStrategic=" << myStrategicParam
126 : << " lcCooperative=" << myCooperativeParam
127 : << " lcSpeedGain=" << mySpeedGainParam
128 : << " lcKeepRight=" << myKeepRightParam
129 : << "\n";
130 : }
131 : #endif
132 3571874 : }
133 :
134 7143618 : MSLCM_LC2013::~MSLCM_LC2013() {
135 3571809 : changed();
136 7143618 : }
137 :
138 :
139 : void
140 3580700 : MSLCM_LC2013::initDerivedParameters() {
141 3580700 : if (mySpeedGainParam <= 0) {
142 7096 : myChangeProbThresholdRight = std::numeric_limits<double>::max();
143 7096 : myChangeProbThresholdLeft = std::numeric_limits<double>::max();
144 : } else {
145 3573604 : myChangeProbThresholdRight = (0.2 / mySpeedGainRight) / mySpeedGainParam;
146 3573604 : myChangeProbThresholdLeft = 0.2 / mySpeedGainParam;
147 : }
148 3580700 : }
149 :
150 :
151 : bool
152 3273 : MSLCM_LC2013::debugVehicle() const {
153 3273 : return DEBUG_COND;
154 : }
155 :
156 :
157 : int
158 150730930 : MSLCM_LC2013::wantsChange(
159 : int laneOffset,
160 : MSAbstractLaneChangeModel::MSLCMessager& msgPass,
161 : int blocked,
162 : const std::pair<MSVehicle*, double>& leader,
163 : const std::pair<MSVehicle*, double>& follower,
164 : const std::pair<MSVehicle*, double>& neighLead,
165 : const std::pair<MSVehicle*, double>& neighFollow,
166 : const MSLane& neighLane,
167 : const std::vector<MSVehicle::LaneQ>& preb,
168 : MSVehicle** lastBlocked,
169 : MSVehicle** firstBlocked) {
170 :
171 : #ifdef DEBUG_WANTS_CHANGE
172 : if (DEBUG_COND) {
173 : std::cout << "\nWANTS_CHANGE\n" << SIMTIME
174 : << std::setprecision(gPrecision)
175 : << " veh=" << myVehicle.getID()
176 : << " lane=" << myVehicle.getLane()->getID()
177 : << " pos=" << myVehicle.getPositionOnLane()
178 : << " posLat=" << myVehicle.getLateralPositionOnLane()
179 : << " speed=" << myVehicle.getSpeed()
180 : << " considerChangeTo=" << (laneOffset == -1 ? "right" : "left")
181 : << "\n";
182 : }
183 : #endif
184 :
185 150730930 : const int result = _wantsChange(laneOffset, msgPass, blocked, leader, follower, neighLead, neighFollow, neighLane, preb, lastBlocked, firstBlocked);
186 :
187 : #ifdef DEBUG_WANTS_CHANGE
188 : if (DEBUG_COND) {
189 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " result=" << toString((LaneChangeAction)result) << " blocked=" << toString((LaneChangeAction)blocked) << "\n\n\n";
190 : }
191 : #endif
192 :
193 150730930 : return result;
194 : }
195 :
196 :
197 : double
198 424733113 : MSLCM_LC2013::patchSpeed(const double min, const double wanted, const double max, const MSCFModel& cfModel) {
199 :
200 : #ifdef DEBUG_PATCH_SPEED
201 : if (DEBUG_COND) {
202 : std::cout << "\nPATCH_SPEED\n"
203 : << SIMTIME
204 : << " veh=" << myVehicle.getID()
205 : << " lane=" << myVehicle.getLane()->getID()
206 : << " pos=" << myVehicle.getPositionOnLane()
207 : << " v=" << myVehicle.getSpeed()
208 : << " min=" << min
209 : << " wanted=" << wanted
210 : << " max=" << max
211 : << "\n";
212 : }
213 : #endif
214 :
215 : // negative min speed may be passed when using ballistic updated
216 424733113 : const double newSpeed = _patchSpeed(MAX2(min, 0.0), wanted, max, cfModel);
217 :
218 : #ifdef DEBUG_PATCH_SPEED
219 : if (DEBUG_COND) {
220 : const std::string patched = (wanted != newSpeed ? " patched=" + toString(newSpeed) : "");
221 : std::cout << patched
222 : << "\n";
223 : }
224 : #endif
225 :
226 424733113 : return newSpeed;
227 : }
228 :
229 :
230 : double
231 424733113 : MSLCM_LC2013::_patchSpeed(double min, const double wanted, double max, const MSCFModel& cfModel) {
232 424733113 : int state = myOwnState;
233 : #ifdef DEBUG_PATCH_SPEED
234 : if (DEBUG_COND) {
235 : std::cout
236 : << "\n" << SIMTIME << std::setprecision(gPrecision)
237 : << " patchSpeed state=" << toString((LaneChangeAction)state) << " myLCAccelerationAdvices=" << toString(myLCAccelerationAdvices)
238 : << "\n speed=" << myVehicle.getSpeed() << " min=" << min << " wanted=" << wanted
239 : << "\n myLeadingBlockerLength=" << myLeadingBlockerLength
240 : << "\n";
241 : }
242 : #endif
243 :
244 : // letting vehicles merge in at the end of the lane in case of counter-lane change, step#2
245 : double nVSafe = wanted;
246 : bool gotOne = false;
247 : // if we want to change and have a blocking leader and there is enough room for him in front of us
248 424733113 : if (myLeadingBlockerLength != 0) {
249 555982 : double space = myLeftSpace - myLeadingBlockerLength - MAGIC_OFFSET - myVehicle.getVehicleType().getMinGap();
250 : #ifdef DEBUG_PATCH_SPEED
251 : if (DEBUG_COND) {
252 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " myLeftSpace=" << myLeftSpace << " myLeadingBlockerLength=" << myLeadingBlockerLength << " space=" << space << "\n";
253 : }
254 : #endif
255 555982 : if (space > 0 && (myVehicle.getLane()->isNormal() || myVehicle.getCurrentEdge()->isRoundabout())) { // XXX space > -MAGIC_OFFSET
256 : // compute speed for decelerating towards a place which allows the blocking leader to merge in in front
257 370881 : const double vMinEmergency = myVehicle.getCarFollowModel().minNextSpeedEmergency(myVehicle.getSpeed(), &myVehicle);
258 370881 : double safe = cfModel.stopSpeed(&myVehicle, myVehicle.getSpeed(), space, MSCFModel::CalcReason::LANE_CHANGE);
259 : max = MIN2(max, MAX2(safe, vMinEmergency));
260 : // if we are approaching this place
261 370881 : if (safe < wanted) {
262 : // return this speed as the speed to use
263 46832 : if (safe < min) {
264 8727 : if (safe >= vMinEmergency) {
265 : // permit harder braking if needed and helpful
266 : min = MAX2(vMinEmergency, safe);
267 : }
268 : }
269 : #ifdef DEBUG_PATCH_SPEED
270 : if (DEBUG_COND) {
271 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " slowing down for leading blocker, safe=" << safe << (safe + NUMERICAL_EPS < min ? " (not enough)" : "") << "\n";
272 : }
273 : #endif
274 : nVSafe = MAX2(min, safe);
275 : gotOne = true;
276 : }
277 : }
278 : }
279 :
280 424733113 : const double coopWeight = MAX2(0.0, MIN2(1.0, myCooperativeSpeed));
281 447938462 : for (auto i : myLCAccelerationAdvices) {
282 : double a = i.first;
283 23205349 : double v = myVehicle.getSpeed() + ACCEL2SPEED(a);
284 :
285 23205349 : if (v >= min && v <= max && (MSGlobals::gSemiImplicitEulerUpdate
286 : // ballistic update: (negative speeds may appear, e.g. min<0, v<0), BUT:
287 : // XXX: LaneChanging returns -1 to indicate no restrictions, which leads to probs here (Leo), refs. #2577
288 : // As a quick fix, we just dismiss cases where v=-1
289 : // VERY rarely (whenever a requested help-acceleration is really indicated by v=-1)
290 : // this can lead to failing lane-change attempts, though)
291 253373 : || v != -1)) {
292 5290017 : if (i.second) {
293 : // own advice, no scaling needed
294 : nVSafe = MIN2(v, nVSafe);
295 : } else {
296 2186675 : nVSafe = MIN2(v * coopWeight + (1 - coopWeight) * wanted, nVSafe);
297 : }
298 : gotOne = true;
299 : #ifdef DEBUG_PATCH_SPEED
300 : if (DEBUG_COND) {
301 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " got nVSafe=" << nVSafe << " isOwn: " << i.second << " rawV=" << v << "\n";
302 : }
303 : #endif
304 : } else {
305 : if (v < min) {
306 : #ifdef DEBUG_PATCH_SPEED
307 : if (DEBUG_COND) {
308 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " ignoring low nVSafe=" << v << " min=" << min << "\n";
309 : }
310 : #endif
311 : } else {
312 : #ifdef DEBUG_PATCH_SPEED
313 : if (DEBUG_COND) {
314 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " ignoring high nVSafe=" << v << " max=" << max << "\n";
315 : }
316 : #endif
317 : }
318 : }
319 : }
320 : // myDontBrake is used in counter-lane-change situations with relief connection
321 424733113 : if (gotOne && !myDontBrake) {
322 : #ifdef DEBUG_PATCH_SPEED
323 : if (DEBUG_COND) {
324 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " got vSafe\n";
325 : }
326 : #endif
327 : return nVSafe;
328 : }
329 :
330 : // check whether the vehicle is blocked
331 420334856 : if ((state & LCA_WANTS_LANECHANGE) != 0 && (state & LCA_BLOCKED) != 0) {
332 9771799 : if ((state & LCA_STRATEGIC) != 0) {
333 : // necessary decelerations are controlled via vSafe. If there are
334 : // none it means we should speed up
335 : #ifdef DEBUG_PATCH_SPEED
336 : if (DEBUG_COND) {
337 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_WANTS_LANECHANGE (strat, no vSafe)\n";
338 : }
339 : #endif
340 2687348 : return (max + wanted) / 2.0;
341 7084451 : } else if ((state & LCA_COOPERATIVE) != 0) {
342 : // only minor adjustments in speed should be done
343 710719 : if ((state & LCA_BLOCKED_BY_LEADER) != 0) {
344 : #ifdef DEBUG_PATCH_SPEED
345 : if (DEBUG_COND) {
346 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_LEADER (coop)\n";
347 : }
348 : #endif
349 576495 : if (wanted >= 0.) {
350 576495 : return (MAX2(0., min) + wanted) / 2.0;
351 : } else {
352 : return wanted;
353 : }
354 : }
355 134224 : if ((state & LCA_BLOCKED_BY_FOLLOWER) != 0) {
356 : #ifdef DEBUG_PATCH_SPEED
357 : if (DEBUG_COND) {
358 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_FOLLOWER (coop)\n";
359 : }
360 : #endif
361 134224 : return (max + wanted) / 2.0;
362 : }
363 : //} else { // VARIANT_16
364 : // // only accelerations should be performed
365 : // if ((state & LCA_BLOCKED_BY_FOLLOWER) != 0) {
366 : // if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_FOLLOWER\n";
367 : // return (max + wanted) / 2.0;
368 : // }
369 : }
370 : }
371 :
372 : /*
373 : // decelerate if being a blocking follower
374 : // (and does not have to change lanes)
375 : if ((state & LCA_AMBLOCKINGFOLLOWER) != 0) {
376 : if (fabs(max - myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle)) < 0.001 && min == 0) { // !!! was standing
377 : if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER (standing)\n";
378 : return 0;
379 : }
380 : if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER\n";
381 :
382 : //return min; // VARIANT_3 (brakeStrong)
383 : return (min + wanted) / 2.0;
384 : }
385 : if ((state & LCA_AMBACKBLOCKER) != 0) {
386 : if (max <= myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle) && min == 0) { // !!! was standing
387 : if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBACKBLOCKER (standing)\n";
388 : //return min; VARIANT_9 (backBlockVSafe)
389 : return nVSafe;
390 : }
391 : }
392 : if ((state & LCA_AMBACKBLOCKER_STANDING) != 0) {
393 : if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBACKBLOCKER_STANDING\n";
394 : //return min;
395 : return nVSafe;
396 : }
397 : */
398 :
399 : // accelerate if being a blocking leader or blocking follower not able to brake
400 : // (and does not have to change lanes)
401 416936789 : if ((state & LCA_AMBLOCKINGLEADER) != 0 && myCooperativeSpeed >= 0) {
402 : #ifdef DEBUG_PATCH_SPEED
403 : if (DEBUG_COND) {
404 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGLEADER\n";
405 : }
406 : #endif
407 1439757 : return (max + wanted) / 2.0;
408 : }
409 :
410 : if ((state & LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0) {
411 : #ifdef DEBUG_PATCH_SPEED
412 : if (DEBUG_COND) {
413 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER_DONTBRAKE\n";
414 : }
415 : #endif
416 : /*
417 : // VARIANT_4 (dontbrake)
418 : if (max <= myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle) && min == 0) { // !!! was standing
419 : return wanted;
420 : }
421 : return (min + wanted) / 2.0;
422 : */
423 : }
424 415497032 : if (!myVehicle.getLane()->getEdge().hasLaneChanger()) {
425 : // remove chaning information if on a road with a single lane
426 261216027 : changed();
427 : }
428 : return wanted;
429 : }
430 :
431 :
432 : void*
433 4236666 : MSLCM_LC2013::inform(void* info, MSVehicle* sender) {
434 : UNUSED_PARAMETER(sender);
435 : Info* pinfo = (Info*)info;
436 : assert(pinfo->first >= 0 || !MSGlobals::gSemiImplicitEulerUpdate);
437 4236666 : addLCSpeedAdvice(pinfo->first, false);
438 4236666 : myOwnState |= pinfo->second;
439 : #ifdef DEBUG_INFORMED
440 : if (DEBUG_COND) {
441 : std::cout << SIMTIME
442 : << " veh=" << myVehicle.getID()
443 : << " informedBy=" << sender->getID()
444 : << " info=" << pinfo->second
445 : << " vSafe=" << pinfo->first
446 : << "\n";
447 : }
448 : #endif
449 4236666 : delete pinfo;
450 4236666 : return (void*) true;
451 : }
452 :
453 : double
454 4164933 : MSLCM_LC2013::overtakeDistance(const MSVehicle* follower, const MSVehicle* leader, const double gap, double followerSpeed, double leaderSpeed) {
455 4164933 : followerSpeed = followerSpeed == INVALID_SPEED ? follower->getSpeed() : followerSpeed;
456 4164933 : leaderSpeed = leaderSpeed == INVALID_SPEED ? leader->getSpeed() : leaderSpeed;
457 : double overtakeDist = (gap // drive to back of leader
458 4164933 : + leader->getVehicleType().getLengthWithGap() // drive to front of leader
459 4164933 : + follower->getVehicleType().getLength() // follower back reaches leader front
460 4164933 : + leader->getCarFollowModel().getSecureGap( // save gap to leader
461 4164933 : leader, follower, leaderSpeed, followerSpeed, follower->getCarFollowModel().getMaxDecel()));
462 4164933 : return MAX2(overtakeDist, 0.);
463 : }
464 :
465 :
466 : double
467 4290567 : MSLCM_LC2013::informLeader(MSAbstractLaneChangeModel::MSLCMessager& msgPass,
468 : int blocked,
469 : int dir,
470 : const std::pair<MSVehicle*, double>& neighLead,
471 : double remainingSeconds) {
472 4290567 : double plannedSpeed = myVehicle.getSpeed();
473 4290567 : if (!isOpposite()) {
474 4191702 : plannedSpeed = MIN2(plannedSpeed,
475 4191702 : myVehicle.getCarFollowModel().stopSpeed(&myVehicle, myVehicle.getSpeed(), myLeftSpace - myLeadingBlockerLength));
476 : }
477 4908109 : for (auto i : myLCAccelerationAdvices) {
478 : const double a = i.first;
479 617542 : if (a >= -myVehicle.getCarFollowModel().getMaxDecel()) {
480 617520 : plannedSpeed = MIN2(plannedSpeed, myVehicle.getSpeed() + ACCEL2SPEED(a));
481 : }
482 : }
483 : #ifdef DEBUG_INFORMER
484 : if (DEBUG_COND) {
485 : std::cout << "\nINFORM_LEADER"
486 : << "\nspeed=" << myVehicle.getSpeed() << " planned=" << plannedSpeed << "\n";
487 : }
488 : #endif
489 :
490 4290567 : const MSVehicle* const nv = neighLead.first;
491 4290567 : if (nv == nullptr) {
492 : // not overtaking
493 : return plannedSpeed;
494 : }
495 4072889 : const double neighNextSpeed = nv->getSpeed() - ACCEL2SPEED(MAX2(1.0, -nv->getAcceleration()));
496 : double neighNextGap;
497 4072889 : if (MSGlobals::gSemiImplicitEulerUpdate) {
498 3845596 : neighNextGap = neighLead.second + SPEED2DIST(neighNextSpeed - plannedSpeed);
499 : } else {
500 227293 : neighNextGap = neighLead.second + SPEED2DIST((nv->getSpeed() + neighNextSpeed) / 2) - SPEED2DIST((myVehicle.getSpeed() + plannedSpeed) / 2);
501 : }
502 4072889 : if ((blocked & LCA_BLOCKED_BY_LEADER) != 0) {
503 3300798 : if (MSLCHelper::divergentRoute(myVehicle, *nv)) {
504 : //std::cout << SIMTIME << " ego=" << myVehicle.getID() << " ignoresDivergentBlockingLeader=" << nv->getID() << "\n";
505 : return plannedSpeed;
506 : }
507 : #ifdef DEBUG_INFORMER
508 : if (DEBUG_COND) {
509 : std::cout << " blocked by leader nv=" << nv->getID() << " nvSpeed=" << nv->getSpeed() << " needGap="
510 : << myVehicle.getCarFollowModel().getSecureGap(&myVehicle, nv, myVehicle.getSpeed(), nv->getSpeed(), nv->getCarFollowModel().getMaxDecel()) << "\n";
511 : }
512 : #endif
513 : // decide whether we want to overtake the leader or follow it
514 : double overtakeTime;
515 3296240 : const double overtakeDist = overtakeDistance(&myVehicle, nv, neighLead.second);
516 3296240 : const double dv = plannedSpeed - nv->getSpeed();
517 :
518 3296240 : if (dv > myOvertakeDeltaSpeedFactor * myVehicle.getLane()->getSpeedLimit()) {
519 1418665 : overtakeTime = overtakeDist / dv;
520 : } else if (nv->getWaitingSeconds() > BLOCKER_IS_BLOCKED_TIME_THRESHOLD
521 1072074 : && !isOpposite()
522 2924275 : && (myVehicle.getVehicleType().getLengthWithGap() + nv->getVehicleType().getLengthWithGap()) <= myLeftSpace) {
523 : // -> set overtakeTime to indicate possibility of overtaking (only if there is enough space)
524 932413 : overtakeTime = remainingSeconds - 1;
525 : } else {
526 : // -> set overtakeTime to something indicating impossibility of overtaking
527 945162 : overtakeTime = remainingSeconds + 1;
528 : }
529 :
530 : #ifdef DEBUG_INFORMER
531 : if (DEBUG_COND) {
532 : std::cout << SIMTIME << " informLeader() of " << myVehicle.getID()
533 : << "\nnv = " << nv->getID()
534 : << "\nplannedSpeed = " << plannedSpeed
535 : << "\nleaderSpeed = " << nv->getSpeed()
536 : << "\nmyLeftSpace = " << myLeftSpace
537 : << "\nremainingSeconds = " << remainingSeconds
538 : << "\novertakeDist = " << overtakeDist
539 : << "\novertakeTime = " << overtakeTime
540 : << std::endl;
541 : }
542 : #endif
543 :
544 3296240 : if ((dv < myOvertakeDeltaSpeedFactor * myVehicle.getLane()->getSpeedLimit()
545 : // overtaking on the right on an uncongested highway is forbidden (noOvertakeLCLeft)
546 2496361 : || (dir == LCA_MLEFT && !myVehicle.congested() && !myAllowOvertakingRight)
547 : // not enough space to overtake?
548 4841623 : || (MSGlobals::gSemiImplicitEulerUpdate && myLeftSpace - myLeadingBlockerLength - myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed()) < overtakeDist)
549 : // using brakeGap() without headway seems adequate in a situation where the obstacle (the lane end) is not moving [XXX implemented in branch ticket860, can be used in general if desired, refs. #2575] (Leo).
550 2341053 : || (!MSGlobals::gSemiImplicitEulerUpdate && myLeftSpace - myLeadingBlockerLength - myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed(), getCarFollowModel().getMaxDecel(), 0.) < overtakeDist)
551 : // not enough time to overtake? (skipped for a stopped leader [currently only for ballistic update XXX: check if appropriate for euler, too, refs. #2575] to ensure that it can be overtaken if only enough space is exists) (Leo)
552 2327893 : || (remainingSeconds < overtakeTime && (MSGlobals::gSemiImplicitEulerUpdate || !nv->isStopped())))
553 : // opposite driving and must overtake
554 3956352 : && (!neighLead.first->isStopped() || (isOpposite() && neighLead.second >= 0))) {
555 : // cannot overtake
556 1456952 : msgPass.informNeighLeader(new Info(std::numeric_limits<double>::max(), dir | LCA_AMBLOCKINGLEADER), &myVehicle);
557 : // slow down smoothly to follow leader
558 : // account for minor decelerations by the leader (dawdling)
559 4370856 : double targetSpeed = MAX3(myVehicle.getCarFollowModel().minNextSpeed(myVehicle.getSpeed(), &myVehicle),
560 1456952 : getCarFollowModel().followSpeed(&myVehicle, myVehicle.getSpeed(), neighNextGap, neighNextSpeed, nv->getCarFollowModel().getMaxDecel()),
561 : // avoid changing on intersection
562 1475026 : (myVehicle.getLane()->isNormal() || myVehicle.getCurrentEdge()->isRoundabout()) ? 0 : ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxAccel()));
563 1456952 : if (targetSpeed < myVehicle.getSpeed()) {
564 : // slow down smoothly to follow leader
565 494084 : const double decel = remainingSeconds == 0. ? myVehicle.getCarFollowModel().getMaxDecel() :
566 494084 : MIN2(myVehicle.getCarFollowModel().getMaxDecel(),
567 494084 : MAX2(MIN_FALLBEHIND, (myVehicle.getSpeed() - targetSpeed) / remainingSeconds));
568 494084 : const double nextSpeed = MIN2(plannedSpeed, MAX2(0.0, myVehicle.getSpeed() - ACCEL2SPEED(decel)));
569 : #ifdef DEBUG_INFORMER
570 : if (DEBUG_COND) {
571 : std::cout << SIMTIME
572 : << " cannot overtake leader nv=" << nv->getID()
573 : << " dv=" << dv
574 : << " myLookAheadSpeed=" << myLookAheadSpeed
575 : << " myLeftSpace=" << myLeftSpace
576 : << " overtakeDist=" << overtakeDist
577 : << " overtakeTime=" << overtakeTime
578 : << " remainingSeconds=" << remainingSeconds
579 : << " currentGap=" << neighLead.second
580 : << " brakeGap=" << myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed(), getCarFollowModel().getMaxDecel(), 0.)
581 : << " neighNextSpeed=" << neighNextSpeed
582 : << " neighNextGap=" << neighNextGap
583 : << " targetSpeed=" << targetSpeed
584 : << " nextSpeed=" << nextSpeed
585 : << "\n";
586 : }
587 : #endif
588 494084 : addLCSpeedAdvice(nextSpeed);
589 494084 : return nextSpeed;
590 : } else {
591 : // leader is fast enough anyway
592 : #ifdef DEBUG_INFORMER
593 : if (DEBUG_COND) {
594 : std::cout << SIMTIME
595 : << " cannot overtake fast leader nv=" << nv->getID()
596 : << " dv=" << dv
597 : << " myLookAheadSpeed=" << myLookAheadSpeed
598 : << " myLeftSpace=" << myLeftSpace
599 : << " overtakeDist=" << overtakeDist
600 : << " myLeadingBlockerLength=" << myLeadingBlockerLength
601 : << " overtakeTime=" << overtakeTime
602 : << " remainingSeconds=" << remainingSeconds
603 : << " currentGap=" << neighLead.second
604 : << " neighNextSpeed=" << neighNextSpeed
605 : << " neighNextGap=" << neighNextGap
606 : << " targetSpeed=" << targetSpeed
607 : << "\n";
608 : }
609 : #endif
610 962868 : addLCSpeedAdvice(targetSpeed);
611 962868 : return plannedSpeed;
612 : }
613 : } else {
614 : // overtaking, leader should not accelerate
615 : #ifdef DEBUG_INFORMER
616 : if (DEBUG_COND) {
617 : std::cout << SIMTIME
618 : << " wants to overtake leader nv=" << nv->getID()
619 : << " dv=" << dv
620 : << " overtakeDist=" << overtakeDist
621 : << " remainingSeconds=" << remainingSeconds
622 : << " overtakeTime=" << overtakeTime
623 : << " currentGap=" << neighLead.second
624 : << " secureGap=" << nv->getCarFollowModel().getSecureGap(nv, &myVehicle, nv->getSpeed(), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel())
625 : << "\n";
626 : }
627 : #endif
628 : // no need to pass a message if the neighbor is waiting/stuck anyway (but sending it would risk deadlock)
629 1839288 : if (nv->getWaitingSeconds() <= BLOCKER_IS_BLOCKED_TIME_THRESHOLD) {
630 821667 : msgPass.informNeighLeader(new Info(nv->getSpeed(), dir | LCA_AMBLOCKINGLEADER), &myVehicle);
631 : }
632 1839288 : return -1; // XXX: using -1 is ambiguous for the ballistic update! Currently this is being catched in patchSpeed() (Leo), consider returning INVALID_SPEED, refs. #2577
633 : }
634 : } else { // (remainUnblocked)
635 : // we are not blocked now. make sure we stay far enough from the leader
636 1544182 : const double targetSpeed = MAX2(
637 772091 : myVehicle.getCarFollowModel().minNextSpeed(myVehicle.getSpeed(), &myVehicle),
638 772091 : getCarFollowModel().followSpeed(&myVehicle, myVehicle.getSpeed(), neighNextGap, neighNextSpeed, nv->getCarFollowModel().getMaxDecel()));
639 772091 : addLCSpeedAdvice(targetSpeed);
640 : #ifdef DEBUG_INFORMER
641 : if (DEBUG_COND) {
642 : std::cout << " not blocked by leader nv=" << nv->getID()
643 : << " nvSpeed=" << nv->getSpeed()
644 : << " gap=" << neighLead.second
645 : << " neighNextSpeed=" << neighNextSpeed
646 : << " neighNextGap=" << neighNextGap
647 : << " needGap=" << myVehicle.getCarFollowModel().getSecureGap(&myVehicle, nv, myVehicle.getSpeed(), nv->getSpeed(), nv->getCarFollowModel().getMaxDecel())
648 : << " targetSpeed=" << targetSpeed
649 : << "\n";
650 : }
651 : #endif
652 : return MIN2(targetSpeed, plannedSpeed);
653 : }
654 : }
655 :
656 : void
657 2448541 : MSLCM_LC2013::informFollower(MSAbstractLaneChangeModel::MSLCMessager& msgPass,
658 : int blocked,
659 : int dir,
660 : const std::pair<MSVehicle*, double>& neighFollow,
661 : double remainingSeconds,
662 : double plannedSpeed) {
663 :
664 2448541 : MSVehicle* nv = neighFollow.first;
665 2448541 : const double plannedAccel = SPEED2ACCEL(MAX2(MIN2(getCarFollowModel().getMaxAccel(), plannedSpeed - myVehicle.getSpeed()), -getCarFollowModel().getMaxDecel()));
666 :
667 : #ifdef DEBUG_INFORMER
668 : if (DEBUG_COND) {
669 : std::cout << "\nINFORM_FOLLOWER"
670 : << "\nspeed=" << myVehicle.getSpeed() << " planned=" << plannedSpeed << "\n";
671 : }
672 :
673 : #endif
674 2448541 : if ((blocked & LCA_BLOCKED_BY_FOLLOWER) != 0 && nv != nullptr) {
675 1427480 : if (MSLCHelper::divergentRoute(myVehicle, *nv)) {
676 : //std::cout << SIMTIME << " ego=" << myVehicle.getID() << " ignoresDivergentBlockingFollower=" << nv->getID() << "\n";
677 : return;
678 : }
679 : #ifdef DEBUG_INFORMER
680 : if (DEBUG_COND) {
681 : std::cout << " blocked by follower nv=" << nv->getID() << " nvSpeed=" << nv->getSpeed() << " needGap="
682 : << nv->getCarFollowModel().getSecureGap(nv, &myVehicle, nv->getSpeed(), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel()) << " planned=" << plannedSpeed << "\n";
683 : }
684 : #endif
685 :
686 : // are we fast enough to cut in without any help?
687 1427326 : if (MAX2(plannedSpeed, 0.) - nv->getSpeed() >= HELP_OVERTAKE) {
688 80335 : const double neededGap = nv->getCarFollowModel().getSecureGap(nv, &myVehicle, nv->getSpeed(), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel());
689 80335 : if ((neededGap - neighFollow.second) / remainingSeconds < (MAX2(plannedSpeed, 0.) - nv->getSpeed())) {
690 : #ifdef DEBUG_INFORMER
691 : if (DEBUG_COND) {
692 : std::cout << " wants to cut in before nv=" << nv->getID() << " without any help." << "\nneededGap = " << neededGap << "\n";
693 : }
694 : #endif
695 : // follower might even accelerate but not to much
696 : // XXX: I don't understand this. The needed gap was determined for nv->getSpeed(), not for (plannedSpeed - HELP_OVERTAKE)?! (Leo), refs. #2578
697 69987 : msgPass.informNeighFollower(new Info(MAX2(plannedSpeed, 0.) - HELP_OVERTAKE, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
698 69987 : return;
699 : }
700 : }
701 :
702 : // decide whether we will request help to cut in before the follower or allow to be overtaken
703 :
704 : // PARAMETERS
705 : // assume other vehicle will assume the equivalent of 1 second of
706 : // maximum deceleration to help us (will probably be spread over
707 : // multiple seconds)
708 : // -----------
709 : const double helpDecel = nv->getCarFollowModel().getMaxDecel() * HELP_DECEL_FACTOR;
710 :
711 : // follower's new speed in next step
712 : double neighNewSpeed;
713 : // follower's new speed after 1s.
714 : double neighNewSpeed1s;
715 : // velocity difference, gap after follower-deceleration
716 : double dv, decelGap;
717 :
718 1357339 : if (MSGlobals::gSemiImplicitEulerUpdate) {
719 : // euler
720 1228960 : neighNewSpeed = MAX2(0., nv->getSpeed() - ACCEL2SPEED(helpDecel));
721 1228960 : neighNewSpeed1s = MAX2(0., nv->getSpeed() - helpDecel); // TODO: consider introduction of a configurable anticipationTime here (see far below in the !blocked part). Refs. #2578
722 : // change in the gap between ego and blocker over 1 second (not STEP!)
723 : // XXX: though here it is calculated as if it were one step!? (Leo) Refs. #2578
724 1228960 : dv = plannedSpeed - neighNewSpeed1s; // XXX: what is this quantity (if TS!=1)?
725 : // new gap between follower and self in case the follower does brake for 1s
726 : // XXX: if the step-length is not 1s., this is not the gap after 1s. deceleration!
727 : // And this formula overestimates the real gap. Isn't that problematic? (Leo)
728 : // Below, it seems that decelGap > secureGap is taken to indicate the possibility
729 : // to cut in within the next time-step. However, this is not the case, if TS<1s.,
730 : // since decelGap is (not exactly, though!) the gap after 1s. Refs. #2578
731 1228960 : decelGap = neighFollow.second + dv;
732 : } else {
733 : // ballistic
734 : // negative newSpeed-extrapolation possible, if stop lies within the next time-step
735 : // XXX: this code should work for the euler case as well, since gapExtrapolation() takes
736 : // care of this, but for TS!=1 we will have different behavior (see previous remark) Refs. #2578
737 128379 : neighNewSpeed = nv->getSpeed() - ACCEL2SPEED(helpDecel);
738 128379 : neighNewSpeed1s = nv->getSpeed() - helpDecel;
739 :
740 128379 : dv = myVehicle.getSpeed() - nv->getSpeed(); // current velocity difference
741 256758 : decelGap = getCarFollowModel().gapExtrapolation(1., neighFollow.second, myVehicle.getSpeed(),
742 128379 : nv->getSpeed(), plannedAccel, -helpDecel, myVehicle.getMaxSpeedOnLane(), nv->getMaxSpeedOnLane());
743 : }
744 :
745 1357339 : const double secureGap = nv->getCarFollowModel().getSecureGap(nv, &myVehicle, MAX2(neighNewSpeed1s, 0.),
746 1357339 : MAX2(plannedSpeed, 0.), myVehicle.getCarFollowModel().getMaxDecel());
747 :
748 1357339 : const double onRampThreshold = myVehicle.getLane()->getSpeedLimit() * 0.8 * myExperimentalParam1 * (1 - myVehicle.getImpatience());
749 :
750 : #ifdef DEBUG_INFORMER
751 : if (DEBUG_COND) {
752 : std::cout << SIMTIME
753 : << " speed=" << myVehicle.getSpeed()
754 : << " plannedSpeed=" << plannedSpeed
755 : << " threshold=" << onRampThreshold
756 : << " neighNewSpeed=" << neighNewSpeed
757 : << " neighNewSpeed1s=" << neighNewSpeed1s
758 : << " dv=" << dv
759 : << " gap=" << neighFollow.second
760 : << " decelGap=" << decelGap
761 : << " secureGap=" << secureGap
762 : << "\n";
763 : }
764 : #endif
765 : // prevent vehicles on an on ramp stopping the main flow
766 : if (dir == LCA_MLEFT
767 505342 : && myVehicle.getLane()->isAccelLane()
768 1420219 : && neighNewSpeed1s < onRampThreshold) {
769 : return;
770 : }
771 :
772 1338654 : if (decelGap > 0 && decelGap >= secureGap) {
773 : // XXX: This does not assure that the leader can cut in in the next step if TS < 1 (see above)
774 : // this seems to be supposed in the following (euler code)...?! (Leo) Refs. #2578
775 :
776 : // if the blocking follower brakes it could help
777 : // how hard does it actually need to be?
778 : // to be safe in the next step the following equation has to hold for the follower's vsafe:
779 : // vsafe <= followSpeed(gap=currentGap - SPEED2DIST(vsafe), ...)
780 : double vsafe, vsafe1;
781 :
782 90918 : if (MSGlobals::gSemiImplicitEulerUpdate) {
783 : // euler
784 : // we compute an upper bound on vsafe by doing the computation twice
785 80901 : vsafe1 = MAX2(neighNewSpeed, nv->getCarFollowModel().followSpeed(
786 80901 : nv, nv->getSpeed(), neighFollow.second + SPEED2DIST(plannedSpeed), plannedSpeed, getCarFollowModel().getMaxDecel()));
787 80901 : vsafe = MAX2(neighNewSpeed, nv->getCarFollowModel().followSpeed(
788 80901 : nv, nv->getSpeed(), neighFollow.second + SPEED2DIST(plannedSpeed - vsafe1), plannedSpeed, getCarFollowModel().getMaxDecel()));
789 : //assert(vsafe <= vsafe1); assertion does not hold for models with randomness in followSpeed (W99)
790 : } else {
791 : // ballistic
792 :
793 : // XXX: This block should actually do as well for euler update (TODO: test!), refs #2575
794 : // we compute an upper bound on vsafe
795 : // next step's gap without help deceleration (nv's speed assumed constant)
796 10017 : double nextGap = getCarFollowModel().gapExtrapolation(TS,
797 10017 : neighFollow.second, myVehicle.getSpeed(),
798 10017 : nv->getSpeed(), plannedAccel, 0,
799 10017 : myVehicle.getMaxSpeedOnLane(), nv->getMaxSpeedOnLane());
800 : #ifdef DEBUG_INFORMER
801 : if (DEBUG_COND) {
802 : std::cout << "nextGap=" << nextGap << " (without help decel) \n";
803 : }
804 : #endif
805 :
806 : // NOTE: the second argument of MIN2() can get larger than nv->getSpeed()
807 20034 : vsafe1 = MIN2(nv->getSpeed(), MAX2(neighNewSpeed,
808 10017 : nv->getCarFollowModel().followSpeed(nv,
809 10017 : nv->getSpeed(), nextGap,
810 : MAX2(0., plannedSpeed),
811 : getCarFollowModel().getMaxDecel())));
812 :
813 :
814 : // next step's gap with possibly less than maximal help deceleration (in case vsafe1 > neighNewSpeed)
815 10017 : double decel2 = SPEED2ACCEL(nv->getSpeed() - vsafe1);
816 10017 : nextGap = getCarFollowModel().gapExtrapolation(TS,
817 10017 : neighFollow.second, myVehicle.getSpeed(),
818 10017 : nv->getSpeed(), plannedAccel, -decel2,
819 10017 : myVehicle.getMaxSpeedOnLane(), nv->getMaxSpeedOnLane());
820 :
821 : // vsafe = MAX(neighNewSpeed, safe speed assuming next_gap)
822 : // Thus, the gap resulting from vsafe is larger or equal to next_gap
823 : // in contrast to the euler case, where nv's follow speed doesn't depend on the actual speed,
824 : // we need to assure, that nv doesn't accelerate
825 20034 : vsafe = MIN2(nv->getSpeed(), MAX2(neighNewSpeed,
826 10017 : nv->getCarFollowModel().followSpeed(nv,
827 10017 : nv->getSpeed(), nextGap,
828 : MAX2(0., plannedSpeed),
829 : getCarFollowModel().getMaxDecel())));
830 :
831 : assert(vsafe >= vsafe1 - NUMERICAL_EPS);
832 :
833 : #ifdef DEBUG_INFORMER
834 : if (DEBUG_COND) {
835 : std::cout << "nextGap=" << nextGap
836 : << " (with vsafe1 and help decel) \nvsafe1=" << vsafe1
837 : << " vsafe=" << vsafe
838 : << "\n";
839 : }
840 : #endif
841 :
842 : // For subsecond simulation, this might not lead to secure gaps for a long time,
843 : // we seek to establish a secure gap as soon as possible
844 10017 : double nextSecureGap = nv->getCarFollowModel().getSecureGap(nv, &myVehicle, vsafe, plannedSpeed, getCarFollowModel().getMaxDecel());
845 :
846 10017 : if (nextGap < nextSecureGap) {
847 : // establish a secureGap as soon as possible
848 : vsafe = neighNewSpeed;
849 : }
850 :
851 : #ifdef DEBUG_INFORMER
852 : if (DEBUG_COND) {
853 : std::cout << "nextGap=" << nextGap
854 : << " minNextSecureGap=" << nextSecureGap
855 : << " vsafe=" << vsafe << "\n";
856 : }
857 : #endif
858 :
859 : }
860 90918 : msgPass.informNeighFollower(
861 90918 : new Info(vsafe, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
862 :
863 : #ifdef DEBUG_INFORMER
864 : if (DEBUG_COND) {
865 : std::cout << " wants to cut in before nv=" << nv->getID()
866 : << " vsafe1=" << vsafe1 << " vsafe=" << vsafe
867 : << " newSecGap="
868 : << nv->getCarFollowModel().getSecureGap(nv, &myVehicle, vsafe,
869 : plannedSpeed,
870 : myVehicle.getCarFollowModel().getMaxDecel())
871 : << "\n";
872 : }
873 : #endif
874 1247736 : } else if ((MSGlobals::gSemiImplicitEulerUpdate && dv > 0 && dv * remainingSeconds > (secureGap - decelGap + POSITION_EPS))
875 1178064 : || (!MSGlobals::gSemiImplicitEulerUpdate && dv > 0 && dv * (remainingSeconds - 1) > secureGap - decelGap + POSITION_EPS)
876 : ) {
877 :
878 : // XXX: Alternative formulation (encapsulating differences of euler and ballistic) TODO: test, refs. #2575
879 : // double eventualGap = getCarFollowModel().gapExtrapolation(remainingSeconds - 1., decelGap, plannedSpeed, neighNewSpeed1s);
880 : // } else if (eventualGap > secureGap + POSITION_EPS) {
881 :
882 :
883 : // NOTE: This case corresponds to the situation, where some time is left to perform the lc
884 : // For the ballistic case this is interpreted as follows:
885 : // If the follower breaks with helpDecel for one second, this vehicle maintains the plannedSpeed,
886 : // and both continue with their speeds for remainingSeconds seconds the gap will suffice for a laneChange
887 : // For the euler case we had the following comment:
888 : // 'decelerating once is sufficient to open up a large enough gap in time', but:
889 : // XXX: 1) Decelerating *once* does not necessarily lead to the gap decelGap! (if TS<1s.) (Leo)
890 : // 2) Probably, the if() for euler should test for dv * (remainingSeconds-1) > ..., too ?!, refs. #2578
891 72589 : msgPass.informNeighFollower(new Info(neighNewSpeed, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
892 : #ifdef DEBUG_INFORMER
893 : if (DEBUG_COND) {
894 : std::cout << " wants to cut in before nv=" << nv->getID() << " (eventually)\n";
895 : }
896 : #endif
897 1175147 : } else if (dir == LCA_MRIGHT && !myAllowOvertakingRight && !nv->congested()) {
898 : // XXX: check if this requires a special treatment for the ballistic update, refs. #2575
899 : const double vhelp = MAX2(neighNewSpeed, HELP_OVERTAKE);
900 1342 : msgPass.informNeighFollower(new Info(vhelp, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
901 : #ifdef DEBUG_INFORMER
902 : if (DEBUG_COND) {
903 : std::cout << " wants to cut in before nv=" << nv->getID() << " (nv cannot overtake right)\n";
904 : }
905 : #endif
906 : } else {
907 1173805 : double vhelp = MAX2(nv->getSpeed(), myVehicle.getSpeed() + HELP_OVERTAKE);
908 : //if (dir == LCA_MRIGHT && myVehicle.getWaitingSeconds() > LCA_RIGHT_IMPATIENCE &&
909 : // nv->getSpeed() > myVehicle.getSpeed()) {
910 1173805 : if (nv->getSpeed() > myVehicle.getSpeed() &&
911 318802 : ((dir == LCA_MRIGHT && myVehicle.getWaitingSeconds() > LCA_RIGHT_IMPATIENCE) // NOTE: it might be considered to use myVehicle.getAccumulatedWaitingSeconds() > LCA_RIGHT_IMPATIENCE instead (Leo). Refs. #2578
912 202433 : || (dir == LCA_MLEFT && plannedSpeed > CUT_IN_LEFT_SPEED_THRESHOLD) // VARIANT_22 (slowDownLeft)
913 : // XXX this is a hack to determine whether the vehicles is on an on-ramp. This information should be retrieved from the network itself
914 202401 : || (dir == LCA_MLEFT && myVehicle.getLane()->getLength() > MAX_ONRAMP_LENGTH)
915 : )) {
916 : // let the follower slow down to increase the likelihood that later vehicles will be slow enough to help
917 : // follower should still be fast enough to open a gap
918 : // XXX: The probability for that success would be larger if the slow down of the appropriate following vehicle
919 : // would take place without the immediate follower slowing down. We might consider to model reactions of
920 : // vehicles that are not immediate followers. (Leo) -> see ticket #2532
921 361536 : vhelp = MAX2(neighNewSpeed, myVehicle.getSpeed() + HELP_OVERTAKE);
922 : #ifdef DEBUG_INFORMER
923 : if (DEBUG_COND) {
924 : // NOTE: the condition labeled "VARIANT_22" seems to imply that this could as well concern the *left* follower?! (Leo)
925 : // Further, vhelp might be larger than nv->getSpeed(), so the request issued below is not to slow down!? (see below) Refs. #2578
926 : std::cout << " wants right follower to slow down a bit\n";
927 : }
928 : #endif
929 361536 : if (MSGlobals::gSemiImplicitEulerUpdate) {
930 : // euler
931 308591 : if ((nv->getSpeed() - myVehicle.getSpeed()) / helpDecel < remainingSeconds) {
932 :
933 : #ifdef DEBUG_INFORMER
934 : if (DEBUG_COND) {
935 : // NOTE: the condition labeled "VARIANT_22" seems to imply that this could as well concern the *left* follower?! Refs. #2578
936 : std::cout << " wants to cut in before right follower nv=" << nv->getID() << " (eventually)\n";
937 : }
938 : #endif
939 : // XXX: I don't understand. This vhelp might be larger than nv->getSpeed() but the above condition seems to rely
940 : // on the reasoning that if nv breaks with helpDecel for remaining Seconds, nv will be so slow, that this
941 : // vehicle will be able to cut in. But nv might have overtaken this vehicle already (or am I missing sth?). (Leo)
942 : // Ad: To my impression, the intention behind allowing larger speeds for the blocking follower is to prevent a
943 : // situation, where an overlapping follower keeps blocking the ego vehicle. Refs. #2578
944 295859 : msgPass.informNeighFollower(new Info(vhelp, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
945 295859 : return;
946 : }
947 : } else {
948 :
949 : // ballistic (this block is a bit different to the logic in the euler part, but in general suited to work on euler as well.. must be tested <- TODO, refs. #2575)
950 : // estimate gap after remainingSeconds.
951 : // Assumptions:
952 : // (A1) leader continues with currentSpeed. (XXX: That might be wrong: Think of accelerating on an on-ramp or of a congested region ahead!)
953 : // (A2) follower breaks with helpDecel.
954 52945 : const double gapAfterRemainingSecs = getCarFollowModel().gapExtrapolation(
955 52945 : remainingSeconds, neighFollow.second, myVehicle.getSpeed(), nv->getSpeed(), 0, -helpDecel, myVehicle.getMaxSpeedOnLane(), nv->getMaxSpeedOnLane());
956 52945 : const double secureGapAfterRemainingSecs = nv->getCarFollowModel().getSecureGap(nv, &myVehicle,
957 52945 : MAX2(nv->getSpeed() - remainingSeconds * helpDecel, 0.), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel());
958 52945 : if (gapAfterRemainingSecs >= secureGapAfterRemainingSecs) { // XXX: here it would be wise to check whether there is enough space for eventual braking if the maneuver doesn't succeed
959 : #ifdef DEBUG_INFORMER
960 : if (DEBUG_COND) {
961 : std::cout << " wants to cut in before follower nv=" << nv->getID() << " (eventually)\n";
962 : }
963 : #endif
964 : // NOTE: ballistic uses neighNewSpeed instead of vhelp, see my note above. (Leo)
965 : // TODO: recheck if this might cause suboptimal behaviour in some LC-situations. Refs. #2578
966 9253 : msgPass.informNeighFollower(new Info(neighNewSpeed, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
967 9253 : return;
968 : }
969 : }
970 :
971 :
972 : }
973 :
974 : #ifdef DEBUG_INFORMER
975 : if (DEBUG_COND) {
976 : std::cout << SIMTIME
977 : << " veh=" << myVehicle.getID()
978 : << " informs follower " << nv->getID()
979 : << " vhelp=" << vhelp
980 : << "\n";
981 : }
982 : #endif
983 :
984 868693 : msgPass.informNeighFollower(new Info(vhelp, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
985 : // This follower is supposed to overtake us. Slow down smoothly to allow this.
986 868693 : const double overtakeDist = overtakeDistance(nv, &myVehicle, neighFollow.second, vhelp, plannedSpeed);
987 : // speed difference to create a sufficiently large gap
988 868693 : const double needDV = overtakeDist / remainingSeconds;
989 : // make sure the deceleration is not to strong (XXX: should be assured in finalizeSpeed -> TODO: remove the MAX2 if agreed) -> prob with possibly non-existing maximal deceleration for som CF Models(?) Refs. #2578
990 964838 : addLCSpeedAdvice(MAX2(vhelp - needDV, myVehicle.getSpeed() - ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxDecel())));
991 :
992 : #ifdef DEBUG_INFORMER
993 : if (DEBUG_COND) {
994 : std::cout << SIMTIME
995 : << " veh=" << myVehicle.getID()
996 : << " wants to be overtaken by=" << nv->getID()
997 : << " overtakeDist=" << overtakeDist
998 : << " vneigh=" << nv->getSpeed()
999 : << " vhelp=" << vhelp
1000 : << " needDV=" << needDV
1001 : << " vsafe=" << myLCAccelerationAdvices.back().first
1002 : << "\n";
1003 : }
1004 : #endif
1005 : }
1006 1021061 : } else if (neighFollow.first != nullptr && (blocked & LCA_BLOCKED_BY_LEADER)) {
1007 : // we are not blocked by the follower now, make sure it remains that way
1008 546940 : const double vsafe = MSLCHelper::getSpeedPreservingSecureGap(myVehicle, *neighFollow.first, neighFollow.second, plannedSpeed);
1009 546940 : msgPass.informNeighFollower(new Info(vsafe, dir), &myVehicle);
1010 :
1011 : #ifdef DEBUG_INFORMER
1012 : if (DEBUG_COND) {
1013 : std::cout << " wants to cut in before non-blocking follower nv=" << nv->getID() << "\n";
1014 : }
1015 : #endif
1016 : }
1017 : }
1018 :
1019 :
1020 : void
1021 422441330 : MSLCM_LC2013::prepareStep() {
1022 422441330 : MSAbstractLaneChangeModel::prepareStep();
1023 : // keep information about strategic change direction
1024 422441330 : if (!isChangingLanes()) {
1025 421918819 : myOwnState = (myOwnState & LCA_STRATEGIC) ? (myOwnState & LCA_WANTS_LANECHANGE) : 0;
1026 : }
1027 422441330 : myLeadingBlockerLength = 0;
1028 422441330 : myLeftSpace = 0;
1029 : myLCAccelerationAdvices.clear();
1030 422441330 : myDontBrake = false;
1031 : // truncate to work around numerical instability between different builds
1032 422441330 : mySpeedGainProbability = ceil(mySpeedGainProbability * 100000.0) * 0.00001;
1033 422441330 : myKeepRightProbability = ceil(myKeepRightProbability * 100000.0) * 0.00001;
1034 422441330 : if (mySigma > 0 && !isChangingLanes()) {
1035 : // disturb lateral position directly
1036 1154 : const double maxDist = SPEED2DIST(myVehicle.getVehicleType().getMaxSpeedLat());
1037 1154 : const double oldPosLat = myVehicle.getLateralPositionOnLane();
1038 1154 : const double overlap = myVehicle.getLateralOverlap();
1039 : double scaledDelta;
1040 1154 : if (overlap > 0) {
1041 : // return to within lane boundary
1042 : scaledDelta = MIN2(overlap, maxDist);
1043 23 : if (myVehicle.getLateralPositionOnLane() > 0) {
1044 7 : scaledDelta *= -1;
1045 : }
1046 : } else {
1047 : // random drift
1048 1131 : double deltaPosLat = OUProcess::step(oldPosLat,
1049 1131 : myVehicle.getActionStepLengthSecs(),
1050 1131 : MAX2(NUMERICAL_EPS, (1 - mySigma) * 100), mySigma) - oldPosLat;
1051 1131 : deltaPosLat = MAX2(MIN2(deltaPosLat, maxDist), -maxDist);
1052 1131 : scaledDelta = deltaPosLat * myVehicle.getSpeed() / myVehicle.getLane()->getSpeedLimit();
1053 : }
1054 1154 : myVehicle.setLateralPositionOnLane(oldPosLat + scaledDelta);
1055 1154 : setSpeedLat(DIST2SPEED(scaledDelta));
1056 : } else {
1057 422440176 : resetSpeedLat();
1058 : }
1059 422441330 : }
1060 :
1061 :
1062 : double
1063 2429067 : MSLCM_LC2013::getExtraReservation(int bestLaneOffset) const {
1064 2429067 : if (bestLaneOffset < -1) {
1065 : return 20;
1066 2423476 : } else if (bestLaneOffset > 1) {
1067 10673 : return 40;
1068 : }
1069 : return 0;
1070 : }
1071 :
1072 : void
1073 265466986 : MSLCM_LC2013::changed() {
1074 265466986 : myOwnState = 0;
1075 265466986 : mySpeedGainProbability = 0;
1076 265466986 : myKeepRightProbability = 0;
1077 265466986 : if (myVehicle.getBestLaneOffset() == 0) {
1078 : // if we are not yet on our best lane there might still be unseen blockers
1079 : // (during patchSpeed)
1080 265064608 : myLeadingBlockerLength = 0;
1081 265064608 : myLeftSpace = 0;
1082 : }
1083 265466986 : myLookAheadSpeed = LOOK_AHEAD_MIN_SPEED;
1084 : myLCAccelerationAdvices.clear();
1085 265466986 : myDontBrake = false;
1086 265466986 : }
1087 :
1088 :
1089 : void
1090 4652 : MSLCM_LC2013::resetState() {
1091 4652 : myOwnState = 0;
1092 4652 : mySpeedGainProbability = 0;
1093 4652 : myKeepRightProbability = 0;
1094 4652 : myLeadingBlockerLength = 0;
1095 4652 : myLeftSpace = 0;
1096 4652 : myLookAheadSpeed = LOOK_AHEAD_MIN_SPEED;
1097 : myLCAccelerationAdvices.clear();
1098 4652 : myDontBrake = false;
1099 4652 : }
1100 :
1101 :
1102 : int
1103 150730930 : MSLCM_LC2013::_wantsChange(
1104 : int laneOffset,
1105 : MSAbstractLaneChangeModel::MSLCMessager& msgPass,
1106 : int blocked,
1107 : const std::pair<MSVehicle*, double>& leader,
1108 : const std::pair<MSVehicle*, double>& follower,
1109 : const std::pair<MSVehicle*, double>& neighLead,
1110 : const std::pair<MSVehicle*, double>& neighFollow,
1111 : const MSLane& neighLane,
1112 : const std::vector<MSVehicle::LaneQ>& preb,
1113 : MSVehicle** lastBlocked,
1114 : MSVehicle** firstBlocked) {
1115 : assert(laneOffset == 1 || laneOffset == -1);
1116 150730930 : const SUMOTime currentTime = MSNet::getInstance()->getCurrentTimeStep();
1117 : // compute bestLaneOffset
1118 : MSVehicle::LaneQ curr, neigh, best;
1119 : int bestLaneOffset = 0;
1120 : // What do these "dists" mean? Please comment. (Leo) Ad: I now think the following:
1121 : // currentDist is the distance that the vehicle can go on its route without having to
1122 : // change lanes from the current lane. neighDist as currentDist for the considered target lane (i.e., neigh)
1123 : // If this is true I suggest to put this into the docu of wantsChange()
1124 : double currentDist = 0;
1125 : double neighDist = 0;
1126 : int currIdx = 0;
1127 150730930 : const bool checkOpposite = &neighLane.getEdge() != &myVehicle.getLane()->getEdge();
1128 150730930 : const MSLane* prebLane = myVehicle.getLane();
1129 150730930 : if (prebLane->getEdge().isInternal()) {
1130 : // internal edges are not kept inside the bestLanes structure
1131 1071455 : if (isOpposite()) {
1132 550 : prebLane = prebLane->getNormalPredecessorLane();
1133 : } else {
1134 1070905 : prebLane = prebLane->getLinkCont()[0]->getLane();
1135 : }
1136 : }
1137 : // special case: vehicle considers changing to the opposite direction edge
1138 : const int prebOffset = laneOffset;
1139 256516419 : for (int p = 0; p < (int) preb.size(); ++p) {
1140 : //if (DEBUG_COND) {
1141 : // std::cout << " p=" << p << " prebLane=" << prebLane->getID() << " preb.p=" << preb[p].lane->getID() << "\n";
1142 : //}
1143 256516419 : if (preb[p].lane == prebLane && p + laneOffset >= 0) {
1144 : assert(p + prebOffset < (int)preb.size());
1145 : curr = preb[p];
1146 150730930 : neigh = preb[p + prebOffset];
1147 150730930 : currentDist = curr.length;
1148 150730930 : neighDist = neigh.length;
1149 150730930 : bestLaneOffset = curr.bestLaneOffset;
1150 150730930 : if (bestLaneOffset == 0 && preb[p + prebOffset].bestLaneOffset == 0 && !checkOpposite) {
1151 : #ifdef DEBUG_WANTS_CHANGE
1152 : if (DEBUG_COND) {
1153 : std::cout << STEPS2TIME(currentTime)
1154 : << " veh=" << myVehicle.getID()
1155 : << " bestLaneOffsetOld=" << bestLaneOffset
1156 : << " bestLaneOffsetNew=" << laneOffset
1157 : << "\n";
1158 : }
1159 : #endif
1160 : bestLaneOffset = prebOffset;
1161 : }
1162 150730930 : best = preb[p + bestLaneOffset];
1163 : currIdx = p;
1164 : break;
1165 : }
1166 : }
1167 : assert(curr.lane != nullptr);
1168 : assert(neigh.lane != nullptr);
1169 : assert(best.lane != nullptr);
1170 : // direction specific constants
1171 150730930 : const bool right = (laneOffset == -1);
1172 150730930 : const double posOnLane = getForwardPos();
1173 : double driveToNextStop = -std::numeric_limits<double>::max();
1174 150730930 : if (myVehicle.nextStopDist() < std::numeric_limits<double>::max()
1175 150730930 : && &myVehicle.getNextStop().lane->getEdge() == &myVehicle.getLane()->getEdge()) {
1176 : // vehicle can always drive up to stop distance
1177 : // @note this information is dynamic and thus not available in updateBestLanes()
1178 : // @note: nextStopDist was compute before the vehicle moved
1179 1825440 : driveToNextStop = myVehicle.nextStopDist();
1180 1825440 : const double stopPos = posOnLane + myVehicle.nextStopDist() - myVehicle.getLastStepDist();
1181 : #ifdef DEBUG_WANTS_CHANGE
1182 : if (DEBUG_COND) {
1183 : std::cout << SIMTIME << std::setprecision(gPrecision) << " veh=" << myVehicle.getID()
1184 : << " stopDist=" << myVehicle.nextStopDist()
1185 : << " lastDist=" << myVehicle.getLastStepDist()
1186 : << " stopPos=" << stopPos
1187 : << " currentDist=" << currentDist
1188 : << " neighDist=" << neighDist
1189 : << "\n";
1190 : }
1191 : #endif
1192 : currentDist = MAX2(currentDist, stopPos);
1193 : neighDist = MAX2(neighDist, stopPos);
1194 : }
1195 150730930 : const int lca = (right ? LCA_RIGHT : LCA_LEFT);
1196 : const int myLca = (right ? LCA_MRIGHT : LCA_MLEFT);
1197 : const int lcaCounter = (right ? LCA_LEFT : LCA_RIGHT);
1198 150730930 : bool changeToBest = (right && bestLaneOffset < 0) || (!right && bestLaneOffset > 0);
1199 : // keep information about being a leader/follower
1200 150730930 : int ret = (myOwnState & 0xffff0000);
1201 : int req = 0; // the request to change or stay
1202 :
1203 150730930 : ret = slowDownForBlocked(lastBlocked, ret);
1204 150730930 : if (lastBlocked != firstBlocked) {
1205 150730930 : ret = slowDownForBlocked(firstBlocked, ret);
1206 : }
1207 :
1208 : #ifdef DEBUG_WANTS_CHANGE
1209 : if (DEBUG_COND) {
1210 : std::cout << SIMTIME
1211 : << " veh=" << myVehicle.getID()
1212 : << " _wantsChange state=" << myOwnState
1213 : << " myLCAccelerationAdvices=" << toString(myLCAccelerationAdvices)
1214 : << " firstBlocked=" << Named::getIDSecure(*firstBlocked)
1215 : << " lastBlocked=" << Named::getIDSecure(*lastBlocked)
1216 : << " leader=" << Named::getIDSecure(leader.first)
1217 : << " leaderGap=" << leader.second
1218 : << " follower=" << Named::getIDSecure(follower.first)
1219 : << " followerGap=" << follower.second
1220 : << " neighLead=" << Named::getIDSecure(neighLead.first)
1221 : << " neighLeadGap=" << neighLead.second
1222 : << " neighFollow=" << Named::getIDSecure(neighFollow.first)
1223 : << " neighFollowGap=" << neighFollow.second
1224 : << "\n";
1225 : }
1226 : #endif
1227 :
1228 : // we try to estimate the distance which is necessary to get on a lane
1229 : // we have to get on in order to keep our route
1230 : // we assume we need something that depends on our velocity
1231 : // and compare this with the free space on our wished lane
1232 : //
1233 : // if the free space is somehow(<-?) less than the space we need, we should
1234 : // definitely try to get to the desired lane
1235 : //
1236 : // this rule forces our vehicle to change the lane if a lane changing is necessary soon
1237 :
1238 :
1239 : // we do not want the lookahead distance to change all the time so we let it decay slowly
1240 : // (in contrast, growth is applied instantaneously)
1241 150730930 : if (myVehicle.getSpeed() > myLookAheadSpeed) {
1242 31038345 : myLookAheadSpeed = myVehicle.getSpeed();
1243 : } else {
1244 : // memory decay factor for this action step
1245 119692585 : const double memoryFactor = 1. - (1. - LOOK_AHEAD_SPEED_MEMORY) * myVehicle.getActionStepLengthSecs();
1246 : assert(memoryFactor > 0.);
1247 119692585 : myLookAheadSpeed = MAX2(LOOK_AHEAD_MIN_SPEED,
1248 119692585 : (memoryFactor * myLookAheadSpeed + (1 - memoryFactor) * myVehicle.getSpeed()));
1249 : }
1250 150730930 : double laDist = myLookAheadSpeed * LOOK_FORWARD * myStrategicParam * (right ? 1 : myLookaheadLeft);
1251 150730930 : laDist += myVehicle.getVehicleType().getLengthWithGap() * 2.;
1252 150730930 : const bool hasStoppedLeader = leader.first != 0 && leader.first->isStopped() && leader.second < (currentDist - posOnLane);
1253 150730930 : const bool hasBidiLeader = myVehicle.getLane()->getBidiLane() != nullptr && MSLCHelper::isBidiLeader(leader.first, curr.bestContinuations);
1254 150730930 : const bool hasBidiNeighLeader = neighLane.getBidiLane() != nullptr && MSLCHelper::isBidiLeader(neighLead.first, neigh.bestContinuations);
1255 :
1256 150730930 : if (bestLaneOffset == 0 && hasBidiLeader) {
1257 : // getting out of the way is enough to clear the blockage
1258 : laDist = 0;
1259 150729301 : } else if (bestLaneOffset == 0 && hasStoppedLeader) {
1260 : // react to a stopped leader on the current lane
1261 : // The value of laDist is doubled below for the check whether the lc-maneuver can be taken out
1262 : // on the remaining distance (because the vehicle has to change back and forth). Therefore multiply with 0.5.
1263 68703 : laDist = 0.5 * (myVehicle.getVehicleType().getLengthWithGap()
1264 68703 : + leader.first->getVehicleType().getLengthWithGap()
1265 68703 : + leader.second);
1266 150660598 : } else if (bestLaneOffset == laneOffset && neighLead.first != 0 && (neighLead.first->isStopped() || hasBidiNeighLeader) && neighLead.second < (currentDist - posOnLane)) {
1267 : // react to a stopped leader on the target lane (if it is the bestLane)
1268 274370 : if (isOpposite()) {
1269 : // always allow changing back
1270 52356 : laDist = (myVehicle.getVehicleType().getLengthWithGap()
1271 52356 : + neighLead.first->getVehicleType().getLengthWithGap()
1272 52356 : + neighLead.second);
1273 222014 : } else if (!hasStoppedLeader &&
1274 202638 : ((neighLead.second + myVehicle.getVehicleType().getLengthWithGap() + neighLead.first->getVehicleType().getLengthWithGap()) < (currentDist - posOnLane)
1275 4894 : || hasBidiNeighLeader)) {
1276 : // do not change to the target lane until passing the stopped vehicle
1277 : // (unless the vehicle blocks our intended stopping position, then we have to wait anyway)
1278 : changeToBest = false;
1279 : }
1280 : }
1281 150730930 : if (myStrategicParam < 0) {
1282 : laDist = -1e3; // never perform strategic change
1283 : }
1284 :
1285 : // free space that is available for changing
1286 : //const double neighSpeed = (neighLead.first != 0 ? neighLead.first->getSpeed() :
1287 : // neighFollow.first != 0 ? neighFollow.first->getSpeed() :
1288 : // best.lane->getSpeedLimit());
1289 : // @note: while this lets vehicles change earlier into the correct direction
1290 : // it also makes the vehicles more "selfish" and prevents changes which are necessary to help others
1291 :
1292 :
1293 :
1294 : // Next we assign to roundabout edges a larger distance than to normal edges
1295 : // in order to decrease sense of lc urgency and induce higher usage of inner roundabout lanes.
1296 150730930 : const double roundaboutBonus = MSLCHelper::getRoundaboutDistBonus(myVehicle, myRoundaboutBonus, curr, neigh, best);
1297 150730930 : currentDist += roundaboutBonus;
1298 150730930 : neighDist += roundaboutBonus;
1299 :
1300 150730930 : const double usableDist = MAX2(currentDist - posOnLane - best.occupation * JAM_FACTOR, driveToNextStop);
1301 : //- (best.lane->getVehicleNumber() * neighSpeed)); // VARIANT 9 jfSpeed
1302 150730930 : const double maxJam = MAX2(preb[currIdx + prebOffset].occupation, preb[currIdx].occupation);
1303 150730930 : const double vMax = myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle);
1304 150730930 : const double neighVMax = neighLane.getVehicleMaxSpeed(&myVehicle);
1305 : // upper bound which will be restricted successively
1306 150730930 : double thisLaneVSafe = vMax;
1307 150730930 : const bool checkOverTakeRight = avoidOvertakeRight();
1308 :
1309 150730930 : double neighLeftPlace = MAX2(0.0, neighDist - posOnLane - maxJam);
1310 150730930 : if (neighLead.first != 0 && neighLead.first->isStopped()) {
1311 247443 : neighLeftPlace = MIN2(neighLeftPlace, neighLead.second);
1312 : }
1313 :
1314 : #ifdef DEBUG_WANTS_CHANGE
1315 : if (DEBUG_COND) {
1316 : std::cout << STEPS2TIME(currentTime)
1317 : << " veh=" << myVehicle.getID()
1318 : << " laSpeed=" << myLookAheadSpeed
1319 : << " laDist=" << laDist
1320 : << " currentDist=" << currentDist
1321 : << " usableDist=" << usableDist
1322 : << " bestLaneOffset=" << bestLaneOffset
1323 : << " best.occupation=" << best.occupation
1324 : << " best.length=" << best.length
1325 : << "\n roundaboutBonus=" << roundaboutBonus
1326 : << " maxJam=" << maxJam
1327 : << " neighDist=" << neighDist
1328 : << " neighLeftPlace=" << neighLeftPlace
1329 : << (hasBidiLeader ? " bidiLeader" : "")
1330 : << (hasBidiNeighLeader ? " bidiNeighLeader" : "")
1331 : << "\n";
1332 : }
1333 : #endif
1334 :
1335 : bool changeLeftToAvoidOvertakeRight = false;
1336 112681482 : if (changeToBest && bestLaneOffset == curr.bestLaneOffset
1337 161304415 : && currentDistDisallows(usableDist, bestLaneOffset, laDist)) {
1338 : /// @brief we urgently need to change lanes to follow our route
1339 4232646 : ret = ret | lca | LCA_STRATEGIC | LCA_URGENT;
1340 : } else {
1341 : // VARIANT_20 (noOvertakeRight)
1342 146498284 : if (neighLead.first != 0 && checkOverTakeRight && !right) {
1343 : // check for slower leader on the left. we should not overtake but
1344 : // rather move left ourselves (unless congested)
1345 : MSVehicle* nv = neighLead.first;
1346 11357614 : const double deltaV = MAX2(vMax - neighLane.getVehicleMaxSpeed(nv),
1347 11357614 : myVehicle.getSpeed() - nv->getSpeed());
1348 11357614 : if (deltaV > 0) {
1349 2257079 : const double vMaxDecel = getCarFollowModel().getSpeedAfterMaxDecel(myVehicle.getSpeed());
1350 2257079 : const double vSafeFollow = getCarFollowModel().followSpeed(
1351 2257079 : &myVehicle, myVehicle.getSpeed(), neighLead.second, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel());
1352 2257079 : const double vStayBehind = nv->getSpeed() - HELP_OVERTAKE;
1353 : double vSafe;
1354 2257079 : if (vSafeFollow >= vMaxDecel) {
1355 : vSafe = vSafeFollow;
1356 : } else {
1357 : vSafe = MAX2(vMaxDecel, vStayBehind);
1358 : }
1359 2257079 : if (mySpeedGainProbability < myChangeProbThresholdLeft) {
1360 1533876 : vSafe = MAX2(vSafe, nv->getSpeed());
1361 : }
1362 2257079 : thisLaneVSafe = MIN2(thisLaneVSafe, vSafe);
1363 2257079 : addLCSpeedAdvice(vSafe);
1364 : // only generate impulse for overtaking left shortly before braking would be necessary
1365 2257079 : const double deltaGapFuture = deltaV * 8;
1366 2257079 : const double vSafeFuture = getCarFollowModel().followSpeed(
1367 2257079 : &myVehicle, myVehicle.getSpeed(), neighLead.second - deltaGapFuture, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel());
1368 2257079 : if (vSafeFuture < vSafe) {
1369 : const double relativeGain = deltaV / MAX2(vMax,
1370 1857347 : RELGAIN_NORMALIZATION_MIN_SPEED);
1371 1857347 : mySpeedGainProbability += myVehicle.getActionStepLengthSecs() * relativeGain;
1372 : changeLeftToAvoidOvertakeRight = true;
1373 : }
1374 : #ifdef DEBUG_WANTS_CHANGE
1375 : if (DEBUG_COND) {
1376 : std::cout << STEPS2TIME(currentTime)
1377 : << " avoid overtaking on the right nv=" << nv->getID()
1378 : << " deltaV=" << deltaV
1379 : << " nvSpeed=" << nv->getSpeed()
1380 : << " mySpeedGainProbability=" << mySpeedGainProbability
1381 : << " planned acceleration =" << myLCAccelerationAdvices.back().first
1382 : << "\n";
1383 : }
1384 : #endif
1385 : }
1386 : }
1387 146498284 : const bool currFreeUntilNeighEnd = leader.first == nullptr || neighDist - posOnLane <= leader.second;
1388 281451834 : const double overtakeDist = (leader.first == 0 || hasBidiLeader ? -1 :
1389 134953550 : leader.second + myVehicle.getVehicleType().getLength() + leader.first->getVehicleType().getLengthWithGap());
1390 146741558 : const double overtakeDist2 = (neighLead.first == 0 || !neighLead.first->isStopped() ? -1 :
1391 243274 : neighLead.second + myVehicle.getVehicleType().getLength() + neighLead.first->getVehicleType().getLengthWithGap());
1392 134959727 : if (leader.first != 0 && (leader.first->isStopped() || hasBidiLeader) && leader.second < REACT_TO_STOPPED_DISTANCE
1393 : // current destination leaves enough space to overtake the leader
1394 112076 : && MIN2(neighDist, currentDist) - posOnLane > overtakeDist
1395 : // maybe do not overtake on the right at high speed
1396 78321 : && (!checkOverTakeRight || !right)
1397 76742 : && myStrategicParam >= 0
1398 146574438 : && (neighLead.first == 0 || !neighLead.first->isStopped()
1399 : // neighboring stopped vehicle leaves enough space to overtake leader
1400 21261 : || neighLead.second > overtakeDist
1401 : // if we cannot pass neighLead before reaching leader we must find another free lane
1402 18406 : || (overtakeDist2 > leader.second && hasFreeLane(laneOffset, neighLead)))) {
1403 : // avoid becoming stuck behind a stopped leader
1404 58234 : currentDist = myVehicle.getPositionOnLane() + leader.second;
1405 : #ifdef DEBUG_WANTS_CHANGE
1406 : if (DEBUG_COND) {
1407 : std::cout << " veh=" << myVehicle.getID() << " overtake stopped leader=" << leader.first->getID()
1408 : << " overtakeDist=" << overtakeDist
1409 : << " overtakeDist2=" << overtakeDist
1410 : << " hasFreeLane=" << hasFreeLane(laneOffset, neighLead)
1411 : << " remaining=" << MIN2(neighDist, currentDist) - posOnLane
1412 : << "\n";
1413 : }
1414 : #endif
1415 58234 : ret = ret | lca | LCA_STRATEGIC | LCA_URGENT;
1416 146440050 : } else if (!changeToBest && currentDistDisallows(neighLeftPlace, abs(bestLaneOffset) + 2, laDist) && !hasBidiLeader) {
1417 : // the opposite lane-changing direction should be done than the one examined herein
1418 : // we'll check whether we assume we could change anyhow and get back in time...
1419 : //
1420 : // this rule prevents the vehicle from moving in opposite direction of the best lane
1421 : // unless the way till the end where the vehicle has to be on the best lane
1422 : // is long enough
1423 : #ifdef DEBUG_WANTS_CHANGE
1424 : if (DEBUG_COND) {
1425 : std::cout << " veh=" << myVehicle.getID() << " could not change back and forth in time (1) neighLeftPlace=" << neighLeftPlace << "\n";
1426 : }
1427 : #endif
1428 33815378 : ret = ret | LCA_STAY | LCA_STRATEGIC;
1429 112624672 : } else if (bestLaneOffset == 0 && (neighLeftPlace * 2. < laDist)) {
1430 : // the current lane is the best and a lane-changing would cause a situation
1431 : // of which we assume we will not be able to return to the lane we have to be on.
1432 : // this rule prevents the vehicle from leaving the current, best lane when it is
1433 : // close to this lane's end
1434 : #ifdef DEBUG_WANTS_CHANGE
1435 : if (DEBUG_COND) {
1436 : std::cout << " veh=" << myVehicle.getID() << " could not change back and forth in time (2) neighLeftPlace=" << neighLeftPlace << "\n";
1437 : }
1438 : #endif
1439 0 : ret = ret | LCA_STAY | LCA_STRATEGIC;
1440 : } else if (bestLaneOffset == 0
1441 3810996 : && (leader.first == 0 || !leader.first->isStopped())
1442 3801552 : && !hasBidiLeader
1443 3801549 : && neigh.bestContinuations.back()->getLinkCont().size() != 0
1444 2144734 : && roundaboutBonus == 0
1445 708265 : && !checkOpposite
1446 604334 : && ((myStrategicParam >= 0 && neighDist < TURN_LANE_DIST)
1447 : // lane changing cannot possibly help
1448 559602 : || (myStrategicParam < 0 && currFreeUntilNeighEnd))
1449 : ) {
1450 : // VARIANT_21 (stayOnBest)
1451 : // we do not want to leave the best lane for a lane which leads elsewhere
1452 : // unless our leader is stopped or we are approaching a roundabout
1453 : #ifdef DEBUG_WANTS_CHANGE
1454 : if (DEBUG_COND) {
1455 : std::cout << " veh=" << myVehicle.getID() << " does not want to leave the bestLane (neighDist=" << neighDist << ")\n";
1456 : }
1457 : #endif
1458 44812 : ret = ret | LCA_STAY | LCA_STRATEGIC;
1459 : }
1460 : }
1461 : // check for overriding TraCI requests
1462 : #ifdef DEBUG_WANTS_CHANGE
1463 : if (DEBUG_COND) {
1464 : std::cout << STEPS2TIME(currentTime) << " veh=" << myVehicle.getID() << " ret=" << toString((LaneChangeAction)ret);
1465 : }
1466 : #endif
1467 : // store state before canceling
1468 150730930 : getCanceledState(laneOffset) |= ret | blocked;
1469 150730930 : ret = myVehicle.influenceChangeDecision(ret);
1470 150730930 : if ((ret & lcaCounter) != 0) {
1471 : // we are not interested in traci requests for the opposite direction here
1472 30 : ret &= ~(LCA_TRACI | lcaCounter | LCA_URGENT);
1473 : }
1474 : #ifdef DEBUG_WANTS_CHANGE
1475 : if (DEBUG_COND) {
1476 : std::cout << " retAfterInfluence=" << toString((LaneChangeAction)ret) << "\n";
1477 : }
1478 : #endif
1479 :
1480 150730930 : if ((ret & LCA_STAY) != 0) {
1481 : // remove TraCI flags because it should not be included in "state-without-traci"
1482 33938977 : ret = getCanceledState(laneOffset);
1483 33938977 : return ret;
1484 : }
1485 116791953 : if ((ret & LCA_URGENT) != 0) {
1486 : // prepare urgent lane change maneuver
1487 : // save the left space
1488 4290600 : myLeftSpace = currentDist - posOnLane;
1489 4290600 : if (changeToBest && abs(bestLaneOffset) > 1) {
1490 : // there might be a vehicle which needs to counter-lane-change one lane further and we cannot see it yet
1491 617436 : myLeadingBlockerLength = MAX2((right ? 20.0 : 40.0), myLeadingBlockerLength);
1492 : #ifdef DEBUG_WANTS_CHANGE
1493 : if (DEBUG_COND) {
1494 : std::cout << " reserving space for unseen blockers myLeadingBlockerLength=" << myLeadingBlockerLength << "\n";
1495 : }
1496 : #endif
1497 : }
1498 :
1499 : // letting vehicles merge in at the end of the lane in case of counter-lane change, step#1
1500 : // if there is a leader and he wants to change to the opposite direction
1501 4290600 : const bool canContinue = curr.bestContinuations.size() > 1;
1502 4290600 : bool canReserve = MSLCHelper::updateBlockerLength(myVehicle, neighLead.first, lcaCounter, myLeftSpace - MAGIC_OFFSET, canContinue, myLeadingBlockerLength);
1503 4290600 : if (*firstBlocked != neighLead.first) {
1504 4047689 : canReserve &= MSLCHelper::updateBlockerLength(myVehicle, *firstBlocked, lcaCounter, myLeftSpace - MAGIC_OFFSET, canContinue, myLeadingBlockerLength);
1505 : }
1506 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
1507 : if (DEBUG_COND) {
1508 : std::cout << SIMTIME << " canReserve=" << canReserve << " canContinue=" << canContinue << "\n";
1509 : }
1510 : #endif
1511 4290600 : if (!canReserve && !isOpposite()) {
1512 : // we have a low-priority relief connection
1513 : // std::cout << SIMTIME << " veh=" << myVehicle.getID() << " cannotReserve for blockers\n";
1514 4372 : myDontBrake = canContinue;
1515 : }
1516 :
1517 4290600 : const int remainingLanes = MAX2(1, abs(bestLaneOffset));
1518 4290600 : const double urgency = isOpposite() ? OPPOSITE_URGENCY : URGENCY;
1519 4290600 : const double remainingSeconds = ((ret & LCA_TRACI) == 0 ?
1520 : //MAX2(STEPS2TIME(TS), (myLeftSpace-myLeadingBlockerLength) / MAX2(myLookAheadSpeed, NUMERICAL_EPS) / remainingLanes / urgency) :
1521 4477608 : MAX2(STEPS2TIME(TS), myLeftSpace / MAX2(myLookAheadSpeed, NUMERICAL_EPS) / remainingLanes / urgency) :
1522 220 : myVehicle.getInfluencer().changeRequestRemainingSeconds(currentTime));
1523 4290600 : if (!hasBidiNeighLeader) {
1524 4290567 : const double plannedSpeed = informLeader(msgPass, blocked, myLca, neighLead, remainingSeconds);
1525 : // NOTE: for the ballistic update case negative speeds may indicate a stop request,
1526 : // while informLeader returns -1 in that case. Refs. #2577
1527 4290567 : if (plannedSpeed >= 0 || (!MSGlobals::gSemiImplicitEulerUpdate && plannedSpeed != -1)) {
1528 : // maybe we need to deal with a blocking follower
1529 2451271 : const bool hasBidiNeighFollower = neighLane.getBidiLane() != nullptr && MSLCHelper::isBidiFollower(&myVehicle, neighFollow.first);
1530 : if (!hasBidiNeighFollower) {
1531 2448541 : informFollower(msgPass, blocked, myLca, neighFollow, remainingSeconds, plannedSpeed);
1532 : }
1533 : }
1534 : #ifdef DEBUG_WANTS_CHANGE
1535 : if (DEBUG_COND) {
1536 : std::cout << STEPS2TIME(currentTime)
1537 : << " veh=" << myVehicle.getID()
1538 : << " myLeftSpace=" << myLeftSpace
1539 : << " remainingSeconds=" << remainingSeconds
1540 : << " plannedSpeed=" << plannedSpeed
1541 : << "\n";
1542 : }
1543 : #endif
1544 : } else {
1545 : #ifdef DEBUG_WANTS_CHANGE
1546 : if (DEBUG_COND) {
1547 : std::cout << STEPS2TIME(currentTime)
1548 : << " veh=" << myVehicle.getID()
1549 : << " myLeftSpace=" << myLeftSpace
1550 : << " remainingSeconds=" << remainingSeconds
1551 : << " hasBidiNeighLeader\n";
1552 : }
1553 : #endif
1554 : }
1555 :
1556 :
1557 : // remove TraCI flags because it should not be included in "state-without-traci"
1558 4290600 : ret = getCanceledState(laneOffset);
1559 4290600 : return ret;
1560 : }
1561 :
1562 : // we wish to anticipate future speeds. This is difficult when the leading
1563 : // vehicles are still accelerating so we resort to comparing speeds for the near future (1s) in this case
1564 102498893 : const bool acceleratingLeader = (neighLead.first != 0 && neighLead.first->getAcceleration() > 0)
1565 163844335 : || (leader.first != 0 && leader.first->getAcceleration() > 0);
1566 112501353 : double neighLaneVSafe = MIN2(neighVMax, anticipateFollowSpeed(neighLead, neighDist, neighVMax, acceleratingLeader));
1567 112501353 : thisLaneVSafe = MIN2(thisLaneVSafe, anticipateFollowSpeed(leader, currentDist, vMax, acceleratingLeader));
1568 : //std::cout << SIMTIME << " veh=" << myVehicle.getID() << " thisLaneVSafe=" << thisLaneVSafe << " neighLaneVSafe=" << neighLaneVSafe << "\n";
1569 :
1570 :
1571 : // a high inconvenience prevents cooperative changes and the following things are inconvenient:
1572 : // - a desire to change in the opposite direction for speedGain
1573 : // - low anticipated speed on the neighboring lane
1574 : // - high occupancy on the neighboring lane while in a roundabout
1575 :
1576 : double inconvenience = laneOffset < 0
1577 112501353 : ? mySpeedGainProbability / myChangeProbThresholdRight
1578 57956685 : : -mySpeedGainProbability / myChangeProbThresholdLeft;
1579 :
1580 151390999 : const double relSpeedDiff = thisLaneVSafe == 0 ? 0 : (thisLaneVSafe - neighLaneVSafe) / MAX2(thisLaneVSafe, neighLaneVSafe);
1581 : inconvenience = MAX2(relSpeedDiff, inconvenience);
1582 : inconvenience = MIN2(1.0, inconvenience);
1583 :
1584 112501353 : const bool speedGainInconvenient = inconvenience > myCooperativeParam;
1585 112501353 : const bool neighOccupancyInconvenient = neigh.lane->getBruttoOccupancy() > curr.lane->getBruttoOccupancy();
1586 : #ifdef DEBUG_WANTS_CHANGE
1587 : if (DEBUG_COND) {
1588 : std::cout << STEPS2TIME(currentTime)
1589 : << " veh=" << myVehicle.getID()
1590 : << " speedGainProb=" << mySpeedGainProbability
1591 : << " neighSpeedFactor=" << (thisLaneVSafe / neighLaneVSafe - 1)
1592 : << " inconvenience=" << inconvenience
1593 : << " speedInconv=" << speedGainInconvenient
1594 : << " occInconv=" << neighOccupancyInconvenient
1595 : << "\n";
1596 : }
1597 : #endif
1598 :
1599 : // VARIANT_15
1600 112501353 : if (roundaboutBonus > 0) {
1601 :
1602 : #ifdef DEBUG_WANTS_CHANGE
1603 : if (DEBUG_COND) {
1604 : std::cout << STEPS2TIME(currentTime)
1605 : << " veh=" << myVehicle.getID()
1606 : << " roundaboutBonus=" << roundaboutBonus
1607 : << " myLeftSpace=" << myLeftSpace
1608 : << "\n";
1609 : }
1610 : #endif
1611 : // try to use the inner lanes of a roundabout to increase throughput
1612 : // unless we are approaching the exit
1613 4567004 : if (lca == LCA_LEFT) {
1614 : // if inconvenience is not too high, request collaborative change (currently only for ballistic update)
1615 : // TODO: test this for euler update! Refs. #2575
1616 764153 : if (MSGlobals::gSemiImplicitEulerUpdate || !neighOccupancyInconvenient) {
1617 : // if(MSGlobals::gSemiImplicitEulerUpdate || !speedGainInconvenient){
1618 764153 : req = ret | lca | LCA_COOPERATIVE;
1619 : }
1620 : } else {
1621 : // if inconvenience is not too high, request collaborative change (currently only for ballistic update)
1622 3802851 : if (MSGlobals::gSemiImplicitEulerUpdate || neighOccupancyInconvenient) {
1623 : // if(MSGlobals::gSemiImplicitEulerUpdate || speedGainInconvenient){
1624 3802851 : req = ret | LCA_STAY | LCA_COOPERATIVE;
1625 : }
1626 : }
1627 4567004 : if (!cancelRequest(req, laneOffset)) {
1628 4567004 : return ret | req;
1629 : }
1630 : }
1631 :
1632 : // let's also regard the case where the vehicle is driving on a highway...
1633 : // in this case, we do not want to get to the dead-end of an on-ramp
1634 107934349 : if (right) {
1635 50741817 : if (bestLaneOffset == 0 && myVehicle.getLane()->getSpeedLimit() > 80. / 3.6 && myLookAheadSpeed > SUMO_const_haltingSpeed) {
1636 : #ifdef DEBUG_WANTS_CHANGE
1637 : if (DEBUG_COND) {
1638 : std::cout << " veh=" << myVehicle.getID() << " does not want to get stranded on the on-ramp of a highway\n";
1639 : }
1640 : #endif
1641 89775 : req = ret | LCA_STAY | LCA_STRATEGIC;
1642 89775 : if (!cancelRequest(req, laneOffset)) {
1643 : return ret | req;
1644 : }
1645 : }
1646 : }
1647 : // --------
1648 :
1649 : // -------- make place on current lane if blocking follower
1650 : //if (amBlockingFollowerPlusNB()) {
1651 : // std::cout << myVehicle.getID() << ", " << currentDistAllows(neighDist, bestLaneOffset, laDist)
1652 : // << " neighDist=" << neighDist
1653 : // << " currentDist=" << currentDist
1654 : // << "\n";
1655 : //}
1656 :
1657 : if (amBlockingFollowerPlusNB()
1658 136273 : && (!speedGainInconvenient)
1659 135748 : && ((myOwnState & myLca) != 0) // VARIANT_6 : counterNoHelp
1660 107891071 : && (changeToBest || currentDistAllows(neighDist, abs(bestLaneOffset) + 1, laDist))) {
1661 :
1662 : // VARIANT_2 (nbWhenChangingToHelp)
1663 : #ifdef DEBUG_COOPERATE
1664 : if (DEBUG_COND) {
1665 : std::cout << STEPS2TIME(currentTime)
1666 : << " veh=" << myVehicle.getID()
1667 : << " wantsChangeToHelp=" << (right ? "right" : "left")
1668 : << " state=" << myOwnState
1669 : << (((myOwnState & myLca) == 0) ? " (counter)" : "")
1670 : << "\n";
1671 : }
1672 : #endif
1673 46472 : req = ret | lca | LCA_COOPERATIVE | LCA_URGENT ;//| LCA_CHANGE_TO_HELP;
1674 46472 : if (!cancelRequest(req, laneOffset)) {
1675 46466 : if ((blocked & LCA_BLOCKED_BY_LEFT_FOLLOWER) && !right && mySpeedGainProbability > mySpeedGainUrgency) {
1676 191 : MSVehicle* nv = neighFollow.first;
1677 191 : const bool hasBidiNeighFollower = neighLane.getBidiLane() != nullptr && MSLCHelper::isBidiFollower(&myVehicle, nv);
1678 191 : if (nv != nullptr && !hasBidiNeighFollower) {
1679 191 : const double helpSpeed = MAX2(nv->getCarFollowModel().minNextSpeed(nv->getSpeed(), nv), myVehicle.getSpeed() - 1);
1680 191 : msgPass.informNeighFollower(new Info(helpSpeed, myLca | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
1681 : }
1682 : }
1683 46466 : return ret | req;
1684 : }
1685 : }
1686 :
1687 : // --------
1688 :
1689 :
1690 : //// -------- security checks for krauss
1691 : //// (vsafe fails when gap<0)
1692 : //if ((blocked & LCA_BLOCKED) != 0) {
1693 : // return ret;
1694 : //}
1695 : //// --------
1696 :
1697 : // -------- higher speed
1698 : //if ((congested(neighLead.first) && neighLead.second < 20) || predInteraction(leader.first)) { //!!!
1699 : // return ret;
1700 : //}
1701 :
1702 107798133 : if (neighLane.getEdge().getPersons().size() > 0) {
1703 : // react to pedestrians
1704 47366 : adaptSpeedToPedestrians(myVehicle.getLane(), thisLaneVSafe);
1705 47366 : adaptSpeedToPedestrians(&neighLane, neighLaneVSafe);
1706 : }
1707 :
1708 107798133 : const double relativeGain = (neighLaneVSafe - thisLaneVSafe) / MAX2(neighLaneVSafe,
1709 107798133 : RELGAIN_NORMALIZATION_MIN_SPEED);
1710 :
1711 : #ifdef DEBUG_WANTS_CHANGE
1712 : if (DEBUG_COND) {
1713 : std::cout << STEPS2TIME(currentTime)
1714 : << " veh=" << myVehicle.getID()
1715 : << " currentDist=" << currentDist
1716 : << " neighDist=" << neighDist
1717 : << " thisVSafe=" << thisLaneVSafe
1718 : << " neighVSafe=" << neighLaneVSafe
1719 : << " relGain=" << toString(relativeGain, 8)
1720 : << "\n";
1721 : }
1722 : #endif
1723 :
1724 107798133 : if (right) {
1725 : // ONLY FOR CHANGING TO THE RIGHT
1726 50648421 : if (thisLaneVSafe - 5 / 3.6 > neighLaneVSafe) {
1727 : // ok, the current lane is faster than the right one...
1728 31105950 : if (mySpeedGainProbability < 0) {
1729 3828478 : mySpeedGainProbability *= pow(0.5, myVehicle.getActionStepLengthSecs());
1730 : //myKeepRightProbability /= 2.0;
1731 : }
1732 : } else {
1733 : // ok, the current lane is not (much) faster than the right one
1734 : // @todo recheck the 5 km/h discount on thisLaneVSafe, refs. #2068
1735 :
1736 : // do not promote changing to the left just because changing to the right is bad
1737 : // XXX: The following code may promote it, though!? (recheck!)
1738 : // (Think of small negative mySpeedGainProbability and larger negative relativeGain)
1739 : // One might think of replacing '||' by '&&' to exclude that possibility...
1740 : // Still, for negative relativeGain, we might want to decrease the inclination for
1741 : // changing to the left. Another solution could be the seperation of mySpeedGainProbability into
1742 : // two variables (one for left and one for right). Refs #2578
1743 19542471 : if (mySpeedGainProbability < 0 || relativeGain > 0) {
1744 7058462 : mySpeedGainProbability -= myVehicle.getActionStepLengthSecs() * relativeGain;
1745 : }
1746 :
1747 : // honor the obligation to keep right (Rechtsfahrgebot)
1748 19542471 : const double roadSpeedFactor = vMax / myVehicle.getLane()->getSpeedLimit(); // differse from speedFactor if vMax < speedLimit
1749 : double acceptanceTime;
1750 19542471 : if (myKeepRightAcceptanceTime == -1) {
1751 : // legacy behavior: scale acceptance time with current speed and
1752 : // use old hard-coded constant
1753 39082334 : acceptanceTime = 7 * roadSpeedFactor * MAX2(1.0, myVehicle.getSpeed());
1754 : } else {
1755 1304 : acceptanceTime = myKeepRightAcceptanceTime * roadSpeedFactor;
1756 1511 : if (follower.first != nullptr && follower.second < 2 * follower.first->getCarFollowModel().brakeGap(follower.first->getSpeed())) {
1757 : // reduce acceptanceTime if the follower vehicle is faster or wants to drive faster
1758 207 : if (follower.first->getSpeed() >= myVehicle.getSpeed()) {
1759 380 : acceptanceTime *= MAX2(1.0, myVehicle.getSpeed()) / MAX2(1.0, follower.first->getSpeed());
1760 190 : const double fRSF = follower.first->getLane()->getVehicleMaxSpeed(follower.first) / follower.first->getLane()->getSpeedLimit();
1761 190 : if (fRSF > roadSpeedFactor) {
1762 120 : acceptanceTime /= fRSF;
1763 : }
1764 : }
1765 : }
1766 : }
1767 19542471 : double fullSpeedGap = MAX2(0., neighDist - myVehicle.getCarFollowModel().brakeGap(vMax));
1768 19542471 : double fullSpeedDrivingSeconds = MIN2(acceptanceTime, fullSpeedGap / vMax);
1769 19542471 : if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
1770 15108221 : fullSpeedGap = MAX2(0., MIN2(fullSpeedGap,
1771 15108221 : neighLead.second - myVehicle.getCarFollowModel().getSecureGap(&myVehicle, neighLead.first,
1772 15108221 : vMax, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
1773 15108221 : fullSpeedDrivingSeconds = MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
1774 : }
1775 : // stay on the current lane if we cannot overtake a slow leader on the right
1776 4628868 : if (checkOverTakeRight && leader.first != 0
1777 23658684 : && leader.first->getLane()->getVehicleMaxSpeed(leader.first) < vMax) {
1778 1313175 : fullSpeedGap = MIN2(fullSpeedGap, leader.second);
1779 1313175 : fullSpeedDrivingSeconds = MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - leader.first->getSpeed()));
1780 1313175 : const double relGain = (vMax - leader.first->getLane()->getVehicleMaxSpeed(leader.first)) / MAX2(vMax,
1781 1313175 : RELGAIN_NORMALIZATION_MIN_SPEED);
1782 : // tiebraker to avoid buridans paradox see #1312
1783 1313175 : mySpeedGainProbability += myVehicle.getActionStepLengthSecs() * relGain;
1784 : }
1785 :
1786 19542471 : const double deltaProb = (myChangeProbThresholdRight * (fullSpeedDrivingSeconds / acceptanceTime) / KEEP_RIGHT_TIME);
1787 19542471 : myKeepRightProbability -= myVehicle.getActionStepLengthSecs() * deltaProb;
1788 :
1789 : //std::cout << STEPS2TIME(currentTime)
1790 : // << " veh=" << myVehicle.getID()
1791 : // << " acceptanceTime=" << acceptanceTime
1792 : // << " fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
1793 : // << " dProb=" << deltaProb
1794 : // << " myKeepRightProbability=" << myKeepRightProbability
1795 : // << "\n";
1796 :
1797 : #ifdef DEBUG_WANTS_CHANGE
1798 : if (DEBUG_COND) {
1799 : std::cout << STEPS2TIME(currentTime)
1800 : << " veh=" << myVehicle.getID()
1801 : << " vMax=" << vMax
1802 : << " neighDist=" << neighDist
1803 : << " brakeGap=" << myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed())
1804 : << " leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
1805 : << " secGap=" << (neighLead.first == 0 ? -1 : myVehicle.getCarFollowModel().getSecureGap(&myVehicle, neighLead.first,
1806 : myVehicle.getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
1807 : << " acceptanceTime=" << acceptanceTime
1808 : << " fullSpeedGap=" << fullSpeedGap
1809 : << " fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
1810 : << " dProb=" << deltaProb
1811 : << " myKeepRightProbability=" << myKeepRightProbability
1812 : << "\n";
1813 : }
1814 : #endif
1815 19542471 : if (myKeepRightProbability * myKeepRightParam < -myChangeProbThresholdRight) {
1816 2408951 : req = ret | lca | LCA_KEEPRIGHT;
1817 2408951 : if (!cancelRequest(req, laneOffset)) {
1818 2408526 : return ret | req;
1819 : }
1820 : }
1821 : }
1822 :
1823 : #ifdef DEBUG_WANTS_CHANGE
1824 : if (DEBUG_COND) {
1825 : std::cout << STEPS2TIME(currentTime)
1826 : << " veh=" << myVehicle.getID()
1827 : << " speed=" << myVehicle.getSpeed()
1828 : << " mySpeedGainProbability=" << mySpeedGainProbability
1829 : << " thisLaneVSafe=" << thisLaneVSafe
1830 : << " neighLaneVSafe=" << neighLaneVSafe
1831 : << " relativeGain=" << relativeGain
1832 : << " blocked=" << blocked
1833 : << "\n";
1834 : }
1835 : #endif
1836 :
1837 48239895 : if (mySpeedGainProbability < -myChangeProbThresholdRight
1838 48631498 : && neighDist / MAX2(.1, myVehicle.getSpeed()) > mySpeedGainRemainTime) { //./MAX2( .1, myVehicle.getSpeed())) { // -.1
1839 377241 : req = ret | lca | LCA_SPEEDGAIN;
1840 377241 : if (abs(mySpeedGainProbability) > mySpeedGainUrgency) {
1841 146 : req |= LCA_URGENT;
1842 : }
1843 377241 : if (!cancelRequest(req, laneOffset)) {
1844 377241 : return ret | req;
1845 : }
1846 : }
1847 : } else {
1848 : // ONLY FOR CHANGING TO THE LEFT
1849 57149712 : if (thisLaneVSafe > neighLaneVSafe) {
1850 : // this lane is better
1851 29726279 : if (mySpeedGainProbability > 0) {
1852 16983787 : mySpeedGainProbability *= pow(0.5, myVehicle.getActionStepLengthSecs());
1853 : }
1854 27423433 : } else if (thisLaneVSafe == neighLaneVSafe) {
1855 15178070 : if (mySpeedGainProbability > 0) {
1856 4245405 : mySpeedGainProbability *= pow(0.8, myVehicle.getActionStepLengthSecs());
1857 : }
1858 : } else {
1859 : // left lane is better
1860 12245363 : mySpeedGainProbability += myVehicle.getActionStepLengthSecs() * relativeGain;
1861 : }
1862 : // VARIANT_19 (stayRight)
1863 : //if (neighFollow.first != 0) {
1864 : // MSVehicle* nv = neighFollow.first;
1865 : // const double secGap = nv->getCarFollowModel().getSecureGap(nv, &myVehicle, nv->getSpeed(), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel());
1866 : // if (neighFollow.second < secGap * KEEP_RIGHT_HEADWAY) {
1867 : // // do not change left if it would inconvenience faster followers
1868 : // return ret | LCA_STAY | LCA_SPEEDGAIN;
1869 : // }
1870 : //}
1871 :
1872 : #ifdef DEBUG_WANTS_CHANGE
1873 : if (DEBUG_COND) {
1874 : std::cout << STEPS2TIME(currentTime)
1875 : << " veh=" << myVehicle.getID()
1876 : << " speed=" << myVehicle.getSpeed()
1877 : << " mySpeedGainProbability=" << mySpeedGainProbability
1878 : << " thisLaneVSafe=" << thisLaneVSafe
1879 : << " neighLaneVSafe=" << neighLaneVSafe
1880 : << " relativeGain=" << relativeGain
1881 : << " blocked=" << blocked
1882 : << "\n";
1883 : }
1884 : #endif
1885 :
1886 57149712 : if (mySpeedGainProbability > myChangeProbThresholdLeft
1887 6332022 : && (relativeGain > NUMERICAL_EPS || changeLeftToAvoidOvertakeRight)
1888 67114170 : && neighDist / MAX2(.1, myVehicle.getSpeed()) > mySpeedGainRemainTime) { // .1
1889 4685352 : req = ret | lca | LCA_SPEEDGAIN;
1890 4685352 : if (abs(mySpeedGainProbability) > mySpeedGainUrgency) {
1891 2366 : req |= LCA_URGENT;
1892 : }
1893 4685352 : if (!cancelRequest(req, laneOffset)) {
1894 4685221 : if ((req & LCA_URGENT) && (blocked & LCA_BLOCKED_BY_LEFT_FOLLOWER)) {
1895 2275 : MSVehicle* nv = neighFollow.first;
1896 2275 : const bool hasBidiNeighFollower = neighLane.getBidiLane() != nullptr && MSLCHelper::isBidiFollower(&myVehicle, nv);
1897 2275 : if (nv != nullptr && !hasBidiNeighFollower) {
1898 2275 : const double helpSpeed = MAX2(nv->getCarFollowModel().minNextSpeed(nv->getSpeed(), nv), myVehicle.getSpeed() - 1);
1899 2275 : msgPass.informNeighFollower(new Info(helpSpeed, myLca | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
1900 : }
1901 : }
1902 4685221 : return ret | req;
1903 : }
1904 : }
1905 : }
1906 : // --------
1907 100327145 : if (changeToBest && bestLaneOffset == curr.bestLaneOffset
1908 2477312 : && myStrategicParam >= 0
1909 2470642 : && relativeGain >= 0
1910 837739 : && (right ? mySpeedGainProbability < 0 : mySpeedGainProbability > 0)) {
1911 : // change towards the correct lane, speedwise it does not hurt
1912 131131 : req = ret | lca | LCA_STRATEGIC;
1913 131131 : if (!cancelRequest(req, laneOffset)) {
1914 131131 : return ret | req;
1915 : }
1916 : }
1917 : #ifdef DEBUG_WANTS_CHANGE
1918 : if (DEBUG_COND) {
1919 : std::cout << STEPS2TIME(currentTime)
1920 : << " veh=" << myVehicle.getID()
1921 : << " mySpeedGainProbability=" << mySpeedGainProbability
1922 : << " myKeepRightProbability=" << myKeepRightProbability
1923 : << " thisLaneVSafe=" << thisLaneVSafe
1924 : << " neighLaneVSafe=" << neighLaneVSafe
1925 : << "\n";
1926 : }
1927 : #endif
1928 :
1929 : return ret;
1930 : }
1931 :
1932 :
1933 : double
1934 225002706 : MSLCM_LC2013::anticipateFollowSpeed(const std::pair<MSVehicle*, double>& leaderDist, double dist, double vMax, bool acceleratingLeader) {
1935 225002706 : const MSVehicle* leader = leaderDist.first;
1936 225002706 : const double gap = leaderDist.second;
1937 : double futureSpeed;
1938 225002706 : if (acceleratingLeader) {
1939 : // XXX see #6562
1940 148980440 : const double maxSpeed1s = (myVehicle.getSpeed() + myVehicle.getCarFollowModel().getMaxAccel()
1941 148980440 : - ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxAccel()));
1942 148980440 : if (leader == nullptr) {
1943 4068271 : if (hasBlueLight()) {
1944 : // can continue from any lane if necessary
1945 : futureSpeed = vMax;
1946 : } else {
1947 4068229 : futureSpeed = getCarFollowModel().followSpeed(&myVehicle, maxSpeed1s, dist, 0, 0);
1948 : }
1949 : } else {
1950 144912169 : futureSpeed = getCarFollowModel().followSpeed(&myVehicle, maxSpeed1s, gap, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel());
1951 : }
1952 : } else {
1953 : // onInsertion = true because the vehicle has already moved
1954 76022266 : if (leader == nullptr) {
1955 13620705 : if (hasBlueLight()) {
1956 : // can continue from any lane if necessary
1957 : futureSpeed = vMax;
1958 : } else {
1959 13619181 : futureSpeed = getCarFollowModel().maximumSafeStopSpeed(dist, getCarFollowModel().getMaxDecel(), myVehicle.getSpeed(), true);
1960 : }
1961 : } else {
1962 62401561 : futureSpeed = getCarFollowModel().maximumSafeFollowSpeed(gap, myVehicle.getSpeed(), leader->getSpeed(), leader->getCarFollowModel().getMaxDecel(), true);
1963 : }
1964 : }
1965 : futureSpeed = MIN2(vMax, futureSpeed);
1966 225002706 : if (leader != nullptr && gap > 0 && mySpeedGainLookahead > 0) {
1967 1004 : const double futureLeaderSpeed = acceleratingLeader ? leader->getLane()->getVehicleMaxSpeed(leader) : leader->getSpeed();
1968 1004 : const double deltaV = vMax - futureLeaderSpeed;
1969 1004 : if (deltaV > 0 && gap > 0) {
1970 986 : const double secGap = getCarFollowModel().getSecureGap(&myVehicle, leader, futureSpeed, leader->getSpeed(), getCarFollowModel().getMaxDecel());
1971 986 : const double fullSpeedGap = gap - secGap;
1972 986 : if (fullSpeedGap / deltaV < mySpeedGainLookahead) {
1973 : // anticipate future braking by computing the average
1974 : // speed over the next few seconds
1975 : const double gapClosingTime = MAX2(0.0, fullSpeedGap / deltaV);
1976 190 : const double foreCastTime = mySpeedGainLookahead * 2;
1977 : //if (DEBUG_COND) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " leader=" << leader->getID() << " gap=" << gap << " deltaV=" << deltaV << " futureSpeed=" << futureSpeed << " futureLeaderSpeed=" << futureLeaderSpeed;
1978 190 : futureSpeed = MIN2(futureSpeed, (gapClosingTime * futureSpeed + (foreCastTime - gapClosingTime) * futureLeaderSpeed) / foreCastTime);
1979 : //if (DEBUG_COND) std::cout << " newFutureSpeed=" << futureSpeed << "\n";
1980 : }
1981 : }
1982 : }
1983 225002706 : return futureSpeed;
1984 : }
1985 :
1986 :
1987 : int
1988 301461860 : MSLCM_LC2013::slowDownForBlocked(MSVehicle** blocked, int state) {
1989 : // if this vehicle is blocking someone in front, we maybe decelerate to let him in
1990 301461860 : if ((*blocked) != nullptr) {
1991 20165110 : double gap = (*blocked)->getPositionOnLane() - (*blocked)->getVehicleType().getLength() - myVehicle.getPositionOnLane() - myVehicle.getVehicleType().getMinGap();
1992 : #ifdef DEBUG_SLOW_DOWN
1993 : if (DEBUG_COND) {
1994 : std::cout << SIMTIME
1995 : << " veh=" << myVehicle.getID()
1996 : << " blocked=" << Named::getIDSecure(*blocked)
1997 : << " gap=" << gap
1998 : << "\n";
1999 : }
2000 : #endif
2001 20165110 : if (gap > POSITION_EPS) {
2002 : //const bool blockedWantsUrgentRight = (((*blocked)->getLaneChangeModel().getOwnState() & LCA_RIGHT != 0)
2003 : // && ((*blocked)->getLaneChangeModel().getOwnState() & LCA_URGENT != 0));
2004 :
2005 16092716 : if (myVehicle.getSpeed() < myVehicle.getCarFollowModel().getMaxDecel()
2006 : //|| blockedWantsUrgentRight // VARIANT_10 (helpblockedRight)
2007 : ) {
2008 13898953 : if ((*blocked)->getSpeed() < SUMO_const_haltingSpeed) {
2009 7689845 : state |= LCA_AMBACKBLOCKER_STANDING;
2010 : } else {
2011 6209108 : state |= LCA_AMBACKBLOCKER;
2012 : }
2013 27797906 : addLCSpeedAdvice(getCarFollowModel().followSpeed(
2014 13898953 : &myVehicle, myVehicle.getSpeed(),
2015 13898953 : gap - POSITION_EPS, (*blocked)->getSpeed(),
2016 13898953 : (*blocked)->getCarFollowModel().getMaxDecel()), false);
2017 :
2018 : //(*blocked) = 0; // VARIANT_14 (furtherBlock)
2019 : #ifdef DEBUG_SLOW_DOWN
2020 : if (DEBUG_COND) {
2021 : std::cout << SIMTIME
2022 : << " veh=" << myVehicle.getID()
2023 : << " slowing down for"
2024 : << " blocked=" << Named::getIDSecure(*blocked)
2025 : << " helpSpeed=" << myLCAccelerationAdvices.back().first
2026 : << "\n";
2027 : }
2028 : #endif
2029 : } /*else if ((*blocked)->getWaitingSeconds() > 30 && gap > myVehicle.getBrakeGap()) {
2030 : // experimental else-branch...
2031 :
2032 : state |= LCA_AMBACKBLOCKER;
2033 : addLCSpeedAdvice(getCarFollowModel().followSpeed(
2034 : &myVehicle, myVehicle.getSpeed(),
2035 : (gap - POSITION_EPS), (*blocked)->getSpeed(),
2036 : (*blocked)->getCarFollowModel().getMaxDecel()));
2037 : } */
2038 : }
2039 : }
2040 301461860 : return state;
2041 : }
2042 :
2043 :
2044 : void
2045 94732 : MSLCM_LC2013::adaptSpeedToPedestrians(const MSLane* lane, double& v) {
2046 94732 : if (lane->hasPedestrians()) {
2047 : #ifdef DEBUG_WANTS_CHANGE
2048 : if (DEBUG_COND) {
2049 : std::cout << SIMTIME << " adapt to pedestrians on lane=" << lane->getID() << "\n";
2050 : }
2051 : #endif
2052 20845 : PersonDist leader = lane->nextBlocking(myVehicle.getPositionOnLane(),
2053 20845 : myVehicle.getRightSideOnLane(), myVehicle.getRightSideOnLane() + myVehicle.getVehicleType().getWidth(),
2054 20845 : ceil(myVehicle.getSpeed() / myVehicle.getCarFollowModel().getMaxDecel()));
2055 20845 : if (leader.first != 0) {
2056 7763 : const double stopSpeed = myVehicle.getCarFollowModel().stopSpeed(&myVehicle, myVehicle.getSpeed(), leader.second - myVehicle.getVehicleType().getMinGap());
2057 12756 : v = MIN2(v, stopSpeed);
2058 : #ifdef DEBUG_WANTS_CHANGE
2059 : if (DEBUG_COND) {
2060 : std::cout << SIMTIME << " pedLeader=" << leader.first->getID() << " dist=" << leader.second << " v=" << v << "\n";
2061 : }
2062 : #endif
2063 : }
2064 : }
2065 94732 : }
2066 :
2067 :
2068 : double
2069 593022 : MSLCM_LC2013::computeSpeedLat(double latDist, double& maneuverDist, bool urgent) const {
2070 593022 : double result = MSAbstractLaneChangeModel::computeSpeedLat(latDist, maneuverDist, urgent);
2071 : #ifdef DEBUG_WANTS_CHANGE
2072 : if (DEBUG_COND) {
2073 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " myLeftSpace=" << myLeftSpace << " latDist=" << latDist << " maneuverDist=" << maneuverDist << " result=" << result << "\n";
2074 : }
2075 : #endif
2076 593022 : if (myLeftSpace > POSITION_EPS || !urgent) {
2077 477827 : double speedBound = myMaxSpeedLatStanding + myMaxSpeedLatFactor * myVehicle.getSpeed();
2078 477827 : if (isChangingLanes()) {
2079 : speedBound = MAX2(LC_RESOLUTION_SPEED_LAT, speedBound);
2080 : }
2081 477827 : result = MAX2(-speedBound, MIN2(speedBound, result));
2082 : }
2083 593022 : return result;
2084 : }
2085 :
2086 :
2087 : double
2088 295003888 : MSLCM_LC2013::getSafetyFactor() const {
2089 295003888 : return 1 / myAssertive;
2090 : }
2091 :
2092 : double
2093 9164137 : MSLCM_LC2013::getOppositeSafetyFactor() const {
2094 9164137 : return myOppositeParam <= 0 ? std::numeric_limits<double>::max() : 1 / myOppositeParam;
2095 : }
2096 :
2097 : bool
2098 182442 : MSLCM_LC2013::saveBlockerLength(double length, double foeLeftSpace) {
2099 182442 : const bool canReserve = MSLCHelper::canSaveBlockerLength(myVehicle, length, myLeftSpace);
2100 182442 : if (!isOpposite() && (canReserve || myLeftSpace > foeLeftSpace)) {
2101 178028 : myLeadingBlockerLength = MAX2(length, myLeadingBlockerLength);
2102 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
2103 : if (DEBUG_COND) {
2104 : std::cout << SIMTIME << " saveBlockerLength veh=" << myVehicle.getID() << " canReserve=" << canReserve << " myLeftSpace=" << myLeftSpace << " foeLeftSpace=" << foeLeftSpace << "\n";
2105 : }
2106 : #endif
2107 178028 : if (myLeftSpace == 0 && foeLeftSpace < 0) {
2108 : // called from opposite overtaking, myLeftSpace must be initialized
2109 171351 : myLeftSpace = myVehicle.getBestLanes()[myVehicle.getLane()->getIndex()].length - myVehicle.getPositionOnLane();
2110 : }
2111 178028 : return true;
2112 : } else {
2113 : return false;
2114 : }
2115 : }
2116 :
2117 :
2118 : bool
2119 25116 : MSLCM_LC2013::hasFreeLane(int laneOffset, const std::pair<MSVehicle*, double>& neighLeadStopped) const {
2120 25116 : if (neighLeadStopped.first == nullptr) {
2121 : return true;
2122 : }
2123 25116 : int dir = (laneOffset > 0 ? 1 : -1);
2124 25116 : const MSLane* neigh = myVehicle.getLane()->getParallelLane(laneOffset);
2125 25116 : if (dir > 0 && !neigh->allowsChangingLeft(myVehicle.getVClass())) {
2126 : return false;
2127 25116 : } else if (dir < 0 && !neigh->allowsChangingRight(myVehicle.getVClass())) {
2128 : return false;
2129 : }
2130 25116 : int nextOffset = laneOffset + dir;
2131 25116 : const MSLane* next = myVehicle.getLane()->getParallelLane(nextOffset);
2132 25116 : if (next == nullptr || !next->allowsVehicleClass(myVehicle.getVClass())) {
2133 17805 : return false;
2134 : }
2135 7311 : const double overtakeDist = neighLeadStopped.second + neighLeadStopped.first->getVehicleType().getLengthWithGap() + myVehicle.getLength() + POSITION_EPS;
2136 7311 : std::pair<MSVehicle* const, double> nextLead = next->getLeader(&myVehicle, myVehicle.getPositionOnLane(), myVehicle.getBestLanesContinuation(next), overtakeDist);
2137 7797 : return nextLead.first == nullptr || nextLead.second >= overtakeDist || hasFreeLane(nextOffset, nextLead);
2138 : }
2139 :
2140 :
2141 : std::string
2142 154 : MSLCM_LC2013::getParameter(const std::string& key) const {
2143 154 : if (key == toString(SUMO_ATTR_LCA_STRATEGIC_PARAM)) {
2144 36 : return toString(myStrategicParam);
2145 118 : } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_PARAM)) {
2146 36 : return toString(myCooperativeParam);
2147 82 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_PARAM)) {
2148 36 : return toString(mySpeedGainParam);
2149 46 : } else if (key == toString(SUMO_ATTR_LCA_KEEPRIGHT_PARAM)) {
2150 0 : return toString(myKeepRightParam);
2151 46 : } else if (key == toString(SUMO_ATTR_LCA_OPPOSITE_PARAM)) {
2152 0 : return toString(myOppositeParam);
2153 46 : } else if (key == toString(SUMO_ATTR_LCA_LOOKAHEADLEFT)) {
2154 0 : return toString(myLookaheadLeft);
2155 46 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAINRIGHT)) {
2156 0 : return toString(mySpeedGainRight);
2157 46 : } else if (key == toString(SUMO_ATTR_LCA_ASSERTIVE)) {
2158 0 : return toString(myAssertive);
2159 46 : } else if (key == toString(SUMO_ATTR_LCA_OVERTAKE_RIGHT)) {
2160 0 : return toString(myOvertakeRightParam);
2161 46 : } else if (key == toString(SUMO_ATTR_LCA_SIGMA)) {
2162 0 : return toString(mySigma);
2163 46 : } else if (key == toString(SUMO_ATTR_LCA_KEEPRIGHT_ACCEPTANCE_TIME)) {
2164 0 : return toString(myKeepRightAcceptanceTime);
2165 46 : } else if (key == toString(SUMO_ATTR_LCA_OVERTAKE_DELTASPEED_FACTOR)) {
2166 0 : return toString(myOvertakeDeltaSpeedFactor);
2167 46 : } else if (key == toString(SUMO_ATTR_LCA_STRATEGIC_LOOKAHEAD)) {
2168 0 : return toString(myStrategicLookahead);
2169 46 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD)) {
2170 0 : return toString(mySpeedGainLookahead);
2171 46 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_REMAIN_TIME)) {
2172 0 : return toString(mySpeedGainRemainTime);
2173 46 : } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_ROUNDABOUT)) {
2174 0 : return toString(myRoundaboutBonus);
2175 46 : } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_SPEED)) {
2176 0 : return toString(myCooperativeSpeed);
2177 46 : } else if (key == toString(SUMO_ATTR_LCA_MAXSPEEDLATSTANDING)) {
2178 0 : return toString(myMaxSpeedLatStanding);
2179 46 : } else if (key == toString(SUMO_ATTR_LCA_MAXSPEEDLATFACTOR)) {
2180 0 : return toString(myMaxSpeedLatFactor);
2181 46 : } else if (key == toString(SUMO_ATTR_LCA_MAXDISTLATSTANDING)) {
2182 0 : return toString(myMaxDistLatStanding);
2183 : // access to internal state for debugging in sumo-gui (not documented since it may change at any time)
2184 46 : } else if (key == "speedGainProbabilityRight") {
2185 0 : return toString(-mySpeedGainProbability);
2186 46 : } else if (key == "speedGainProbabilityLeft") {
2187 0 : return toString(mySpeedGainProbability);
2188 46 : } else if (key == "keepRightProbability") {
2189 0 : return toString(-myKeepRightProbability);
2190 46 : } else if (key == "lookAheadSpeed") {
2191 0 : return toString(myLookAheadSpeed);
2192 : // motivation relative to threshold
2193 46 : } else if (key == "speedGainRP") {
2194 0 : return toString(-mySpeedGainProbability / myChangeProbThresholdRight);
2195 46 : } else if (key == "speedGainLP") {
2196 0 : return toString(mySpeedGainProbability / myChangeProbThresholdLeft);
2197 46 : } else if (key == "keepRightP") {
2198 0 : return toString(myKeepRightProbability * myKeepRightParam / -myChangeProbThresholdRight);
2199 : }
2200 138 : throw InvalidArgument("Parameter '" + key + "' is not supported for laneChangeModel of type '" + toString(myModel) + "'");
2201 : }
2202 :
2203 :
2204 : void
2205 11698 : MSLCM_LC2013::setParameter(const std::string& key, const std::string& value) {
2206 : double doubleValue;
2207 : try {
2208 11698 : doubleValue = StringUtils::toDouble(value);
2209 0 : } catch (NumberFormatException&) {
2210 0 : throw InvalidArgument("Setting parameter '" + key + "' requires a number for laneChangeModel of type '" + toString(myModel) + "'");
2211 0 : }
2212 11698 : if (key == toString(SUMO_ATTR_LCA_STRATEGIC_PARAM)) {
2213 3090 : myStrategicParam = doubleValue;
2214 8608 : } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_PARAM)) {
2215 0 : myCooperativeParam = doubleValue;
2216 8608 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_PARAM)) {
2217 0 : mySpeedGainParam = doubleValue;
2218 8608 : } else if (key == toString(SUMO_ATTR_LCA_KEEPRIGHT_PARAM)) {
2219 0 : myKeepRightParam = doubleValue;
2220 8608 : } else if (key == toString(SUMO_ATTR_LCA_OPPOSITE_PARAM)) {
2221 0 : myOppositeParam = doubleValue;
2222 8608 : } else if (key == toString(SUMO_ATTR_LCA_LOOKAHEADLEFT)) {
2223 0 : myLookaheadLeft = doubleValue;
2224 8608 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAINRIGHT)) {
2225 0 : mySpeedGainRight = doubleValue;
2226 8608 : } else if (key == toString(SUMO_ATTR_LCA_ASSERTIVE)) {
2227 0 : myAssertive = doubleValue;
2228 8608 : } else if (key == toString(SUMO_ATTR_LCA_OVERTAKE_RIGHT)) {
2229 0 : myOvertakeRightParam = doubleValue;
2230 8608 : } else if (key == toString(SUMO_ATTR_LCA_SIGMA)) {
2231 0 : mySigma = doubleValue;
2232 8608 : } else if (key == toString(SUMO_ATTR_LCA_KEEPRIGHT_ACCEPTANCE_TIME)) {
2233 0 : myKeepRightAcceptanceTime = doubleValue;
2234 8608 : } else if (key == toString(SUMO_ATTR_LCA_OVERTAKE_DELTASPEED_FACTOR)) {
2235 0 : myOvertakeDeltaSpeedFactor = doubleValue;
2236 8608 : } else if (key == toString(SUMO_ATTR_LCA_STRATEGIC_LOOKAHEAD)) {
2237 0 : myStrategicLookahead = doubleValue;
2238 8608 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD)) {
2239 2868 : mySpeedGainLookahead = doubleValue;
2240 5740 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_REMAIN_TIME)) {
2241 2868 : mySpeedGainRemainTime = doubleValue;
2242 2872 : } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_ROUNDABOUT)) {
2243 0 : myRoundaboutBonus = doubleValue;
2244 2872 : } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_SPEED)) {
2245 0 : myCooperativeSpeed = doubleValue;
2246 2872 : } else if (key == toString(SUMO_ATTR_LCA_MAXSPEEDLATSTANDING)) {
2247 0 : myMaxSpeedLatStanding = doubleValue;
2248 2872 : } else if (key == toString(SUMO_ATTR_LCA_MAXSPEEDLATFACTOR)) {
2249 0 : myMaxSpeedLatFactor = doubleValue;
2250 2872 : } else if (key == toString(SUMO_ATTR_LCA_MAXDISTLATSTANDING)) {
2251 0 : myMaxDistLatStanding = doubleValue;
2252 : // access to internal state
2253 2872 : } else if (key == "speedGainProbabilityRight") {
2254 0 : mySpeedGainProbability = -doubleValue;
2255 2872 : } else if (key == "speedGainProbabilityLeft") {
2256 0 : mySpeedGainProbability = doubleValue;
2257 2872 : } else if (key == "keepRightProbability") {
2258 0 : myKeepRightProbability = -doubleValue;
2259 2872 : } else if (key == "lookAheadSpeed") {
2260 0 : myLookAheadSpeed = doubleValue;
2261 : } else {
2262 8616 : throw InvalidArgument("Setting parameter '" + key + "' is not supported for laneChangeModel of type '" + toString(myModel) + "'");
2263 : }
2264 8826 : initDerivedParameters();
2265 8826 : }
2266 :
2267 :
2268 : /****************************************************************************/
|
