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-2017 German Aerospace Center (DLR) and others.
4 /****************************************************************************/
5 //
6 // This program and the accompanying materials
7 // are made available under the terms of the Eclipse Public License v2.0
8 // which accompanies this distribution, and is available at
9 // http://www.eclipse.org/legal/epl-v20.html
10 //
11 /****************************************************************************/
23 // A lane change model developed by J. Erdmann
24 // based on the model of D. Krajzewicz developed between 2004 and 2011 (MSLCM_DK2004)
25 /****************************************************************************/
26 
27 
28 // ===========================================================================
29 // included modules
30 // ===========================================================================
31 #ifdef _MSC_VER
32 #include <windows_config.h>
33 #else
34 #include <config.h>
35 #endif
36 
37 #include <iostream>
41 #include <microsim/MSEdge.h>
42 #include <microsim/MSLane.h>
43 #include <microsim/MSNet.h>
44 #include "MSLCM_LC2013.h"
45 
46 
47 // ===========================================================================
48 // variable definitions
49 // ===========================================================================
50 #define LOOK_FORWARD (double)10.
51 
52 #define JAM_FACTOR (double)1.
53 
54 #define LCA_RIGHT_IMPATIENCE (double)-1.
55 #define CUT_IN_LEFT_SPEED_THRESHOLD (double)27.
56 
57 #define LOOK_AHEAD_MIN_SPEED 0.0
58 #define LOOK_AHEAD_SPEED_MEMORY 0.9
59 
60 #define HELP_DECEL_FACTOR (double)1.0
61 
62 #define HELP_OVERTAKE (double)(10.0 / 3.6)
63 #define MIN_FALLBEHIND (double)(7.0 / 3.6)
64 
65 // allow overtaking to the right below this speed difference
66 #define OVERTAKE_RIGHT_THRESHOLD (double)(5/3.6)
67 
68 #define RELGAIN_NORMALIZATION_MIN_SPEED (double)10.0
69 #define URGENCY (double)2.0
70 
71 #define KEEP_RIGHT_TIME (double)5.0 // the number of seconds after which a vehicle should move to the right lane
72 #define KEEP_RIGHT_ACCEPTANCE (double)7.0 // calibration factor for determining the desire to keep right
73 #define ROUNDABOUT_DIST_BONUS (double)100.0 // valence (distance) for to faked per roundabout edge in front (inducing inner lane usage in roundabouts by decreasing sense of lc-urgency)
74 
75 #define ROUNDABOUT_DIST_FACTOR (double)10.0 // Must be >=1.0, serves an alternative way of decreasing sense lc-urgency by multiplying the distance along the next roundabout
76 #define ROUNDABOUT_DIST_TRESH (double)10.0 // roundabout distances below ROUNDABOUT_DIST_TRESH are not multiplied by ROUNDABOUT_DIST_FACTOR
77 
78 #define KEEP_RIGHT_HEADWAY (double)2.0
79 #define MAX_ONRAMP_LENGTH (double)200.
80 #define TURN_LANE_DIST (double)200.0 // the distance at which a lane leading elsewhere is considered to be a turn-lane that must be avoided
81 
82 // ===========================================================================
83 // debug defines
84 // ===========================================================================
85 //#define DEBUG_PATCH_SPEED
86 //#define DEBUG_INFORMED
87 //#define DEBUG_INFORMER
88 //#define DEBUG_CONSTRUCTOR
89 //#define DEBUG_WANTS_CHANGE
90 //#define DEBUG_SLOW_DOWN
91 //#define DEBUG_SAVE_BLOCKER_LENGTH
92 
93 //#define DEBUG_COND (myVehicle.getID() == "disabled")
94 #define DEBUG_COND (myVehicle.isSelected())
95 //#define DEBUG_COND (false)
96 
97 // ===========================================================================
98 // member method definitions
99 // ===========================================================================
102  mySpeedGainProbability(0),
103  myKeepRightProbability(0),
104  myLeadingBlockerLength(0),
105  myLeftSpace(0),
106  myLookAheadSpeed(LOOK_AHEAD_MIN_SPEED),
107  myStrategicParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_STRATEGIC_PARAM, 1)),
108  myCooperativeParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_PARAM, 1)),
109  mySpeedGainParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAIN_PARAM, 1)),
110  myKeepRightParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_KEEPRIGHT_PARAM, 1)),
111  myLookaheadLeft(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_LOOKAHEADLEFT, 2.0)),
112  mySpeedGainRight(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAINRIGHT, 0.1)),
113  myExperimentalParam1(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_EXPERIMENTAL1, 0)) {
115 #ifdef DEBUG_CONSTRUCTOR
116  if (DEBUG_COND) {
117  std::cout << SIMTIME
118  << " create lcModel veh=" << myVehicle.getID()
119  << " lcStrategic=" << myStrategicParam
120  << " lcCooperative=" << myCooperativeParam
121  << " lcSpeedGain=" << mySpeedGainParam
122  << " lcKeepRight=" << myKeepRightParam
123  << "\n";
124  }
125 #endif
126 }
127 
129  changed();
130 }
131 
132 
133 void
137 }
138 
139 
140 bool
142  return DEBUG_COND;
143 }
144 
145 
146 int
148  int laneOffset,
150  int blocked,
151  const std::pair<MSVehicle*, double>& leader,
152  const std::pair<MSVehicle*, double>& neighLead,
153  const std::pair<MSVehicle*, double>& neighFollow,
154  const MSLane& neighLane,
155  const std::vector<MSVehicle::LaneQ>& preb,
156  MSVehicle** lastBlocked,
157  MSVehicle** firstBlocked) {
158 
159 #ifdef DEBUG_WANTS_CHANGE
160  if (DEBUG_COND) {
161  std::cout << "\nWANTS_CHANGE\n" << SIMTIME
162  << std::setprecision(gPrecision)
163  << " veh=" << myVehicle.getID()
164  << " lane=" << myVehicle.getLane()->getID()
165  << " pos=" << myVehicle.getPositionOnLane()
166  << " posLat=" << myVehicle.getLateralPositionOnLane()
167  << " speed=" << myVehicle.getSpeed()
168  << " considerChangeTo=" << (laneOffset == -1 ? "right" : "left")
169  << "\n";
170  }
171 #endif
172 
173  const int result = _wantsChange(laneOffset, msgPass, blocked, leader, neighLead, neighFollow, neighLane, preb, lastBlocked, firstBlocked);
174 
175 #ifdef DEBUG_WANTS_CHANGE
176  if (DEBUG_COND) {
177  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " result=" << toString((LaneChangeAction)result) << " blocked=" << toString((LaneChangeAction)blocked) << "\n\n\n";
178  }
179 #endif
180 
181  return result;
182 }
183 
184 
185 double
186 MSLCM_LC2013::patchSpeed(const double min, const double wanted, const double max, const MSCFModel& cfModel) {
187 
188 #ifdef DEBUG_PATCH_SPEED
189  if (DEBUG_COND) {
190  std::cout << "\nPATCH_SPEED\n"
191  << SIMTIME
192  << " veh=" << myVehicle.getID()
193  << " lane=" << myVehicle.getLane()->getID()
194  << " pos=" << myVehicle.getPositionOnLane()
195  << " v=" << myVehicle.getSpeed()
196  << " min=" << min
197  << " wanted=" << wanted
198  << " max=" << max
199  << "\n";
200  }
201 #endif
202 
203  // negative min speed may be passed when using ballistic updated
204  const double newSpeed = _patchSpeed(MAX2(min, 0.0), wanted, max, cfModel);
205 
206 #ifdef DEBUG_PATCH_SPEED
207  if (DEBUG_COND) {
208  const std::string patched = (wanted != newSpeed ? " patched=" + toString(newSpeed) : "");
209  std::cout << patched
210  << "\n";
211  }
212 #endif
213 
214  return newSpeed;
215 }
216 
217 
218 double
219 MSLCM_LC2013::_patchSpeed(const double min, const double wanted, const double max, const MSCFModel& cfModel) {
220  int state = myOwnState;
221 #ifdef DEBUG_PATCH_SPEED
222  if (DEBUG_COND) {
223  std::cout
224  << "\n" << SIMTIME << " patchSpeed state=" << state << " myLCAccelerationAdvices=" << toString(myLCAccelerationAdvices)
225  << " \nspeed=" << myVehicle.getSpeed()
226  << " min=" << min
227  << " wanted=" << wanted << std::endl;
228  }
229 #endif
230 
231  // letting vehicles merge in at the end of the lane in case of counter-lane change, step#2
232  double MAGIC_offset = 1.;
233  // if we want to change and have a blocking leader and there is enough room for him in front of us
234  if (myLeadingBlockerLength != 0) {
235  double space = myLeftSpace - myLeadingBlockerLength - MAGIC_offset - myVehicle.getVehicleType().getMinGap();
236 #ifdef DEBUG_PATCH_SPEED
237  if (DEBUG_COND) {
238  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " myLeadingBlockerLength=" << myLeadingBlockerLength << " space=" << space << "\n";
239  }
240 #endif
241  if (space > 0) { // XXX space > -MAGIC_offset
242  // compute speed for decelerating towards a place which allows the blocking leader to merge in in front
243  double safe = cfModel.stopSpeed(&myVehicle, myVehicle.getSpeed(), space);
244  // if we are approaching this place
245  if (safe < wanted) {
246  // return this speed as the speed to use
247 #ifdef DEBUG_PATCH_SPEED
248  if (DEBUG_COND) {
249  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " slowing down for leading blocker, safe=" << safe << (safe + NUMERICAL_EPS < min ? " (not enough)" : "") << "\n";
250  }
251 #endif
252  return MAX2(min, safe);
253  }
254  }
255  }
256 
257  double nVSafe = wanted;
258  bool gotOne = false;
259  for (std::vector<double>::const_iterator i = myLCAccelerationAdvices.begin(); i != myLCAccelerationAdvices.end(); ++i) {
260  double a = (*i);
261  double v = myVehicle.getSpeed() + ACCEL2SPEED(a);
262 
263  if (v >= min && v <= max && (MSGlobals::gSemiImplicitEulerUpdate
264  // ballistic update: (negative speeds may appear, e.g. min<0, v<0), BUT:
265  // XXX: LaneChanging returns -1 to indicate no restrictions, which leads to probs here (Leo), refs. #2577
266  // As a quick fix, we just dismiss cases where v=-1
267  // VERY rarely (whenever a requested help-acceleration is really indicated by v=-1)
268  // this can lead to failing lane-change attempts, though)
269  || v != -1)) {
270  nVSafe = MIN2(v * myCooperativeParam + (1 - myCooperativeParam) * wanted, nVSafe);
271  gotOne = true;
272 #ifdef DEBUG_PATCH_SPEED
273  if (DEBUG_COND) {
274  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " got nVSafe=" << nVSafe << "\n";
275  }
276 #endif
277  } else {
278  if (v < min) {
279 #ifdef DEBUG_PATCH_SPEED
280  if (DEBUG_COND) {
281  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " ignoring low nVSafe=" << v << " min=" << min << "\n";
282  }
283 #endif
284  } else {
285 #ifdef DEBUG_PATCH_SPEED
286  if (DEBUG_COND) {
287  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " ignoring high nVSafe=" << v << " max=" << max << "\n";
288  }
289 #endif
290  }
291  }
292  }
293 
294  if (gotOne && !myDontBrake) { // XXX: myDontBrake is initialized as false and seems not to be changed anywhere... What's its purpose???
295 #ifdef DEBUG_PATCH_SPEED
296  if (DEBUG_COND) {
297  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " got vSafe\n";
298  }
299 #endif
300  return nVSafe;
301  }
302 
303  // check whether the vehicle is blocked
304  if ((state & LCA_WANTS_LANECHANGE) != 0 && (state & LCA_BLOCKED) != 0) {
305  if ((state & LCA_STRATEGIC) != 0) {
306  // necessary decelerations are controlled via vSafe. If there are
307  // none it means we should speed up
308 #ifdef DEBUG_PATCH_SPEED
309  if (DEBUG_COND) {
310  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_WANTS_LANECHANGE (strat, no vSafe)\n";
311  }
312 #endif
313  return (max + wanted) / (double) 2.0;
314  } else if ((state & LCA_COOPERATIVE) != 0) {
315  // only minor adjustments in speed should be done
316  if ((state & LCA_BLOCKED_BY_LEADER) != 0) {
317 #ifdef DEBUG_PATCH_SPEED
318  if (DEBUG_COND) {
319  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_LEADER (coop)\n";
320  }
321 #endif
322  if (wanted >= 0.) {
323  return (MAX2(0., min) + wanted) / (double) 2.0;
324  } else {
325  return wanted;
326  }
327  }
328  if ((state & LCA_BLOCKED_BY_FOLLOWER) != 0) {
329 #ifdef DEBUG_PATCH_SPEED
330  if (DEBUG_COND) {
331  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_FOLLOWER (coop)\n";
332  }
333 #endif
334  return (max + wanted) / (double) 2.0;
335  }
336  //} else { // VARIANT_16
337  // // only accelerations should be performed
338  // if ((state & LCA_BLOCKED_BY_FOLLOWER) != 0) {
339  // if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_FOLLOWER\n";
340  // return (max + wanted) / (double) 2.0;
341  // }
342  }
343  }
344 
345  /*
346  // decelerate if being a blocking follower
347  // (and does not have to change lanes)
348  if ((state & LCA_AMBLOCKINGFOLLOWER) != 0) {
349  if (fabs(max - myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle)) < 0.001 && min == 0) { // !!! was standing
350  if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER (standing)\n";
351  return 0;
352  }
353  if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER\n";
354 
355  //return min; // VARIANT_3 (brakeStrong)
356  return (min + wanted) / (double) 2.0;
357  }
358  if ((state & LCA_AMBACKBLOCKER) != 0) {
359  if (max <= myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle) && min == 0) { // !!! was standing
360  if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBACKBLOCKER (standing)\n";
361  //return min; VARIANT_9 (backBlockVSafe)
362  return nVSafe;
363  }
364  }
365  if ((state & LCA_AMBACKBLOCKER_STANDING) != 0) {
366  if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBACKBLOCKER_STANDING\n";
367  //return min;
368  return nVSafe;
369  }
370  */
371 
372  // accelerate if being a blocking leader or blocking follower not able to brake
373  // (and does not have to change lanes)
374  if ((state & LCA_AMBLOCKINGLEADER) != 0) {
375 #ifdef DEBUG_PATCH_SPEED
376  if (DEBUG_COND) {
377  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGLEADER\n";
378  }
379 #endif
380  return (max + wanted) / (double) 2.0;
381  }
382 
383  if ((state & LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0) {
384 #ifdef DEBUG_PATCH_SPEED
385  if (DEBUG_COND) {
386  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER_DONTBRAKE\n";
387  }
388 #endif
389  /*
390  // VARIANT_4 (dontbrake)
391  if (max <= myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle) && min == 0) { // !!! was standing
392  return wanted;
393  }
394  return (min + wanted) / (double) 2.0;
395  */
396  }
397  if (!myVehicle.getLane()->getEdge().hasLaneChanger()) {
398  // remove chaning information if on a road with a single lane
399  changed();
400  }
401  return wanted;
402 }
403 
404 
405 void*
406 MSLCM_LC2013::inform(void* info, MSVehicle* sender) {
407  UNUSED_PARAMETER(sender);
408  Info* pinfo = (Info*)info;
409  assert(pinfo->first >= 0 || !MSGlobals::gSemiImplicitEulerUpdate);
410  addLCSpeedAdvice(pinfo->first);
411  myOwnState |= pinfo->second;
412 #ifdef DEBUG_INFORMED
413  if (DEBUG_COND) {
414  std::cout << SIMTIME
415  << " veh=" << myVehicle.getID()
416  << " informedBy=" << sender->getID()
417  << " info=" << pinfo->second
418  << " vSafe=" << pinfo->first
419  << "\n";
420  }
421 #endif
422  delete pinfo;
423  return (void*) true;
424 }
425 
426 double
427 MSLCM_LC2013::overtakeDistance(const MSVehicle* follower, const MSVehicle* leader, const double gap, double followerSpeed, double leaderSpeed) {
428  followerSpeed = followerSpeed == INVALID_SPEED ? follower->getSpeed() : followerSpeed;
429  leaderSpeed = leaderSpeed == INVALID_SPEED ? leader->getSpeed() : leaderSpeed;
430  double overtakeDist = (gap // drive to back of leader
431  + leader->getVehicleType().getLengthWithGap() // drive to front of leader
432  + follower->getVehicleType().getLength() // follower back reaches leader front
433  + leader->getCarFollowModel().getSecureGap( // save gap to leader
434  leaderSpeed, followerSpeed, follower->getCarFollowModel().getMaxDecel()));
435  return MAX2(overtakeDist, 0.);
436 }
437 
438 
439 double
441  int blocked,
442  int dir,
443  const std::pair<MSVehicle*, double>& neighLead,
444  double remainingSeconds) {
445  double plannedSpeed = MIN2(myVehicle.getSpeed(),
447  for (std::vector<double>::const_iterator i = myLCAccelerationAdvices.begin(); i != myLCAccelerationAdvices.end(); ++i) {
448  const double a = *i;
449  if (a >= -myVehicle.getCarFollowModel().getMaxDecel()) {
450  plannedSpeed = MIN2(plannedSpeed, myVehicle.getSpeed() + ACCEL2SPEED(a));
451  }
452  }
453 #ifdef DEBUG_INFORMER
454  if (DEBUG_COND) {
455  std::cout << "\nINFORM_LEADER"
456  << "\nspeed=" << myVehicle.getSpeed() << " planned=" << plannedSpeed << "\n";
457  }
458 #endif
459 
460  if ((blocked & LCA_BLOCKED_BY_LEADER) != 0) {
461  assert(neighLead.first != 0);
462  MSVehicle* nv = neighLead.first;
463 #ifdef DEBUG_INFORMER
464  if (DEBUG_COND) {
465  std::cout << " blocked by leader nv=" << nv->getID() << " nvSpeed=" << nv->getSpeed() << " needGap="
467  }
468 #endif
469  // decide whether we want to overtake the leader or follow it
470  double overtakeTime;
471  const double overtakeDist = overtakeDistance(&myVehicle, nv, neighLead.second);
472  const double dv = plannedSpeed - nv->getSpeed();
473 
474  if (dv > 0) {
475  overtakeTime = overtakeDist / dv;
476  } else {
477  // -> set overtakeTime to something indicating impossibility of overtaking
478  overtakeTime = remainingSeconds + 1;
479  }
480 
481 #ifdef DEBUG_INFORMER
482  if (DEBUG_COND) {
483  std::cout << SIMTIME << " informLeader() of " << myVehicle.getID()
484  << "\nnv = " << nv->getID()
485  << "\nplannedSpeed = " << plannedSpeed
486  << "\nleaderSpeed = " << nv->getSpeed()
487  << "\nmyLeftSpace = " << myLeftSpace
488  << "\nremainingSeconds = " << remainingSeconds
489  << "\novertakeDist = " << overtakeDist
490  << "\novertakeTime = " << overtakeTime
491  << std::endl;
492  }
493 #endif
494 
495  if ((dv < 0
496  // overtaking on the right on an uncongested highway is forbidden (noOvertakeLCLeft)
498  // not enough space to overtake?
500  // 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).
502  // 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)
503  || (remainingSeconds < overtakeTime && (MSGlobals::gSemiImplicitEulerUpdate || !nv->isStopped())))
504  // opposite driving and must overtake
505  && !(isOpposite() && neighLead.second < 0 && neighLead.first->isStopped())) {
506  // cannot overtake
507  msgPass.informNeighLeader(new Info(std::numeric_limits<double>::max(), dir | LCA_AMBLOCKINGLEADER), &myVehicle);
508  // slow down smoothly to follow leader
509  const double targetSpeed = myCarFollowModel.followSpeed(
510  &myVehicle, myVehicle.getSpeed(), neighLead.second, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel());
511  if (targetSpeed < myVehicle.getSpeed()) {
512  // slow down smoothly to follow leader
513  const double decel = remainingSeconds == 0. ? myVehicle.getCarFollowModel().getMaxDecel() :
515  MAX2(MIN_FALLBEHIND, (myVehicle.getSpeed() - targetSpeed) / remainingSeconds));
516  const double nextSpeed = MIN2(plannedSpeed, myVehicle.getSpeed() - ACCEL2SPEED(decel));
517 #ifdef DEBUG_INFORMER
518  if (DEBUG_COND) {
519  std::cout << SIMTIME
520  << " cannot overtake leader nv=" << nv->getID()
521  << " dv=" << dv
522  << " myLookAheadSpeed=" << myLookAheadSpeed
523  << " myLeftSpace=" << myLeftSpace
524  << " overtakeDist=" << overtakeDist
525  << " overtakeTime=" << overtakeTime
526  << " remainingSeconds=" << remainingSeconds
527  << " currentGap=" << neighLead.second
530  << " targetSpeed=" << targetSpeed
531  << " nextSpeed=" << nextSpeed
532  << "\n";
533  }
534 #endif
535  addLCSpeedAdvice(nextSpeed);
536  return nextSpeed;
537  } else {
538  // leader is fast enough anyway
539 #ifdef DEBUG_INFORMER
540  if (DEBUG_COND) {
541  std::cout << SIMTIME
542  << " cannot overtake fast leader nv=" << nv->getID()
543  << " dv=" << dv
544  << " myLookAheadSpeed=" << myLookAheadSpeed
545  << " myLeftSpace=" << myLeftSpace
546  << " overtakeDist=" << overtakeDist
547  << " myLeadingBlockerLength=" << myLeadingBlockerLength
548  << " overtakeTime=" << overtakeTime
549  << " remainingSeconds=" << remainingSeconds
550  << " currentGap=" << neighLead.second
551  << " targetSpeed=" << targetSpeed
552  << "\n";
553  }
554 #endif
555  addLCSpeedAdvice(targetSpeed);
556  return plannedSpeed;
557  }
558  } else {
559  // overtaking, leader should not accelerate
560 #ifdef DEBUG_INFORMER
561  if (DEBUG_COND) {
562  std::cout << SIMTIME
563  << " wants to overtake leader nv=" << nv->getID()
564  << " dv=" << dv
565  << " overtakeDist=" << overtakeDist
566  << " remainingSeconds=" << remainingSeconds
567  << " overtakeTime=" << overtakeTime
568  << " currentGap=" << neighLead.second
570  << "\n";
571  }
572 #endif
573  msgPass.informNeighLeader(new Info(nv->getSpeed(), dir | LCA_AMBLOCKINGLEADER), &myVehicle);
574  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
575  }
576  } else if (neighLead.first != 0) { // (remainUnblocked)
577  // we are not blocked now. make sure we stay far enough from the leader
578  MSVehicle* nv = neighLead.first;
579  double dv, nextNVSpeed;
581  // XXX: the decrement (HELP_OVERTAKE) should be scaled with timestep length, I think.
582  // It seems to function as an estimate nv's speed in the next simstep!? (so HELP_OVERTAKE should be an acceleration value.)
583  nextNVSpeed = nv->getSpeed() - HELP_OVERTAKE; // conservative
584  dv = SPEED2DIST(myVehicle.getSpeed() - nextNVSpeed);
585  } else {
586  // Estimate neigh's speed after actionstep length
587  // @note The possible breaking can be underestimated by the formula, so this is a potential
588  // source of collisions if actionsteplength>simsteplength.
589  const double nvMaxDecel = HELP_OVERTAKE;
590  nextNVSpeed = nv->getSpeed() - nvMaxDecel * myVehicle.getActionStepLengthSecs(); // conservative
591  // Estimated gap reduction until next action step if own speed stays constant
592  dv = SPEED2DIST(myVehicle.getSpeed() - nextNVSpeed);
593  }
594  const double targetSpeed = myCarFollowModel.followSpeed(
595  &myVehicle, myVehicle.getSpeed(), neighLead.second - dv, nextNVSpeed, nv->getCarFollowModel().getMaxDecel());
596  addLCSpeedAdvice(targetSpeed);
597 #ifdef DEBUG_INFORMER
598  if (DEBUG_COND) {
599  std::cout << " not blocked by leader nv=" << nv->getID()
600  << " nvSpeed=" << nv->getSpeed()
601  << " gap=" << neighLead.second
602  << " nextGap=" << neighLead.second - dv
604  << " targetSpeed=" << targetSpeed
605  << "\n";
606  }
607 #endif
608  return MIN2(targetSpeed, plannedSpeed);
609  } else {
610  // not overtaking
611  return plannedSpeed;
612  }
613 }
614 
615 void
617  int blocked,
618  int dir,
619  const std::pair<MSVehicle*, double>& neighFollow,
620  double remainingSeconds,
621  double plannedSpeed) {
622 
623  MSVehicle* nv = neighFollow.first;
624  const double plannedAccel = SPEED2ACCEL(MAX2(MIN2(myCarFollowModel.getMaxAccel(), plannedSpeed - myVehicle.getSpeed()), -myCarFollowModel.getMaxDecel()));
625 
626 #ifdef DEBUG_INFORMER
627  if (DEBUG_COND) {
628  std::cout << "\nINFORM_FOLLOWER"
629  << "\nspeed=" << myVehicle.getSpeed() << " planned=" << plannedSpeed << "\n";
630  }
631 
632 #endif
633  if ((blocked & LCA_BLOCKED_BY_FOLLOWER) != 0) {
634  assert(nv != 0);
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->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->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(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);
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(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(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
870  || (dir == LCA_MLEFT && myLeftSpace > MAX_ONRAMP_LENGTH)
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(
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 moveHelper -> 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->getSpeed(), anticipatedSpeed, myCarFollowModel.getMaxDecel());
984 
985  // propose follower speed corresponding to first estimation of gap
986  double 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(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
1023  myLeftSpace = 0;
1024  myLCAccelerationAdvices.clear();
1025  myDontBrake = false;
1026  // truncate to work around numerical instability between different builds
1027  mySpeedGainProbability = ceil(mySpeedGainProbability * 100000.0) * 0.00001;
1028  myKeepRightProbability = ceil(myKeepRightProbability * 100000.0) * 0.00001;
1029 }
1030 
1031 
1032 void
1034  myOwnState = 0;
1037  if (myVehicle.getBestLaneOffset() == 0) {
1038  // if we are not yet on our best lane there might still be unseen blockers
1039  // (during patchSpeed)
1041  myLeftSpace = 0;
1042  }
1044  myLCAccelerationAdvices.clear();
1045  myDontBrake = false;
1046 }
1047 
1048 
1049 int
1051  int laneOffset,
1053  int blocked,
1054  const std::pair<MSVehicle*, double>& leader,
1055  const std::pair<MSVehicle*, double>& neighLead,
1056  const std::pair<MSVehicle*, double>& neighFollow,
1057  const MSLane& neighLane,
1058  const std::vector<MSVehicle::LaneQ>& preb, // XXX: What does "preb" stand for? Please comment. (Leo) Refs. #2578, #2604
1059  MSVehicle** lastBlocked,
1060  MSVehicle** firstBlocked) {
1061  assert(laneOffset == 1 || laneOffset == -1);
1062  const SUMOTime currentTime = MSNet::getInstance()->getCurrentTimeStep();
1063  // compute bestLaneOffset
1064  MSVehicle::LaneQ curr, neigh, best;
1065  int bestLaneOffset = 0;
1066  // What do these "dists" mean? Please comment. (Leo) Ad: I now think the following:
1067  // currentDist is the distance that the vehicle can go on its route without having to
1068  // change lanes from the current lane. neighDist as currentDist for the considered target lane (i.e., neigh)
1069  // If this is true I suggest to put this into the docu of wantsChange()
1070  // Another thing: "preb" probably means "previous best (lanes)"?!
1071  double currentDist = 0;
1072  double neighDist = 0;
1073  int currIdx = 0;
1074  MSLane* prebLane = myVehicle.getLane();
1075  if (prebLane->getEdge().isInternal()) {
1076  // internal edges are not kept inside the bestLanes structure
1077  prebLane = prebLane->getLinkCont()[0]->getLane();
1078  }
1079  // XXX: What does the following code do? Please comment. (Leo) Refs. #2578
1080  const bool checkOpposite = &neighLane.getEdge() != &myVehicle.getLane()->getEdge();
1081  const int prebOffset = (checkOpposite ? 0 : laneOffset);
1082  for (int p = 0; p < (int) preb.size(); ++p) {
1083  if (preb[p].lane == prebLane && p + laneOffset >= 0) {
1084  assert(p + prebOffset < (int)preb.size());
1085  curr = preb[p];
1086  neigh = preb[p + prebOffset];
1087  currentDist = curr.length;
1088  neighDist = neigh.length;
1089  bestLaneOffset = curr.bestLaneOffset;
1090  if (bestLaneOffset == 0 && preb[p + prebOffset].bestLaneOffset == 0) {
1091 #ifdef DEBUG_WANTS_CHANGE
1092  if (DEBUG_COND) {
1093  std::cout << STEPS2TIME(currentTime)
1094  << " veh=" << myVehicle.getID()
1095  << " bestLaneOffsetOld=" << bestLaneOffset
1096  << " bestLaneOffsetNew=" << laneOffset
1097  << "\n";
1098  }
1099 #endif
1100  bestLaneOffset = prebOffset;
1101  }
1102  best = preb[p + bestLaneOffset];
1103  currIdx = p;
1104  break;
1105  }
1106  }
1107  // direction specific constants
1108  const bool right = (laneOffset == -1);
1109  if (isOpposite() && right) {
1110  neigh = preb[preb.size() - 1];
1111  curr = neigh;
1112  best = neigh;
1113  bestLaneOffset = -1;
1114  curr.bestLaneOffset = -1;
1115  neighDist = neigh.length;
1116  currentDist = curr.length;
1117  }
1119  const int lca = (right ? LCA_RIGHT : LCA_LEFT);
1120  const int myLca = (right ? LCA_MRIGHT : LCA_MLEFT);
1121  const int lcaCounter = (right ? LCA_LEFT : LCA_RIGHT);
1122  const bool changeToBest = (right && bestLaneOffset < 0) || (!right && bestLaneOffset > 0);
1123  // keep information about being a leader/follower
1124  int ret = (myOwnState & 0xffff0000);
1125  int req = 0; // the request to change or stay
1126 
1127  ret = slowDownForBlocked(lastBlocked, ret);
1128  if (lastBlocked != firstBlocked) {
1129  ret = slowDownForBlocked(firstBlocked, ret);
1130  }
1131 
1132 #ifdef DEBUG_WANTS_CHANGE
1133  if (DEBUG_COND) {
1134  std::cout << SIMTIME
1135  << " veh=" << myVehicle.getID()
1136  << " _wantsChange state=" << myOwnState
1137  << " myLCAccelerationAdvices=" << toString(myLCAccelerationAdvices)
1138  << " firstBlocked=" << Named::getIDSecure(*firstBlocked)
1139  << " lastBlocked=" << Named::getIDSecure(*lastBlocked)
1140  << " leader=" << Named::getIDSecure(leader.first)
1141  << " leaderGap=" << leader.second
1142  << " neighLead=" << Named::getIDSecure(neighLead.first)
1143  << " neighLeadGap=" << neighLead.second
1144  << " neighFollow=" << Named::getIDSecure(neighFollow.first)
1145  << " neighFollowGap=" << neighFollow.second
1146  << "\n";
1147  }
1148 #endif
1149 
1150  // we try to estimate the distance which is necessary to get on a lane
1151  // we have to get on in order to keep our route
1152  // we assume we need something that depends on our velocity
1153  // and compare this with the free space on our wished lane
1154  //
1155  // if the free space is somehow(<-?) less than the space we need, we should
1156  // definitely try to get to the desired lane
1157  //
1158  // this rule forces our vehicle to change the lane if a lane changing is necessary soon
1159 
1160 
1161  // we do not want the lookahead distance to change all the time so we let it decay slowly
1162  // (in contrast, growth is applied instantaneously)
1165  } else {
1166  // memory decay factor for this action step
1167  const double memoryFactor = 1. - (1. - LOOK_AHEAD_SPEED_MEMORY) * myVehicle.getActionStepLengthSecs();
1168  assert(memoryFactor > 0.);
1170  (memoryFactor * myLookAheadSpeed + (1 - memoryFactor) * myVehicle.getSpeed()));
1171  }
1172  double laDist = myLookAheadSpeed * LOOK_FORWARD * myStrategicParam * (right ? 1 : myLookaheadLeft);
1173  laDist += myVehicle.getVehicleType().getLengthWithGap() * (double) 2.;
1174 
1175 
1176  if (bestLaneOffset == 0 && leader.first != 0 && leader.first->isStopped()) {
1177  // react to a stopped leader on the current lane
1178  // The value of laDist is doubled below for the check whether the lc-maneuver can be taken out
1179  // on the remaining distance (because the vehicle has to change back and forth). Therefore multiply with 0.5.
1180  laDist = 0.5 * (myVehicle.getVehicleType().getLengthWithGap()
1181  + leader.first->getVehicleType().getLengthWithGap());
1182  } else if (bestLaneOffset == laneOffset && neighLead.first != 0 && neighLead.first->isStopped()) {
1183  // react to a stopped leader on the target lane (if it is the bestLane)
1185  + neighLead.first->getVehicleType().getLengthWithGap();
1186  }
1187 
1188  // free space that is available for changing
1189  //const double neighSpeed = (neighLead.first != 0 ? neighLead.first->getSpeed() :
1190  // neighFollow.first != 0 ? neighFollow.first->getSpeed() :
1191  // best.lane->getSpeedLimit());
1192  // @note: while this lets vehicles change earlier into the correct direction
1193  // it also makes the vehicles more "selfish" and prevents changes which are necessary to help others
1194 
1195 
1196 
1197  // Next we assign to roundabout edges a larger distance than to normal edges
1198  // in order to decrease sense of lc urgency and induce higher usage of inner roundabout lanes.
1199  // TODO: include ticket860 code
1200  // 1) get information about the next upcoming roundabout
1201  double roundaboutDistanceAhead = 0;
1202  double roundaboutDistanceAheadNeigh = 0;
1203  int roundaboutEdgesAhead = 0;
1204  int roundaboutEdgesAheadNeigh = 0;
1205  if (!isOpposite()) {
1206  getRoundaboutAheadInfo(this, curr, neigh, roundaboutDistanceAhead, roundaboutDistanceAheadNeigh, roundaboutEdgesAhead, roundaboutEdgesAheadNeigh);
1207  }
1208  // 2) add a distance bonus for roundabout edges
1209  currentDist += roundaboutDistBonus(roundaboutDistanceAhead, roundaboutEdgesAhead);
1210  neighDist += roundaboutDistBonus(roundaboutDistanceAheadNeigh, roundaboutEdgesAheadNeigh);
1211 
1212 #ifdef DEBUG_WANTS_CHANGE
1213  if (DEBUG_COND) {
1214  if (roundaboutEdgesAhead > 0) {
1215  std::cout << " roundaboutEdgesAhead=" << roundaboutEdgesAhead << " roundaboutEdgesAheadNeigh=" << roundaboutEdgesAheadNeigh << "\n";
1216 // std::cout << " roundaboutDistanceAhead=" << roundaboutDistanceAhead << " roundaboutDistanceAheadNeigh=" << roundaboutDistanceAheadNeigh << "\n";
1217  }
1218  }
1219 #endif
1220 
1221  const double usableDist = (currentDist - posOnLane - best.occupation * JAM_FACTOR);
1222  //- (best.lane->getVehicleNumber() * neighSpeed)); // VARIANT 9 jfSpeed
1223  const double maxJam = MAX2(preb[currIdx + prebOffset].occupation, preb[currIdx].occupation);
1224  const double neighLeftPlace = MAX2((double) 0, neighDist - posOnLane - maxJam);
1225  const double vMax = myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle);
1226  // upper bound which will be restricted successively
1227  double thisLaneVSafe = vMax;
1228  const bool checkOverTakeRight = (!myAllowOvertakingRight
1229  && !myVehicle.congested()
1231 
1232 #ifdef DEBUG_WANTS_CHANGE
1233  if (DEBUG_COND) {
1234  std::cout << STEPS2TIME(currentTime)
1235  << " veh=" << myVehicle.getID()
1236  << " laSpeed=" << myLookAheadSpeed
1237  << " laDist=" << laDist
1238  << " currentDist=" << currentDist
1239  << " usableDist=" << usableDist
1240  << " bestLaneOffset=" << bestLaneOffset
1241  << " best.occupation=" << best.occupation
1242  << " best.length=" << best.length
1243  << " maxJam=" << maxJam
1244  << " neighLeftPlace=" << neighLeftPlace
1245  << "\n";
1246  }
1247 #endif
1248 
1249  bool changeLeftToAvoidOvertakeRight = false;
1250  if (changeToBest && bestLaneOffset == curr.bestLaneOffset
1251  && currentDistDisallows(usableDist, bestLaneOffset, laDist)) {
1253  ret = ret | lca | LCA_STRATEGIC | LCA_URGENT;
1254  } else {
1255  // VARIANT_20 (noOvertakeRight)
1256  if (neighLead.first != 0 && checkOverTakeRight && !right) {
1257  // check for slower leader on the left. we should not overtake but
1258  // rather move left ourselves (unless congested)
1259  MSVehicle* nv = neighLead.first;
1260  const double deltaV = MAX2(vMax - neighLane.getVehicleMaxSpeed(nv),
1261  myVehicle.getSpeed() - nv->getSpeed());
1262  if (deltaV > 0) {
1263  double vSafe = MAX2(
1266  &myVehicle, myVehicle.getSpeed(), neighLead.second, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel()));
1268  vSafe = MAX2(vSafe, nv->getSpeed());
1269  }
1270  thisLaneVSafe = MIN2(thisLaneVSafe, vSafe);
1271  addLCSpeedAdvice(vSafe);
1272  // only generate impulse for overtaking left shortly before braking would be necessary
1273  const double deltaGapFuture = deltaV * 8;
1274  const double vSafeFuture = myCarFollowModel.followSpeed(
1275  &myVehicle, myVehicle.getSpeed(), neighLead.second - deltaGapFuture, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel());
1276  if (vSafeFuture < vSafe) {
1277  const double relativeGain = deltaV / MAX2(vMax,
1280  changeLeftToAvoidOvertakeRight = true;
1281  }
1282 #ifdef DEBUG_WANTS_CHANGE
1283  if (DEBUG_COND) {
1284  std::cout << STEPS2TIME(currentTime)
1285  << " avoid overtaking on the right nv=" << nv->getID()
1286  << " deltaV=" << deltaV
1287  << " nvSpeed=" << nv->getSpeed()
1288  << " mySpeedGainProbability=" << mySpeedGainProbability
1289  << " planned acceleration =" << myLCAccelerationAdvices.back()
1290  << "\n";
1291  }
1292 #endif
1293  }
1294  }
1295 
1296  if (!changeToBest && (currentDistDisallows(neighLeftPlace, abs(bestLaneOffset) + 2, laDist))) {
1297  // the opposite lane-changing direction should be done than the one examined herein
1298  // we'll check whether we assume we could change anyhow and get back in time...
1299  //
1300  // this rule prevents the vehicle from moving in opposite direction of the best lane
1301  // unless the way till the end where the vehicle has to be on the best lane
1302  // is long enough
1303 #ifdef DEBUG_WANTS_CHANGE
1304  if (DEBUG_COND) {
1305  std::cout << " veh=" << myVehicle.getID() << " could not change back and forth in time (1) neighLeftPlace=" << neighLeftPlace << "\n";
1306  }
1307 #endif
1308  ret = ret | LCA_STAY | LCA_STRATEGIC;
1309  } else if (bestLaneOffset == 0 && (neighLeftPlace * 2. < laDist)) {
1310  // the current lane is the best and a lane-changing would cause a situation
1311  // of which we assume we will not be able to return to the lane we have to be on.
1312  // this rule prevents the vehicle from leaving the current, best lane when it is
1313  // close to this lane's end
1314 #ifdef DEBUG_WANTS_CHANGE
1315  if (DEBUG_COND) {
1316  std::cout << " veh=" << myVehicle.getID() << " could not change back and forth in time (2) neighLeftPlace=" << neighLeftPlace << "\n";
1317  }
1318 #endif
1319  ret = ret | LCA_STAY | LCA_STRATEGIC;
1320  } else if (bestLaneOffset == 0
1321  && (leader.first == 0 || !leader.first->isStopped())
1322  && neigh.bestContinuations.back()->getLinkCont().size() != 0
1323  && roundaboutEdgesAhead == 0
1324  && !checkOpposite
1325  && neighDist < TURN_LANE_DIST) {
1326  // VARIANT_21 (stayOnBest)
1327  // we do not want to leave the best lane for a lane which leads elsewhere
1328  // unless our leader is stopped or we are approaching a roundabout
1329 #ifdef DEBUG_WANTS_CHANGE
1330  if (DEBUG_COND) {
1331  std::cout << " veh=" << myVehicle.getID() << " does not want to leave the bestLane (neighDist=" << neighDist << ")\n";
1332  }
1333 #endif
1334  ret = ret | LCA_STAY | LCA_STRATEGIC;
1335  }
1336  }
1337  // check for overriding TraCI requests
1338 #ifdef DEBUG_WANTS_CHANGE
1339  if (DEBUG_COND) {
1340  std::cout << STEPS2TIME(currentTime) << " veh=" << myVehicle.getID() << " ret=" << ret;
1341  }
1342 #endif
1344  if ((ret & lcaCounter) != 0) {
1345  // we are not interested in traci requests for the opposite direction here
1346  ret &= ~(LCA_TRACI | lcaCounter | LCA_URGENT);
1347  }
1348 #ifdef DEBUG_WANTS_CHANGE
1349  if (DEBUG_COND) {
1350  std::cout << " retAfterInfluence=" << ret << "\n";
1351  }
1352 #endif
1353 
1354  if ((ret & LCA_STAY) != 0) {
1355  return ret;
1356  }
1357  if ((ret & LCA_URGENT) != 0) {
1358  // prepare urgent lane change maneuver
1359  // save the left space
1360  myLeftSpace = currentDist - posOnLane;
1361  if (changeToBest && abs(bestLaneOffset) > 1) {
1362  // there might be a vehicle which needs to counter-lane-change one lane further and we cannot see it yet
1363  myLeadingBlockerLength = MAX2((double)(right ? 20.0 : 40.0), myLeadingBlockerLength);
1364 #ifdef DEBUG_WANTS_CHANGE
1365  if (DEBUG_COND) {
1366  std::cout << " reserving space for unseen blockers myLeadingBlockerLength=" << myLeadingBlockerLength << "\n";
1367  }
1368 #endif
1369  }
1370 
1371  // letting vehicles merge in at the end of the lane in case of counter-lane change, step#1
1372  // if there is a leader and he wants to change to the opposite direction
1373  saveBlockerLength(neighLead.first, lcaCounter);
1374  if (*firstBlocked != neighLead.first) {
1375  saveBlockerLength(*firstBlocked, lcaCounter);
1376  }
1377 
1378  const double remainingSeconds = ((ret & LCA_TRACI) == 0 ?
1379  // MAX2((double)STEPS2TIME(TS), (myLeftSpace-myLeadingBlockerLength) / MAX2(myLookAheadSpeed, NUMERICAL_EPS) / abs(bestLaneOffset) / URGENCY) :
1380  MAX2((double)STEPS2TIME(TS), myLeftSpace / MAX2(myLookAheadSpeed, NUMERICAL_EPS) / abs(bestLaneOffset) / URGENCY) :
1382  const double plannedSpeed = informLeader(msgPass, blocked, myLca, neighLead, remainingSeconds);
1383  // NOTE: for the ballistic update case negative speeds may indicate a stop request,
1384  // while informLeader returns -1 in that case. Refs. #2577
1385  if (plannedSpeed >= 0 || (!MSGlobals::gSemiImplicitEulerUpdate && plannedSpeed != -1)) {
1386  // maybe we need to deal with a blocking follower
1387  informFollower(msgPass, blocked, myLca, neighFollow, remainingSeconds, plannedSpeed);
1388  }
1389 
1390 #ifdef DEBUG_WANTS_CHANGE
1391  if (DEBUG_COND) {
1392  std::cout << STEPS2TIME(currentTime)
1393  << " veh=" << myVehicle.getID()
1394  << " myLeftSpace=" << myLeftSpace
1395  << " remainingSeconds=" << remainingSeconds
1396  << " plannedSpeed=" << plannedSpeed
1397  << "\n";
1398  }
1399 #endif
1400 
1401  return ret;
1402  }
1403  // a high inconvenience prevents cooperative changes.
1404  const double inconvenience = MIN2((double)1.0, (laneOffset < 0
1407  const bool speedGainInconvenient = inconvenience > myCooperativeParam;
1408  const bool neighOccupancyInconvenient = neigh.lane->getBruttoOccupancy() > curr.lane->getBruttoOccupancy();
1409 
1410  // VARIANT_15
1411  if (roundaboutEdgesAhead > 1) {
1412 
1413 #ifdef DEBUG_WANTS_CHANGE
1414  if (DEBUG_COND) {
1415  std::cout << STEPS2TIME(currentTime)
1416  << " veh=" << myVehicle.getID()
1417  << " roundaboutEdgesAhead=" << roundaboutEdgesAhead
1418  << " myLeftSpace=" << myLeftSpace
1419  << "\n";
1420  }
1421 #endif
1422  // try to use the inner lanes of a roundabout to increase throughput
1423  // unless we are approaching the exit
1424  if (lca == LCA_LEFT) {
1425  // if inconvenience is not too high, request collaborative change (currently only for ballistic update)
1426  // TODO: test this for euler update! Refs. #2575
1427  if (MSGlobals::gSemiImplicitEulerUpdate || !neighOccupancyInconvenient) {
1428 // if(MSGlobals::gSemiImplicitEulerUpdate || !speedGainInconvenient){
1429  req = ret | lca | LCA_COOPERATIVE;
1430  }
1431  } else {
1432  // if inconvenience is not too high, request collaborative change (currently only for ballistic update)
1433  if (MSGlobals::gSemiImplicitEulerUpdate || neighOccupancyInconvenient) {
1434 // if(MSGlobals::gSemiImplicitEulerUpdate || speedGainInconvenient){
1435  req = ret | LCA_STAY | LCA_COOPERATIVE;
1436  }
1437  }
1438  if (!cancelRequest(req)) {
1439  return ret | req;
1440  }
1441  }
1442 
1443  // let's also regard the case where the vehicle is driving on a highway...
1444  // in this case, we do not want to get to the dead-end of an on-ramp
1445  if (right) {
1446  if (bestLaneOffset == 0 && myVehicle.getLane()->getSpeedLimit() > 80. / 3.6 && myLookAheadSpeed > SUMO_const_haltingSpeed) {
1447 #ifdef DEBUG_WANTS_CHANGE
1448  if (DEBUG_COND) {
1449  std::cout << " veh=" << myVehicle.getID() << " does not want to get stranded on the on-ramp of a highway\n";
1450  }
1451 #endif
1452  req = ret | LCA_STAY | LCA_STRATEGIC;
1453  if (!cancelRequest(req)) {
1454  return ret | req;
1455  }
1456  }
1457  }
1458  // --------
1459 
1460  // -------- make place on current lane if blocking follower
1461  //if (amBlockingFollowerPlusNB()) {
1462  // std::cout << myVehicle.getID() << ", " << currentDistAllows(neighDist, bestLaneOffset, laDist)
1463  // << " neighDist=" << neighDist
1464  // << " currentDist=" << currentDist
1465  // << "\n";
1466  //}
1467 
1469  && (!speedGainInconvenient)
1470  //&& ((myOwnState & lcaCounter) == 0) // VARIANT_6 : counterNoHelp
1471  && (changeToBest || currentDistAllows(neighDist, abs(bestLaneOffset) + 1, laDist))) {
1472 
1473  // VARIANT_2 (nbWhenChangingToHelp)
1474 #ifdef DEBUG_WANTS_CHANGE
1475  if (DEBUG_COND) {
1476  std::cout << STEPS2TIME(currentTime)
1477  << " veh=" << myVehicle.getID()
1478  << " wantsChangeToHelp=" << (right ? "right" : "left")
1479  << " state=" << myOwnState
1480  // << (((myOwnState & lcaCounter) != 0) ? " (counter)" : "")
1481  << "\n";
1482  }
1483 #endif
1484  req = ret | lca | LCA_COOPERATIVE | LCA_URGENT ;//| LCA_CHANGE_TO_HELP;
1485  if (!cancelRequest(req)) {
1486  return ret | req;
1487  }
1488  }
1489 
1490  // --------
1491 
1492 
1495  //if ((blocked & LCA_BLOCKED) != 0) {
1496  // return ret;
1497  //}
1499 
1500  // -------- higher speed
1501  //if ((congested(neighLead.first) && neighLead.second < 20) || predInteraction(leader.first)) { //!!!
1502  // return ret;
1503  //}
1504 
1505  double neighLaneVSafe = neighLane.getVehicleMaxSpeed(&myVehicle);
1506 
1507  // we wish to anticipate future speeds. This is difficult when the leading
1508  // vehicles are still accelerating so we resort to comparing next speeds in this case
1509  const bool acceleratingLeader = (neighLead.first != 0 && neighLead.first->getAcceleration() > 0)
1510  || (leader.first != 0 && leader.first->getAcceleration() > 0);
1511 
1512  if (acceleratingLeader) {
1513  // followSpeed allows acceleration for 1 step, to always compare speeds
1514  // after 1 second of acceleration we have call the function with a correct speed value
1515  // TODO: This should be explained better. Refs #2
1516  const double correctedSpeed = (myVehicle.getSpeed() + myVehicle.getCarFollowModel().getMaxAccel()
1518 
1519  if (neighLead.first == 0) {
1520  neighLaneVSafe = MIN2(neighLaneVSafe, myCarFollowModel.followSpeed(&myVehicle, correctedSpeed, neighDist, 0, 0));
1521  } else {
1522  neighLaneVSafe = MIN2(neighLaneVSafe, myCarFollowModel.followSpeed(
1523  &myVehicle, correctedSpeed, neighLead.second, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()));
1524  }
1525  if (leader.first == 0) {
1526  thisLaneVSafe = MIN2(thisLaneVSafe, myCarFollowModel.followSpeed(&myVehicle, correctedSpeed, currentDist, 0, 0));
1527  } else {
1528  thisLaneVSafe = MIN2(thisLaneVSafe, myCarFollowModel.followSpeed(
1529  &myVehicle, correctedSpeed, leader.second, leader.first->getSpeed(), leader.first->getCarFollowModel().getMaxDecel()));
1530  }
1531  } else {
1532  if (neighLead.first == 0) {
1533  neighLaneVSafe = MIN2(neighLaneVSafe, myCarFollowModel.maximumSafeStopSpeed(neighDist, myVehicle.getSpeed(), true));
1534  } else {
1535  neighLaneVSafe = MIN2(neighLaneVSafe, myCarFollowModel.maximumSafeFollowSpeed(neighLead.second, myVehicle.getSpeed(),
1536  neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel(), true));
1537  }
1538  if (leader.first == 0) {
1539  thisLaneVSafe = MIN2(thisLaneVSafe, myCarFollowModel.maximumSafeStopSpeed(currentDist, myVehicle.getSpeed(), true));
1540  } else {
1541  thisLaneVSafe = MIN2(thisLaneVSafe, myCarFollowModel.maximumSafeFollowSpeed(leader.second, myVehicle.getSpeed(),
1542  leader.first->getSpeed(), leader.first->getCarFollowModel().getMaxDecel(), true));
1543  }
1544  }
1545 
1546  if (neighLane.getEdge().getPersons().size() > 0) {
1547  // react to pedestrians
1548  adaptSpeedToPedestrians(myVehicle.getLane(), thisLaneVSafe);
1549  adaptSpeedToPedestrians(&neighLane, neighLaneVSafe);
1550  }
1551 
1552  const double relativeGain = (neighLaneVSafe - thisLaneVSafe) / MAX2(neighLaneVSafe,
1554 
1555 #ifdef DEBUG_WANTS_CHANGE
1556  if (DEBUG_COND) {
1557  std::cout << STEPS2TIME(currentTime)
1558  << " veh=" << myVehicle.getID()
1559  << " currentDist=" << currentDist
1560  << " neighDist=" << neighDist
1561  << " thisVSafe=" << thisLaneVSafe
1562  << " neighVSafe=" << neighLaneVSafe
1563  << " relGain=" << toString(relativeGain, 8)
1564  << "\n";
1565  }
1566 #endif
1567 
1568  if (right) {
1569  // ONLY FOR CHANGING TO THE RIGHT
1570  if (thisLaneVSafe - 5 / 3.6 > neighLaneVSafe) {
1571  // ok, the current lane is faster than the right one...
1572  if (mySpeedGainProbability < 0) {
1574  //myKeepRightProbability /= 2.0;
1575  }
1576  } else {
1577  // ok, the current lane is not (much) faster than the right one
1578  // @todo recheck the 5 km/h discount on thisLaneVSafe, refs. #2068
1579 
1580  // do not promote changing to the left just because changing to the right is bad
1581  // XXX: The following code may promote it, though!? (recheck!)
1582  // (Think of small negative mySpeedGainProbability and larger negative relativeGain)
1583  // One might think of replacing '||' by '&&' to exclude that possibility...
1584  // Still, for negative relativeGain, we might want to decrease the inclination for
1585  // changing to the left. Another solution could be the seperation of mySpeedGainProbability into
1586  // two variables (one for left and one for right). Refs #2578
1587  if (mySpeedGainProbability < 0 || relativeGain > 0) {
1589  }
1590 
1591  // honor the obligation to keep right (Rechtsfahrgebot)
1592  // XXX consider fast approaching followers on the current lane. Refs #2578
1593  //const double vMax = myLookAheadSpeed;
1594  const double acceptanceTime = KEEP_RIGHT_ACCEPTANCE * vMax * MAX2((double)1, myVehicle.getSpeed()) / myVehicle.getLane()->getSpeedLimit();
1595  double fullSpeedGap = MAX2(0., neighDist - myVehicle.getCarFollowModel().brakeGap(vMax));
1596  double fullSpeedDrivingSeconds = MIN2(acceptanceTime, fullSpeedGap / vMax);
1597  if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
1598  fullSpeedGap = MAX2(0., MIN2(fullSpeedGap,
1599  neighLead.second - myVehicle.getCarFollowModel().getSecureGap(
1600  vMax, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
1601  fullSpeedDrivingSeconds = MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
1602  }
1603  // stay on the current lane if we cannot overtake a slow leader on the right
1604  if (checkOverTakeRight && leader.first != 0
1605  && leader.first->getLane()->getVehicleMaxSpeed(leader.first) < vMax) {
1606  fullSpeedGap = MIN2(fullSpeedGap, leader.second);
1607  fullSpeedDrivingSeconds = MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - leader.first->getSpeed()));
1608  const double relativeGain = (vMax - leader.first->getLane()->getVehicleMaxSpeed(leader.first)) / MAX2(vMax,
1610  // tiebraker to avoid buridans paradox see #1312
1612  }
1613 
1614  const double deltaProb = (myChangeProbThresholdRight * (fullSpeedDrivingSeconds / acceptanceTime) / KEEP_RIGHT_TIME);
1616 
1617 #ifdef DEBUG_WANTS_CHANGE
1618  if (DEBUG_COND) {
1619  std::cout << STEPS2TIME(currentTime)
1620  << " veh=" << myVehicle.getID()
1621  << " vMax=" << vMax
1622  << " neighDist=" << neighDist
1623  << " brakeGap=" << myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed())
1624  << " leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
1625  << " secGap=" << (neighLead.first == 0 ? -1 : myVehicle.getCarFollowModel().getSecureGap(
1626  myVehicle.getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
1627  << " acceptanceTime=" << acceptanceTime
1628  << " fullSpeedGap=" << fullSpeedGap
1629  << " fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
1630  << " dProb=" << deltaProb
1631  << " myKeepRightProbability=" << myKeepRightProbability
1632  << "\n";
1633  }
1634 #endif
1636  req = ret | lca | LCA_KEEPRIGHT;
1637  if (!cancelRequest(req)) {
1638  return ret | req;
1639  }
1640  }
1641  }
1642 
1643 #ifdef DEBUG_WANTS_CHANGE
1644  if (DEBUG_COND) {
1645  std::cout << STEPS2TIME(currentTime)
1646  << " veh=" << myVehicle.getID()
1647  << " speed=" << myVehicle.getSpeed()
1648  << " mySpeedGainProbability=" << mySpeedGainProbability
1649  << " thisLaneVSafe=" << thisLaneVSafe
1650  << " neighLaneVSafe=" << neighLaneVSafe
1651  << " relativeGain=" << relativeGain
1652  << " blocked=" << blocked
1653  << "\n";
1654  }
1655 #endif
1656 
1658  && neighDist / MAX2((double) .1, myVehicle.getSpeed()) > 20.) { //./MAX2((double) .1, myVehicle.getSpeed())) { // -.1
1659  req = ret | lca | LCA_SPEEDGAIN;
1660  if (!cancelRequest(req)) {
1661  return ret | req;
1662  }
1663  }
1664  } else {
1665  // ONLY FOR CHANGING TO THE LEFT
1666  if (thisLaneVSafe > neighLaneVSafe) {
1667  // this lane is better
1668  if (mySpeedGainProbability > 0) {
1670  }
1671  } else if (thisLaneVSafe == neighLaneVSafe) {
1672  if (mySpeedGainProbability > 0) {
1674  }
1675  } else {
1676  // left lane is better
1678  }
1679  // VARIANT_19 (stayRight)
1680  //if (neighFollow.first != 0) {
1681  // MSVehicle* nv = neighFollow.first;
1682  // const double secGap = nv->getCarFollowModel().getSecureGap(nv->getSpeed(), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel());
1683  // if (neighFollow.second < secGap * KEEP_RIGHT_HEADWAY) {
1684  // // do not change left if it would inconvenience faster followers
1685  // return ret | LCA_STAY | LCA_SPEEDGAIN;
1686  // }
1687  //}
1688 
1689 #ifdef DEBUG_WANTS_CHANGE
1690  if (DEBUG_COND) {
1691  std::cout << STEPS2TIME(currentTime)
1692  << " veh=" << myVehicle.getID()
1693  << " speed=" << myVehicle.getSpeed()
1694  << " mySpeedGainProbability=" << mySpeedGainProbability
1695  << " thisLaneVSafe=" << thisLaneVSafe
1696  << " neighLaneVSafe=" << neighLaneVSafe
1697  << " relativeGain=" << relativeGain
1698  << " blocked=" << blocked
1699  << "\n";
1700  }
1701 #endif
1702 
1704  && (relativeGain > NUMERICAL_EPS || changeLeftToAvoidOvertakeRight)
1705  && neighDist / MAX2((double) .1, myVehicle.getSpeed()) > 20.) { // .1
1706  req = ret | lca | LCA_SPEEDGAIN;
1707  if (!cancelRequest(req)) {
1708  return ret | req;
1709  }
1710  }
1711  }
1712  // --------
1713  if (changeToBest && bestLaneOffset == curr.bestLaneOffset
1714  && (right ? mySpeedGainProbability < 0 : mySpeedGainProbability > 0)) {
1715  // change towards the correct lane, speedwise it does not hurt
1716  req = ret | lca | LCA_STRATEGIC;
1717  if (!cancelRequest(req)) {
1718  return ret | req;
1719  }
1720  }
1721 #ifdef DEBUG_WANTS_CHANGE
1722  if (DEBUG_COND) {
1723  std::cout << STEPS2TIME(currentTime)
1724  << " veh=" << myVehicle.getID()
1725  << " mySpeedGainProbability=" << mySpeedGainProbability
1726  << " myKeepRightProbability=" << myKeepRightProbability
1727  << " thisLaneVSafe=" << thisLaneVSafe
1728  << " neighLaneVSafe=" << neighLaneVSafe
1729  << "\n";
1730  }
1731 #endif
1732 
1733  return ret;
1734 }
1735 
1736 
1737 void
1739  double& roundaboutDistanceAhead, double& roundaboutDistanceAheadNeigh, int& roundaboutEdgesAhead, int& roundaboutEdgesAheadNeigh) {
1740 
1741  const MSVehicle& veh = lcm->myVehicle;
1742 
1743  // In what follows, we check whether a roundabout is ahead (or the vehicle is on a roundabout)
1744  // We calculate the lengths of the continuations described by curr and neigh,
1745  // which are part of the roundabout. Currently only takes effect for ballistic update, refs #1807, #2576 (Leo)
1746  double pos = lcm->isOpposite() ? veh.getLane()->getLength() - veh.getPositionOnLane() : veh.getPositionOnLane();
1747  roundaboutDistanceAhead = distanceAlongNextRoundabout(pos, veh.getLane(), curr.bestContinuations);
1748 
1749  // For the distance on the neigh.lane, we need to do a little hack since we may not
1750  // have access to the right initial lane (neigh.lane is only the first non-null lane of neigh.bestContinuations).
1751  roundaboutDistanceAheadNeigh = 0;
1752  double neighPosition = pos;
1753  if (veh.getLane()->getEdge().isInternal()) {
1754  // take care of the distance on internal lanes
1755  neighPosition = 0.;
1756  if (veh.getLane()->getEdge().isRoundabout()) {
1757  // vehicle is on internal roundabout lane -> neigh.lane is not a parallel internal lane, but the next non-internal lane
1758  // add remaining length on current, internal lane to roundabout distance on neigh continuation
1759  roundaboutDistanceAheadNeigh = veh.getLane()->getLength() - veh.getPositionOnLane();
1760  MSLane* nextLane = 0;
1761  for (std::vector<MSLane*>::const_iterator i = curr.bestContinuations.begin(); i != curr.bestContinuations.end(); i++) {
1762  if (*i != 0 && *i != veh.getLane()) {
1763  nextLane = *i;
1764  break;
1765  }
1766  }
1767  assert(nextLane != 0);
1768  // add all lengths remaining internal lanes of current continuations until nextLane
1769  const MSLink* link = MSLinkContHelper::getConnectingLink(*veh.getLane(), *nextLane);
1770  roundaboutDistanceAheadNeigh += link->getInternalLengthsAfter();
1771  }
1772  }
1773  // add roundabout distance from neigh.lane on
1774  roundaboutDistanceAheadNeigh += distanceAlongNextRoundabout(neighPosition, neigh.lane, neigh.bestContinuations);
1775 
1776 #ifdef DEBUG_WANTS_CHANGE
1777  if (lcm->debugVehicle()) {
1778  std::cout << "roundaboutDistanceAhead = " << roundaboutDistanceAhead
1779  << " roundaboutDistanceAheadNeigh = " << roundaboutDistanceAheadNeigh
1780  << "\n";
1781  }
1782 #endif
1783 
1784  // count the number of roundabout edges ahead to determine whether
1785  // special LC behavior is required (promoting the use of the inner lane, mainly)
1786  roundaboutEdgesAhead = 0;
1787  for (std::vector<MSLane*>::const_iterator it = curr.bestContinuations.begin(); it != curr.bestContinuations.end(); ++it) {
1788  const MSLane* lane = *it;
1789  if (lane != 0 && lane->getEdge().isRoundabout()) {
1790  roundaboutEdgesAhead += 1;
1791  } else if (roundaboutEdgesAhead > 0) {
1792  // only check the next roundabout
1793  break;
1794  }
1795  }
1796  roundaboutEdgesAheadNeigh = 0;
1797  for (std::vector<MSLane*>::const_iterator it = neigh.bestContinuations.begin(); it != neigh.bestContinuations.end(); ++it) {
1798  if ((*it) != 0 && (*it)->getEdge().isRoundabout()) {
1799  roundaboutEdgesAheadNeigh += 1;
1800  } else if (roundaboutEdgesAheadNeigh > 0) {
1801  // only check the next roundabout
1802  break;
1803  }
1804  }
1805  return;
1806 }
1807 
1808 
1809 double
1810 MSLCM_LC2013::roundaboutDistBonus(double roundaboutDistAhead, int roundaboutEdgesAhead) const {
1811  // NOTE: Currently there are two variants, one taking into account only the number
1812  // of upcoming non-internal roundabout edges and adding ROUNDABOUT_DIST_BONUS per upcoming edge except the first.
1813  // Another variant uses the actual distance and multiplies it by a factor ROUNDABOUT_DIST_FACTOR.
1814  // Currently, the update rule decides which variant to take (because the second was experimentally implemented
1815  // in the ballistic branch (ticket860)). Both variants may be combined in future. Refs. #2576
1817  if (roundaboutEdgesAhead > 1) {
1818  // Here we add a bonus length for each upcoming roundabout edge to the distance.
1819  // XXX: That becomes problematic, if the vehicle enters the last round about edge,
1820  // realizes suddenly that the change is very urgent and finds itself with very
1821  // few space to complete the urgent strategic change frequently leading to
1822  // a hang up on the inner lane.
1823  return roundaboutEdgesAhead * ROUNDABOUT_DIST_BONUS * myCooperativeParam;
1824  } else {
1825  return 0.;
1826  }
1827  } else {
1828  // This weighs the roundabout edges' distance with a larger factor
1829  if (roundaboutDistAhead > ROUNDABOUT_DIST_TRESH) {
1830  return (roundaboutDistAhead - ROUNDABOUT_DIST_TRESH) * (ROUNDABOUT_DIST_FACTOR - 1.);
1831  } else {
1832  return 0.;
1833  }
1834  }
1835 }
1836 
1837 
1838 double
1839 MSLCM_LC2013::distanceAlongNextRoundabout(double position, const MSLane* initialLane, const std::vector<MSLane*>& continuationLanes) {
1840  for (std::vector<MSLane*>::const_iterator i = continuationLanes.begin(); i != continuationLanes.end(); i++) {
1841  assert((*i) == 0 || !(*i)->getEdge().isInternal());
1842  }
1843 
1844  // We start with the current edge.
1845  bool encounteredRoundabout = false;
1846  double roundaboutDistanceAhead = 0.;
1847 
1848  // set an iterator to the first non-zero entry of continuationLanes
1849  std::vector<MSLane*>::const_iterator j = continuationLanes.begin();
1850  while (j != continuationLanes.end() && *j == 0) {
1851  ++j;
1852  }
1853 
1854  // differentiate possible situations
1855  if (j == continuationLanes.end()) {
1856  // continuations end here
1857  assert(initialLane == 0);
1858  return 0.;
1859  } else if (initialLane == 0) {
1860  // NOTE: this may occur when calling distanceAlongNextRoundabout() with neigh.lane in _wantsChange().
1861  // In that case, the possible internal lengths have been taken into account in _wantsChange().
1862  // Thus we set initialLane to the first non-zero lane in continuationLanes
1863  initialLane = *j;
1864  } else if (!initialLane->isInternal() && initialLane != *j) {
1865  // this may occur during opposite direction driving where the initial Lane
1866  // is the reverse lane of *j. This should not happen on a roundabout! Therefore we can skip initialLane.
1867  assert(!initialLane->getEdge().isRoundabout());
1868  initialLane = *j;
1869  } else if (initialLane->getEdge().isRoundabout()) {
1870  // initial lane is on roundabout
1871  assert(position >= 0. && position <= initialLane->getLength());
1872  if (!initialLane->isInternal()) {
1873  assert(initialLane == *j);
1874  roundaboutDistanceAhead += initialLane->getLength() - position;
1875  if (j + 1 == continuationLanes.end() || *(j + 1) == 0 || !(*(j + 1))->getEdge().isRoundabout()) {
1876  // following connection is not part of the roundabout
1877  } else {
1878  // add internal lengths
1879  const MSLane* nextLane = *(j + 1);
1880  const MSLink* link = MSLinkContHelper::getConnectingLink(*initialLane, *nextLane);
1881  assert(link != 0);
1882  roundaboutDistanceAhead += link->getInternalLengthsAfter();
1883  }
1884  j++;
1885  } else {
1886  // initialLane is an internal roundabout lane -> add length to roundaboutDistanceAhead
1887  roundaboutDistanceAhead += initialLane->getLength() - position;
1888  assert(initialLane->getLinkCont().size() == 1);
1889  roundaboutDistanceAhead += initialLane->getLinkCont()[0]->getInternalLengthsAfter();
1890  }
1891  }
1892 
1893  // treat lanes beyond the initial one
1894  for (std::vector<MSLane*>::const_iterator it = j; it != continuationLanes.end(); ++it) {
1895  const MSLane* lane = *it;
1896  assert(lane != 0); // possible leading NULL lanes in continuationLanes were treated above
1897  if (lane->getEdge().isRoundabout()) {
1898  encounteredRoundabout = true;
1899  // add roundabout lane length
1900  roundaboutDistanceAhead += lane->getLength();
1901 
1902  // since bestContinuations contains no internal edges
1903  // add consecutive connection lengths if it is part of the route and the
1904  // consecutive edge is on the roundabout as well
1905  if (it + 1 != continuationLanes.end() && *(it + 1) != 0 && (*(it + 1))->getEdge().isRoundabout()) {
1906  // find corresponding link for the current lane
1907  const MSLink* link = MSLinkContHelper::getConnectingLink(*lane, **(it + 1));
1908  assert(link != 0);
1909  double linkLength = link->getInternalLengthsAfter();
1910  roundaboutDistanceAhead += linkLength;
1911  }
1912  } else if (encounteredRoundabout) {
1913  // only check the next roundabout
1914  break;
1915  }
1916  }
1917  return roundaboutDistanceAhead;
1918 }
1919 
1920 
1921 
1922 int
1924  // if this vehicle is blocking someone in front, we maybe decelerate to let him in
1925  if ((*blocked) != 0) {
1926  double gap = (*blocked)->getPositionOnLane() - (*blocked)->getVehicleType().getLength() - myVehicle.getPositionOnLane() - myVehicle.getVehicleType().getMinGap();
1927 #ifdef DEBUG_SLOW_DOWN
1928  if (DEBUG_COND) {
1929  std::cout << SIMTIME
1930  << " veh=" << myVehicle.getID()
1931  << " blocked=" << Named::getIDSecure(*blocked)
1932  << " gap=" << gap
1933  << "\n";
1934  }
1935 #endif
1936  if (gap > POSITION_EPS) {
1937  //const bool blockedWantsUrgentRight = (((*blocked)->getLaneChangeModel().getOwnState() & LCA_RIGHT != 0)
1938  // && ((*blocked)->getLaneChangeModel().getOwnState() & LCA_URGENT != 0));
1939 
1941  //|| blockedWantsUrgentRight // VARIANT_10 (helpblockedRight)
1942  ) {
1943  if ((*blocked)->getSpeed() < SUMO_const_haltingSpeed) {
1944  state |= LCA_AMBACKBLOCKER_STANDING;
1945  } else {
1946  state |= LCA_AMBACKBLOCKER;
1947  }
1950  (double)(gap - POSITION_EPS), (*blocked)->getSpeed(),
1951  (*blocked)->getCarFollowModel().getMaxDecel()));
1952 
1953  //(*blocked) = 0; // VARIANT_14 (furtherBlock)
1954 #ifdef DEBUG_SLOW_DOWN
1955  if (DEBUG_COND) {
1956  std::cout << SIMTIME
1957  << " veh=" << myVehicle.getID()
1958  << " slowing down for"
1959  << " blocked=" << Named::getIDSecure(*blocked)
1960  << " helpSpeed=" << myLCAccelerationAdvices.back()
1961  << "\n";
1962  }
1963 #endif
1964  } /* else {
1965  // experimental else-branch...
1966  state |= LCA_AMBACKBLOCKER;
1967  myVSafes.push_back(myCarFollowModel.followSpeed(
1968  &myVehicle, myVehicle.getSpeed(),
1969  (double)(gap - POSITION_EPS), (*blocked)->getSpeed(),
1970  (*blocked)->getCarFollowModel().getMaxDecel()));
1971  }*/
1972  }
1973  }
1974  return state;
1975 }
1976 
1977 
1978 void
1979 MSLCM_LC2013::saveBlockerLength(MSVehicle* blocker, int lcaCounter) {
1980 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
1981  if (DEBUG_COND) {
1982  std::cout << SIMTIME
1983  << " veh=" << myVehicle.getID()
1984  << " saveBlockerLength blocker=" << Named::getIDSecure(blocker)
1985  << " bState=" << (blocker == 0 ? "None" : toString(blocker->getLaneChangeModel().getOwnState()))
1986  << "\n";
1987  }
1988 #endif
1989  if (blocker != 0 && (blocker->getLaneChangeModel().getOwnState() & lcaCounter) != 0) {
1990  // is there enough space in front of us for the blocker?
1991  const double potential = myLeftSpace - myVehicle.getCarFollowModel().brakeGap(
1993  if (blocker->getVehicleType().getLengthWithGap() <= potential) {
1994  // save at least his length in myLeadingBlockerLength
1996 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
1997  if (DEBUG_COND) {
1998  std::cout << SIMTIME
1999  << " veh=" << myVehicle.getID()
2000  << " blocker=" << Named::getIDSecure(blocker)
2001  << " saving myLeadingBlockerLength=" << myLeadingBlockerLength
2002  << "\n";
2003  }
2004 #endif
2005  } else {
2006  // we cannot save enough space for the blocker. It needs to save
2007  // space for ego instead
2008 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
2009  if (DEBUG_COND) {
2010  std::cout << SIMTIME
2011  << " veh=" << myVehicle.getID()
2012  << " blocker=" << Named::getIDSecure(blocker)
2013  << " cannot save space=" << blocker->getVehicleType().getLengthWithGap()
2014  << " potential=" << potential
2015  << "\n";
2016  }
2017 #endif
2019  }
2020  }
2021 }
2022 
2023 
2024 void
2026  if (MSPModel::getModel()->hasPedestrians(lane)) {
2027 #ifdef DEBUG_WANTS_CHANGE
2028  if (DEBUG_COND) {
2029  std::cout << SIMTIME << " adapt to pedestrians on lane=" << lane->getID() << "\n";
2030  }
2031 #endif
2035  if (leader.first != 0) {
2036  const double stopSpeed = myVehicle.getCarFollowModel().stopSpeed(&myVehicle, myVehicle.getSpeed(), leader.second - myVehicle.getVehicleType().getMinGap());
2037  v = MIN2(v, stopSpeed);
2038 #ifdef DEBUG_WANTS_CHANGE
2039  if (DEBUG_COND) {
2040  std::cout << SIMTIME << " pedLeader=" << leader.first->getID() << " dist=" << leader.second << " v=" << v << "\n";
2041  }
2042 #endif
2043  }
2044  }
2045 }
2046 
2047 
2048 void MSLCM_LC2013::addLCSpeedAdvice(const double vSafe) {
2049  const double accel = SPEED2ACCEL(vSafe - myVehicle.getSpeed());
2050  myLCAccelerationAdvices.push_back(accel);
2051 }
2052 
2053 
2054 std::string
2055 MSLCM_LC2013::getParameter(const std::string& key) const {
2057  return toString(myStrategicParam);
2058  } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_PARAM)) {
2059  return toString(myCooperativeParam);
2060  } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_PARAM)) {
2061  return toString(mySpeedGainParam);
2062  } else if (key == toString(LCA_KEEPRIGHT)) {
2063  return toString(myKeepRightParam);
2064  } else if (key == toString(SUMO_ATTR_LCA_LOOKAHEADLEFT)) {
2065  return toString(myLookaheadLeft);
2066  } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAINRIGHT)) {
2067  return toString(mySpeedGainRight);
2068  }
2069  throw InvalidArgument("Parameter '" + key + "' is not supported for laneChangeModel of type '" + toString(myModel) + "'");
2070 }
2071 
2072 
2073 void
2074 MSLCM_LC2013::setParameter(const std::string& key, const std::string& value) {
2075  double doubleValue;
2076  try {
2077  doubleValue = TplConvert::_2double(value.c_str());
2078  } catch (NumberFormatException) {
2079  throw InvalidArgument("Setting parameter '" + key + "' requires a number for laneChangeModel of type '" + toString(myModel) + "'");
2080  }
2082  myStrategicParam = doubleValue;
2083  } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_PARAM)) {
2084  myCooperativeParam = doubleValue;
2085  } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_PARAM)) {
2086  mySpeedGainParam = doubleValue;
2087  } else if (key == toString(LCA_KEEPRIGHT)) {
2088  myKeepRightParam = doubleValue;
2089  } else if (key == toString(SUMO_ATTR_LCA_LOOKAHEADLEFT)) {
2090  myLookaheadLeft = doubleValue;
2091  } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAINRIGHT)) {
2092  mySpeedGainRight = doubleValue;
2093  } else {
2094  throw InvalidArgument("Setting parameter '" + key + "' is not supported for laneChangeModel of type '" + toString(myModel) + "'");
2095  }
2097 }
2098 
2099 /****************************************************************************/
2100 
double myLeadingBlockerLength
Definition: MSLCM_LC2013.h:256
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 ...
double getBruttoOccupancy() const
Returns the brutto (including minGaps) occupancy of this lane during the last step.
Definition: MSLane.cpp:2383
#define LOOK_FORWARD
double getLengthWithGap() const
Get vehicle&#39;s length including the minimum gap [m].
double getVehicleMaxSpeed(const SUMOVehicle *const veh) const
Returns the lane&#39;s maximum speed, given a vehicle&#39;s speed limit adaptation.
Definition: MSLane.h:475
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:770
void saveBlockerLength(MSVehicle *blocker, int lcaCounter)
save space for vehicles which need to counter-lane-change
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver&#39;s reaction time tau (i...
Definition: MSCFModel.h:289
MSEdge & getEdge() const
Returns the lane&#39;s edge.
Definition: MSLane.h:607
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:83
MSLCM_LC2013(MSVehicle &v)
The action is due to the default of keeping right "Rechtsfahrgebot".
#define SPEED2DIST(x)
Definition: SUMOTime.h:54
The action is done to help someone else.
double getWaitingSeconds() const
Returns the number of seconds waited (speed was lesser than 0.1m/s)
Definition: MSVehicle.h:660
#define MIN_FALLBEHIND
#define ACCEL2SPEED(x)
Definition: SUMOTime.h:60
MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:564
int bestLaneOffset
The (signed) number of lanes to be crossed to get to the lane which allows to continue the drive...
Definition: MSVehicle.h:811
int gPrecision
the precision for floating point outputs
Definition: StdDefs.cpp:29
virtual double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel) const =0
Computes the vehicle&#39;s follow speed (no dawdling)
double myKeepRightProbability
Definition: MSLCM_LC2013.h:254
#define KEEP_RIGHT_ACCEPTANCE
The car-following model abstraction.
Definition: MSCFModel.h:59
void addLCSpeedAdvice(const double vSafe)
Takes a vSafe (speed advice for speed in the next simulation step), converts it into an acceleration ...
double getPositionOnLane() const
Get the vehicle&#39;s position along the lane.
Definition: MSVehicle.h:402
double myChangeProbThresholdLeft
Definition: MSLCM_LC2013.h:288
void * informNeighFollower(void *info, MSVehicle *sender)
Informs the follower on the desired lane.
#define MAX_ONRAMP_LENGTH
double myLookAheadSpeed
Definition: MSLCM_LC2013.h:261
int getBestLaneOffset() const
Definition: MSVehicle.cpp:4055
Wants go to the right.
double myStrategicParam
Definition: MSLCM_LC2013.h:271
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
double myKeepRightParam
Definition: MSLCM_LC2013.h:274
void initDerivedParameters()
init cached parameters derived directly from model parameters
bool isRoundabout() const
Definition: MSEdge.h:619
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:167
T MAX2(T a, T b)
Definition: StdDefs.h:73
double _patchSpeed(const double min, const double wanted, const double max, const MSCFModel &cfModel)
double getLength() const
Returns the lane&#39;s length.
Definition: MSLane.h:497
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:67
double getRightSideOnLane() const
Get the vehicle&#39;s lateral position on the lane:
Definition: MSVehicle.cpp:4331
const std::string & getID() const
Returns the id.
Definition: Named.h:74
std::pair< double, int > Info
information regarding save velocity (unused) and state flags of the ego vehicle
Definition: MSLCM_LC2013.h:247
#define TS
Definition: SUMOTime.h:51
double length
The overall length which may be driven when using this lane without a lane change.
Definition: MSVehicle.h:803
Wants go to the left.
The action is due to the wish to be faster (tactical lc)
#define SPEED2ACCEL(x)
Definition: SUMOTime.h:62
#define UNUSED_PARAMETER(x)
Definition: StdDefs.h:39
bool currentDistDisallows(double dist, int laneOffset, double lookForwardDist)
Definition: MSLCM_LC2013.h:229
bool hasLaneChanger() const
Definition: MSEdge.h:631
#define ROUNDABOUT_DIST_FACTOR
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:3674
#define SIMTIME
Definition: SUMOTime.h:71
static double distanceAlongNextRoundabout(double position, const MSLane *initialLane, const std::vector< MSLane *> &continuationLanes)
compute the distance on the next upcoming roundabout along a given sequence of lanes.
double getMaxSpeedOnLane() const
Returns the maximal speed for the vehicle on its current lane (including speed factor and deviation...
Definition: MSVehicle.h:574
double getMaxAccel() const
Get the vehicle type&#39;s maximum acceleration [m/s^2].
Definition: MSCFModel.h:203
Needs to stay on the current lane.
const LaneChangeModel myModel
the type of this model
#define ROUNDABOUT_DIST_BONUS
#define DEBUG_COND
double changeRequestRemainingSeconds(const SUMOTime currentTime) const
Return the remaining number of seconds of the current laneTimeLine assuming one exists.
Definition: MSVehicle.cpp:430
bool isInternal() const
Definition: MSLane.cpp:1774
static bool myAllowOvertakingRight
whether overtaking on the right is permitted
bool debugVehicle() const
whether the current vehicles shall be debugged
A class responsible for exchanging messages between cars involved in lane-change interaction.
static double overtakeDistance(const MSVehicle *follower, const MSVehicle *leader, const double gap, double followerSpeed=INVALID_SPEED, double leaderSpeed=INVALID_SPEED)
MSLane * lane
The described lane.
Definition: MSVehicle.h:801
const MSCFModel & getCarFollowModel() const
Returns the vehicle&#39;s car following model definition.
Definition: MSVehicle.h:882
A lane change model developed by D. Krajzewicz, J. Erdmann et al. between 2004 and 2013...
Definition: MSLCM_LC2013.h:54
double mySpeedGainParam
Definition: MSLCM_LC2013.h:273
bool cancelRequest(int state)
whether the influencer cancels the given request
double getActionStepLengthSecs() const
Returns the vehicle&#39;s action step length in secs, i.e. the interval between two action points...
Definition: MSVehicle.h:516
double mySpeedGainProbability
a value for tracking the probability that a change to the offset with the same sign is beneficial ...
Definition: MSLCM_LC2013.h:250
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
blocked in all directions
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:55
The action is urgent (to be defined by lc-model)
static MSPModel * getModel()
Definition: MSPModel.cpp:65
double mySpeedGainRight
Definition: MSLCM_LC2013.h:279
#define CUT_IN_LEFT_SPEED_THRESHOLD
bool currentDistAllows(double dist, int laneOffset, double lookForwardDist)
Definition: MSLCM_LC2013.h:232
const std::set< MSTransportable * > & getPersons() const
Returns this edge&#39;s persons set.
Definition: MSEdge.h:176
#define LOOK_AHEAD_SPEED_MEMORY
const double myExperimentalParam1
Definition: MSLCM_LC2013.h:281
double getSpeedLimit() const
Returns the lane&#39;s maximum allowed speed.
Definition: MSLane.h:489
#define STEPS2TIME(x)
Definition: SUMOTime.h:64
int slowDownForBlocked(MSVehicle **blocked, int state)
compute useful slowdowns for blocked vehicles
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
Definition: MSNet.h:253
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...
T MIN2(T a, T b)
Definition: StdDefs.h:67
#define RELGAIN_NORMALIZATION_MIN_SPEED
The action is needed to follow the route (navigational lc)
virtual double getSpeedAfterMaxDecel(double v) const
Returns the velocity after maximum deceleration.
Definition: MSCFModel.h:318
#define POSITION_EPS
Definition: config.h:175
double getImpatience() const
Returns this vehicles impatience.
A structure representing the best lanes for continuing the current route starting at &#39;lane&#39;...
Definition: MSVehicle.h:799
virtual PersonDist nextBlocking(const MSLane *lane, double minPos, double minRight, double maxLeft, double stopTime=0)
returns the next pedestrian beyond minPos that is laterally between minRight and maxLeft or 0 ...
Definition: MSPModel.h:96
double getMinGap() const
Get the free space in front of vehicles of this class.
double getMaxDecel() const
Get the vehicle type&#39;s maximal comfortable deceleration [m/s^2].
Definition: MSCFModel.h:211
int myOwnState
The current state of the vehicle.
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:662
bool isInternal() const
return whether this edge is an internal edge
Definition: MSEdge.h:229
std::vector< double > myLCAccelerationAdvices
vector of LC-related acceleration recommendations Filled in wantsChange() and applied in patchSpeed()...
Definition: MSLCM_LC2013.h:265
double getLateralPositionOnLane() const
Get the vehicle&#39;s lateral position on the lane.
Definition: MSVehicle.h:439
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:458
double myLeftSpace
Definition: MSLCM_LC2013.h:257
std::pair< const MSPerson *, double > PersonDist
Definition: MSPModel.h:43
double myLookaheadLeft
Definition: MSLCM_LC2013.h:277
double myChangeProbThresholdRight
Definition: MSLCM_LC2013.h:287
double getWidth() const
Get the width which vehicles of this class shall have when being drawn.
MSVehicle & myVehicle
The vehicle this lane-changer belongs to.
void * informNeighLeader(void *info, MSVehicle *sender)
Informs the leader on the desired lane.
Influencer & getInfluencer()
Returns the velocity/lane influencer.
Definition: MSVehicle.cpp:4812
LaneChangeAction
The state of a vehicle&#39;s lane-change behavior.
#define JAM_FACTOR
static void getRoundaboutAheadInfo(const MSLCM_LC2013 *lcm, const MSVehicle::LaneQ &curr, const MSVehicle::LaneQ &neigh, double &roundaboutDistanceAhead, double &roundaboutDistanceAheadNeigh, int &roundaboutEdgesAhead, int &roundaboutEdgesAheadNeigh)
computes the distance and number of edges in the next upcoming roundabout along the lane continuation...
void adaptSpeedToPedestrians(const MSLane *lane, double &v)
react to pedestrians on the given lane
double getOppositePos(double pos) const
return the corresponding position on the opposite lane
Definition: MSLane.cpp:3021
double occupation
The overall vehicle sum on consecutive lanes which can be passed without a lane change.
Definition: MSVehicle.h:807
const MSVehicleType & getVehicleType() const
Returns the vehicle&#39;s type definition.
void setParameter(const std::string &key, const std::string &value)
try to set the given parameter for this laneChangeModel. Throw exception for unsupported key ...
std::vector< MSLane * > bestContinuations
Definition: MSVehicle.h:819
static double _2double(const E *const data)
converts a char-type array into the double value described by it
Definition: TplConvert.h:311
#define URGENCY
double myCooperativeParam
Definition: MSLCM_LC2013.h:272
virtual ~MSLCM_LC2013()
#define INVALID_SPEED
Definition: MSCFModel.h:39
std::string getParameter(const std::string &key) const
try to retrieve the given parameter from this device. Throw exception for unsupported key ...
double getLength() const
Get vehicle&#39;s length [m].
const double SUMO_const_haltingSpeed
the speed threshold at which vehicles are considered as halting
Definition: StdDefs.h:57
virtual void saveBlockerLength(double length)
reserve space at the end of the lane to avoid dead locks
The action is due to a TraCI request.
static bool gSemiImplicitEulerUpdate
Definition: MSGlobals.h:62
long long int SUMOTime
Definition: TraCIDefs.h:51
#define NUMERICAL_EPS
Definition: config.h:151
bool amBlockingFollowerPlusNB()
Definition: MSLCM_LC2013.h:226
#define ROUNDABOUT_DIST_TRESH
#define HELP_OVERTAKE
double informLeader(MSAbstractLaneChangeModel::MSLCMessager &msgPass, int blocked, int dir, const std::pair< MSVehicle *, double > &neighLead, double remainingSeconds)
bool isStopped() const
Returns whether the vehicle is at a stop.
Definition: MSVehicle.cpp:1332
double getSecureGap(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:304
#define LOOK_AHEAD_MIN_SPEED
const MSLinkCont & getLinkCont() const
returns the container with all links !!!
Definition: MSLane.cpp:1874
#define LCA_RIGHT_IMPATIENCE
double getSpeed() const
Returns the vehicle&#39;s current speed.
Definition: MSVehicle.h:482
int influenceChangeDecision(int state)
allow TraCI to influence a lane change decision
Definition: MSVehicle.cpp:4836
public emergency vehicles
void * inform(void *info, MSVehicle *sender)
blocker by follower
const std::string & getID() const
Returns the name of the vehicle.
Representation of a lane in the micro simulation.
Definition: MSLane.h:77
const MSCFModel & myCarFollowModel
The vehicle&#39;s car following model.
Interface for lane-change models.
virtual double stopSpeed(const MSVehicle *const veh, const double speed, double gap) const =0
Computes the vehicle&#39;s safe speed for approaching a non-moving obstacle (no dawdling) ...
bool isAccelLane() const
return whether this lane is an acceleration lane
Definition: MSLane.h:449
SUMOVehicleClass getVehicleClass() const
Get this vehicle type&#39;s vehicle class.
#define HELP_DECEL_FACTOR
double roundaboutDistBonus(double roundaboutDistAhead, int roundaboutEdgesAhead) const
Computes the artificial bonus distance for roundabout lanes this additional distance reduces the sens...
#define TURN_LANE_DIST
#define KEEP_RIGHT_TIME
bool congested() const
Definition: MSVehicle.h:714