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