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