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