Eclipse SUMO - Simulation of Urban MObility
MSLCM_SL2015.cpp
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2013-2019 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
15 // A lane change model for heterogeneous traffic (based on sub-lanes)
16 /****************************************************************************/
17 
18 
19 // ===========================================================================
20 // included modules
21 // ===========================================================================
22 #include <config.h>
23 
24 #include <iostream>
27 #include <microsim/MSEdge.h>
28 #include <microsim/MSLane.h>
29 #include <microsim/MSNet.h>
30 #include <microsim/MSGlobals.h>
32 #include "MSLCM_SL2015.h"
33 
34 // ===========================================================================
35 // variable definitions
36 // ===========================================================================
37 #define MAGIC_OFFSET 1.
38 #define LOOK_FORWARD (double)10.
39 
40 #define JAM_FACTOR (double)1.
41 //#define JAM_FACTOR 2. // VARIANT_8 (makes vehicles more focused but also more "selfish")
42 
43 #define LCA_RIGHT_IMPATIENCE (double)-1.
44 #define CUT_IN_LEFT_SPEED_THRESHOLD (double)27.
45 #define MAX_ONRAMP_LENGTH (double)200.
46 
47 #define LOOK_AHEAD_MIN_SPEED 0.0
48 #define LOOK_AHEAD_SPEED_MEMORY 0.9
49 
50 #define HELP_DECEL_FACTOR (double)1.0
51 
52 #define HELP_OVERTAKE (double)(10.0 / 3.6)
53 #define MIN_FALLBEHIND (double)(7.0 / 3.6)
54 
55 #define KEEP_RIGHT_HEADWAY (double)2.0
56 
57 #define URGENCY (double)2.0
58 
59 #define ROUNDABOUT_DIST_BONUS (double)100.0
60 
61 #define KEEP_RIGHT_TIME (double)5.0 // the number of seconds after which a vehicle should move to the right lane
62 #define KEEP_RIGHT_ACCEPTANCE (double)7.0 // calibration factor for determining the desire to keep right
63 
64 #define RELGAIN_NORMALIZATION_MIN_SPEED (double)10.0
65 
66 #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
67 #define GAIN_PERCEPTION_THRESHOLD 0.05 // the minimum relative speed gain which affects the behavior
68 
69 #define SPEED_GAIN_MIN_SECONDS 20.0
70 
71 #define ARRIVALPOS_LAT_THRESHOLD 100.0
72 
73 // the speed at which the desired lateral gap grows now further
74 #define LATGAP_SPEED_THRESHOLD (50 / 3.6)
75 // the speed at which the desired lateral gap shrinks now further.
76 // @note: when setting LATGAP_SPEED_THRESHOLD = LATGAP_SPEED_THRESHOLD2, no speed-specif reduction of minGapLat is done
77 #define LATGAP_SPEED_THRESHOLD2 (50 / 3.6)
78 
79 // intention to change decays over time
80 #define SPEEDGAIN_DECAY_FACTOR 0.5
81 // exponential averaging factor for expected sublane speeds
82 #define SPEEDGAIN_MEMORY_FACTOR 0.5
83 
84 
85 
86 // ===========================================================================
87 // Debug flags
88 // ===========================================================================
89 //#define DEBUG_ACTIONSTEPS
90 //#define DEBUG_STATE
91 //#define DEBUG_SURROUNDING
92 //#define DEBUG_MANEUVER
93 //#define DEBUG_COMMITTED_SPEED
94 //#define DEBUG_PATCHSPEED
95 //#define DEBUG_INFORM
96 //#define DEBUG_ROUNDABOUTS
97 //#define DEBUG_WANTSCHANGE
98 //#define DEBUG_COOPERATE
99 //#define DEBUG_SLOWDOWN
100 //#define DEBUG_SAVE_BLOCKER_LENGTH
101 //#define DEBUG_BLOCKING
102 //#define DEBUG_TRACI
103 //#define DEBUG_STRATEGIC_CHANGE
104 //#define DEBUG_KEEP_LATGAP
105 //#define DEBUG_EXPECTED_SLSPEED
106 //#define DEBUG_COND (myVehicle.getID() == "moped.18" || myVehicle.getID() == "moped.16")
107 //#define DEBUG_COND (myVehicle.getID() == "Togliatti_71_0")
108 #define DEBUG_COND (myVehicle.isSelected())
109 //#define DEBUG_COND (myVehicle.getID() == "pkw150478" || myVehicle.getID() == "pkw150494" || myVehicle.getID() == "pkw150289")
110 //#define DEBUG_COND (myVehicle.getID() == "A" || myVehicle.getID() == "B") // fail change to left
111 //#define DEBUG_COND (myVehicle.getID() == "disabled") // test stops_overtaking
112 //#define DEBUG_COND false
113 
114 
115 // ===========================================================================
116 // member method definitions
117 // ===========================================================================
120  mySpeedGainProbabilityRight(0),
121  mySpeedGainProbabilityLeft(0),
122  myKeepRightProbability(0),
123  myLeadingBlockerLength(0),
124  myLeftSpace(0),
125  myLookAheadSpeed(LOOK_AHEAD_MIN_SPEED),
126  myLastEdge(nullptr),
127  myCanChangeFully(true),
128  mySafeLatDistRight(0),
129  mySafeLatDistLeft(0),
130  myStrategicParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_STRATEGIC_PARAM, 1)),
131  myCooperativeParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_PARAM, 1)),
132  mySpeedGainParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAIN_PARAM, 1)),
133  myKeepRightParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_KEEPRIGHT_PARAM, 1)),
134  mySublaneParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SUBLANE_PARAM, 1)),
135  // by default use SUMO_ATTR_LCA_PUSHY. If that is not set, try SUMO_ATTR_LCA_PUSHYGAP
136  myPushy(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_PUSHY,
137  1 - (v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_PUSHYGAP,
138  MAX2(NUMERICAL_EPS, v.getVehicleType().getMinGapLat())) /
139  MAX2(NUMERICAL_EPS, v.getVehicleType().getMinGapLat())))),
140  myAssertive(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_ASSERTIVE, 1)),
141  myImpatience(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_IMPATIENCE, 0)),
142  myMinImpatience(myImpatience),
143  myTimeToImpatience(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_TIME_TO_IMPATIENCE, std::numeric_limits<double>::max())),
144  myAccelLat(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_ACCEL_LAT, 1.0)),
145  myTurnAlignmentDist(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_TURN_ALIGNMENT_DISTANCE, 0.0)),
146  myLookaheadLeft(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_LOOKAHEADLEFT, 2.0)),
147  mySpeedGainRight(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAINRIGHT, 0.1)) {
149 }
150 
152  changed();
153 }
154 
155 
156 void
160  mySpeedLossProbThreshold = (-0.1 + (1 - mySublaneParam));
161 }
162 
163 
164 bool
166  return DEBUG_COND;
167 }
168 
169 
170 int
172  int laneOffset,
173  LaneChangeAction alternatives,
174  const MSLeaderDistanceInfo& leaders,
175  const MSLeaderDistanceInfo& followers,
176  const MSLeaderDistanceInfo& blockers,
177  const MSLeaderDistanceInfo& neighLeaders,
178  const MSLeaderDistanceInfo& neighFollowers,
179  const MSLeaderDistanceInfo& neighBlockers,
180  const MSLane& neighLane,
181  const std::vector<MSVehicle::LaneQ>& preb,
182  MSVehicle** lastBlocked,
183  MSVehicle** firstBlocked,
184  double& latDist, double& maneuverDist, int& blocked) {
185 
187  const std::string changeType = laneOffset == -1 ? "right" : (laneOffset == 1 ? "left" : "current");
188 
189 #ifdef DEBUG_MANEUVER
190  if (gDebugFlag2) {
191  std::cout << "\n" << SIMTIME
192  << std::setprecision(gPrecision)
193  << " veh=" << myVehicle.getID()
194  << " lane=" << myVehicle.getLane()->getID()
195  << " pos=" << myVehicle.getPositionOnLane()
196  << " posLat=" << myVehicle.getLateralPositionOnLane()
197  << " speed=" << myVehicle.getSpeed()
198  << " considerChangeTo=" << changeType
199  << "\n";
200  }
201 #endif
202 
203  int result = _wantsChangeSublane(laneOffset,
204  alternatives,
205  leaders, followers, blockers,
206  neighLeaders, neighFollowers, neighBlockers,
207  neighLane, preb,
208  lastBlocked, firstBlocked, latDist, maneuverDist, blocked);
209 
210  result = keepLatGap(result, leaders, followers, blockers,
211  neighLeaders, neighFollowers, neighBlockers,
212  neighLane, laneOffset, latDist, maneuverDist, blocked);
213 
214  result |= getLCA(result, latDist);
215  // take into account lateral acceleration
216 #if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE)
217  double latDistTmp = latDist;
218 #endif
219  latDist = SPEED2DIST(computeSpeedLat(latDist, maneuverDist));
220 #if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE)
221  if (gDebugFlag2 && latDist != latDistTmp) {
222  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " maneuverDist=" << maneuverDist << " latDist=" << latDistTmp << " mySpeedPrev=" << mySpeedLat << " speedLat=" << DIST2SPEED(latDist) << " latDist2=" << latDist << "\n";
223  }
224 
225  if (gDebugFlag2) {
226  if (result & LCA_WANTS_LANECHANGE) {
227  std::cout << SIMTIME
228  << " veh=" << myVehicle.getID()
229  << " wantsChangeTo=" << changeType
230  << " latDist=" << latDist
231  << " maneuverDist=" << maneuverDist
232  << " state=" << toString((LaneChangeAction)result)
233  << ((blocked & LCA_BLOCKED) ? " (blocked)" : "")
234  << ((blocked & LCA_OVERLAPPING) ? " (overlap)" : "")
235  << "\n\n";
236  } else {
237  std::cout << SIMTIME
238  << " veh=" << myVehicle.getID()
239  << " wantsNoChangeTo=" << changeType
240  << " state=" << toString((LaneChangeAction)result)
241  << "\n\n";
242  }
243  }
244 #endif
245  gDebugFlag2 = false;
246  return result;
247 }
248 
249 void
250 MSLCM_SL2015::setOwnState(const int state) {
252  if (myVehicle.isActive()) {
253  if ((state & (LCA_STRATEGIC | LCA_SPEEDGAIN)) != 0 && (state & LCA_BLOCKED) != 0) {
255  } else {
256  // impatience decays only to the driver-specific level
258  }
259 #ifdef DEBUG_STATE
260  if (DEBUG_COND) {
261  std::cout << SIMTIME << " veh=" << myVehicle.getID()
262  << " setOwnState=" << toString((LaneChangeAction)state)
263  << " myMinImpatience=" << myMinImpatience
264  << " myImpatience=" << myImpatience
265  << "\n";
266  }
267 #endif
268  if ((state & LCA_STAY) != 0) {
269  myManeuverDist = 0;
270  myCanChangeFully = true;
271 // if (DEBUG_COND) {
272 // std::cout << " myCanChangeFully=true\n";
273 // }
274  }
275  }
276 }
277 
278 
279 void
280 MSLCM_SL2015::updateSafeLatDist(const double travelledLatDist) {
281  mySafeLatDistLeft -= travelledLatDist;
282  mySafeLatDistRight += travelledLatDist;
283 
284  if (fabs(mySafeLatDistLeft) < NUMERICAL_EPS) {
285  mySafeLatDistLeft = 0.;
286  }
287  if (fabs(mySafeLatDistRight) < NUMERICAL_EPS) {
288  mySafeLatDistRight = 0.;
289  }
290 }
291 
292 
293 double
294 MSLCM_SL2015::patchSpeed(const double min, const double wanted, const double max, const MSCFModel& cfModel) {
296  // negative min speed may be passed when using ballistic updated
297  const double newSpeed = _patchSpeed(MAX2(min, 0.0), wanted, max, cfModel);
298 #ifdef DEBUG_PATCHSPEED
299  if (gDebugFlag2) {
300  const std::string patched = (wanted != newSpeed ? " patched=" + toString(newSpeed) : "");
301  std::cout << SIMTIME
302  << " veh=" << myVehicle.getID()
303  << " lane=" << myVehicle.getLane()->getID()
304  << " pos=" << myVehicle.getPositionOnLane()
305  << " v=" << myVehicle.getSpeed()
306  << " min=" << min
307  << " wanted=" << wanted
308  << " max=" << max
309  << patched
310  << "\n\n";
311  }
312 #endif
313  gDebugFlag2 = false;
314  return newSpeed;
315 }
316 
317 
318 double
319 MSLCM_SL2015::_patchSpeed(const double min, const double wanted, const double max, const MSCFModel& cfModel) {
320  if (wanted <= 0) {
321  return wanted;
322  }
323 
324  int state = myOwnState;
325 
326  // letting vehicles merge in at the end of the lane in case of counter-lane change, step#2
327  // if we want to change and have a blocking leader and there is enough room for him in front of us
328  if (myLeadingBlockerLength != 0) {
330 #ifdef DEBUG_PATCHSPEED
331  if (gDebugFlag2) {
332  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " myLeadingBlockerLength=" << myLeadingBlockerLength << " space=" << space << "\n";
333  }
334 #endif
335  if (space > 0) { // XXX space > -MAGIC_OFFSET
336  // compute speed for decelerating towards a place which allows the blocking leader to merge in in front
337  double safe = cfModel.stopSpeed(&myVehicle, myVehicle.getSpeed(), space);
338  // if we are approaching this place
339  if (safe < wanted) {
340 #ifdef DEBUG_PATCHSPEED
341  if (gDebugFlag2) {
342  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " slowing down for leading blocker, safe=" << safe << (safe + NUMERICAL_EPS < min ? " (not enough)" : "") << "\n";
343  }
344 #endif
345  return MAX2(min, safe);
346  }
347  }
348  }
349  double nVSafe = wanted;
350  bool gotOne = false;
351  for (std::vector<double>::const_iterator i = myLCAccelerationAdvices.begin(); i != myLCAccelerationAdvices.end(); ++i) {
352  double v = myVehicle.getSpeed() + ACCEL2SPEED(*i);
353  if (v >= min && v <= max) {
354  nVSafe = MIN2(v, nVSafe);
355  gotOne = true;
356 #ifdef DEBUG_PATCHSPEED
357  if (gDebugFlag2) {
358  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " got accel=" << (*i) << " nVSafe=" << nVSafe << "\n";
359  }
360 #endif
361  } else {
362 #ifdef DEBUG_PATCHSPEED
363  if (v < min) {
364  if (gDebugFlag2) {
365  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " ignoring low nVSafe=" << v << " min=" << min << "\n";
366  }
367  } else {
368  if (gDebugFlag2) {
369  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " ignoring high nVSafe=" << v << " max=" << max << "\n";
370  }
371  }
372 #endif
373  }
374  }
375 
376  if (gotOne && !myDontBrake) {
377 #ifdef DEBUG_PATCHSPEED
378  if (gDebugFlag2) {
379  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " got vSafe\n";
380  }
381 #endif
382  return nVSafe;
383  }
384 
385  // check whether the vehicle is blocked
386  if ((state & LCA_WANTS_LANECHANGE) != 0 && (state & LCA_BLOCKED) != 0) {
387  if ((state & LCA_STRATEGIC) != 0) {
388  // necessary decelerations are controlled via vSafe. If there are
389  // none it means we should speed up
390 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
391  if (gDebugFlag2) {
392  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_WANTS_LANECHANGE (strat, no vSafe)\n";
393  }
394 #endif
395  return (max + wanted) / (double) 2.0;
396  } else if ((state & LCA_COOPERATIVE) != 0) {
397  // only minor adjustments in speed should be done
398  if ((state & LCA_BLOCKED_BY_LEADER) != 0) {
399 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
400  if (gDebugFlag2) {
401  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_LEADER (coop)\n";
402  }
403 #endif
404  return (min + wanted) / (double) 2.0;
405  }
406  if ((state & LCA_BLOCKED_BY_FOLLOWER) != 0) {
407 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
408  if (gDebugFlag2) {
409  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_FOLLOWER (coop)\n";
410  }
411 #endif
412  return (max + wanted) / (double) 2.0;
413  }
414  //} else { // VARIANT_16
415  // // only accelerations should be performed
416  // if ((state & LCA_BLOCKED_BY_FOLLOWER) != 0) {
417  // if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_FOLLOWER\n";
418  // return (max + wanted) / (double) 2.0;
419  // }
420  }
421  }
422 
423  /*
424  // decelerate if being a blocking follower
425  // (and does not have to change lanes)
426  if ((state & LCA_AMBLOCKINGFOLLOWER) != 0) {
427  if (fabs(max - myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle)) < 0.001 && min == 0) { // !!! was standing
428  if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER (standing)\n";
429  return 0;
430  }
431  if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER\n";
432 
433  //return min; // VARIANT_3 (brakeStrong)
434  return (min + wanted) / (double) 2.0;
435  }
436  if ((state & LCA_AMBACKBLOCKER) != 0) {
437  if (max <= myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle) && min == 0) { // !!! was standing
438  if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBACKBLOCKER (standing)\n";
439  //return min; VARIANT_9 (backBlockVSafe)
440  return nVSafe;
441  }
442  }
443  if ((state & LCA_AMBACKBLOCKER_STANDING) != 0) {
444  if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBACKBLOCKER_STANDING\n";
445  //return min;
446  return nVSafe;
447  }
448  */
449 
450  // accelerate if being a blocking leader or blocking follower not able to brake
451  // (and does not have to change lanes)
452  if ((state & LCA_AMBLOCKINGLEADER) != 0) {
453 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
454  if (gDebugFlag2) {
455  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGLEADER\n";
456  }
457 #endif
458  return (max + wanted) / (double) 2.0;
459  }
460 
461  if ((state & LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0) {
462 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
463  if (gDebugFlag2) {
464  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER_DONTBRAKE\n";
465  }
466 #endif
467  /*
468  // VARIANT_4 (dontbrake)
469  if (max <= myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle) && min == 0) { // !!! was standing
470  return wanted;
471  }
472  return (min + wanted) / (double) 2.0;
473  */
474  }
475  return wanted;
476 }
477 
478 
479 void*
480 MSLCM_SL2015::inform(void* info, MSVehicle* sender) {
481  Info* pinfo = (Info*) info;
482  if (pinfo->first >= 0) {
483  addLCSpeedAdvice(pinfo->first);
484  }
485  //myOwnState &= 0xffffffff; // reset all bits of MyLCAEnum but only those
486  myOwnState |= pinfo->second;
487 #ifdef DEBUG_INFORM
488  if (gDebugFlag2 || DEBUG_COND || sender->getLaneChangeModel().debugVehicle()) {
489  std::cout << SIMTIME
490  << " veh=" << myVehicle.getID()
491  << " informedBy=" << sender->getID()
492  << " info=" << pinfo->second
493  << " vSafe=" << pinfo->first
494  << "\n";
495  }
496 #else
497  UNUSED_PARAMETER(sender);
498 #endif
499  delete pinfo;
500  return (void*) true;
501 }
502 
503 
504 void
505 MSLCM_SL2015::msg(const CLeaderDist& cld, double speed, int state) {
506  assert(cld.first != 0);
507  ((MSVehicle*)cld.first)->getLaneChangeModel().inform(new Info(speed, state), &myVehicle);
508 }
509 
510 
511 double
513  int dir,
514  const CLeaderDist& neighLead,
515  double remainingSeconds) {
516  double plannedSpeed = MIN2(myVehicle.getSpeed(),
518  for (std::vector<double>::const_iterator i = myLCAccelerationAdvices.begin(); i != myLCAccelerationAdvices.end(); ++i) {
519  double v = myVehicle.getSpeed() + ACCEL2SPEED(*i);
521  plannedSpeed = MIN2(plannedSpeed, v);
522  }
523  }
524 #ifdef DEBUG_INFORM
525  if (gDebugFlag2) {
526  std::cout << " informLeader speed=" << myVehicle.getSpeed() << " planned=" << plannedSpeed << "\n";
527  }
528 #endif
529 
530  if ((blocked & LCA_BLOCKED_BY_LEADER) != 0 && neighLead.first != 0) {
531  const MSVehicle* nv = neighLead.first;
532 #ifdef DEBUG_INFORM
533  if (gDebugFlag2) std::cout << " blocked by leader nv=" << nv->getID() << " nvSpeed=" << nv->getSpeed() << " needGap="
535 #endif
536  // decide whether we want to overtake the leader or follow it
537  const double dv = plannedSpeed - nv->getSpeed();
538  const double overtakeDist = (neighLead.second // drive to back of follower
539  + nv->getVehicleType().getLengthWithGap() // drive to front of follower
540  + myVehicle.getVehicleType().getLength() // ego back reaches follower front
541  + nv->getCarFollowModel().getSecureGap( // save gap to follower
543 
544  if (dv < NUMERICAL_EPS
545  // overtaking on the right on an uncongested highway is forbidden (noOvertakeLCLeft)
547  // not enough space to overtake? (we will start to brake when approaching a dead end)
549  // not enough time to overtake?
550  || dv * remainingSeconds < overtakeDist) {
551  // cannot overtake
552  msg(neighLead, -1, dir | LCA_AMBLOCKINGLEADER);
553  // slow down smoothly to follow leader
554  const double targetSpeed = myCarFollowModel.followSpeed(
555  &myVehicle, myVehicle.getSpeed(), neighLead.second, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel());
556  if (targetSpeed < myVehicle.getSpeed()) {
557  // slow down smoothly to follow leader
558  const double decel = ACCEL2SPEED(MIN2(myVehicle.getCarFollowModel().getMaxDecel(),
559  MAX2(MIN_FALLBEHIND, (myVehicle.getSpeed() - targetSpeed) / remainingSeconds)));
560  //const double nextSpeed = MAX2(0., MIN2(plannedSpeed, myVehicle.getSpeed() - decel));
561  const double nextSpeed = MIN2(plannedSpeed, myVehicle.getSpeed() - decel);
562 #ifdef DEBUG_INFORM
563  if (gDebugFlag2) {
564  std::cout << SIMTIME
565  << " cannot overtake leader nv=" << nv->getID()
566  << " dv=" << dv
567  << " remainingSeconds=" << remainingSeconds
568  << " targetSpeed=" << targetSpeed
569  << " nextSpeed=" << nextSpeed
570  << "\n";
571  }
572 #endif
573  addLCSpeedAdvice(nextSpeed);
574  return nextSpeed;
575  } else {
576  // leader is fast enough anyway
577 #ifdef DEBUG_INFORM
578  if (gDebugFlag2) {
579  std::cout << SIMTIME
580  << " cannot overtake fast leader nv=" << nv->getID()
581  << " dv=" << dv
582  << " remainingSeconds=" << remainingSeconds
583  << " targetSpeed=" << targetSpeed
584  << "\n";
585  }
586 #endif
587  addLCSpeedAdvice(targetSpeed);
588  return plannedSpeed;
589  }
590  } else {
591 #ifdef DEBUG_INFORM
592  if (gDebugFlag2) {
593  std::cout << SIMTIME
594  << " wants to overtake leader nv=" << nv->getID()
595  << " dv=" << dv
596  << " remainingSeconds=" << remainingSeconds
597  << " currentGap=" << neighLead.second
599  << " overtakeDist=" << overtakeDist
600  << " leftSpace=" << myLeftSpace
601  << " blockerLength=" << myLeadingBlockerLength
602  << "\n";
603  }
604 #endif
605  // overtaking, leader should not accelerate
606  msg(neighLead, nv->getSpeed(), dir | LCA_AMBLOCKINGLEADER);
607  return -1;
608  }
609  } else if (neighLead.first != 0) { // (remainUnblocked)
610  // we are not blocked now. make sure we stay far enough from the leader
611  const MSVehicle* nv = neighLead.first;
612  double dv, nextNVSpeed;
614  // XXX: the decrement (HELP_OVERTAKE) should be scaled with timestep length, I think.
615  // It seems to function as an estimate nv's speed in the next simstep!? (so HELP_OVERTAKE should be an acceleration value.)
616  nextNVSpeed = nv->getSpeed() - HELP_OVERTAKE; // conservative
617  dv = SPEED2DIST(myVehicle.getSpeed() - nextNVSpeed);
618  } else {
619  // Estimate neigh's speed after actionstep length
620  // @note The possible breaking can be underestimated by the formula, so this is a potential
621  // source of collisions if actionsteplength>simsteplength.
622  const double nvMaxDecel = HELP_OVERTAKE;
623  nextNVSpeed = nv->getSpeed() - nvMaxDecel * myVehicle.getActionStepLengthSecs(); // conservative
624  // Estimated gap reduction until next action step if own speed stays constant
625  dv = SPEED2DIST(myVehicle.getSpeed() - nextNVSpeed);
626  }
627  const double targetSpeed = myCarFollowModel.followSpeed(
628  &myVehicle, myVehicle.getSpeed(), neighLead.second - dv, nextNVSpeed, nv->getCarFollowModel().getMaxDecel());
629  addLCSpeedAdvice(targetSpeed);
630 #ifdef DEBUG_INFORM
631  if (gDebugFlag2) {
632  std::cout << " not blocked by leader nv=" << nv->getID()
633  << " nvSpeed=" << nv->getSpeed()
634  << " gap=" << neighLead.second
635  << " nextGap=" << neighLead.second - dv
637  << " targetSpeed=" << targetSpeed
638  << "\n";
639  }
640 #endif
641  return MIN2(targetSpeed, plannedSpeed);
642  } else {
643  // not overtaking
644  return plannedSpeed;
645  }
646 }
647 
648 
649 void
651  int dir,
652  const CLeaderDist& neighFollow,
653  double remainingSeconds,
654  double plannedSpeed) {
655  if ((blocked & LCA_BLOCKED_BY_FOLLOWER) != 0 && neighFollow.first != 0) {
656  const MSVehicle* nv = neighFollow.first;
657 #ifdef DEBUG_INFORM
658  if (gDebugFlag2) std::cout << " blocked by follower nv=" << nv->getID() << " nvSpeed=" << nv->getSpeed() << " needGap="
660 #endif
661 
662  // are we fast enough to cut in without any help?
663  if (plannedSpeed - nv->getSpeed() >= HELP_OVERTAKE) {
664  const double neededGap = nv->getCarFollowModel().getSecureGap(nv->getSpeed(), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel());
665  if ((neededGap - neighFollow.second) / remainingSeconds < (plannedSpeed - nv->getSpeed())) {
666 #ifdef DEBUG_INFORM
667  if (gDebugFlag2) {
668  std::cout << " wants to cut in before nv=" << nv->getID() << " without any help neededGap=" << neededGap << "\n";
669  }
670 #endif
671  // follower might even accelerate but not to much
672  msg(neighFollow, plannedSpeed - HELP_OVERTAKE, dir | LCA_AMBLOCKINGFOLLOWER);
673  return;
674  }
675  }
676  // decide whether we will request help to cut in before the follower or allow to be overtaken
677 
678  // PARAMETERS
679  // assume other vehicle will assume the equivalent of 1 second of
680  // maximum deceleration to help us (will probably be spread over
681  // multiple seconds)
682  // -----------
683  const double helpDecel = nv->getCarFollowModel().getMaxDecel() * HELP_DECEL_FACTOR ;
684 
685  // change in the gap between ego and blocker over 1 second (not STEP!)
686  const double neighNewSpeed = MAX2(0., nv->getSpeed() - ACCEL2SPEED(helpDecel));
687  const double neighNewSpeed1s = MAX2(0., nv->getSpeed() - helpDecel);
688  const double dv = plannedSpeed - neighNewSpeed1s;
689  // new gap between follower and self in case the follower does brake for 1s
690  const double decelGap = neighFollow.second + dv;
691  const double secureGap = nv->getCarFollowModel().getSecureGap(neighNewSpeed1s, plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel());
692 #ifdef DEBUG_INFORM
693  if (gDebugFlag2) {
694  std::cout << SIMTIME
695  << " egoV=" << myVehicle.getSpeed()
696  << " egoNV=" << plannedSpeed
697  << " nvNewSpeed=" << neighNewSpeed
698  << " nvNewSpeed1s=" << neighNewSpeed1s
699  << " deltaGap=" << dv
700  << " decelGap=" << decelGap
701  << " secGap=" << secureGap
702  << "\n";
703  }
704 #endif
705  if (decelGap > 0 && decelGap >= secureGap) {
706  // if the blocking neighbor brakes it could actually help
707  // how hard does it actually need to be?
708  // to be safe in the next step the following equation has to hold:
709  // vsafe <= followSpeed(gap=currentGap - SPEED2DIST(vsafe), ...)
710  // we compute an upper bound on vsafe by doing the computation twice
711  const double vsafe1 = MAX2(neighNewSpeed, nv->getCarFollowModel().followSpeed(
712  nv, nv->getSpeed(), neighFollow.second + SPEED2DIST(plannedSpeed), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel()));
713  const double vsafe = MAX2(neighNewSpeed, nv->getCarFollowModel().followSpeed(
714  nv, nv->getSpeed(), neighFollow.second + SPEED2DIST(plannedSpeed - vsafe1), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel()));
715  // the following assertion cannot be guaranteed because the CFModel handles small gaps differently, see MSCFModel::maximumSafeStopSpeed
716  // assert(vsafe <= vsafe1);
717  msg(neighFollow, vsafe, dir | LCA_AMBLOCKINGFOLLOWER);
718 #ifdef DEBUG_INFORM
719  if (gDebugFlag2) {
720  std::cout << " wants to cut in before nv=" << nv->getID()
721  << " vsafe1=" << vsafe1
722  << " vsafe=" << vsafe
723  << " newSecGap=" << nv->getCarFollowModel().getSecureGap(vsafe, plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel())
724  << "\n";
725  }
726 #endif
727  } else if (dv > 0 && dv * remainingSeconds > (secureGap - decelGap + POSITION_EPS)) {
728  // decelerating once is sufficient to open up a large enough gap in time
729  msg(neighFollow, neighNewSpeed, dir | LCA_AMBLOCKINGFOLLOWER);
730 #ifdef DEBUG_INFORM
731  if (gDebugFlag2) {
732  std::cout << " wants to cut in before nv=" << nv->getID() << " (eventually)\n";
733  }
734 #endif
735  } else if (dir == LCA_MRIGHT && !myAllowOvertakingRight && !nv->congested()) {
736  const double vhelp = MAX2(neighNewSpeed, HELP_OVERTAKE);
737  msg(neighFollow, vhelp, dir | LCA_AMBLOCKINGFOLLOWER);
738 #ifdef DEBUG_INFORM
739  if (gDebugFlag2) {
740  std::cout << " wants to cut in before nv=" << nv->getID() << " (nv cannot overtake right)\n";
741  }
742 #endif
743  } else {
744  double vhelp = MAX2(nv->getSpeed(), myVehicle.getSpeed() + HELP_OVERTAKE);
745  if (nv->getSpeed() > myVehicle.getSpeed() &&
747  || (dir == LCA_MLEFT && plannedSpeed > CUT_IN_LEFT_SPEED_THRESHOLD) // VARIANT_22 (slowDownLeft)
748  // XXX this is a hack to determine whether the vehicles is on an on-ramp. This information should be retrieved from the network itself
749  || (dir == LCA_MLEFT && myLeftSpace > MAX_ONRAMP_LENGTH)
750  )) {
751  // let the follower slow down to increase the likelyhood that later vehicles will be slow enough to help
752  // follower should still be fast enough to open a gap
753  vhelp = MAX2(neighNewSpeed, myVehicle.getSpeed() + HELP_OVERTAKE);
754 #ifdef DEBUG_INFORM
755  if (gDebugFlag2) {
756  std::cout << " wants right follower to slow down a bit\n";
757  }
758 #endif
759  if ((nv->getSpeed() - myVehicle.getSpeed()) / helpDecel < remainingSeconds) {
760 #ifdef DEBUG_INFORM
761  if (gDebugFlag2) {
762  std::cout << " wants to cut in before right follower nv=" << nv->getID() << " (eventually)\n";
763  }
764 #endif
765  msg(neighFollow, vhelp, dir | LCA_AMBLOCKINGFOLLOWER);
766  return;
767  }
768  }
769  msg(neighFollow, vhelp, dir | LCA_AMBLOCKINGFOLLOWER);
770  // this follower is supposed to overtake us. slow down smoothly to allow this
771  const double overtakeDist = (neighFollow.second // follower reaches ego back
772  + myVehicle.getVehicleType().getLengthWithGap() // follower reaches ego front
773  + nv->getVehicleType().getLength() // follower back at ego front
774  + myVehicle.getCarFollowModel().getSecureGap( // follower has safe dist to ego
775  plannedSpeed, vhelp, nv->getCarFollowModel().getMaxDecel()));
776  // speed difference to create a sufficiently large gap
777  const double needDV = overtakeDist / remainingSeconds;
778  // make sure the deceleration is not to strong
780 
781 #ifdef DEBUG_INFORM
782  if (gDebugFlag2) {
783  std::cout << SIMTIME
784  << " veh=" << myVehicle.getID()
785  << " wants to be overtaken by=" << nv->getID()
786  << " overtakeDist=" << overtakeDist
787  << " vneigh=" << nv->getSpeed()
788  << " vhelp=" << vhelp
789  << " needDV=" << needDV
790  << " vsafe=" << myVehicle.getSpeed() + ACCEL2SPEED(myLCAccelerationAdvices.back())
791  << "\n";
792  }
793 #endif
794  }
795  } else if (neighFollow.first != 0) {
796  // we are not blocked no, make sure it remains that way
797  const MSVehicle* nv = neighFollow.first;
798  const double vsafe1 = nv->getCarFollowModel().followSpeed(
799  nv, nv->getSpeed(), neighFollow.second + SPEED2DIST(plannedSpeed), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel());
800  const double vsafe = nv->getCarFollowModel().followSpeed(
801  nv, nv->getSpeed(), neighFollow.second + SPEED2DIST(plannedSpeed - vsafe1), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel());
802  msg(neighFollow, vsafe, dir | LCA_AMBLOCKINGFOLLOWER);
803 #ifdef DEBUG_INFORM
804  if (gDebugFlag2) {
805  std::cout << " wants to cut in before non-blocking follower nv=" << nv->getID() << "\n";
806  }
807 #endif
808  }
809 }
810 
811 double
812 MSLCM_SL2015::informLeaders(int blocked, int dir,
813  const std::vector<CLeaderDist>& blockers,
814  double remainingSeconds) {
815  double plannedSpeed = myVehicle.getSpeed();
816  if (myLeadingBlockerLength != 0) {
817  // see patchSpeed @todo: refactor
819  if (space > 0) {
820  double safe = myVehicle.getCarFollowModel().stopSpeed(&myVehicle, myVehicle.getSpeed(), space);
821  plannedSpeed = MIN2(plannedSpeed, safe);
822  }
823  }
824  for (std::vector<CLeaderDist>::const_iterator it = blockers.begin(); it != blockers.end(); ++it) {
825  plannedSpeed = MIN2(plannedSpeed, informLeader(blocked, dir, *it, remainingSeconds));
826  }
827  return plannedSpeed;
828 }
829 
830 
831 void
832 MSLCM_SL2015::informFollowers(int blocked, int dir,
833  const std::vector<CLeaderDist>& blockers,
834  double remainingSeconds,
835  double plannedSpeed) {
836  // #3727
837  for (std::vector<CLeaderDist>::const_iterator it = blockers.begin(); it != blockers.end(); ++it) {
838  informFollower(blocked, dir, *it, remainingSeconds, plannedSpeed);
839  }
840 }
841 
842 
843 void
846  // keep information about strategic change direction
848  if (myCanChangeFully) {
849  myManeuverDist = 0;
850  }
851 #ifdef DEBUG_INFORM
852  if (debugVehicle()) {
853  std::cout << SIMTIME
854  << " veh=" << myVehicle.getID()
855  << " prepareStep"
856  << " myCanChangeFully=" << myCanChangeFully
857  << "\n";
858  }
859 #endif
861  myLeftSpace = 0;
862  myLCAccelerationAdvices.clear();
863  myDontBrake = false;
864  myCFRelated.clear();
865  myCFRelatedReady = false;
866  const double halfWidth = getWidth() * 0.5;
867  const double center = myVehicle.getCenterOnEdge();
868  mySafeLatDistRight = center - halfWidth;
869  mySafeLatDistLeft = myVehicle.getLane()->getEdge().getWidth() - center - halfWidth;
870  // truncate to work around numerical instability between different builds
871  mySpeedGainProbabilityRight = ceil(mySpeedGainProbabilityRight * 100000.0) * 0.00001;
872  mySpeedGainProbabilityLeft = ceil(mySpeedGainProbabilityLeft * 100000.0) * 0.00001;
873  myKeepRightProbability = ceil(myKeepRightProbability * 100000.0) * 0.00001;
874  // updated myExpectedSublaneSpeeds
875  // XXX only do this when (sub)lane changing is possible
876  std::vector<double> newExpectedSpeeds;
877 #ifdef DEBUG_INFORM
878  if (DEBUG_COND) {
879  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " myExpectedSublaneSpeeds=" << toString(myExpectedSublaneSpeeds) << "\n";
880  }
881 #endif
882  if (myExpectedSublaneSpeeds.size() != myVehicle.getLane()->getEdge().getSubLaneSides().size()) {
883  // initialize
884  const MSEdge* currEdge = &myVehicle.getLane()->getEdge();
885  const std::vector<MSLane*>& lanes = currEdge->getLanes();
886  for (std::vector<MSLane*>::const_iterator it_lane = lanes.begin(); it_lane != lanes.end(); ++it_lane) {
887  const int subLanes = MAX2(1, int(ceil((*it_lane)->getWidth() / MSGlobals::gLateralResolution)));
888  for (int i = 0; i < subLanes; ++i) {
889  newExpectedSpeeds.push_back((*it_lane)->getVehicleMaxSpeed(&myVehicle));
890  }
891  }
892  if (myExpectedSublaneSpeeds.size() > 0) {
893  // copy old values
894  assert(myLastEdge != 0);
895  if (myLastEdge->getSubLaneSides().size() == myExpectedSublaneSpeeds.size()) {
896  const int subLaneShift = computeSublaneShift(myLastEdge, currEdge);
897  if (subLaneShift < std::numeric_limits<int>::max()) {
898  for (int i = 0; i < (int)myExpectedSublaneSpeeds.size(); ++i) {
899  const int newI = i + subLaneShift;
900  if (newI > 0 && newI < (int)newExpectedSpeeds.size()) {
901  newExpectedSpeeds[newI] = myExpectedSublaneSpeeds[i];
902  }
903  }
904  }
905  }
906  }
907  myExpectedSublaneSpeeds = newExpectedSpeeds;
908  myLastEdge = currEdge;
909  }
910  assert(myExpectedSublaneSpeeds.size() == myVehicle.getLane()->getEdge().getSubLaneSides().size());
911 }
912 
913 
914 int
915 MSLCM_SL2015::computeSublaneShift(const MSEdge* prevEdge, const MSEdge* curEdge) {
916  // find the first lane that targets the new edge
917  int prevShift = 0;
918  const std::vector<MSLane*>& lanes = prevEdge->getLanes();
919  for (std::vector<MSLane*>::const_iterator it_lane = lanes.begin(); it_lane != lanes.end(); ++it_lane) {
920  const MSLane* lane = *it_lane;
921  for (MSLinkCont::const_iterator it_link = lane->getLinkCont().begin(); it_link != lane->getLinkCont().end(); ++it_link) {
922  if (&((*it_link)->getLane()->getEdge()) == curEdge) {
923  int curShift = 0;
924  const MSLane* target = (*it_link)->getLane();
925  const std::vector<MSLane*>& lanes2 = curEdge->getLanes();
926  for (std::vector<MSLane*>::const_iterator it_lane2 = lanes2.begin(); it_lane2 != lanes2.end(); ++it_lane2) {
927  const MSLane* lane2 = *it_lane2;
928  if (lane2 == target) {
929  return prevShift + curShift;
930  }
931  MSLeaderInfo ahead(lane2);
932  curShift += ahead.numSublanes();
933  }
934  assert(false);
935  }
936  }
937  MSLeaderInfo ahead(lane);
938  prevShift -= ahead.numSublanes();
939  }
940  return std::numeric_limits<int>::max();
941 }
942 
943 
944 void
946  if (!myCanChangeFully) {
947  // do not reset state yet
948 #ifdef DEBUG_STATE
949  if (DEBUG_COND) {
950  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " state not reset\n";
951  }
952 #endif
953  return;
954  }
955  myOwnState = 0;
956  // XX do not reset values for unfinished maneuvers
960 
961  if (myVehicle.getBestLaneOffset() == 0) {
962  // if we are not yet on our best lane there might still be unseen blockers
963  // (during patchSpeed)
965  myLeftSpace = 0;
966  }
968  myLCAccelerationAdvices.clear();
969  myDontBrake = false;
970 #if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE)
971  if (DEBUG_COND) {
972  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " changed()\n";
973  }
974 #endif
975 }
976 
977 
978 int
980  int laneOffset,
981  LaneChangeAction alternatives,
982  const MSLeaderDistanceInfo& leaders,
983  const MSLeaderDistanceInfo& followers,
984  const MSLeaderDistanceInfo& blockers,
985  const MSLeaderDistanceInfo& neighLeaders,
986  const MSLeaderDistanceInfo& neighFollowers,
987  const MSLeaderDistanceInfo& neighBlockers,
988  const MSLane& neighLane,
989  const std::vector<MSVehicle::LaneQ>& preb,
990  MSVehicle** lastBlocked,
991  MSVehicle** firstBlocked,
992  double& latDist, double& maneuverDist, int& blocked) {
993 
994  const SUMOTime currentTime = MSNet::getInstance()->getCurrentTimeStep();
995  // compute bestLaneOffset
996  MSVehicle::LaneQ curr, neigh;
997  int bestLaneOffset = 0;
998  double currentDist = 0;
999  double neighDist = 0;
1000  int currIdx = 0;
1001  MSLane* prebLane = myVehicle.getLane();
1002  if (prebLane->getEdge().isInternal()) {
1003  // internal edges are not kept inside the bestLanes structure
1004  prebLane = prebLane->getLinkCont()[0]->getLane();
1005  }
1006  for (int p = 0; p < (int) preb.size(); ++p) {
1007  if (preb[p].lane == prebLane && p + laneOffset >= 0) {
1008  assert(p + laneOffset < (int)preb.size());
1009  curr = preb[p];
1010  neigh = preb[p + laneOffset];
1011  currentDist = curr.length;
1012  neighDist = neigh.length;
1013  bestLaneOffset = curr.bestLaneOffset;
1014  // VARIANT_13 (equalBest)
1015  if (bestLaneOffset == 0 && preb[p + laneOffset].bestLaneOffset == 0) {
1016 #ifdef DEBUG_WANTSCHANGE
1017  if (gDebugFlag2) {
1018  std::cout << STEPS2TIME(currentTime)
1019  << " veh=" << myVehicle.getID()
1020  << " bestLaneOffsetOld=" << bestLaneOffset
1021  << " bestLaneOffsetNew=" << laneOffset
1022  << "\n";
1023  }
1024 #endif
1025  bestLaneOffset = laneOffset;
1026  }
1027  currIdx = p;
1028  break;
1029  }
1030  }
1031  double driveToNextStop = -std::numeric_limits<double>::max();
1032  UNUSED_PARAMETER(driveToNextStop); // XXX use when computing usableDist
1033  if (myVehicle.nextStopDist() < std::numeric_limits<double>::max()
1035  // vehicle can always drive up to stop distance
1036  // @note this information is dynamic and thus not available in updateBestLanes()
1037  // @note: nextStopDist was compute before the vehicle moved
1038  driveToNextStop = myVehicle.nextStopDist();
1040 #ifdef DEBUG_WANTS_CHANGE
1041  if (DEBUG_COND) {
1042  std::cout << SIMTIME << std::setprecision(gPrecision) << " veh=" << myVehicle.getID()
1043  << " stopDist=" << myVehicle.nextStopDist()
1044  << " lastDist=" << myVehicle.getLastStepDist()
1045  << " stopPos=" << stopPos
1046  << " currentDist=" << currentDist
1047  << " neighDist=" << neighDist
1048  << "\n";
1049  }
1050 #endif
1051  currentDist = MAX2(currentDist, stopPos);
1052  neighDist = MAX2(neighDist, stopPos);
1053  }
1054  // direction specific constants
1055  const bool right = (laneOffset == -1);
1056  const bool left = (laneOffset == 1);
1057  const int myLca = (right ? LCA_MRIGHT : (left ? LCA_MLEFT : 0));
1058  const int lcaCounter = (right ? LCA_LEFT : (left ? LCA_RIGHT : LCA_NONE));
1059  const bool changeToBest = (right && bestLaneOffset < 0) || (left && bestLaneOffset > 0) || (laneOffset == 0 && bestLaneOffset == 0);
1060  // keep information about being a leader/follower but remove information
1061  // about previous lane change request or urgency
1062  int ret = (myOwnState & 0xffff0000);
1063 
1064  // compute the distance when changing to the neighboring lane
1065  // (ensure we do not lap into the line behind neighLane since there might be unseen blockers)
1066  // minimum distance to move the vehicle fully onto the new lane
1067  double latLaneDist = laneOffset == 0 ? 0. : myVehicle.lateralDistanceToLane(laneOffset);
1068 
1069  // VARIANT_5 (disableAMBACKBLOCKER1)
1070  /*
1071  if (leader.first != 0
1072  && (myOwnState & LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0
1073  && (leader.first->getLaneChangeModel().getOwnState() & LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0) {
1074 
1075  myOwnState &= (0xffffffff - LCA_AMBLOCKINGFOLLOWER_DONTBRAKE);
1076  if (myVehicle.getSpeed() > SUMO_const_haltingSpeed) {
1077  myOwnState |= LCA_AMBACKBLOCKER;
1078  } else {
1079  ret |= LCA_AMBACKBLOCKER;
1080  myDontBrake = true;
1081  }
1082  }
1083  */
1084 
1085 #ifdef DEBUG_WANTSCHANGE
1086  if (gDebugFlag2) {
1087  std::cout << STEPS2TIME(currentTime)
1088  << " veh=" << myVehicle.getID()
1089  << " myState=" << toString((LaneChangeAction)myOwnState)
1090  << " firstBlocked=" << Named::getIDSecure(*firstBlocked)
1091  << " lastBlocked=" << Named::getIDSecure(*lastBlocked)
1092  << "\n leaders=" << leaders.toString()
1093  << "\n followers=" << followers.toString()
1094  << "\n blockers=" << blockers.toString()
1095  << "\n neighLeaders=" << neighLeaders.toString()
1096  << "\n neighFollowers=" << neighFollowers.toString()
1097  << "\n neighBlockers=" << neighBlockers.toString()
1098  << "\n changeToBest=" << changeToBest
1099  << " latLaneDist=" << latLaneDist
1100  << "\n expectedSpeeds=" << toString(myExpectedSublaneSpeeds)
1101  << std::endl;
1102  }
1103 #endif
1104 
1105  ret = slowDownForBlocked(lastBlocked, ret);
1106  // VARIANT_14 (furtherBlock)
1107  if (lastBlocked != firstBlocked) {
1108  ret = slowDownForBlocked(firstBlocked, ret);
1109  }
1110 
1111 
1112  // we try to estimate the distance which is necessary to get on a lane
1113  // we have to get on in order to keep our route
1114  // we assume we need something that depends on our velocity
1115  // and compare this with the free space on our wished lane
1116  //
1117  // if the free space is somehow less than the space we need, we should
1118  // definitely try to get to the desired lane
1119  //
1120  // this rule forces our vehicle to change the lane if a lane changing is necessary soon
1121  // lookAheadDistance:
1122  // we do not want the lookahead distance to change all the time so we discrectize the speed a bit
1123 
1124  // VARIANT_18 (laHyst)
1127  } else {
1128  // FIXME: This strongly dependent on the value of TS, see LC2013 for the fix (l.1153, currently)
1131  }
1132  //myLookAheadSpeed = myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle);
1133 
1134  //double laDist = laSpeed > LOOK_FORWARD_SPEED_DIVIDER
1135  // ? laSpeed * LOOK_FORWARD_FAR
1136  // : laSpeed * LOOK_FORWARD_NEAR;
1137  double laDist = myLookAheadSpeed * LOOK_FORWARD * myStrategicParam * (right ? 1 : myLookaheadLeft);
1138  laDist += myVehicle.getVehicleType().getLengthWithGap() * (double) 2.;
1139  // aggressive drivers may elect to use reduced strategic lookahead to optimize speed
1140  /*
1141  if (mySpeedGainProbabilityRight > myChangeProbThresholdRight
1142  || mySpeedGainProbabilityLeft > myChangeProbThresholdLeft) {
1143  laDist *= MAX2(0.0, (1 - myPushy));
1144  laDist *= MAX2(0,0, (1 - myAssertive));
1145  laDist *= MAX2(0,0, (2 - mySpeedGainParam));
1146  }
1147  */
1148 
1149  // react to a stopped leader on the current lane
1150  if (bestLaneOffset == 0 && leaders.hasStoppedVehicle()) {
1151  // value is doubled for the check since we change back and forth
1152  // laDist = 0.5 * (myVehicle.getVehicleType().getLengthWithGap() + leader.first->getVehicleType().getLengthWithGap());
1153  // XXX determine lenght of longest stopped vehicle
1155  }
1156 
1157  // free space that is available for changing
1158  //const double neighSpeed = (neighLead.first != 0 ? neighLead.first->getSpeed() :
1159  // neighFollow.first != 0 ? neighFollow.first->getSpeed() :
1160  // best.lane->getSpeedLimit());
1161  // @note: while this lets vehicles change earlier into the correct direction
1162  // it also makes the vehicles more "selfish" and prevents changes which are necessary to help others
1163 
1164 
1165  // TODO: include updated roundabout distance code from LC2013 (probably best to put it to AbstractLCModel class)
1166  // VARIANT_15 (insideRoundabout)
1167  int roundaboutEdgesAhead = 0;
1168  for (std::vector<MSLane*>::iterator it = curr.bestContinuations.begin(); it != curr.bestContinuations.end(); ++it) {
1169  if ((*it) != nullptr && (*it)->getEdge().isRoundabout()) {
1170  roundaboutEdgesAhead += 1;
1171  } else if (roundaboutEdgesAhead > 0) {
1172  // only check the next roundabout
1173  break;
1174  }
1175  }
1176  int roundaboutEdgesAheadNeigh = 0;
1177  for (std::vector<MSLane*>::iterator it = neigh.bestContinuations.begin(); it != neigh.bestContinuations.end(); ++it) {
1178  if ((*it) != nullptr && (*it)->getEdge().isRoundabout()) {
1179  roundaboutEdgesAheadNeigh += 1;
1180  } else if (roundaboutEdgesAheadNeigh > 0) {
1181  // only check the next roundabout
1182  break;
1183  }
1184  }
1185  if (roundaboutEdgesAhead > 1) {
1186  currentDist += roundaboutEdgesAhead * ROUNDABOUT_DIST_BONUS * myCooperativeParam;
1187  neighDist += roundaboutEdgesAheadNeigh * ROUNDABOUT_DIST_BONUS * myCooperativeParam;
1188  }
1189  if (roundaboutEdgesAhead > 0) {
1190 #ifdef DEBUG_ROUNDABOUTS
1191  if (gDebugFlag2) {
1192  std::cout << " roundaboutEdgesAhead=" << roundaboutEdgesAhead << " roundaboutEdgesAheadNeigh=" << roundaboutEdgesAheadNeigh << "\n";
1193  }
1194 #endif
1195  }
1196 
1197  if (laneOffset != 0) {
1198  ret = checkStrategicChange(ret,
1199  laneOffset,
1200  preb,
1201  leaders,
1202  neighLeaders,
1203  currIdx,
1204  bestLaneOffset,
1205  changeToBest,
1206  currentDist,
1207  neighDist,
1208  laDist,
1209  roundaboutEdgesAhead,
1210  latLaneDist,
1211  latDist);
1212  }
1213 
1214  if ((ret & LCA_STAY) != 0 && latDist == 0) {
1215  // ensure that mySafeLatDistLeft / mySafeLatDistRight are up to date for the
1216  // subsquent check with laneOffset = 0
1217  const double center = myVehicle.getCenterOnEdge();
1218  updateGaps(neighLeaders, neighLane.getRightSideOnEdge(), center, 1.0, mySafeLatDistRight, mySafeLatDistLeft);
1219  updateGaps(neighFollowers, neighLane.getRightSideOnEdge(), center, 1.0, mySafeLatDistRight, mySafeLatDistLeft);
1220  return ret;
1221  }
1222  if ((ret & LCA_URGENT) != 0) {
1223  // prepare urgent lane change maneuver
1224  if (changeToBest && abs(bestLaneOffset) > 1
1225  && curr.bestContinuations.back()->getLinkCont().size() != 0
1226  ) {
1227  // there might be a vehicle which needs to counter-lane-change one lane further and we cannot see it yet
1228  myLeadingBlockerLength = MAX2((double)(right ? 20.0 : 40.0), myLeadingBlockerLength);
1229 #ifdef DEBUG_WANTSCHANGE
1230  if (gDebugFlag2) {
1231  std::cout << " reserving space for unseen blockers myLeadingBlockerLength=" << myLeadingBlockerLength << "\n";
1232  }
1233 #endif
1234  }
1235 
1236  // letting vehicles merge in at the end of the lane in case of counter-lane change, step#1
1237  // if there is a leader and he wants to change to the opposite direction
1238  const MSVehicle* neighLeadLongest = getLongest(neighLeaders).first;
1239  saveBlockerLength(neighLeadLongest, lcaCounter);
1240  if (*firstBlocked != neighLeadLongest) {
1241  saveBlockerLength(*firstBlocked, lcaCounter);
1242  }
1243  std::vector<CLeaderDist> collectLeadBlockers;
1244  std::vector<CLeaderDist> collectFollowBlockers;
1245  int blockedFully = 0;
1246 
1247  const double gapFactor = computeGapFactor(LCA_STRATEGIC);
1248  blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1249  leaders, followers, blockers,
1250  neighLeaders, neighFollowers, neighBlockers, &collectLeadBlockers, &collectFollowBlockers,
1251  false, gapFactor, &blockedFully);
1252 
1253  const double absLaneOffset = fabs(bestLaneOffset != 0 ? bestLaneOffset : latDist / SUMO_const_laneWidth);
1254  const double remainingSeconds = ((ret & LCA_TRACI) == 0 ?
1257  const double plannedSpeed = informLeaders(blocked, myLca, collectLeadBlockers, remainingSeconds);
1258  // coordinate with direct obstructions
1259  if (plannedSpeed >= 0) {
1260  // maybe we need to deal with a blocking follower
1261  informFollowers(blocked, myLca, collectFollowBlockers, remainingSeconds, plannedSpeed);
1262  }
1263  if (plannedSpeed > 0) {
1264  commitManoeuvre(blocked, blockedFully, leaders, neighLeaders, neighLane, maneuverDist);
1265  }
1266 #if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE)
1267  if (gDebugFlag2) {
1268  std::cout << STEPS2TIME(currentTime)
1269  << " veh=" << myVehicle.getID()
1270  << " myLeftSpace=" << myLeftSpace
1271  << " changeFully=" << myCanChangeFully
1272  << " blockedFully=" << toString((LaneChangeAction)blockedFully)
1273  << " remainingSeconds=" << remainingSeconds
1274  << " plannedSpeed=" << plannedSpeed
1275  << " mySafeLatDistRight=" << mySafeLatDistRight
1276  << " mySafeLatDistLeft=" << mySafeLatDistLeft
1277  << "\n";
1278  }
1279 #endif
1280  return ret;
1281  }
1282 
1283  // VARIANT_15
1284  if (roundaboutEdgesAhead > 1) {
1285  // try to use the inner lanes of a roundabout to increase throughput
1286  // unless we are approaching the exit
1287  if (left) {
1288  ret |= LCA_COOPERATIVE;
1289  } else {
1290  ret |= LCA_STAY | LCA_COOPERATIVE;
1291  }
1292  if (!cancelRequest(ret, laneOffset)) {
1293  if ((ret & LCA_STAY) == 0) {
1294  latDist = latLaneDist;
1295  blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1296  leaders, followers, blockers,
1297  neighLeaders, neighFollowers, neighBlockers);
1298  }
1299  return ret;
1300  }
1301  }
1302 
1303  // --------
1304 
1305  // -------- make place on current lane if blocking follower
1306  //if (amBlockingFollowerPlusNB()) {
1307  // std::cout << myVehicle.getID() << ", " << currentDistAllows(neighDist, bestLaneOffset, laDist)
1308  // << " neighDist=" << neighDist
1309  // << " currentDist=" << currentDist
1310  // << "\n";
1311  //}
1312  const double inconvenience = (latLaneDist < 0
1315  if (laneOffset != 0
1317  // VARIANT_6 : counterNoHelp
1318  && ((myOwnState & myLca) != 0))
1319  ||
1320  // continue previous cooperative change
1322  && !myCanChangeFully
1323  // change is in the right direction
1324  && (laneOffset * myManeuverDist > 0)))
1325  && (inconvenience < myCooperativeParam)
1326  && (changeToBest || currentDistAllows(neighDist, abs(bestLaneOffset) + 1, laDist))) {
1327 
1328  // VARIANT_2 (nbWhenChangingToHelp)
1329 #ifdef DEBUG_COOPERATE
1330  if (gDebugFlag2) {
1331  std::cout << STEPS2TIME(currentTime)
1332  << " veh=" << myVehicle.getID()
1333  << " amBlocking=" << amBlockingFollowerPlusNB()
1334  << " prevState=" << toString((LaneChangeAction)myPreviousState)
1335  << " origLatDist=" << myManeuverDist
1336  << " wantsChangeToHelp=" << (right ? "right" : "left")
1337  << " state=" << myOwnState
1338  //<< (((myOwnState & myLca) == 0) ? " (counter)" : "")
1339  << "\n";
1340  }
1341 #endif
1342 
1343  ret |= LCA_COOPERATIVE | LCA_URGENT ;//| LCA_CHANGE_TO_HELP;
1344  if (!cancelRequest(ret, laneOffset)) {
1345  latDist = amBlockingFollowerPlusNB() ? latLaneDist : myManeuverDist;
1346  blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1347  leaders, followers, blockers,
1348  neighLeaders, neighFollowers, neighBlockers);
1349  return ret;
1350  }
1351  }
1352 
1353  // --------
1354 
1355 
1358  //if ((blocked & LCA_BLOCKED) != 0) {
1359  // return ret;
1360  //}
1362 
1363  // -------- higher speed
1364  //if ((congested(neighLead.first) && neighLead.second < 20) || predInteraction(leader.first)) { //!!!
1365  // return ret;
1366  //}
1367 
1368  // iterate over all possible combinations of sublanes this vehicle might cover and check the potential speed
1369  const MSEdge& edge = myVehicle.getLane()->getEdge();
1370  const std::vector<double>& sublaneSides = edge.getSubLaneSides();
1371  assert(sublaneSides.size() == myExpectedSublaneSpeeds.size());
1372  const double vehWidth = getWidth();
1373  const double rightVehSide = myVehicle.getCenterOnEdge() - 0.5 * vehWidth;
1374  const double leftVehSide = rightVehSide + vehWidth;
1375  // figure out next speed when staying where we are
1376  double defaultNextSpeed = std::numeric_limits<double>::max();
1378  int leftmostOnEdge = (int)sublaneSides.size() - 1;
1379  while (leftmostOnEdge > 0 && sublaneSides[leftmostOnEdge] > leftVehSide) {
1380  leftmostOnEdge--;
1381  }
1382  int rightmostOnEdge = leftmostOnEdge;
1383  while (rightmostOnEdge > 0 && sublaneSides[rightmostOnEdge] > rightVehSide + NUMERICAL_EPS) {
1384  defaultNextSpeed = MIN2(defaultNextSpeed, myExpectedSublaneSpeeds[rightmostOnEdge]);
1385 #ifdef DEBUG_WANTSCHANGE
1386  if (gDebugFlag2) {
1387  std::cout << " adapted to current sublane=" << rightmostOnEdge << " defaultNextSpeed=" << defaultNextSpeed << "\n";
1388  std::cout << " sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] << " rightVehSide=" << rightVehSide << "\n";
1389  }
1390 #endif
1391  rightmostOnEdge--;
1392  }
1393  defaultNextSpeed = MIN2(defaultNextSpeed, myExpectedSublaneSpeeds[rightmostOnEdge]);
1394 #ifdef DEBUG_WANTSCHANGE
1395  if (gDebugFlag2) {
1396  std::cout << " adapted to current sublane=" << rightmostOnEdge << " defaultNextSpeed=" << defaultNextSpeed << "\n";
1397  std::cout << " sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] << " rightVehSide=" << rightVehSide << "\n";
1398  }
1399 #endif
1400  double maxGain = -std::numeric_limits<double>::max();
1401  double maxGainRight = -std::numeric_limits<double>::max();
1402  double maxGainLeft = -std::numeric_limits<double>::max();
1403  double latDistNice = std::numeric_limits<double>::max();
1404 
1405  const int iMin = MIN2(myVehicle.getLane()->getRightmostSublane(), neighLane.getRightmostSublane());
1406  const double leftMax = MAX2(
1408  neighLane.getRightSideOnEdge() + neighLane.getWidth());
1409  assert(leftMax <= edge.getWidth());
1410  int sublaneCompact = MAX2(iMin, rightmostOnEdge - 1); // try to compactify to the right by default
1411 
1412 #ifdef DEBUG_WANTSCHANGE
1413  if (gDebugFlag2) std::cout
1414  << " checking sublanes rightmostOnEdge=" << rightmostOnEdge
1415  << " leftmostOnEdge=" << leftmostOnEdge
1416  << " iMin=" << iMin
1417  << " leftMax=" << leftMax
1418  << " sublaneCompact=" << sublaneCompact
1419  << "\n";
1420 #endif
1421  for (int i = iMin; i < (int)sublaneSides.size(); ++i) {
1422  if (sublaneSides[i] + vehWidth < leftMax) {
1423  // i is the rightmost sublane and the left side of vehicles still fits on the edge,
1424  // compute min speed of all sublanes covered by the vehicle in this case
1425  double vMin = myExpectedSublaneSpeeds[i];
1426  //std::cout << " i=" << i << "\n";
1427  int j = i;
1428  while (vMin > 0 && j < (int)sublaneSides.size() && sublaneSides[j] < sublaneSides[i] + vehWidth) {
1429  vMin = MIN2(vMin, myExpectedSublaneSpeeds[j]);
1430  //std::cout << " j=" << j << " vMin=" << vMin << " sublaneSides[j]=" << sublaneSides[j] << " leftVehSide=" << leftVehSide << " rightVehSide=" << rightVehSide << "\n";
1431  ++j;
1432  }
1433  const double relativeGain = (vMin - defaultNextSpeed) / MAX2(vMin, RELGAIN_NORMALIZATION_MIN_SPEED);
1434  const double currentLatDist = sublaneSides[i] - rightVehSide;
1435  // @note this is biased for changing to the left since we compare the sublanes in ascending order
1436  if (relativeGain > maxGain) {
1437  maxGain = relativeGain;
1438  if (maxGain > GAIN_PERCEPTION_THRESHOLD) {
1439  sublaneCompact = i;
1440  latDist = currentLatDist;
1441 #ifdef DEBUG_WANTSCHANGE
1442  if (gDebugFlag2) {
1443  std::cout << " i=" << i << " newLatDist=" << latDist << " relGain=" << relativeGain << "\n";
1444  }
1445 #endif
1446  }
1447  } else {
1448  // if anticipated gains to the left are higher, prefer this
1449  if (currentLatDist > 0
1450  //&& latDist < 0 @todo check why this causes some timeloss in tests
1452  && relativeGain > GAIN_PERCEPTION_THRESHOLD
1453  && maxGain - relativeGain < NUMERICAL_EPS) {
1454  latDist = currentLatDist;
1455  }
1456  }
1457 #ifdef DEBUG_WANTSCHANGE
1458  if (gDebugFlag2) {
1459  std::cout << " i=" << i << " rightmostOnEdge=" << rightmostOnEdge << " vMin=" << vMin << " relGain=" << relativeGain << " sublaneCompact=" << sublaneCompact << " curLatDist=" << currentLatDist << "\n";
1460  }
1461 #endif
1462  if (currentLatDist < -NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) {
1463  maxGainRight = MAX2(maxGainRight, relativeGain);
1464  } else if (currentLatDist > NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) {
1465  maxGainLeft = MAX2(maxGainLeft, relativeGain);
1466  }
1467  const double subAlignDist = sublaneSides[i] - rightVehSide;
1468  if (fabs(subAlignDist) < fabs(latDistNice)) {
1469  latDistNice = subAlignDist;
1470 #ifdef DEBUG_WANTSCHANGE
1471  if (gDebugFlag2) std::cout
1472  << " nicest sublane=" << i
1473  << " side=" << sublaneSides[i]
1474  << " rightSide=" << rightVehSide
1475  << " latDistNice=" << latDistNice
1476  << " maxGainR=" << maxGainRight
1477  << " maxGainL=" << maxGainLeft
1478  << "\n";
1479 #endif
1480  }
1481  }
1482  }
1483  // updated change probabilities
1484  if (maxGainRight != -std::numeric_limits<double>::max()) {
1485 #ifdef DEBUG_WANTSCHANGE
1486  if (gDebugFlag2) {
1487  std::cout << " speedGainR_old=" << mySpeedGainProbabilityRight;
1488  }
1489 #endif
1491 #ifdef DEBUG_WANTSCHANGE
1492  if (gDebugFlag2) {
1493  std::cout << " speedGainR_new=" << mySpeedGainProbabilityRight << "\n";
1494  }
1495 #endif
1496  }
1497  if (maxGainLeft != -std::numeric_limits<double>::max()) {
1498 #ifdef DEBUG_WANTSCHANGE
1499  if (gDebugFlag2) {
1500  std::cout << " speedGainL_old=" << mySpeedGainProbabilityLeft;
1501  }
1502 #endif
1504 #ifdef DEBUG_WANTSCHANGE
1505  if (gDebugFlag2) {
1506  std::cout << " speedGainL_new=" << mySpeedGainProbabilityLeft << "\n";
1507  }
1508 #endif
1509  }
1510  // decay if there is no reason for or against changing (only if we have enough information)
1511  if ((fabs(maxGainRight) < NUMERICAL_EPS || maxGainRight == -std::numeric_limits<double>::max())
1512  && (right || (alternatives & LCA_RIGHT) == 0)) {
1514  }
1515  if ((fabs(maxGainLeft) < NUMERICAL_EPS || maxGainLeft == -std::numeric_limits<double>::max())
1516  && (left || (alternatives & LCA_LEFT) == 0)) {
1518  }
1519 
1520 
1521 #ifdef DEBUG_WANTSCHANGE
1522  if (gDebugFlag2) std::cout << SIMTIME
1523  << " veh=" << myVehicle.getID()
1524  << " defaultNextSpeed=" << defaultNextSpeed
1525  << " maxGain=" << maxGain
1526  << " maxGainRight=" << maxGainRight
1527  << " maxGainLeft=" << maxGainLeft
1528  << " latDist=" << latDist
1529  << " latDistNice=" << latDistNice
1530  << " sublaneCompact=" << sublaneCompact
1531  << "\n";
1532 #endif
1533 
1534  if (!left) {
1535  // ONLY FOR CHANGING TO THE RIGHT
1536  if (right && maxGainRight >= 0) {
1537  // honor the obligation to keep right (Rechtsfahrgebot)
1538  // XXX consider fast approaching followers on the current lane
1539  //const double vMax = myLookAheadSpeed;
1540  const double vMax = myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle);
1541  const double acceptanceTime = KEEP_RIGHT_ACCEPTANCE * vMax * MAX2(1., myVehicle.getSpeed()) / myVehicle.getLane()->getSpeedLimit();
1542  double fullSpeedGap = MAX2(0., neighDist - myVehicle.getCarFollowModel().brakeGap(vMax));
1543  double fullSpeedDrivingSeconds = MIN2(acceptanceTime, fullSpeedGap / vMax);
1544  CLeaderDist neighLead = getSlowest(neighLeaders);
1545  if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
1546  fullSpeedGap = MAX2(0., MIN2(fullSpeedGap,
1547  neighLead.second - myVehicle.getCarFollowModel().getSecureGap(
1548  vMax, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
1549  fullSpeedDrivingSeconds = MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
1550  }
1551  const double deltaProb = (myChangeProbThresholdRight * (fullSpeedDrivingSeconds / acceptanceTime) / KEEP_RIGHT_TIME);
1553 
1554 #ifdef DEBUG_WANTSCHANGE
1555  if (gDebugFlag2) {
1556  std::cout << STEPS2TIME(currentTime)
1557  << " considering keepRight:"
1558  << " vMax=" << vMax
1559  << " neighDist=" << neighDist
1560  << " brakeGap=" << myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed())
1561  << " leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
1562  << " secGap=" << (neighLead.first == 0 ? -1 : myVehicle.getCarFollowModel().getSecureGap(
1563  myVehicle.getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
1564  << " acceptanceTime=" << acceptanceTime
1565  << " fullSpeedGap=" << fullSpeedGap
1566  << " fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
1567  << " dProb=" << deltaProb
1568  << " keepRight=" << myKeepRightProbability
1569  << " speedGainL=" << mySpeedGainProbabilityLeft
1570  << "\n";
1571  }
1572 #endif
1574  /*&& latLaneDist <= -NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()*/) {
1575  ret |= LCA_KEEPRIGHT;
1576  assert(myVehicle.getLane()->getIndex() > neighLane.getIndex());
1577  if (!cancelRequest(ret, laneOffset)) {
1578  latDist = latLaneDist;
1579  blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1580  leaders, followers, blockers,
1581  neighLeaders, neighFollowers, neighBlockers);
1582  return ret;
1583  }
1584  }
1585  }
1586 
1587 #ifdef DEBUG_WANTSCHANGE
1588  if (gDebugFlag2) {
1589  std::cout << STEPS2TIME(currentTime)
1590  << " speedGainR=" << mySpeedGainProbabilityRight
1591  << " speedGainL=" << mySpeedGainProbabilityLeft
1592  << " neighDist=" << neighDist
1593  << " neighTime=" << neighDist / MAX2(.1, myVehicle.getSpeed())
1594  << " rThresh=" << myChangeProbThresholdRight
1595  << " latDist=" << latDist
1596  << "\n";
1597  }
1598 #endif
1599 
1600  if (latDist < 0 && mySpeedGainProbabilityRight >= MAX2(myChangeProbThresholdRight, mySpeedGainProbabilityLeft)
1601  && neighDist / MAX2(.1, myVehicle.getSpeed()) > 20.) {
1602  ret |= LCA_SPEEDGAIN;
1603  if (!cancelRequest(ret, laneOffset)) {
1604  int blockedFully = 0;
1605  blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1606  leaders, followers, blockers,
1607  neighLeaders, neighFollowers, neighBlockers,
1608  nullptr, nullptr, false, 0, &blockedFully);
1609  //commitManoeuvre(blocked, blockedFully, leaders, neighLeaders, neighLane);
1610  return ret;
1611  }
1612  }
1613  }
1614  if (!right) {
1615 
1616  const bool stayInLane = myVehicle.getLateralPositionOnLane() + latDist < 0.5 * myVehicle.getLane()->getWidth();
1617 #ifdef DEBUG_WANTSCHANGE
1618  if (gDebugFlag2) {
1619  std::cout << STEPS2TIME(currentTime)
1620  << " speedGainL=" << mySpeedGainProbabilityLeft
1621  << " speedGainR=" << mySpeedGainProbabilityRight
1622  << " latDist=" << latDist
1623  << " neighDist=" << neighDist
1624  << " neighTime=" << neighDist / MAX2(.1, myVehicle.getSpeed())
1625  << " lThresh=" << myChangeProbThresholdLeft
1626  << " stayInLane=" << stayInLane
1627  << "\n";
1628  }
1629 #endif
1630 
1632  // if we leave our lane, we should be able to stay in the new
1633  // lane for some time
1634  (stayInLane || neighDist / MAX2(.1, myVehicle.getSpeed()) > SPEED_GAIN_MIN_SECONDS)) {
1635  ret |= LCA_SPEEDGAIN;
1636  if (!cancelRequest(ret, laneOffset)) {
1637  int blockedFully = 0;
1638  blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1639  leaders, followers, blockers,
1640  neighLeaders, neighFollowers, neighBlockers,
1641  nullptr, nullptr, false, 0, &blockedFully);
1642  //commitManoeuvre(blocked, blockedFully, leaders, neighLeaders, neighLane);
1643  return ret;
1644  }
1645  }
1646  }
1647 
1648  // only factor in preferred lateral alignment if there is no speedGain motivation
1649  if (fabs(latDist) <= NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) {
1650  double latDistSublane = 0.;
1651  const double halfLaneWidth = myVehicle.getLane()->getWidth() * 0.5;
1652  const double halfVehWidth = getWidth() * 0.5;
1655  && bestLaneOffset == 0
1657  // vehicle is on its final edge, on the correct lane and close to
1658  // its arrival position. Change to the desired lateral position
1660  case ARRIVAL_POSLAT_GIVEN:
1662  break;
1663  case ARRIVAL_POSLAT_RIGHT:
1664  latDistSublane = -halfLaneWidth + halfVehWidth - myVehicle.getLateralPositionOnLane();
1665  break;
1666  case ARRIVAL_POSLAT_CENTER:
1667  latDistSublane = -myVehicle.getLateralPositionOnLane();
1668  break;
1669  case ARRIVAL_POSLAT_LEFT:
1670  latDistSublane = halfLaneWidth - halfVehWidth - myVehicle.getLateralPositionOnLane();
1671  break;
1672  default:
1673  assert(false);
1674  }
1675 #ifdef DEBUG_WANTSCHANGE
1676  if (gDebugFlag2) std::cout << SIMTIME
1677  << " arrivalPosLatProcedure=" << myVehicle.getParameter().arrivalPosLatProcedure
1678  << " arrivalPosLat=" << myVehicle.getParameter().arrivalPosLat << "\n";
1679 #endif
1680 
1681  } else {
1682 
1684  // Check whether the vehicle should adapt its alignment to an upcoming turn
1685  if (myTurnAlignmentDist > 0) {
1686  const std::pair<double, LinkDirection>& turnInfo = myVehicle.getNextTurn();
1687  if (turnInfo.first < myTurnAlignmentDist) {
1688  // Vehicle is close enough to the link to change its default alignment
1689  switch (turnInfo.second) {
1690  case LINKDIR_TURN:
1691  case LINKDIR_LEFT:
1692  case LINKDIR_PARTLEFT:
1693  align = LATALIGN_LEFT;
1694  break;
1695  case LINKDIR_TURN_LEFTHAND:
1696  case LINKDIR_RIGHT:
1697  case LINKDIR_PARTRIGHT:
1698  align = LATALIGN_RIGHT;
1699  break;
1700  case LINKDIR_STRAIGHT:
1701  case LINKDIR_NODIR:
1702  default:
1703  break;
1704  }
1705  }
1706  }
1707  switch (align) {
1708  case LATALIGN_RIGHT:
1709  latDistSublane = -halfLaneWidth + halfVehWidth - myVehicle.getLateralPositionOnLane();
1710  break;
1711  case LATALIGN_LEFT:
1712  latDistSublane = halfLaneWidth - halfVehWidth - myVehicle.getLateralPositionOnLane();
1713  break;
1714  case LATALIGN_CENTER:
1715  latDistSublane = -myVehicle.getLateralPositionOnLane();
1716  break;
1717  case LATALIGN_NICE:
1718  latDistSublane = latDistNice;
1719  break;
1720  case LATALIGN_COMPACT:
1721  latDistSublane = sublaneSides[sublaneCompact] - rightVehSide;
1722  break;
1723  case LATALIGN_ARBITRARY:
1724  latDistSublane = 0;
1725  break;
1726  }
1727  }
1728 
1729 #if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE) || defined(DEBUG_MANEUVER)
1730  if (gDebugFlag2) std::cout << SIMTIME
1732  << " mySpeedGainR=" << mySpeedGainProbabilityRight
1733  << " mySpeedGainL=" << mySpeedGainProbabilityLeft
1734  << " latDist=" << latDist
1735  << " latDistSublane=" << latDistSublane
1736  << " relGainSublane=" << computeSpeedGain(latDistSublane, defaultNextSpeed)
1737  << " maneuverDist=" << maneuverDist
1738  << " myCanChangeFully=" << myCanChangeFully
1739  << " prevState=" << toString((LaneChangeAction)myPreviousState)
1740  << "\n";
1741 #endif
1742 
1743  if ((latDistSublane < 0 && mySpeedGainProbabilityRight < mySpeedLossProbThreshold)
1744  || (latDistSublane > 0 && mySpeedGainProbabilityLeft < mySpeedLossProbThreshold)
1745  || computeSpeedGain(latDistSublane, defaultNextSpeed) < -mySublaneParam) {
1746  // do not risk losing speed
1747 #if defined(DEBUG_WANTSCHANGE)
1748  if (gDebugFlag2) std::cout << " aborting sublane change to avoid speed loss (mySpeedLossProbThreshold=" << mySpeedLossProbThreshold
1749  << " speedGain=" << computeSpeedGain(latDistSublane, defaultNextSpeed) << ")\n";
1750 #endif
1751  latDistSublane = 0;
1752  }
1753  // Ignore preferred lateral alignment if we are in the middle of an unfinished non-alignment maneuver into the opposite direction
1754  if (!myCanChangeFully
1755  && (myPreviousState & (LCA_STRATEGIC | LCA_COOPERATIVE | LCA_KEEPRIGHT | LCA_SPEEDGAIN)) != 0
1756  && ((myManeuverDist < 0 && latDistSublane > 0) || (myManeuverDist > 0 && latDistSublane < 0))) {
1757 #if defined(DEBUG_WANTSCHANGE)
1758  if (gDebugFlag2) {
1759  std::cout << " aborting sublane change due to prior maneuver\n";
1760  }
1761 #endif
1762  latDistSublane = 0;
1763  }
1764  latDist = latDistSublane;
1765  // XXX first compute preferred adaptation and then override with speed
1766  // (this way adaptation is still done if changing for speedgain is
1767  // blocked)
1768  if (fabs(latDist) >= NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) {
1769 #ifdef DEBUG_WANTSCHANGE
1770  if (gDebugFlag2) std::cout << SIMTIME
1771  << " adapting to preferred alignment=" << toString(myVehicle.getVehicleType().getPreferredLateralAlignment())
1772  << " latDist=" << latDist
1773  << "\n";
1774 #endif
1775  ret |= LCA_SUBLANE;
1776  // include prior motivation when sublane-change is part of finishing an ongoing maneuver in the same direction
1777  if (myPreviousManeuverDist * latDist > 0) {
1778  int priorReason = (myPreviousState & LCA_CHANGE_REASONS & ~LCA_SUBLANE);
1779  ret |= priorReason;
1780 #ifdef DEBUG_WANTSCHANGE
1781  if (gDebugFlag2 && priorReason != 0) std::cout << " including prior reason " << toString((LaneChangeAction)priorReason)
1782  << " prevManeuverDist=" << myPreviousManeuverDist << "\n";
1783 #endif
1784  }
1785  if (!cancelRequest(ret, laneOffset)) {
1786  blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1787  leaders, followers, blockers,
1788  neighLeaders, neighFollowers, neighBlockers);
1789  return ret;
1790  }
1791  } else {
1792  return ret | LCA_SUBLANE | LCA_STAY;
1793  }
1794  }
1795  latDist = 0;
1796 
1797 
1798  // --------
1799  /*
1800  if (changeToBest && bestLaneOffset == curr.bestLaneOffset && laneOffset != 0
1801  && (right
1802  ? mySpeedGainProbabilityRight > MAX2(0., mySpeedGainProbabilityLeft)
1803  : mySpeedGainProbabilityLeft > MAX2(0., mySpeedGainProbabilityRight))) {
1804  // change towards the correct lane, speedwise it does not hurt
1805  ret |= LCA_STRATEGIC;
1806  if (!cancelRequest(ret, laneOffset)) {
1807  latDist = latLaneDist;
1808  blocked = checkBlocking(neighLane, latDist, laneOffset,
1809  leaders, followers, blockers,
1810  neighLeaders, neighFollowers, neighBlockers);
1811  return ret;
1812  }
1813  }
1814  */
1815 #ifdef DEBUG_WANTSCHANGE
1816  if (gDebugFlag2) {
1817  std::cout << STEPS2TIME(currentTime)
1818  << " veh=" << myVehicle.getID()
1819  << " mySpeedGainR=" << mySpeedGainProbabilityRight
1820  << " mySpeedGainL=" << mySpeedGainProbabilityLeft
1821  << " myKeepRight=" << myKeepRightProbability
1822  << "\n";
1823  }
1824 #endif
1825  return ret;
1826 }
1827 
1828 
1829 int
1831  // if this vehicle is blocking someone in front, we maybe decelerate to let him in
1832  if ((*blocked) != nullptr) {
1833  double gap = (*blocked)->getPositionOnLane() - (*blocked)->getVehicleType().getLength() - myVehicle.getPositionOnLane() - myVehicle.getVehicleType().getMinGap();
1834 #ifdef DEBUG_SLOWDOWN
1835  if (gDebugFlag2) {
1836  std::cout << SIMTIME
1837  << " veh=" << myVehicle.getID()
1838  << " blocked=" << Named::getIDSecure(*blocked)
1839  << " gap=" << gap
1840  << "\n";
1841  }
1842 #endif
1843  if (gap > POSITION_EPS) {
1844  //const bool blockedWantsUrgentRight = (((*blocked)->getLaneChangeModel().getOwnState() & LCA_RIGHT != 0)
1845  // && ((*blocked)->getLaneChangeModel().getOwnState() & LCA_URGENT != 0));
1846 
1848  //|| blockedWantsUrgentRight // VARIANT_10 (helpblockedRight)
1849  ) {
1850  if ((*blocked)->getSpeed() < SUMO_const_haltingSpeed) {
1851  state |= LCA_AMBACKBLOCKER_STANDING;
1852  } else {
1853  state |= LCA_AMBACKBLOCKER;
1854  }
1857  (double)(gap - POSITION_EPS), (*blocked)->getSpeed(),
1858  (*blocked)->getCarFollowModel().getMaxDecel()));
1859  //(*blocked) = 0; // VARIANT_14 (furtherBlock)
1860  }
1861  }
1862  }
1863  return state;
1864 }
1865 
1866 
1867 void
1868 MSLCM_SL2015::saveBlockerLength(const MSVehicle* blocker, int lcaCounter) {
1869 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
1870  if (gDebugFlag2) {
1871  std::cout << SIMTIME
1872  << " veh=" << myVehicle.getID()
1873  << " saveBlockerLength blocker=" << Named::getIDSecure(blocker)
1874  << " bState=" << (blocker == 0 ? "None" : toString(blocker->getLaneChangeModel().getOwnState()))
1875  << "\n";
1876  }
1877 #endif
1878  if (blocker != nullptr && (blocker->getLaneChangeModel().getOwnState() & lcaCounter) != 0) {
1879  // is there enough space in front of us for the blocker?
1880  const double potential = myLeftSpace - myVehicle.getCarFollowModel().brakeGap(
1882  if (blocker->getVehicleType().getLengthWithGap() <= potential) {
1883  // save at least his length in myLeadingBlockerLength
1885 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
1886  if (gDebugFlag2) {
1887  std::cout << SIMTIME
1888  << " veh=" << myVehicle.getID()
1889  << " blocker=" << Named::getIDSecure(blocker)
1890  << " saving myLeadingBlockerLength=" << myLeadingBlockerLength
1891  << "\n";
1892  }
1893 #endif
1894  } else {
1895  // we cannot save enough space for the blocker. It needs to save
1896  // space for ego instead
1897 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
1898  if (gDebugFlag2) {
1899  std::cout << SIMTIME
1900  << " veh=" << myVehicle.getID()
1901  << " blocker=" << Named::getIDSecure(blocker)
1902  << " cannot save space=" << blocker->getVehicleType().getLengthWithGap()
1903  << " potential=" << potential
1904  << "\n";
1905  }
1906 #endif
1907  ((MSVehicle*)blocker)->getLaneChangeModel().saveBlockerLength(myVehicle.getVehicleType().getLengthWithGap());
1908  }
1909  }
1910 }
1911 
1912 
1913 void MSLCM_SL2015::addLCSpeedAdvice(const double vSafe) {
1914  const double accel = SPEED2ACCEL(vSafe - myVehicle.getSpeed());
1915  myLCAccelerationAdvices.push_back(accel);
1916 #ifdef DEBUG_INFORM
1917  if (DEBUG_COND) {
1918  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " accepted LC speed advice "
1919  << "vSafe=" << vSafe << " -> accel=" << accel << "\n";
1920  }
1921 #endif
1922 }
1923 
1924 
1925 void
1926 MSLCM_SL2015::updateExpectedSublaneSpeeds(const MSLeaderDistanceInfo& ahead, int sublaneOffset, int laneIndex) {
1927  const std::vector<MSLane*>& lanes = myVehicle.getLane()->getEdge().getLanes();
1928  const std::vector<MSVehicle::LaneQ>& preb = myVehicle.getBestLanes();
1929  const MSLane* lane = lanes[laneIndex];
1930  const double vMax = lane->getVehicleMaxSpeed(&myVehicle);
1931  assert(preb.size() == lanes.size());
1932 
1933  for (int sublane = 0; sublane < (int)ahead.numSublanes(); ++sublane) {
1934  const int edgeSublane = sublane + sublaneOffset;
1935  if (edgeSublane >= (int)myExpectedSublaneSpeeds.size()) {
1936  // this may happen if a sibling lane is wider than the changer lane
1937  continue;
1938  }
1940  // lane allowed, find potential leaders and compute safe speeds
1941  // XXX anticipate future braking if leader has a lower speed than myVehicle
1942  const MSVehicle* leader = ahead[sublane].first;
1943  const double gap = ahead[sublane].second;
1944  double vSafe;
1945  if (leader == nullptr) {
1946  vSafe = myCarFollowModel.followSpeed(&myVehicle, vMax, preb[laneIndex].length, 0, 0);
1947  } else {
1948  if (leader->getAcceleration() > 0.5 * leader->getCarFollowModel().getMaxAccel()) {
1949  // assume that the leader will continue accelerating to its maximum speed
1950  vSafe = leader->getLane()->getVehicleMaxSpeed(leader);
1951  } else {
1952  vSafe = myCarFollowModel.followSpeed(
1953  &myVehicle, vMax, gap, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel());
1954 #ifdef DEBUG_EXPECTED_SLSPEED
1955  if (DEBUG_COND) {
1956  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " updateExpectedSublaneSpeeds edgeSublane=" << edgeSublane << " leader=" << leader->getID() << " gap=" << gap << " vSafe=" << vSafe << "\n";
1957  }
1958 #endif
1959  const double deltaV = vMax - leader->getSpeed();
1960  if (deltaV > 0 && gap / deltaV < 5) {
1961  // anticipate future braking by computing the average
1962  // speed over the next few seconds
1963  const double foreCastTime = 10;
1964  const double gapClosingTime = gap / deltaV;
1965  vSafe = (gapClosingTime * vSafe + (foreCastTime - gapClosingTime) * leader->getSpeed()) / foreCastTime;
1966  }
1967  }
1968  }
1969  // take pedestrians into account
1970  if (lane->getEdge().getPersons().size() > 0 && MSPModel::getModel()->hasPedestrians(lane)) {
1972  double foeRight, foeLeft;
1973  ahead.getSublaneBorders(sublane, 0, foeRight, foeLeft);
1974  // get all leaders ahead or overlapping
1976  if (leader.first != 0) {
1977  const double gap = leader.second - myVehicle.getVehicleType().getMinGap() - myVehicle.getVehicleType().getLength();
1978  const double vSafePed = myCarFollowModel.stopSpeed(&myVehicle, vMax, gap);
1979  vSafe = MIN2(vSafe, vSafePed);
1980  }
1981  }
1982  vSafe = MIN2(vMax, vSafe);
1983  const double memoryFactor = pow(SPEEDGAIN_MEMORY_FACTOR, myVehicle.getActionStepLengthSecs());
1984  myExpectedSublaneSpeeds[edgeSublane] = memoryFactor * myExpectedSublaneSpeeds[edgeSublane] + (1 - memoryFactor) * vSafe;
1985  } else {
1986  // lane forbidden
1987  myExpectedSublaneSpeeds[edgeSublane] = -1;
1988  }
1989  }
1990  // XXX deal with leaders on subsequent lanes based on preb
1991 }
1992 
1993 
1994 double
1995 MSLCM_SL2015::computeSpeedGain(double latDistSublane, double defaultNextSpeed) const {
1996  double result = std::numeric_limits<double>::max();
1998  const std::vector<double>& sublaneSides = myVehicle.getLane()->getEdge().getSubLaneSides();
1999  const double vehWidth = getWidth();
2000  const double rightVehSide = myVehicle.getCenterOnEdge() - vehWidth * 0.5 + latDistSublane;
2001  const double leftVehSide = rightVehSide + vehWidth;
2002  for (int i = 0; i < (int)sublaneSides.size(); ++i) {
2003  if (overlap(rightVehSide, leftVehSide, sublaneSides[i], sublaneSides[i] + res)) {
2004  result = MIN2(result, myExpectedSublaneSpeeds[i]);
2005  }
2006  //std::cout << " i=" << i << " rightVehSide=" << rightVehSide << " leftVehSide=" << leftVehSide << " sublaneR=" << sublaneSides[i] << " sublaneL=" << sublaneSides[i] + res
2007  // << " overlap=" << overlap(rightVehSide, leftVehSide, sublaneSides[i], sublaneSides[i] + res) << " speed=" << myExpectedSublaneSpeeds[i] << " result=" << result << "\n";
2008  }
2009  return result - defaultNextSpeed;
2010 }
2011 
2012 
2015  int iMax = 0;
2016  double maxLength = -1;
2017  for (int i = 0; i < ldi.numSublanes(); ++i) {
2018  if (ldi[i].first != 0) {
2019  const double length = ldi[i].first->getVehicleType().getLength();
2020  if (length > maxLength) {
2021  maxLength = length;
2022  iMax = i;
2023  }
2024  }
2025  }
2026  return ldi[iMax];
2027 }
2028 
2029 
2032  int iMax = 0;
2033  double minSpeed = std::numeric_limits<double>::max();
2034  for (int i = 0; i < ldi.numSublanes(); ++i) {
2035  if (ldi[i].first != 0) {
2036  const double speed = ldi[i].first->getSpeed();
2037  if (speed < minSpeed) {
2038  minSpeed = speed;
2039  iMax = i;
2040  }
2041  }
2042  }
2043  return ldi[iMax];
2044 }
2045 
2046 
2047 int
2048 MSLCM_SL2015::checkBlocking(const MSLane& neighLane, double& latDist, double& maneuverDist, int laneOffset,
2049  const MSLeaderDistanceInfo& leaders,
2050  const MSLeaderDistanceInfo& followers,
2051  const MSLeaderDistanceInfo& /*blockers */,
2052  const MSLeaderDistanceInfo& neighLeaders,
2053  const MSLeaderDistanceInfo& neighFollowers,
2054  const MSLeaderDistanceInfo& /* neighBlockers */,
2055  std::vector<CLeaderDist>* collectLeadBlockers,
2056  std::vector<CLeaderDist>* collectFollowBlockers,
2057  bool keepLatGapManeuver,
2058  double gapFactor,
2059  int* retBlockedFully) {
2060  // truncate latDist according to maxSpeedLat
2061  if (!keepLatGapManeuver) {
2062  myManeuverDist = latDist;
2063  maneuverDist = latDist;
2064  }
2065  const double maxDist = SPEED2DIST(myVehicle.getVehicleType().getMaxSpeedLat());
2066  latDist = MAX2(MIN2(latDist, maxDist), -maxDist);
2068  return 0;
2069  }
2070 
2071  if (!myCFRelatedReady) {
2072  updateCFRelated(leaders, myVehicle.getLane()->getRightSideOnEdge(), true);
2073  updateCFRelated(followers, myVehicle.getLane()->getRightSideOnEdge(), false);
2074  if (laneOffset != 0) {
2075  updateCFRelated(neighLeaders, neighLane.getRightSideOnEdge(), true);
2076  updateCFRelated(neighFollowers, neighLane.getRightSideOnEdge(), false);
2077  }
2078  myCFRelatedReady = true;
2079  }
2080 
2081  // reduce latDist to avoid blockage with overlapping vehicles (no minGapLat constraints)
2082  const double center = myVehicle.getCenterOnEdge();
2083  updateGaps(leaders, myVehicle.getLane()->getRightSideOnEdge(), center, gapFactor, mySafeLatDistRight, mySafeLatDistLeft, false, 0, latDist, collectLeadBlockers);
2084  updateGaps(followers, myVehicle.getLane()->getRightSideOnEdge(), center, gapFactor, mySafeLatDistRight, mySafeLatDistLeft, false, 0, latDist, collectFollowBlockers);
2085  if (laneOffset != 0) {
2086  updateGaps(neighLeaders, neighLane.getRightSideOnEdge(), center, gapFactor, mySafeLatDistRight, mySafeLatDistLeft, false, 0, latDist, collectLeadBlockers);
2087  updateGaps(neighFollowers, neighLane.getRightSideOnEdge(), center, gapFactor, mySafeLatDistRight, mySafeLatDistLeft, false, 0, latDist, collectFollowBlockers);
2088  }
2089 #ifdef DEBUG_BLOCKING
2090  if (gDebugFlag2) {
2091  std::cout << " checkBlocking latDist=" << latDist << " mySafeLatDistRight=" << mySafeLatDistRight << " mySafeLatDistLeft=" << mySafeLatDistLeft << "\n";
2092  }
2093 #endif
2094  // if we can move at least a little bit in the desired direction, do so (rather than block)
2095  const bool forcedTraCIChange = (myVehicle.hasInfluencer()
2096  && myVehicle.getInfluencer().getLatDist() != 0
2098  if (latDist < 0) {
2099  if (mySafeLatDistRight <= NUMERICAL_EPS) {
2101  } else if (!forcedTraCIChange) {
2102  latDist = MAX2(latDist, -mySafeLatDistRight);
2103  }
2104  } else {
2105  if (mySafeLatDistLeft <= NUMERICAL_EPS) {
2107  } else if (!forcedTraCIChange) {
2108  latDist = MIN2(latDist, mySafeLatDistLeft);
2109  }
2110  }
2111 
2112  myCanChangeFully = (myManeuverDist == 0 || latDist == myManeuverDist);
2113 #ifdef DEBUG_BLOCKING
2114  if (gDebugFlag2) {
2115  std::cout << " checkBlocking fully=" << myCanChangeFully << " latDist=" << latDist << " myManeuverDist=" << myManeuverDist << "\n";
2116  }
2117 #endif
2118  // destination sublanes must be safe
2119  // intermediate sublanes must not be blocked by overlapping vehicles
2120 
2121  // XXX avoid checking the same leader multiple times
2122  // XXX ensure that only changes within the same lane are undertaken if laneOffset = 0
2123 
2124  int blocked = 0;
2125  blocked |= checkBlockingVehicles(&myVehicle, leaders, latDist, myVehicle.getLane()->getRightSideOnEdge(), true, LCA_BLOCKED_BY_LEADER,
2126  mySafeLatDistRight, mySafeLatDistLeft, collectLeadBlockers);
2127  blocked |= checkBlockingVehicles(&myVehicle, followers, latDist, myVehicle.getLane()->getRightSideOnEdge(), false, LCA_BLOCKED_BY_FOLLOWER,
2128  mySafeLatDistRight, mySafeLatDistLeft, collectFollowBlockers);
2129  if (laneOffset != 0) {
2130  blocked |= checkBlockingVehicles(&myVehicle, neighLeaders, latDist, neighLane.getRightSideOnEdge(), true,
2132  mySafeLatDistRight, mySafeLatDistLeft, collectLeadBlockers);
2133  blocked |= checkBlockingVehicles(&myVehicle, neighFollowers, latDist, neighLane.getRightSideOnEdge(), false,
2135  mySafeLatDistRight, mySafeLatDistLeft, collectFollowBlockers);
2136  }
2137 
2138  int blockedFully = 0;
2140  mySafeLatDistRight, mySafeLatDistLeft, collectLeadBlockers);
2142  mySafeLatDistRight, mySafeLatDistLeft, collectFollowBlockers);
2143  if (laneOffset != 0) {
2144  blockedFully |= checkBlockingVehicles(&myVehicle, neighLeaders, myManeuverDist, neighLane.getRightSideOnEdge(), true,
2146  mySafeLatDistRight, mySafeLatDistLeft, collectLeadBlockers);
2147  blockedFully |= checkBlockingVehicles(&myVehicle, neighFollowers, myManeuverDist, neighLane.getRightSideOnEdge(), false,
2149  mySafeLatDistRight, mySafeLatDistLeft, collectFollowBlockers);
2150  }
2151  if (retBlockedFully != nullptr) {
2152  *retBlockedFully = blockedFully;
2153  }
2154  if (blocked == 0 && !myCanChangeFully && myPushy == 0 && !keepLatGapManeuver) {
2155  // aggressive drivers immediately start moving towards potential
2156  // blockers and only check that the start of their maneuver (latDist) is safe. In
2157  // contrast, cautious drivers need to check latDist and origLatDist to
2158  // ensure that the maneuver can be finished without encroaching on other vehicles.
2159  blocked |= blockedFully;
2160  } else {
2161  // XXX: in case of action step length > simulation step length, pushing may lead to collisions,
2162  // because maneuver is continued until myManeuverDist is reached (perhaps set myManeuverDist=latDist)
2163  }
2164  if (collectFollowBlockers != nullptr && collectLeadBlockers != nullptr) {
2165  // prevent vehicles from being classified as leader and follower simultaneously
2166  for (std::vector<CLeaderDist>::const_iterator it2 = collectLeadBlockers->begin(); it2 != collectLeadBlockers->end(); ++it2) {
2167  for (std::vector<CLeaderDist>::iterator it = collectFollowBlockers->begin(); it != collectFollowBlockers->end();) {
2168  if ((*it2).first == (*it).first) {
2169 #ifdef DEBUG_BLOCKING
2170  if (gDebugFlag2) {
2171  std::cout << " removed follower " << (*it).first->getID() << " because it is already a leader\n";
2172  }
2173 #endif
2174  it = collectFollowBlockers->erase(it);
2175  } else {
2176  ++it;
2177  }
2178  }
2179  }
2180  }
2181  return blocked;
2182 }
2183 
2184 
2185 int
2187  const MSVehicle* ego, const MSLeaderDistanceInfo& vehicles,
2188  double latDist, double foeOffset, bool leaders, LaneChangeAction blockType,
2189  double& safeLatGapRight, double& safeLatGapLeft,
2190  std::vector<CLeaderDist>* collectBlockers) const {
2191  // determine borders where safety/no-overlap conditions must hold
2192  const double vehWidth = getWidth();
2193  const double rightVehSide = ego->getRightSideOnEdge();
2194  const double leftVehSide = rightVehSide + vehWidth;
2195  const double rightVehSideDest = rightVehSide + latDist;
2196  const double leftVehSideDest = leftVehSide + latDist;
2197  const double rightNoOverlap = MIN2(rightVehSideDest, rightVehSide);
2198  const double leftNoOverlap = MAX2(leftVehSideDest, leftVehSide);
2199 #ifdef DEBUG_BLOCKING
2200  if (gDebugFlag2) {
2201  std::cout << " checkBlockingVehicles"
2202  << " latDist=" << latDist
2203  << " foeOffset=" << foeOffset
2204  << " vehRight=" << rightVehSide
2205  << " vehLeft=" << leftVehSide
2206  << " rightNoOverlap=" << rightNoOverlap
2207  << " leftNoOverlap=" << leftNoOverlap
2208  << " destRight=" << rightVehSideDest
2209  << " destLeft=" << leftVehSideDest
2210  << " leaders=" << leaders
2211  << " blockType=" << toString((LaneChangeAction) blockType)
2212  << "\n";
2213  }
2214 #endif
2215  int result = 0;
2216  for (int i = 0; i < vehicles.numSublanes(); ++i) {
2217  CLeaderDist vehDist = vehicles[i];
2218  if (vehDist.first != 0 && myCFRelated.count(vehDist.first) == 0) {
2219  const MSVehicle* leader = vehDist.first;
2220  const MSVehicle* follower = ego;
2221  if (!leaders) {
2222  std::swap(leader, follower);
2223  }
2224  // only check the current stripe occupied by foe (transform into edge-coordinates)
2225  double foeRight, foeLeft;
2226  vehicles.getSublaneBorders(i, foeOffset, foeRight, foeLeft);
2227  const bool overlapBefore = overlap(rightVehSide, leftVehSide, foeRight, foeLeft);
2228  const bool overlapDest = overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft);
2229  const bool overlapAny = overlap(rightNoOverlap, leftNoOverlap, foeRight, foeLeft);
2230 #ifdef DEBUG_BLOCKING
2231  if (gDebugFlag2) {
2232  std::cout << " foe=" << vehDist.first->getID()
2233  << " gap=" << vehDist.second
2234  << " secGap=" << follower->getCarFollowModel().getSecureGap(follower->getSpeed(), leader->getSpeed(), leader->getCarFollowModel().getMaxDecel())
2235  << " foeRight=" << foeRight
2236  << " foeLeft=" << foeLeft
2237  << " overlapBefore=" << overlapBefore
2238  << " overlap=" << overlapAny
2239  << " overlapDest=" << overlapDest
2240  << "\n";
2241  }
2242 #endif
2243  if (overlapAny) {
2244  if (vehDist.second < 0) {
2245  if (overlapBefore && !overlapDest) {
2246 #ifdef DEBUG_BLOCKING
2247  if (gDebugFlag2) {
2248  std::cout << " ignoring current overlap to come clear\n";
2249  }
2250 #endif
2251  } else {
2252 #ifdef DEBUG_BLOCKING
2253  if (gDebugFlag2) {
2254  std::cout << " overlap (" << toString((LaneChangeAction)blockType) << ")\n";
2255  }
2256 #endif
2257  result |= (blockType | LCA_OVERLAPPING);
2258  if (collectBlockers == nullptr) {
2259  return result;
2260  } else {
2261  collectBlockers->push_back(vehDist);
2262  }
2263  }
2264  } else if (overlapDest || !myCanChangeFully) {
2265  // Estimate state after actionstep (follower may be accelerating!)
2266  // A comparison between secure gap depending on the expected speeds and the extrapolated gap
2267  // determines whether the s is blocking the lane change.
2268  // (Note that the longitudinal state update has already taken effect before LC dynamics (thus "-TS" below), would be affected by #3665)
2269 
2270  // Use conservative estimate for time until next action step
2271  // (XXX: how can the ego know the foe's action step length?)
2272  const double timeTillAction = MAX2(follower->getActionStepLengthSecs(), leader->getActionStepLengthSecs()) - TS;
2273  // Ignore decel for follower
2274  const double followerAccel = MAX2(0., follower->getAcceleration());
2275  const double leaderAccel = leader->getAcceleration();
2276  // Expected gap after next actionsteps
2277  const double expectedGap = MSCFModel::gapExtrapolation(timeTillAction, vehDist.second, leader->getSpeed(), follower->getSpeed(), leaderAccel, followerAccel, std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
2278 
2279  // Determine expected speeds and corresponding secure gap at the extrapolated timepoint
2280  const double followerExpectedSpeed = follower->getSpeed() + timeTillAction * followerAccel;
2281  const double leaderExpectedSpeed = MAX2(0., leader->getSpeed() + timeTillAction * leaderAccel);
2282  const double expectedSecureGap = follower->getCarFollowModel().getSecureGap(followerExpectedSpeed, leaderExpectedSpeed, leader->getCarFollowModel().getMaxDecel());
2283 
2284 #if defined(DEBUG_ACTIONSTEPS) && defined(DEBUG_BLOCKING)
2285  if (gDebugFlag2) {
2286  std::cout << " timeTillAction=" << timeTillAction
2287  << " followerAccel=" << followerAccel
2288  << " followerExpectedSpeed=" << followerExpectedSpeed
2289  << " leaderAccel=" << leaderAccel
2290  << " leaderExpectedSpeed=" << leaderExpectedSpeed
2291  << "\n gap=" << vehDist.second
2292  << " gapChange=" << (expectedGap - vehDist.second)
2293  << " expectedGap=" << expectedGap
2294  << " expectedSecureGap=" << expectedSecureGap
2295  << " safeLatGapLeft=" << safeLatGapLeft
2296  << " safeLatGapRight=" << safeLatGapRight
2297  << std::endl;
2298  }
2299 #endif
2300 
2301  // @note for euler-update, a different value for secureGap2 may be obtained when applying safetyFactor to followerDecel rather than secureGap
2302  const double secureGap2 = expectedSecureGap * getSafetyFactor();
2303  if (expectedGap < secureGap2) {
2304  // Foe is a blocker. Update lateral safe gaps accordingly.
2305  if (foeRight > leftVehSide) {
2306  safeLatGapLeft = MIN2(safeLatGapLeft, foeRight - leftVehSide);
2307  } else if (foeLeft < rightVehSide) {
2308  safeLatGapRight = MIN2(safeLatGapRight, rightVehSide - foeLeft);
2309  }
2310 
2311 #ifdef DEBUG_BLOCKING
2312  if (gDebugFlag2) {
2313  std::cout << " blocked by " << vehDist.first->getID() << " gap=" << vehDist.second << " expectedGap=" << expectedGap
2314  << " expectedSecureGap=" << expectedSecureGap << " secGap2=" << secureGap2 << " safetyFactor=" << getSafetyFactor()
2315  << " safeLatGapLeft=" << safeLatGapLeft << " safeLatGapRight=" << safeLatGapRight
2316  << "\n";
2317  }
2318 #endif
2319  result |= blockType;
2320  if (collectBlockers == nullptr) {
2321  return result;
2322  } else {
2323  collectBlockers->push_back(vehDist);
2324  }
2325 #ifdef DEBUG_BLOCKING
2326  } else if (gDebugFlag2 && expectedGap < expectedSecureGap) {
2327  std::cout << " ignore blocker " << vehDist.first->getID() << " gap=" << vehDist.second << " expectedGap=" << expectedGap
2328  << " expectedSecureGap=" << expectedSecureGap << " secGap2=" << secureGap2 << " safetyFactor=" << getSafetyFactor() << "\n";
2329 #endif
2330  }
2331  }
2332  }
2333  }
2334  }
2335  return result;
2336 
2337 }
2338 
2339 
2340 void
2341 MSLCM_SL2015::updateCFRelated(const MSLeaderDistanceInfo& vehicles, double foeOffset, bool leaders) {
2342  // to ensure that we do not ignore the wrong vehicles due to numerical
2343  // instability we slightly reduce the width
2344  const double vehWidth = myVehicle.getVehicleType().getWidth() - NUMERICAL_EPS;
2345  const double rightVehSide = myVehicle.getCenterOnEdge() - 0.5 * vehWidth;
2346  const double leftVehSide = rightVehSide + vehWidth;
2347 #ifdef DEBUG_BLOCKING
2348  if (gDebugFlag2) {
2349  std::cout << " updateCFRelated foeOffset=" << foeOffset << " vehicles=" << vehicles.toString() << "\n";
2350  }
2351 #endif
2352  for (int i = 0; i < vehicles.numSublanes(); ++i) {
2353  CLeaderDist vehDist = vehicles[i];
2354  if (vehDist.first != 0 && myCFRelated.count(vehDist.first) == 0) {
2355  double foeRight, foeLeft;
2356  vehicles.getSublaneBorders(i, foeOffset, foeRight, foeLeft);
2357  if (overlap(rightVehSide, leftVehSide, foeRight, foeLeft) && (vehDist.second >= 0
2358  // avoid deadlock due to #3729
2359  || (!leaders
2362  && vehDist.first->getSpeed() < SUMO_const_haltingSpeed
2363  && -vehDist.second < vehDist.first->getVehicleType().getMinGap()
2364  && &(myVehicle.getLane()->getEdge()) != &(vehDist.first->getLane()->getEdge()))
2365  )) {
2366 #ifdef DEBUG_BLOCKING
2367  if (gDebugFlag2) {
2368  std::cout << " ignoring cfrelated foe=" << vehDist.first->getID() << " gap=" << vehDist.second
2369  << " sublane=" << i
2370  << " foeOffset=" << foeOffset
2371  << " egoR=" << rightVehSide << " egoL=" << leftVehSide
2372  << " iR=" << foeRight << " iL=" << foeLeft
2373  << " egoV=" << myVehicle.getSpeed() << " foeV=" << vehDist.first->getSpeed()
2374  << " egoE=" << myVehicle.getLane()->getEdge().getID() << " egoE=" << vehDist.first->getLane()->getEdge().getID()
2375  << "\n";
2376  }
2377 #endif
2378  myCFRelated.insert(vehDist.first);
2379  }
2380  }
2381  }
2382 }
2383 
2384 
2385 bool
2386 MSLCM_SL2015::overlap(double right, double left, double right2, double left2) {
2387  assert(right <= left);
2388  assert(right2 <= left2);
2389  return left2 >= right + NUMERICAL_EPS && left >= right2 + NUMERICAL_EPS;
2390 }
2391 
2392 
2393 int
2394 MSLCM_SL2015::lowest_bit(int changeReason) {
2395  if ((changeReason & LCA_STRATEGIC) != 0) {
2396  return LCA_STRATEGIC;
2397  }
2398  if ((changeReason & LCA_COOPERATIVE) != 0) {
2399  return LCA_COOPERATIVE;
2400  }
2401  if ((changeReason & LCA_SPEEDGAIN) != 0) {
2402  return LCA_SPEEDGAIN;
2403  }
2404  if ((changeReason & LCA_KEEPRIGHT) != 0) {
2405  return LCA_KEEPRIGHT;
2406  }
2407  if ((changeReason & LCA_TRACI) != 0) {
2408  return LCA_TRACI;
2409  }
2410  return changeReason;
2411 }
2412 
2413 
2416  // ignore dummy decisions (returned if mayChange() failes)
2417  if (sd1.state == 0) {
2418  return sd2;
2419  } else if (sd2.state == 0) {
2420  return sd1;
2421  }
2422  // LCA_SUBLANE is special because LCA_STAY|LCA_SUBLANE may override another LCA_SUBLANE command
2423  const bool want1 = ((sd1.state & LCA_WANTS_LANECHANGE) != 0) || ((sd1.state & LCA_SUBLANE) != 0 && (sd1.state & LCA_STAY) != 0);
2424  const bool want2 = ((sd2.state & LCA_WANTS_LANECHANGE) != 0) || ((sd2.state & LCA_SUBLANE) != 0 && (sd2.state & LCA_STAY) != 0);
2425  const bool can1 = ((sd1.state & LCA_BLOCKED) == 0);
2426  const bool can2 = ((sd2.state & LCA_BLOCKED) == 0);
2427  int reason1 = lowest_bit(sd1.state & LCA_CHANGE_REASONS);
2428  int reason2 = lowest_bit(sd2.state & LCA_CHANGE_REASONS);
2429 #ifdef DEBUG_WANTSCHANGE
2430  if (DEBUG_COND) std::cout << SIMTIME
2431  << " veh=" << myVehicle.getID()
2432  << " state1=" << toString((LaneChangeAction)sd1.state)
2433  << " want1=" << (sd1.state & LCA_WANTS_LANECHANGE)
2434  << " dist1=" << sd1.latDist
2435  << " dir1=" << sd1.dir
2436  << " state2=" << toString((LaneChangeAction)sd2.state)
2437  << " want2=" << (sd2.state & LCA_WANTS_LANECHANGE)
2438  << " dist2=" << sd2.latDist
2439  << " dir2=" << sd2.dir
2440  << " reason1=" << reason1
2441  << " reason2=" << reason2
2442  << "\n";
2443 #endif
2444  if (want1) {
2445  if (want2) {
2446  // decide whether right or left has higher priority (lower value in enum LaneChangeAction)
2447  if (reason1 < reason2) {
2448  //if (DEBUG_COND) std::cout << " " << (sd1.state & LCA_CHANGE_REASONS) << " < " << (sd2.state & LCA_CHANGE_REASONS) << "\n";
2449  return (!can1 && can2 && sd1.sameDirection(sd2)) ? sd2 : sd1;
2450  //return sd1;
2451  } else if (reason1 > reason2) {
2452  //if (DEBUG_COND) std::cout << " " << (sd1.state & LCA_CHANGE_REASONS) << " > " << (sd2.state & LCA_CHANGE_REASONS) << "\n";
2453  return (!can2 && can1 && sd1.sameDirection(sd2)) ? sd1 : sd2;
2454  //return sd2;
2455  } else {
2456  // same priority.
2457  if ((sd1.state & LCA_SUBLANE) != 0) {
2458  // special treatment: prefer action with dir != 0
2459  if (sd1.dir == 0) {
2460  return sd2;
2461  } else if (sd2.dir == 0) {
2462  return sd1;
2463  } else {
2464  // prefer action that knows more about the desired direction
2465  // @note when deciding between right and left, right is always given as sd1
2466  assert(sd1.dir == -1);
2467  assert(sd2.dir == 1);
2468  if (sd1.latDist <= 0) {
2469  return sd1;
2470  } else if (sd2.latDist >= 0) {
2471  return sd2;
2472  }
2473  // when in doubt, prefer moving to the right
2474  return sd1.latDist <= sd2.latDist ? sd1 : sd2;
2475  }
2476  } else {
2477  // see which one is allowed
2478  return can1 ? sd1 : sd2;
2479  }
2480  }
2481  } else {
2482  return sd1;
2483  }
2484  } else {
2485  return sd2;
2486  }
2487 
2488 }
2489 
2490 
2492 MSLCM_SL2015::getLCA(int state, double latDist) {
2493  return ((latDist == 0 || (state & LCA_CHANGE_REASONS) == 0)
2494  ? LCA_NONE : (latDist < 0 ? LCA_RIGHT : LCA_LEFT));
2495 }
2496 
2497 
2498 int
2500  int laneOffset,
2501  const std::vector<MSVehicle::LaneQ>& preb,
2502  const MSLeaderDistanceInfo& leaders,
2503  const MSLeaderDistanceInfo& neighLeaders,
2504  int currIdx,
2505  int bestLaneOffset,
2506  bool changeToBest,
2507  double currentDist,
2508  double neighDist,
2509  double laDist,
2510  int roundaboutEdgesAhead,
2511  double latLaneDist,
2512  double& latDist
2513  ) {
2514  const bool right = (laneOffset == -1);
2515  const bool left = (laneOffset == 1);
2516  const MSVehicle::LaneQ& curr = preb[currIdx];
2517  const MSVehicle::LaneQ& neigh = preb[currIdx + laneOffset];
2518  const MSVehicle::LaneQ& best = preb[currIdx + bestLaneOffset];
2519 
2520  myLeftSpace = currentDist - myVehicle.getPositionOnLane();
2521  const double usableDist = (currentDist - myVehicle.getPositionOnLane() - best.occupation * JAM_FACTOR);
2522  //- (best.lane->getVehicleNumber() * neighSpeed)); // VARIANT 9 jfSpeed
2523  const double maxJam = MAX2(neigh.occupation, curr.occupation);
2524  const double neighLeftPlace = MAX2(0., neighDist - myVehicle.getPositionOnLane() - maxJam);
2525  // save the left space
2526 
2527 #ifdef DEBUG_STRATEGIC_CHANGE
2528  if (gDebugFlag2) {
2529  std::cout << SIMTIME
2530  << " veh=" << myVehicle.getID()
2531  << " laSpeed=" << myLookAheadSpeed
2532  << " laDist=" << laDist
2533  << " currentDist=" << currentDist
2534  << " usableDist=" << usableDist
2535  << " bestLaneOffset=" << bestLaneOffset
2536  << " best.length=" << best.length
2537  << " maxJam=" << maxJam
2538  << " neighLeftPlace=" << neighLeftPlace
2539  << " myLeftSpace=" << myLeftSpace
2540  << "\n";
2541  }
2542 #endif
2543 
2544  if (laneOffset != 0 && changeToBest && bestLaneOffset == curr.bestLaneOffset
2545  && currentDistDisallows(usableDist, bestLaneOffset, laDist)) {
2547  latDist = latLaneDist;
2548  ret |= LCA_STRATEGIC | LCA_URGENT;
2549  } else {
2550  // VARIANT_20 (noOvertakeRight)
2552  // check for slower leader on the left. we should not overtake but
2553  // rather move left ourselves (unless congested)
2554  // XXX only adapt as much as possible to get a lateral gap
2555  CLeaderDist cld = getSlowest(neighLeaders);
2556  const MSVehicle* nv = cld.first;
2557  if (nv->getSpeed() < myVehicle.getSpeed()) {
2558  const double vSafe = myCarFollowModel.followSpeed(
2559  &myVehicle, myVehicle.getSpeed(), cld.second, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel());
2560  addLCSpeedAdvice(vSafe);
2561  if (vSafe < myVehicle.getSpeed()) {
2563  }
2564 #ifdef DEBUG_STRATEGIC_CHANGE
2565  if (gDebugFlag2) {
2566  std::cout << SIMTIME
2567  << " avoid overtaking on the right nv=" << nv->getID()
2568  << " nvSpeed=" << nv->getSpeed()
2569  << " mySpeedGainProbabilityR=" << mySpeedGainProbabilityRight
2570  << " plannedSpeed=" << myVehicle.getSpeed() + ACCEL2SPEED(myLCAccelerationAdvices.back())
2571  << "\n";
2572  }
2573 #endif
2574  }
2575  }
2576 
2577  if (!changeToBest && (currentDistDisallows(neighLeftPlace, abs(bestLaneOffset) + 2, laDist))) {
2578  // the opposite lane-changing direction should be done than the one examined herein
2579  // we'll check whether we assume we could change anyhow and get back in time...
2580  //
2581  // this rule prevents the vehicle from moving in opposite direction of the best lane
2582  // unless the way till the end where the vehicle has to be on the best lane
2583  // is long enough
2584 #ifdef DEBUG_STRATEGIC_CHANGE
2585  if (gDebugFlag2) {
2586  std::cout << " veh=" << myVehicle.getID() << " could not change back and forth in time (1) neighLeftPlace=" << neighLeftPlace << "\n";
2587  }
2588 #endif
2589  ret |= LCA_STAY | LCA_STRATEGIC;
2590  } else if (
2591  laneOffset != 0
2592  && bestLaneOffset == 0
2593  && !leaders.hasStoppedVehicle()
2594  && neigh.bestContinuations.back()->getLinkCont().size() != 0
2595  && roundaboutEdgesAhead == 0
2596  && neighDist < TURN_LANE_DIST) {
2597  // VARIANT_21 (stayOnBest)
2598  // we do not want to leave the best lane for a lane which leads elsewhere
2599  // unless our leader is stopped or we are approaching a roundabout
2600 #ifdef DEBUG_STRATEGIC_CHANGE
2601  if (gDebugFlag2) {
2602  std::cout << " veh=" << myVehicle.getID() << " does not want to leave the bestLane (neighDist=" << neighDist << ")\n";
2603  }
2604 #endif
2605  ret |= LCA_STAY | LCA_STRATEGIC;
2606  } else if (right
2607  && bestLaneOffset == 0
2608  && myVehicle.getLane()->getSpeedLimit() > 80. / 3.6
2610  ) {
2611  // let's also regard the case where the vehicle is driving on a highway...
2612  // in this case, we do not want to get to the dead-end of an on-ramp
2613 #ifdef DEBUG_STRATEGIC_CHANGE
2614  if (gDebugFlag2) {
2615  std::cout << " veh=" << myVehicle.getID() << " does not want to get stranded on the on-ramp of a highway\n";
2616  }
2617 #endif
2618  ret |= LCA_STAY | LCA_STRATEGIC;
2619  }
2620  }
2621  if ((ret & LCA_URGENT) == 0 && getShadowLane() != nullptr &&
2622  // ignore overlap if it goes in the correct direction
2623  bestLaneOffset * myVehicle.getLateralPositionOnLane() <= 0) {
2624  // no decision or decision to stay
2625  // make sure to stay within lane bounds in case the shadow lane ends
2626  //const double requiredDist = MAX2(2 * myVehicle.getLateralOverlap(), getSublaneWidth()) / SUMO_const_laneWidth * laDist;
2627  const double requiredDist = 2 * myVehicle.getLateralOverlap() / SUMO_const_laneWidth * laDist;
2628  double currentShadowDist = -myVehicle.getPositionOnLane();
2629  MSLane* shadowPrev = nullptr;
2630  for (std::vector<MSLane*>::const_iterator it = curr.bestContinuations.begin(); it != curr.bestContinuations.end(); ++it) {
2631  if (*it == nullptr) {
2632  continue;
2633  }
2634  MSLane* shadow = getShadowLane(*it);
2635  if (shadow == nullptr || currentShadowDist >= requiredDist) {
2636  break;
2637  }
2638  if (shadowPrev != nullptr) {
2639  currentShadowDist += shadowPrev->getEdge().getInternalFollowingLengthTo(&shadow->getEdge());
2640  }
2641  currentShadowDist += shadow->getLength();
2642  shadowPrev = shadow;
2643 #ifdef DEBUG_STRATEGIC_CHANGE
2644  if (gDebugFlag2) {
2645  std::cout << " shadow=" << shadow->getID() << " currentShadowDist=" << currentShadowDist << "\n";
2646  }
2647 #endif
2648  }
2649 #ifdef DEBUG_STRATEGIC_CHANGE
2650  if (gDebugFlag2) {
2651  std::cout << " veh=" << myVehicle.getID() << " currentShadowDist=" << currentShadowDist << " requiredDist=" << requiredDist << " overlap=" << myVehicle.getLateralOverlap() << "\n";
2652  }
2653 #endif
2654  if (currentShadowDist < requiredDist && currentShadowDist < usableDist) {
2655  myLeftSpace = currentShadowDist;
2657 #ifdef DEBUG_STRATEGIC_CHANGE
2658  if (gDebugFlag2) {
2659  std::cout << " must change for shadowLane end latDist=" << latDist << " myLeftSpace=" << myLeftSpace << "\n";
2660  }
2661 #endif
2662  ret |= LCA_STRATEGIC | LCA_URGENT | LCA_STAY ;
2663  }
2664  }
2665 
2666  // check for overriding TraCI requests
2667 #if defined(DEBUG_STRATEGIC_CHANGE) || defined(DEBUG_TRACI)
2668  if (gDebugFlag2) {
2669  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " ret=" << ret;
2670  }
2671 #endif
2672  // store state before canceling
2673  getCanceledState(laneOffset) |= ret;
2674  int retTraCI = myVehicle.influenceChangeDecision(ret);
2675  if ((retTraCI & LCA_TRACI) != 0) {
2676  if ((retTraCI & LCA_STAY) != 0) {
2677  ret = retTraCI;
2678  latDist = 0;
2679  } else if (((retTraCI & LCA_RIGHT) != 0 && laneOffset < 0)
2680  || ((retTraCI & LCA_LEFT) != 0 && laneOffset > 0)) {
2681  ret = retTraCI;
2682  latDist = latLaneDist;
2683  }
2684  }
2685 #if defined(DEBUG_STRATEGIC_CHANGE) || defined(DEBUG_TRACI)
2686  if (gDebugFlag2) {
2687  std::cout << " reqAfterInfluence=" << ret << " ret=" << ret << "\n";
2688  }
2689 #endif
2690  return ret;
2691 }
2692 
2693 
2694 double
2696  return (state & LCA_STRATEGIC) != 0 ? MAX2(0.0, (1.0 - myPushy * (1 + 0.5 * myImpatience))) : 1.0;
2697 }
2698 
2699 
2700 int
2702  const MSLeaderDistanceInfo& leaders,
2703  const MSLeaderDistanceInfo& followers,
2704  const MSLeaderDistanceInfo& blockers,
2705  const MSLeaderDistanceInfo& neighLeaders,
2706  const MSLeaderDistanceInfo& neighFollowers,
2707  const MSLeaderDistanceInfo& neighBlockers,
2708  const MSLane& neighLane,
2709  int laneOffset,
2710  double& latDist,
2711  double& maneuverDist,
2712  int& blocked) {
2713 
2714  /* @notes
2715  * vehicles may need to compromise between fulfilling lane change objectives
2716  * (LCA_STRATEGIC, LCA_SPEED etc) and maintaining lateral gap. The minimum
2717  * acceptable lateral gap depends on
2718  * - the cultural context (China vs Europe)
2719  * - the driver agressiveness (willingness to encroach on other vehicles to force them to move laterally as well)
2720  * - see @note in checkBlocking
2721  * - the vehicle type (car vs motorcycle)
2722  * - the current speed
2723  * - the speed difference
2724  * - the importance / urgency of the desired maneuver
2725  *
2726  * the object of this method is to evaluate the above circumstances and
2727  * either:
2728  * - allow the current maneuver (state, latDist)
2729  * - to override the current maneuver with a distance-keeping maneuver
2730  *
2731  *
2732  * laneChangeModel/driver parameters
2733  * - bool pushy (willingness to encroach)
2734  * - float minGap at 100km/h (to be interpolated for lower speeds (assume 0 at speed 0)
2735  * - gapFactors (a factor for each of the change reasons
2736  *
2737  * further assumptions
2738  * - the maximum of egoSpeed and deltaSpeed can be used when interpolating minGap
2739  * - distance keeping to the edges of the road can be ignored (for now)
2740  *
2741  * currentMinGap = minGap * min(1.0, max(v, abs(v - vOther)) / 100) * gapFactor[lc_reason]
2742  *
2743  * */
2744 
2746  double gapFactor = computeGapFactor(state);
2747  const bool stayInLane = laneOffset == 0 || ((state & LCA_STRATEGIC) != 0 && (state & LCA_STAY) != 0);
2748  const double oldLatDist = latDist;
2749 
2750  // compute gaps after maneuver
2751  const double halfWidth = getWidth() * 0.5;
2752  // if the current maneuver is blocked we will stay where we are
2753  const double oldCenter = myVehicle.getCenterOnEdge();
2754  // surplus gaps. these are used to collect various constraints
2755  // if they do not permit the desired maneuvre, should override it to better maintain distance
2756  // stay within the current edge
2757  double surplusGapRight = oldCenter - halfWidth;
2758  double surplusGapLeft = myVehicle.getLane()->getEdge().getWidth() - oldCenter - halfWidth;
2759 #ifdef DEBUG_KEEP_LATGAP
2760  if (gDebugFlag2) {
2761  std::cout << "\n " << SIMTIME << " keepLatGap() laneOffset=" << laneOffset
2762  << " latDist=" << latDist
2763  << " state=" << toString((LaneChangeAction)state)
2764  << " blocked=" << toString((LaneChangeAction)blocked)
2765  << " gapFactor=" << gapFactor
2766  << " stayInLane=" << stayInLane << "\n"
2767  << " stayInEdge: surplusGapRight=" << surplusGapRight << " surplusGapLeft=" << surplusGapLeft << "\n";
2768  }
2769 #endif
2770  // staying within the edge overrides all minGap considerations
2771  if (surplusGapLeft < 0 || surplusGapRight < 0) {
2772  gapFactor = 0;
2773  }
2774 
2775  // maintain gaps to vehicles on the current lane
2776  // ignore vehicles that are too far behind
2777  const double netOverlap = -myVehicle.getVehicleType().getLength() * 0.5;
2778  updateGaps(leaders, myVehicle.getLane()->getRightSideOnEdge(), oldCenter, gapFactor, surplusGapRight, surplusGapLeft, true);
2779  updateGaps(followers, myVehicle.getLane()->getRightSideOnEdge(), oldCenter, gapFactor, surplusGapRight, surplusGapLeft, true, netOverlap);
2780 
2781  if (laneOffset != 0) {
2782  // maintain gaps to vehicles on the target lane
2783  updateGaps(neighLeaders, neighLane.getRightSideOnEdge(), oldCenter, gapFactor, surplusGapRight, surplusGapLeft, true);
2784  updateGaps(neighFollowers, neighLane.getRightSideOnEdge(), oldCenter, gapFactor, surplusGapRight, surplusGapLeft, true, netOverlap);
2785  }
2786 #ifdef DEBUG_KEEP_LATGAP
2787  if (gDebugFlag2) {
2788  std::cout << " minGapLat: surplusGapRight=" << surplusGapRight << " surplusGapLeft=" << surplusGapLeft << "\n"
2789  << " lastGaps: right=" << myLastLateralGapRight << " left=" << myLastLateralGapLeft << "\n";
2790  }
2791 #endif
2792  // we also need to track the physical gap, in addition to the psychological gap
2793  double physicalGapLeft = myLastLateralGapLeft == NO_NEIGHBOR ? surplusGapLeft : myLastLateralGapLeft;
2794  double physicalGapRight = myLastLateralGapRight == NO_NEIGHBOR ? surplusGapRight : myLastLateralGapRight;
2795 
2796  const double halfLaneWidth = myVehicle.getLane()->getWidth() * 0.5;
2797  if (stayInLane || laneOffset == 1) {
2798  // do not move past the right boundary of the current lane (traffic wasn't checked there)
2799  // but assume it's ok to be where we are in case we are already beyond
2800  surplusGapRight = MIN2(surplusGapRight, MAX2(0.0, halfLaneWidth + myVehicle.getLateralPositionOnLane() - halfWidth));
2801  physicalGapRight = MIN2(physicalGapRight, MAX2(0.0, halfLaneWidth + myVehicle.getLateralPositionOnLane() - halfWidth));
2802  }
2803  if (stayInLane || laneOffset == -1) {
2804  // do not move past the left boundary of the current lane (traffic wasn't checked there)
2805  // but assume it's ok to be where we are in case we are already beyond
2806  surplusGapLeft = MIN2(surplusGapLeft, MAX2(0.0, halfLaneWidth - myVehicle.getLateralPositionOnLane() - halfWidth));
2807  physicalGapLeft = MIN2(physicalGapLeft, MAX2(0.0, halfLaneWidth - myVehicle.getLateralPositionOnLane() - halfWidth));
2808  }
2809 #ifdef DEBUG_KEEP_LATGAP
2810  if (gDebugFlag2) {
2811  std::cout << " stayInLane: surplusGapRight=" << surplusGapRight << " surplusGapLeft=" << surplusGapLeft << "\n";
2812  }
2813 #endif
2814 
2815  if (surplusGapRight + surplusGapLeft < 0) {
2816  // insufficient lateral space to fulfill all requirements. apportion space proportionally
2817  if ((state & LCA_CHANGE_REASONS) == 0) {
2818  state |= LCA_SUBLANE;
2819  }
2820  const double equalDeficit = 0.5 * (surplusGapLeft + surplusGapRight);
2821  if (surplusGapRight < surplusGapLeft) {
2822  // shift further to the left but no further than there is physical space
2823  const double delta = MIN2(equalDeficit - surplusGapRight, physicalGapLeft);
2824  latDist = delta;
2825  maneuverDist = delta;
2826 #ifdef DEBUG_KEEP_LATGAP
2827  if (gDebugFlag2) {
2828  std::cout << " insufficient latSpace, move left: delta=" << delta << "\n";
2829  }
2830 #endif
2831  } else {
2832  // shift further to the right but no further than there is physical space
2833  const double delta = MIN2(equalDeficit - surplusGapLeft, physicalGapRight);
2834  latDist = -delta;
2835  maneuverDist = -delta;
2836 #ifdef DEBUG_KEEP_LATGAP
2837  if (gDebugFlag2) {
2838  std::cout << " insufficient latSpace, move right: delta=" << delta << "\n";
2839  }
2840 #endif
2841  }
2842  } else {
2843  // sufficient space. move as far as the gaps permit
2844  latDist = MAX2(MIN2(latDist, surplusGapLeft), -surplusGapRight);
2845  maneuverDist = MAX2(MIN2(maneuverDist, surplusGapLeft), -surplusGapRight);
2846 #ifdef DEBUG_KEEP_LATGAP
2847  if (gDebugFlag2) {
2848  std::cout << " adapted latDist=" << latDist << " maneuverDist=" << maneuverDist << " (old=" << oldLatDist << ")\n";
2849  }
2850 #endif
2851  }
2852  // take into account overriding traci sublane-request
2854  // @note: the influence is reset in MSAbstractLaneChangeModel::setOwnState at the end of the lane-changing code for this vehicle
2855  latDist = myVehicle.getInfluencer().getLatDist();
2856  maneuverDist = myVehicle.getInfluencer().getLatDist();
2857  state |= LCA_TRACI;
2858 #ifdef DEBUG_KEEP_LATGAP
2859  if (gDebugFlag2) {
2860  std::cout << " traci influenced latDist=" << latDist << "\n";
2861  }
2862 #endif
2863  }
2864  // if we cannot move in the desired direction, consider the maneuver blocked anyway
2865  const bool nonSublaneChange = (state & (LCA_STRATEGIC | LCA_COOPERATIVE | LCA_SPEEDGAIN | LCA_KEEPRIGHT)) != 0;
2866  const bool traciChange = (state & LCA_TRACI) != 0;
2867  if (nonSublaneChange && !traciChange) {
2868  if ((latDist < NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) && (oldLatDist > 0)) {
2869 #ifdef DEBUG_KEEP_LATGAP
2870  if (gDebugFlag2) {
2871  std::cout << " wanted changeToLeft oldLatDist=" << oldLatDist << ", blocked latGap changeToRight\n";
2872  }
2873 #endif
2874  latDist = oldLatDist; // restore old request for usage in decideDirection()
2875  blocked = LCA_OVERLAPPING | LCA_BLOCKED_LEFT;
2876  } else if ((latDist > -NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) && (oldLatDist < 0)) {
2877 #ifdef DEBUG_KEEP_LATGAP
2878  if (gDebugFlag2) {
2879  std::cout << " wanted changeToRight oldLatDist=" << oldLatDist << ", blocked latGap changeToLeft\n";
2880  }
2881 #endif
2882  latDist = oldLatDist; // restore old request for usage in decideDirection()
2883  blocked = LCA_OVERLAPPING | LCA_BLOCKED_RIGHT;
2884  }
2885  }
2886  // if we move, even though we wish to stay, update the change reason (except for TraCI)
2887  if (fabs(latDist) > NUMERICAL_EPS * myVehicle.getActionStepLengthSecs() && oldLatDist == 0) {
2888  state &= (~(LCA_CHANGE_REASONS | LCA_STAY) | LCA_TRACI);
2889  }
2890  // update blocked status
2891  if (fabs(latDist - oldLatDist) > NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) {
2892 #ifdef DEBUG_KEEP_LATGAP
2893  if (gDebugFlag2) {
2894  std::cout << " latDistUpdated=" << latDist << " oldLatDist=" << oldLatDist << "\n";
2895  }
2896 #endif
2897  blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset, leaders, followers, blockers, neighLeaders, neighFollowers, neighBlockers, nullptr, nullptr, nonSublaneChange);
2898  }
2899  if (fabs(latDist) > NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) {
2900  state = (state & ~LCA_STAY);
2901  if ((state & LCA_CHANGE_REASONS) == 0) {
2902  state |= LCA_SUBLANE;
2903  }
2904  } else {
2905  if ((state & LCA_SUBLANE) != 0) {
2906  state |= LCA_STAY;
2907  }
2908  // avoid setting blinker due to numerical issues
2909  latDist = 0;
2910  }
2911 #if defined(DEBUG_KEEP_LATGAP) || defined(DEBUG_STATE)
2912  if (gDebugFlag2) {
2913  std::cout << " latDist2=" << latDist
2914  << " state2=" << toString((LaneChangeAction)state)
2915  << " lastGapLeft=" << myLastLateralGapLeft
2916  << " lastGapRight=" << myLastLateralGapRight
2917  << " blockedAfter=" << toString((LaneChangeAction)blocked)
2918  << "\n";
2919  }
2920 #endif
2921  return state;
2922 }
2923 
2924 
2925 void
2926 MSLCM_SL2015::updateGaps(const MSLeaderDistanceInfo& others, double foeOffset, double oldCenter, double gapFactor,
2927  double& surplusGapRight, double& surplusGapLeft,
2928  bool saveMinGap, double netOverlap,
2929  double latDist,
2930  std::vector<CLeaderDist>* collectBlockers) {
2931  if (others.hasVehicles()) {
2932  const double halfWidth = getWidth() * 0.5 + NUMERICAL_EPS;
2933  const double baseMinGap = myVehicle.getVehicleType().getMinGapLat();
2934  for (int i = 0; i < others.numSublanes(); ++i) {
2935  if (others[i].first != 0 && others[i].second <= 0
2936  && myCFRelated.count(others[i].first) == 0
2937  && (netOverlap == 0 || others[i].second + others[i].first->getVehicleType().getMinGap() < netOverlap)) {
2939  const MSVehicle* foe = others[i].first;
2940  const double res = MSGlobals::gLateralResolution > 0 ? MSGlobals::gLateralResolution : others[i].first->getLane()->getWidth();
2941  double foeRight, foeLeft;
2942  others.getSublaneBorders(i, foeOffset, foeRight, foeLeft);
2943  const double foeCenter = foeRight + 0.5 * res;
2944  const double gap = MIN2(fabs(foeRight - oldCenter), fabs(foeLeft - oldCenter)) - halfWidth;
2945  const double deltaV = MIN2(LATGAP_SPEED_THRESHOLD, MAX3(LATGAP_SPEED_THRESHOLD2, myVehicle.getSpeed(), (double)fabs(myVehicle.getSpeed() - foe->getSpeed())));
2946  const double desiredMinGap = baseMinGap * deltaV / LATGAP_SPEED_THRESHOLD;
2947  const double currentMinGap = desiredMinGap * gapFactor; // pushy vehicles may accept a lower lateral gap temporarily
2948  /*
2949  if (netOverlap != 0) {
2950  // foe vehicle is follower with its front ahead of the ego midpoint
2951  // scale gap requirements so it gets lower for foe which are further behind ego
2952  //
2953  // relOverlap approaches 0 as the foe gets closer to the midpoint and it equals 1 if the foe is driving head-to-head
2954  const double relOverlap = 1 - (others[i].second + others[i].first->getVehicleType().getMinGap()) / netOverlap;
2955  currentMinGap *= currOverlap * relOverlap;
2956  }
2957  */
2958 #if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
2959  if (debugVehicle()) {
2960  std::cout << " updateGaps"
2961  << " i=" << i
2962  << " foe=" << foe->getID()
2963  << " foeRight=" << foeRight
2964  << " foeLeft=" << foeLeft
2965  << " oldCenter=" << oldCenter
2966  << " gap=" << others[i].second
2967  << " latgap=" << gap
2968  << " currentMinGap=" << currentMinGap
2969  << " surplusGapRight=" << surplusGapRight
2970  << " surplusGapLeft=" << surplusGapLeft
2971  << "\n";
2972  }
2973 #endif
2974 
2975  // If foe is maneuvering towards ego, reserve some additional distance.
2976  // But don't expect the foe to come closer than currentMinGap if it isn't already there.
2977  // (XXX: How can the ego know the foe's maneuver dist?)
2978  if (foeCenter < oldCenter) {
2979  const double foeManeuverDist = MAX2(0., foe->getLaneChangeModel().getManeuverDist());
2980  surplusGapRight = MIN3(surplusGapRight, gap - currentMinGap, MAX2(currentMinGap, gap - foeManeuverDist));
2981  } else {
2982  const double foeManeuverDist = -MIN2(0., foe->getLaneChangeModel().getManeuverDist());
2983  surplusGapLeft = MIN3(surplusGapLeft, gap - currentMinGap, MAX2(currentMinGap, gap - foeManeuverDist));
2984  }
2985  if (saveMinGap) {
2986  if (foeCenter < oldCenter) {
2987 #if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
2988  if (gDebugFlag2 && gap < myLastLateralGapRight) {
2989  std::cout << " new minimum rightGap=" << gap << "\n";
2990  }
2991 #endif
2993  } else {
2994 #if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
2995  if (gDebugFlag2 && gap < myLastLateralGapLeft) {
2996  std::cout << " new minimum leftGap=" << gap << "\n";
2997  }
2998 #endif
3000  }
3001  }
3002  if (collectBlockers != nullptr) {
3003  // check if the vehicle is blocking a desire lane change
3004  if ((foeCenter < oldCenter && latDist < 0 && gap < (desiredMinGap - latDist))
3005  || (foeCenter > oldCenter && latDist > 0 && gap < (desiredMinGap + latDist))) {
3006  collectBlockers->push_back(others[i]);
3007  }
3008  }
3009  }
3010  }
3011  }
3012 }
3013 
3014 
3015 double
3018 }
3019 
3020 
3021 double
3022 MSLCM_SL2015::computeSpeedLat(double latDist, double& maneuverDist) {
3023  int currentDirection = mySpeedLat >= 0 ? 1 : -1;
3024  int directionWish = latDist >= 0 ? 1 : -1;
3025  const double maxSpeedLat = MIN2(myVehicle.getVehicleType().getMaxSpeedLat(),
3027 
3028 #ifdef DEBUG_MANEUVER
3029  if (debugVehicle()) {
3030  std::cout << SIMTIME
3031  << " veh=" << myVehicle.getID()
3032  << " computeSpeedLat()"
3033  << " currentDirection=" << currentDirection
3034  << " directionWish=" << directionWish
3035  << std::endl;
3036  }
3037 #endif
3038  // reduced lateral speed (in the desired direction). Don't change direction against desired.
3039  double speedDecel;
3040  if (directionWish == 1) {
3041  speedDecel = MAX2(mySpeedLat - ACCEL2SPEED(myAccelLat), 0.);
3042  } else {
3043  speedDecel = MIN2(mySpeedLat + ACCEL2SPEED(myAccelLat), 0.);
3044  }
3045  // Eventually reduce lateral speed even more to ensure safety
3046  double speedDecelSafe = MAX2(MIN2(speedDecel, DIST2SPEED(mySafeLatDistLeft)), DIST2SPEED(-mySafeLatDistRight));
3047 
3048  // increased lateral speed (in the desired direction)
3049  double speedAccel = MAX2(MIN2(mySpeedLat + directionWish * ACCEL2SPEED(myAccelLat), maxSpeedLat), -maxSpeedLat);
3050  // increase lateral speed more strongly to ensure safety (when moving in the wrong direction)
3051  double speedAccelSafe = latDist * speedAccel >= 0 ? speedAccel : 0;
3052 
3053  // can we reach the target distance in a single step? (XXX: assumes "Euler" update)
3054  double speedBound = DIST2SPEED(latDist);
3055  // for lat-gap keeping maneuvres myOrigLatDist may be 0
3056  const double fullLatDist = latDist > 0 ? MIN2(mySafeLatDistLeft, MAX2(maneuverDist, latDist)) : MAX2(-mySafeLatDistRight, MIN2(maneuverDist, latDist));
3057 
3058  // update maneuverDist, if safety constraints apply in its direction
3059  if (maneuverDist * latDist > 0) {
3060  maneuverDist = fullLatDist;
3061  }
3062 
3063 #ifdef DEBUG_MANEUVER
3064  if (debugVehicle()) {
3065  std::cout << SIMTIME
3066  << " veh=" << myVehicle.getID()
3067  << " speedLat=" << mySpeedLat
3068  << " latDist=" << latDist
3069  << " maneuverDist=" << maneuverDist
3070  << " mySafeLatDistRight=" << mySafeLatDistRight
3071  << " mySafeLatDistLeft=" << mySafeLatDistLeft
3072  << " fullLatDist=" << fullLatDist
3073  << " speedAccel=" << speedAccel
3074  << " speedDecel=" << speedDecel
3075  << " speedBound=" << speedBound
3076  << std::endl;
3077  }
3078 #endif
3079  if (speedDecel * speedAccel <= 0 && (
3080  // speedAccel and speedDecel bracket speed 0. This means we can end the maneuver
3081  (latDist >= 0 && speedAccel >= speedBound && speedBound >= speedDecel)
3082  || (latDist <= 0 && speedAccel <= speedBound && speedBound <= speedDecel))) {
3083  // we can reach the desired value in this step
3084 #ifdef DEBUG_MANEUVER
3085  if (debugVehicle()) {
3086  std::cout << " computeSpeedLat a)\n";
3087  }
3088 #endif
3089  return speedBound;
3090  }
3091  // are we currently moving in the wrong direction?
3092  if (latDist * mySpeedLat < 0) {
3093 #ifdef DEBUG_MANEUVER
3094  if (debugVehicle()) {
3095  std::cout << " computeSpeedLat b)\n";
3096  }
3097 #endif
3098  return speedAccelSafe;
3099  }
3100  // check if the remaining distance allows to accelerate laterally
3101  double minDistAccel = SPEED2DIST(speedAccel) + currentDirection * MSCFModel::brakeGapEuler(fabs(speedAccel), myAccelLat, 0); // most we can move in the target direction
3102  if ((fabs(minDistAccel) < fabs(fullLatDist)) || (fabs(minDistAccel - fullLatDist) < NUMERICAL_EPS)) {
3103 #ifdef DEBUG_MANEUVER
3104  if (debugVehicle()) {
3105  std::cout << " computeSpeedLat c)\n";
3106  }
3107 #endif
3108  return speedAccel;
3109  } else {
3110 #ifdef DEBUG_MANEUVER
3111  if (debugVehicle()) {
3112  std::cout << " minDistAccel=" << minDistAccel << "\n";
3113  }
3114 #endif
3115  // check if the remaining distance allows to maintain current lateral speed
3116  double minDistCurrent = SPEED2DIST(mySpeedLat) + currentDirection * MSCFModel::brakeGapEuler(fabs(mySpeedLat), myAccelLat, 0);
3117  if ((fabs(minDistCurrent) < fabs(fullLatDist)) || (fabs(minDistCurrent - fullLatDist) < NUMERICAL_EPS)) {
3118 #ifdef DEBUG_MANEUVER
3119  if (debugVehicle()) {
3120  std::cout << " computeSpeedLat d)\n";
3121  }
3122 #endif
3123  return mySpeedLat;
3124  }
3125  }
3126  // reduce lateral speed
3127 #ifdef DEBUG_MANEUVER
3128  if (debugVehicle()) {
3129  std::cout << " computeSpeedLat e)\n";
3130  }
3131 #endif
3132  return speedDecelSafe;
3133 }
3134 
3135 
3136 void
3137 MSLCM_SL2015::commitManoeuvre(int blocked, int blockedFully,
3138  const MSLeaderDistanceInfo& leaders,
3139  const MSLeaderDistanceInfo& neighLeaders,
3140  const MSLane& neighLane,
3141  double maneuverDist) {
3142  if (!blocked && !blockedFully && !myCanChangeFully) {
3143  // round to full action steps
3144  double secondsToLeaveLane;
3146  secondsToLeaveLane = ceil(fabs(maneuverDist) / myVehicle.getVehicleType().getMaxSpeedLat() / myVehicle.getActionStepLengthSecs()) * myVehicle.getActionStepLengthSecs();
3147  // XXX myAccelLat must be taken into account (refs #3601, see ballistic case for solution)
3148 
3149  // XXX This also causes probs: if the difference between the current speed and the committed is higher than the maximal decel,
3150  // the vehicle may pass myLeftSpace before completing the maneuver.
3151  myCommittedSpeed = MIN3(myLeftSpace / secondsToLeaveLane,
3154 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3155  if (debugVehicle()) {
3156  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " myCommittedSpeed=" << myCommittedSpeed << " leftSpace=" << myLeftSpace << " secondsToLeave=" << secondsToLeaveLane << "\n";
3157  }
3158 #endif
3159  } else {
3160 
3161  // Calculate seconds needed for leaving lane assuming start from lateral speed zero, and lat.accel == -lat.decel
3162  secondsToLeaveLane = MSCFModel::estimateArrivalTime(fabs(maneuverDist), 0., 0., myVehicle.getVehicleType().getMaxSpeedLat(), myAccelLat, myAccelLat);
3163  // round to full action steps
3164  secondsToLeaveLane = ceil(secondsToLeaveLane / myVehicle.getActionStepLengthSecs()) * myVehicle.getActionStepLengthSecs();
3165 
3166  // committed speed will eventually be pushed into a drive item during the next planMove() step. This item
3167  // will not be read before the next action step at current time + actionStepLength-TS, so we need to schedule the corresponding speed.
3168  const double timeTillActionStep = myVehicle.getActionStepLengthSecs() - TS;
3169  const double nextActionStepSpeed = MAX2(0., myVehicle.getSpeed() + timeTillActionStep * myVehicle.getAcceleration());
3170  double nextLeftSpace;
3171  if (nextActionStepSpeed > 0.) {
3172  nextLeftSpace = myLeftSpace - timeTillActionStep * (myVehicle.getSpeed() + nextActionStepSpeed) * 0.5;
3173  } else if (myVehicle.getAcceleration() == 0) {
3174  nextLeftSpace = myLeftSpace;
3175  } else {
3176  assert(myVehicle.getAcceleration() < 0.);
3177  nextLeftSpace = myLeftSpace + (myVehicle.getSpeed() * myVehicle.getSpeed() / myVehicle.getAcceleration()) * 0.5;
3178  }
3179  const double avoidArrivalSpeed = nextActionStepSpeed + ACCEL2SPEED(MSCFModel::avoidArrivalAccel(
3180  nextLeftSpace, secondsToLeaveLane - timeTillActionStep, nextActionStepSpeed, myVehicle.getCarFollowModel().getEmergencyDecel()));
3181 
3182  myCommittedSpeed = MIN3(avoidArrivalSpeed,
3185 
3186 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3187  if (gDebugFlag2) {
3188  std::cout << SIMTIME
3189  << " veh=" << myVehicle.getID()
3190  << " avoidArrivalSpeed=" << avoidArrivalSpeed
3191  << " currentSpeed=" << myVehicle.getSpeed()
3192  << " myLeftSpace=" << myLeftSpace
3193  << "\n nextLeftSpace=" << nextLeftSpace
3194  << " nextActionStepSpeed=" << nextActionStepSpeed
3195  << " nextActionStepRemainingSeconds=" << secondsToLeaveLane - timeTillActionStep
3196  << "\n";
3197  }
3198 #endif
3199  }
3200  myCommittedSpeed = commitFollowSpeed(myCommittedSpeed, maneuverDist, secondsToLeaveLane, leaders, myVehicle.getLane()->getRightSideOnEdge());
3201  myCommittedSpeed = commitFollowSpeed(myCommittedSpeed, maneuverDist, secondsToLeaveLane, neighLeaders, neighLane.getRightSideOnEdge());
3202  if (myCommittedSpeed < myVehicle.getCarFollowModel().minNextSpeed(myVehicle.getSpeed(), &myVehicle)) {
3203  myCommittedSpeed = 0;
3204  }
3205 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3206  if (gDebugFlag2) {
3207  std::cout << SIMTIME
3208  << " veh=" << myVehicle.getID()
3209  << " secondsToLeave=" << secondsToLeaveLane
3211  << " committed=" << myCommittedSpeed
3212  << "\n";
3213  }
3214 #endif
3215  }
3216 }
3217 
3218 double
3219 MSLCM_SL2015::commitFollowSpeed(double speed, double latDist, double secondsToLeaveLane, const MSLeaderDistanceInfo& leaders, double foeOffset) const {
3220  if (leaders.hasVehicles()) {
3221  // we distinguish 3 cases
3222  // - vehicles with lateral overlap at the end of the maneuver: try to follow safely
3223  // - vehicles with overlap at the start of the maneuver: avoid collision within secondsToLeaveLane
3224  // - vehicles without overlap: ignore
3225 
3226  const double maxDecel = myVehicle.getCarFollowModel().getMaxDecel();
3227  // temporarily use another decel value
3228  MSCFModel& cfmodel = const_cast<MSCFModel&>(myVehicle.getCarFollowModel());
3229  cfmodel.setMaxDecel(maxDecel / getSafetyFactor());
3230 
3231  const double vehWidth = getWidth();
3232  const double rightVehSide = myVehicle.getCenterOnEdge() - 0.5 * vehWidth;
3233  const double leftVehSide = rightVehSide + vehWidth;
3234  const double rightVehSideDest = rightVehSide + latDist;
3235  const double leftVehSideDest = leftVehSide + latDist;
3236 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3237  if (gDebugFlag2) {
3238  std::cout << " commitFollowSpeed"
3239  << " latDist=" << latDist
3240  << " foeOffset=" << foeOffset
3241  << " vehRight=" << rightVehSide
3242  << " vehLeft=" << leftVehSide
3243  << " destRight=" << rightVehSideDest
3244  << " destLeft=" << leftVehSideDest
3245  << "\n";
3246  }
3247 #endif
3248  for (int i = 0; i < leaders.numSublanes(); ++i) {
3249  CLeaderDist vehDist = leaders[i];
3250  if (vehDist.first != 0) {
3251  const MSVehicle* leader = vehDist.first;
3252  // only check the current stripe occuped by foe (transform into edge-coordinates)
3253  double foeRight, foeLeft;
3254  leaders.getSublaneBorders(i, foeOffset, foeRight, foeLeft);
3255 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3256  if (gDebugFlag2) {
3257  std::cout << " foe=" << vehDist.first->getID()
3258  << " gap=" << vehDist.second
3259  << " secGap=" << myVehicle.getCarFollowModel().getSecureGap(myVehicle.getSpeed(), leader->getSpeed(), leader->getCarFollowModel().getMaxDecel())
3260  << " foeRight=" << foeRight
3261  << " foeLeft=" << foeLeft
3262  << " overlapBefore=" << overlap(rightVehSide, leftVehSide, foeRight, foeLeft)
3263  << " overlapDest=" << overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)
3264  << "\n";
3265  }
3266 #endif
3267  if (overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)) {
3268  // case 1
3269  const double vSafe = myVehicle.getCarFollowModel().followSpeed(
3270  &myVehicle, speed, vehDist.second, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel());
3271  speed = MIN2(speed, vSafe);
3272 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3273  if (gDebugFlag2) {
3274  std::cout << " case1 vsafe=" << vSafe << " speed=" << speed << "\n";
3275  }
3276 #endif
3277  } else if (overlap(rightVehSide, leftVehSide, foeRight, foeLeft)) {
3278  // case 2
3279  const double vSafe = myVehicle.getCarFollowModel().followSpeedTransient(
3280  secondsToLeaveLane,
3281  &myVehicle, speed, vehDist.second, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel());
3282  speed = MIN2(speed, vSafe);
3283 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3284  if (gDebugFlag2) {
3285  std::cout << " case2 vsafe=" << vSafe << " speed=" << speed << "\n";
3286  }
3287 #endif
3288  }
3289  }
3290  }
3291  // restore original deceleration
3292  cfmodel.setMaxDecel(maxDecel);
3293 
3294  }
3295  return speed;
3296 }
3297 
3298 double
3300  return 1 / ((1 + 0.5 * myImpatience) * myAssertive);
3301 }
3302 
3303 
3304 std::string
3305 MSLCM_SL2015::getParameter(const std::string& key) const {
3307  return toString(myStrategicParam);
3308  } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_PARAM)) {
3309  return toString(myCooperativeParam);
3310  } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_PARAM)) {
3311  return toString(mySpeedGainParam);
3312  } else if (key == toString(SUMO_ATTR_LCA_KEEPRIGHT_PARAM)) {
3313  return toString(myKeepRightParam);
3314  } else if (key == toString(SUMO_ATTR_LCA_SUBLANE_PARAM)) {
3315  return toString(mySublaneParam);
3316  } else if (key == toString(SUMO_ATTR_LCA_PUSHY)) {
3317  return toString(myPushy);
3318  } else if (key == toString(SUMO_ATTR_LCA_ASSERTIVE)) {
3319  return toString(myAssertive);
3320  } else if (key == toString(SUMO_ATTR_LCA_IMPATIENCE)) {
3321  return toString(myImpatience);
3322  } else if (key == toString(SUMO_ATTR_LCA_TIME_TO_IMPATIENCE)) {
3323  return toString(myTimeToImpatience);
3324  } else if (key == toString(SUMO_ATTR_LCA_ACCEL_LAT)) {
3325  return toString(myAccelLat);
3326  } else if (key == toString(SUMO_ATTR_LCA_LOOKAHEADLEFT)) {
3327  return toString(myLookaheadLeft);
3328  } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAINRIGHT)) {
3329  return toString(mySpeedGainRight);
3330  }
3331  throw InvalidArgument("Parameter '" + key + "' is not supported for laneChangeModel of type '" + toString(myModel) + "'");
3332 }
3333 
3334 void
3335 MSLCM_SL2015::setParameter(const std::string& key, const std::string& value) {
3336  double doubleValue;
3337  try {
3338  doubleValue = StringUtils::toDouble(value);
3339  } catch (NumberFormatException&) {
3340  throw InvalidArgument("Setting parameter '" + key + "' requires a number for laneChangeModel of type '" + toString(myModel) + "'");
3341  }
3343  myStrategicParam = doubleValue;
3344  } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_PARAM)) {
3345  myCooperativeParam = doubleValue;
3346  } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_PARAM)) {
3347  mySpeedGainParam = doubleValue;
3348  } else if (key == toString(SUMO_ATTR_LCA_KEEPRIGHT_PARAM)) {
3349  myKeepRightParam = doubleValue;
3350  } else if (key == toString(SUMO_ATTR_LCA_SUBLANE_PARAM)) {
3351  mySublaneParam = doubleValue;
3352  } else if (key == toString(SUMO_ATTR_LCA_PUSHY)) {
3353  myPushy = doubleValue;
3354  } else if (key == toString(SUMO_ATTR_LCA_ASSERTIVE)) {
3355  myAssertive = doubleValue;
3356  } else if (key == toString(SUMO_ATTR_LCA_IMPATIENCE)) {
3357  myImpatience = doubleValue;
3358  myMinImpatience = doubleValue;
3359  } else if (key == toString(SUMO_ATTR_LCA_TIME_TO_IMPATIENCE)) {
3360  myTimeToImpatience = doubleValue;
3361  } else if (key == toString(SUMO_ATTR_LCA_ACCEL_LAT)) {
3362  myAccelLat = doubleValue;
3363  } else if (key == toString(SUMO_ATTR_LCA_TURN_ALIGNMENT_DISTANCE)) {
3364  myTurnAlignmentDist = doubleValue;
3365  } else if (key == toString(SUMO_ATTR_LCA_LOOKAHEADLEFT)) {
3366  myLookaheadLeft = doubleValue;
3367  } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAINRIGHT)) {
3368  mySpeedGainRight = doubleValue;
3369  } else {
3370  throw InvalidArgument("Setting parameter '" + key + "' is not supported for laneChangeModel of type '" + toString(myModel) + "'");
3371  }
3373 }
3374 
3375 
3376 int
3378  int laneOffset,
3380  int blocked,
3381  const std::pair<MSVehicle*, double>& leader,
3382  const std::pair<MSVehicle*, double>& neighLead,
3383  const std::pair<MSVehicle*, double>& neighFollow,
3384  const MSLane& neighLane,
3385  const std::vector<MSVehicle::LaneQ>& preb,
3386  MSVehicle** lastBlocked,
3387  MSVehicle** firstBlocked) {
3388 
3389  const LaneChangeAction alternatives = LCA_NONE; // @todo pas this data
3390 
3391 #ifdef DEBUG_WANTSCHANGE
3392  if (DEBUG_COND) {
3393  std::cout << "\nWANTS_CHANGE\n" << SIMTIME
3394  //<< std::setprecision(10)
3395  << " veh=" << myVehicle.getID()
3396  << " lane=" << myVehicle.getLane()->getID()
3397  << " pos=" << myVehicle.getPositionOnLane()
3398  << " posLat=" << myVehicle.getLateralPositionOnLane()
3399  << " speed=" << myVehicle.getSpeed()
3400  << " considerChangeTo=" << (laneOffset == -1 ? "right" : "left")
3401  << "\n";
3402  }
3403 #endif
3404 
3405  double latDist = 0;
3406  const MSLane* dummy = myVehicle.getLane();
3407  MSLeaderDistanceInfo leaders(leader, dummy);
3408  MSLeaderDistanceInfo followers(std::make_pair((MSVehicle*)nullptr, -1), dummy);
3409  MSLeaderDistanceInfo blockers(std::make_pair((MSVehicle*)nullptr, -1), dummy);
3410  MSLeaderDistanceInfo neighLeaders(neighLead, dummy);
3411  MSLeaderDistanceInfo neighFollowers(neighFollow, dummy);
3412  MSLeaderDistanceInfo neighBlockers(std::make_pair((MSVehicle*)nullptr, -1), dummy);
3413 
3414  double maneuverDist;
3415  int result = _wantsChangeSublane(laneOffset,
3416  alternatives,
3417  leaders, followers, blockers,
3418  neighLeaders, neighFollowers, neighBlockers,
3419  neighLane, preb,
3420  lastBlocked, firstBlocked, latDist, maneuverDist, blocked);
3421 
3422  myManeuverDist = 0;
3423  myCanChangeFully = true;
3424  // ignore sublane motivation
3425  result &= ~LCA_SUBLANE;
3426  result |= getLCA(result, latDist);
3427 
3428 #if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE)
3429  if (DEBUG_COND) {
3430  if (result & LCA_WANTS_LANECHANGE) {
3431  std::cout << SIMTIME
3432  << " veh=" << myVehicle.getID()
3433  << " wantsChangeTo=" << (laneOffset == -1 ? "right" : "left")
3434  << ((result & LCA_URGENT) ? " (urgent)" : "")
3435  << ((result & LCA_CHANGE_TO_HELP) ? " (toHelp)" : "")
3436  << ((result & LCA_STRATEGIC) ? " (strat)" : "")
3437  << ((result & LCA_COOPERATIVE) ? " (coop)" : "")
3438  << ((result & LCA_SPEEDGAIN) ? " (speed)" : "")
3439  << ((result & LCA_KEEPRIGHT) ? " (keepright)" : "")
3440  << ((result & LCA_TRACI) ? " (traci)" : "")
3441  << ((blocked & LCA_BLOCKED) ? " (blocked)" : "")
3442  << ((blocked & LCA_OVERLAPPING) ? " (overlap)" : "")
3443  << "\n\n\n";
3444  }
3445  }
3446 #endif
3447 
3448  return result;
3449 }
3450 
3451 /****************************************************************************/
void * inform(void *info, MSVehicle *sender)
The link is a partial left direction.
double getLengthWithGap() const
Get vehicle&#39;s length including the minimum gap [m].
static double gLateralResolution
Definition: MSGlobals.h:85
void initDerivedParameters()
init cached parameters derived directly from model parameters
#define DIST2SPEED(x)
Definition: SUMOTime.h:49
const std::vector< double > getSubLaneSides() const
Returns the right side offsets of this edge&#39;s sublanes.
Definition: MSEdge.h:558
double myLeftSpace
Definition: MSLCM_SL2015.h:354
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:313
saves leader/follower vehicles and their distances relative to an ego vehicle
Definition: MSLeaderInfo.h:133
double myChangeProbThresholdLeft
Definition: MSLCM_SL2015.h:415
MSEdge & getEdge() const
Returns the lane&#39;s edge.
Definition: MSLane.h:670
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:80
long long int SUMOTime
Definition: SUMOTime.h:35
void msg(const CLeaderDist &cld, double speed, int state)
send a speed recommendation to the given vehicle
void setParameter(const std::string &key, const std::string &value)
try to set the given parameter for this laneChangeModel. Throw exception for unsupported key ...
double mySpeedGainProbabilityLeft
a value for tracking the probability that a change to the left is beneficial
Definition: MSLCM_SL2015.h:346
The action is due to the default of keeping right "Rechtsfahrgebot".
#define SPEED2DIST(x)
Definition: SUMOTime.h:47
The action is done to help someone else.
#define RELGAIN_NORMALIZATION_MIN_SPEED
std::set< const MSVehicle * > myCFRelated
set of vehicles that are in a car-following relationship with ego (leader of followers) ...
Definition: MSLCM_SL2015.h:381
double getWaitingSeconds() const
Returns the number of seconds waited (speed was lesser than 0.1m/s)
Definition: MSVehicle.h:657
virtual bool hasPedestrians(const MSLane *lane)
whether the given lane has pedestrians on it
Definition: MSPModel.h:85
double getSafetyFactor() const
return factor for modifying the safety constraints of the car-following model
#define MIN_FALLBEHIND
virtual std::string toString() const
print a debugging representation
double mySpeedGainParam
Definition: MSLCM_SL2015.h:388
#define ACCEL2SPEED(x)
Definition: SUMOTime.h:53
Stop & getNextStop()
Definition: MSVehicle.cpp:5807
#define ROUNDABOUT_DIST_BONUS
int myPreviousState
lane changing state from the previous simulation step
double informLeaders(int blocked, int dir, const std::vector< CLeaderDist > &blockers, double remainingSeconds)
MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:561
bool myCanChangeFully
whether the current lane changing maneuver can be finished in a single step
Definition: MSLCM_SL2015.h:374
double myLookaheadLeft
Definition: MSLCM_SL2015.h:405
double myKeepRightParam
Definition: MSLCM_SL2015.h:389
double computeGapFactor(int state) const
compute the gap factor for the given state
virtual double minNextSpeed(double speed, const MSVehicle *const veh=0) const
Returns the minimum speed given the current speed (depends on the numerical update scheme and its ste...
Definition: MSCFModel.cpp:245
int bestLaneOffset
The (signed) number of lanes to be crossed to get to the lane which allows to continue the drive...
Definition: MSVehicle.h:823
The vehicle is blocked by left follower.
At the leftmost side of the lane.
int gPrecision
the precision for floating point outputs
Definition: StdDefs.cpp:27
LateralAlignment getPreferredLateralAlignment() const
Get vehicle&#39;s preferred lateral alignment.
double getManeuverDist() const
Returns the remaining unblocked distance for the current maneuver. (only used by sublane model) ...
int checkBlockingVehicles(const MSVehicle *ego, const MSLeaderDistanceInfo &vehicles, double latDist, double foeOffset, bool leaders, LaneChangeAction blockType, double &safeLatGapRight, double &safeLatGapLeft, std::vector< CLeaderDist > *collectBlockers=0) const
check whether any of the vehicles overlaps with ego
const double SUMO_const_laneWidth
Definition: StdDefs.h:50
static int lowest_bit(int changeReason)
return the most important change reason
The car-following model abstraction.
Definition: MSCFModel.h:57
double computeSpeedLat(double latDist, double &maneuverDist)
decides the next lateral speed depending on the remaining lane change distance to be covered and upda...
int checkBlocking(const MSLane &neighLane, double &latDist, double &maneuverDist, int laneOffset, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &followers, const MSLeaderDistanceInfo &blockers, const MSLeaderDistanceInfo &neighLeaders, const MSLeaderDistanceInfo &neighFollowers, const MSLeaderDistanceInfo &neighBlockers, std::vector< CLeaderDist > *collectLeadBlockers=0, std::vector< CLeaderDist > *collectFollowBlockers=0, bool keepLatGapManeuver=false, double gapFactor=0, int *retBlockedFully=0)
restrict latDist to permissible speed and determine blocking state depending on that distance ...
double myStrategicParam
Definition: MSLCM_SL2015.h:386
double getPositionOnLane() const
Get the vehicle&#39;s position along the lane.
Definition: MSVehicle.h:397
double mySpeedLossProbThreshold
Definition: MSLCM_SL2015.h:417
const SUMOVehicleParameter & getParameter() const
Returns the vehicle&#39;s parameter (including departure definition)
void setOwnState(const int state)
static CLeaderDist getSlowest(const MSLeaderDistanceInfo &ldi)
get the slowest vehicle in the given info
int getBestLaneOffset() const
Definition: MSVehicle.cpp:5016
The link is a 180 degree turn.
double myPreviousManeuverDist
Maneuver distance from the previous simulation step.
Wants go to the right.
ArrivalPosLatDefinition arrivalPosLatProcedure
Information how the vehicle shall choose the lateral arrival position.
double lateralDistanceToLane(const int offset) const
Get the minimal lateral distance required to move fully onto the lane at given offset.
Definition: MSVehicle.cpp:5366
double arrivalPosLat
(optional) The lateral position the vehicle shall arrive on
std::vector< double > myExpectedSublaneSpeeds
expected travel speeds on all sublanes on the current edge(!)
Definition: MSLCM_SL2015.h:365
virtual double maxNextSpeed(double speed, const MSVehicle *const veh) const
Returns the maximum speed given the current speed.
Definition: MSCFModel.cpp:239
double myTimeToImpatience
Definition: MSLCM_SL2015.h:399
const std::vector< MSLane * > & getLanes() const
Returns this edge&#39;s lanes.
Definition: MSEdge.h:165
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:168
T MAX2(T a, T b)
Definition: StdDefs.h:80
The position is given.
double getLength() const
Returns the lane&#39;s length.
Definition: MSLane.h:541
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:70
const MSRoute & getRoute() const
Returns the current route.
double getMinGapLat() const
Get the minimum lateral gap that vehicles of this type maintain.
static double avoidArrivalAccel(double dist, double time, double speed, double maxDecel)
Computes the acceleration needed to arrive not before the given time.
Definition: MSCFModel.cpp:460
const MSEdge * getLastEdge() const
returns the destination edge
Definition: MSRoute.cpp:88
const std::string & getID() const
Returns the id.
Definition: Named.h:77
bool sameDirection(const StateAndDist &other) const
align with the closest sublane border
drive on the right side
#define TS
Definition: SUMOTime.h:44
double length
The overall length which may be driven when using this lane without a lane change.
Definition: MSVehicle.h:815
Wants go to the left.
bool debugVehicle() const
whether the current vehicles shall be debugged
The action is due to the wish to be faster (tactical lc)
#define SPEED2ACCEL(x)
Definition: SUMOTime.h:55
double getWidth() const
Returns the lane&#39;s width.
Definition: MSLane.h:557
T MAX3(T a, T b, T c)
Definition: StdDefs.h:94
#define UNUSED_PARAMETER(x)
Definition: StdDefs.h:32
MSLCM_SL2015(MSVehicle &v)
used by the sublane model
#define LCA_RIGHT_IMPATIENCE
The link is a (hard) left direction.
double myTurnAlignmentDist
Definition: MSLCM_SL2015.h:403
LateralAlignment
Numbers representing special SUMO-XML-attribute values Information how vehicles align themselves with...
double myChangeProbThresholdRight
Definition: MSLCM_SL2015.h:413
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:4609
#define SIMTIME
Definition: SUMOTime.h:64
int keepLatGap(int state, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &followers, const MSLeaderDistanceInfo &blockers, const MSLeaderDistanceInfo &neighLeaders, const MSLeaderDistanceInfo &neighFollowers, const MSLeaderDistanceInfo &neighBlockers, const MSLane &neighLane, int laneOffset, double &latDist, double &maneuverDist, int &blocked)
check whether lateral gap requirements are met override the current maneuver if necessary ...
#define MAX_ONRAMP_LENGTH
#define GAIN_PERCEPTION_THRESHOLD
#define ARRIVALPOS_LAT_THRESHOLD
The link is a straight direction.
double mySafeLatDistRight
the lateral distance the vehicle can safely move in the currently considered direction ...
Definition: MSLCM_SL2015.h:377
double getMaxAccel() const
Get the vehicle type&#39;s maximum acceleration [m/s^2].
Definition: MSCFModel.h:210
Needs to stay on the current lane.
void saveBlockerLength(const MSVehicle *blocker, int lcaCounter)
save space for vehicles which need to counter-lane-change
const LaneChangeModel myModel
the type of this model
double changeRequestRemainingSeconds(const SUMOTime currentTime) const
Return the remaining number of seconds of the current laneTimeLine assuming one exists.
Definition: MSVehicle.cpp:750
drive on the left side
double getWidth() const
return the widht of this vehicle (padded for numerical stability)
static bool overlap(double right, double left, double right2, double left2)
return whether the given intervals overlap
static bool myAllowOvertakingRight
whether overtaking on the right is permitted
reasons of lane change
A class responsible for exchanging messages between cars involved in lane-change interaction.
StateAndDist decideDirection(StateAndDist sd1, StateAndDist sd2) const
decide in which direction to move in case both directions are desirable
A road/street connecting two junctions.
Definition: MSEdge.h:76
void commitManoeuvre(int blocked, int blockedFully, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &neighLeaders, const MSLane &neighLane, double maneuverDist)
commit to lane change maneuvre potentially overriding safe speed
double myLeadingBlockerLength
Definition: MSLCM_SL2015.h:353
const MSCFModel & getCarFollowModel() const
Returns the vehicle&#39;s car following model definition.
Definition: MSVehicle.h:894
void prepareStep()
int getIndex() const
Returns the lane&#39;s index.
Definition: MSLane.h:564
double getActionStepLengthSecs() const
Returns the vehicle&#39;s action step length in secs, i.e. the interval between two action points...
Definition: MSVehicle.h:513
double myManeuverDist
The complete lateral distance the vehicle wants to travel to finish its maneuver Only used by sublane...
double commitFollowSpeed(double speed, double latDist, double secondsToLeaveLane, const MSLeaderDistanceInfo &leaders, double foeOffset) const
compute speed when committing to an urgent change that is safe in regard to leading vehicles ...
blocked in all directions
void updateCFRelated(const MSLeaderDistanceInfo &vehicles, double foeOffset, bool leaders)
find leaders/followers that are already in a car-following relationship with ego
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:48
double getLateralOverlap() const
return the amount by which the vehicle extends laterally outside it&#39;s primary lane ...
Definition: MSVehicle.cpp:5416
The action is urgent (to be defined by lc-model)
virtual 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:328
#define MAGIC_OFFSET
static double brakeGapEuler(const double speed, const double decel, const double headwayTime)
Definition: MSCFModel.cpp:90
At the center of the lane.
static MSPModel * getModel()
Definition: MSPModel.cpp:59
void addLCSpeedAdvice(const double vSafe)
Takes a vSafe (speed advice for speed in the next simulation step), converts it into an acceleration ...
static double toDouble(const std::string &sData)
converts a string into the double value described by it by calling the char-type converter ...
const MSEdge * myLastEdge
expected travel speeds on all sublanes on the current edge(!)
Definition: MSLCM_SL2015.h:368
#define URGENCY
double myPushy
Definition: MSLCM_SL2015.h:392
double informLeader(int blocked, int dir, const CLeaderDist &neighLead, double remainingSeconds)
static LaneChangeAction getLCA(int state, double latDist)
compute lane change action from desired lateral distance
void informFollowers(int blocked, int dir, const std::vector< CLeaderDist > &blockers, double remainingSeconds, double plannedSpeed)
call informFollower for multiple followers
double getEmergencyDecel() const
Get the vehicle type&#39;s maximal phisically possible deceleration [m/s^2].
Definition: MSCFModel.h:226
const std::set< MSTransportable * > & getPersons() const
Returns this edge&#39;s persons set.
Definition: MSEdge.h:174
double getCenterOnEdge(const MSLane *lane=0) const
Get the vehicle&#39;s lateral position on the edge of the given lane (or its current edge if lane == 0) ...
Definition: MSVehicle.cpp:5262
const std::pair< double, LinkDirection > & getNextTurn()
Get the distance and direction of the next upcoming turn for the vehicle (within its look-ahead range...
Definition: MSVehicle.h:784
int wantsChangeSublane(int laneOffset, LaneChangeAction alternatives, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &followers, const MSLeaderDistanceInfo &blockers, const MSLeaderDistanceInfo &neighLeaders, const MSLeaderDistanceInfo &neighFollowers, const MSLeaderDistanceInfo &neighBlockers, const MSLane &neighLane, const std::vector< MSVehicle::LaneQ > &preb, MSVehicle **lastBlocked, MSVehicle **firstBlocked, double &latDist, double &maneuverDist, int &blocked)
Called to examine whether the vehicle wants to change with the given laneOffset (using the sublane mo...
At the rightmost side of the lane.
double getSpeedLimit() const
Returns the lane&#39;s maximum allowed speed.
Definition: MSLane.h:533
#define STEPS2TIME(x)
Definition: SUMOTime.h:57
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
Definition: MSNet.h:284
#define KEEP_RIGHT_ACCEPTANCE
T MIN2(T a, T b)
Definition: StdDefs.h:74
The link is a (hard) right direction.
The action is needed to follow the route (navigational lc)
#define POSITION_EPS
Definition: config.h:169
int _wantsChangeSublane(int laneOffset, LaneChangeAction alternatives, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &followers, const MSLeaderDistanceInfo &blockers, const MSLeaderDistanceInfo &neighLeaders, const MSLeaderDistanceInfo &neighFollowers, const MSLeaderDistanceInfo &neighBlockers, const MSLane &neighLane, const std::vector< MSVehicle::LaneQ > &preb, MSVehicle **lastBlocked, MSVehicle **firstBlocked, double &latDist, double &maneuverDist, int &blocked)
helper function for doing the actual work
double myMinImpatience
Definition: MSLCM_SL2015.h:397
A structure representing the best lanes for continuing the current route starting at &#39;lane&#39;...
Definition: MSVehicle.h:811
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:91
bool hasInfluencer() const
Definition: MSVehicle.h:1680
virtual void updateSafeLatDist(const double travelledLatDist)
Updates the value of safe lateral distances (mySafeLatDistLeft and mySafeLatDistRight) during maneuve...
double getMinGap() const
Get the free space in front of vehicles of this class.
double _patchSpeed(const double min, const double wanted, const double max, const MSCFModel &cfModel)
double getMaxDecel() const
Get the vehicle type&#39;s maximal comfortable deceleration [m/s^2].
Definition: MSCFModel.h:218
int myOwnState
The current state of the vehicle.
maintain the current alignment
bool isInternal() const
return whether this edge is an internal edge
Definition: MSEdge.h:233
virtual bool debugVehicle() const
whether the current vehicles shall be debugged
#define LOOK_AHEAD_SPEED_MEMORY
#define LOOK_AHEAD_MIN_SPEED
static CLeaderDist getLongest(const MSLeaderDistanceInfo &ldi)
get the longest vehicle in the given info
The link is a partial right direction.
bool cancelRequest(int state, int laneOffset)
whether the influencer cancels the given request
const MSLane * lane
The lane to stop at.
Definition: MSVehicle.h:927
double myCooperativeParam
Definition: MSLCM_SL2015.h:387
#define JAM_FACTOR
double getLateralPositionOnLane() const
Get the vehicle&#39;s lateral position on the lane.
Definition: MSVehicle.h:434
double getRightSideOnEdge() const
Definition: MSLane.h:1068
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:503
std::pair< double, int > Info
information regarding save velocity (unused) and state flags of the ego vehicle
Definition: MSLCM_SL2015.h:216
double computeSpeedGain(double latDistSublane, double defaultNextSpeed) const
compute speedGain when moving by the given amount
int getRightmostSublane() const
Definition: MSLane.h:1072
std::pair< const MSPerson *, double > PersonDist
Definition: MSPModel.h:38
drive in the middle
int checkStrategicChange(int ret, int laneOffset, const std::vector< MSVehicle::LaneQ > &preb, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &neighLeaders, int currIdx, int bestLaneOffset, bool changeToBest, double currentDist, double neighDist, double laDist, int roundaboutEdgesAhead, double latLaneDist, double &latDist)
compute strategic lane change actions TODO: Better documentation, refs #2
bool hasStoppedVehicle() const
whether a stopped vehicle is leader
double getMaxSpeedLat() const
Get vehicle&#39;s maximum lateral speed [m/s].
double getWidth() const
Get the width which vehicles of this class shall have when being drawn.
double mySpeedGainRight
Definition: MSLCM_SL2015.h:407
MSVehicle & myVehicle
The vehicle this lane-changer belongs to.
virtual ~MSLCM_SL2015()
std::pair< const MSVehicle *, double > CLeaderDist
Definition: MSLeaderInfo.h:35
Influencer & getInfluencer()
Returns the velocity/lane influencer.
Definition: MSVehicle.cpp:5817
LaneChangeAction
The state of a vehicle&#39;s lane-change behavior.
virtual void setOwnState(const int state)
virtual void setMaxDecel(double decel)
Sets a new value for maximal comfortable deceleration [m/s^2].
Definition: MSCFModel.h:474
double occupation
The overall vehicle sum on consecutive lanes which can be passed without a lane change.
Definition: MSVehicle.h:819
const MSVehicleType & getVehicleType() const
Returns the vehicle&#39;s type definition.
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 ...
#define SPEEDGAIN_MEMORY_FACTOR
bool isActive() const
Returns whether the current simulation step is an action point for the vehicle.
Definition: MSVehicle.h:591
double myKeepRightProbability
Definition: MSLCM_SL2015.h:351
The vehicle is blocked being overlapping.
int slowDownForBlocked(MSVehicle **blocked, int state)
compute useful slowdowns for blocked vehicles
std::vector< MSLane * > bestContinuations
Definition: MSVehicle.h:831
void getSublaneBorders(int sublane, double latOffset, double &rightSide, double &leftSide) const
static double estimateArrivalTime(double dist, double speed, double maxSpeed, double accel)
Computes the time needed to travel a distance dist given an initial speed and constant acceleration...
Definition: MSCFModel.cpp:389
bool hasVehicles() const
Definition: MSLeaderInfo.h:96
double getAcceleration() const
Returns the vehicle&#39;s acceleration in m/s (this is computed as the last step&#39;s mean acceleration in c...
Definition: MSVehicle.h:494
void informFollower(int blocked, int dir, const CLeaderDist &neighFollow, double remainingSeconds, double plannedSpeed)
decide whether we will try cut in before the follower or allow to be overtaken
double mySpeedLat
the current lateral speed
double getInternalFollowingLengthTo(const MSEdge *followerAfterInternal) const
returns the length of all internal edges on the junction until reaching the non-internal edge followe...
Definition: MSEdge.cpp:726
No information given; use default.
double getLength() const
Get vehicle&#39;s length [m].
#define SPEEDGAIN_DECAY_FACTOR
bool currentDistDisallows(double dist, int laneOffset, double lookForwardDist)
Definition: MSLCM_SL2015.h:207
align with the rightmost sublane that allows keeping the current speed
bool allowsVehicleClass(SUMOVehicleClass vclass) const
Definition: MSLane.h:806
#define DEBUG_COND
double myAccelLat
Definition: MSLCM_SL2015.h:401
std::vector< double > myLCAccelerationAdvices
vector of LC-related acceleration recommendations Filled in wantsChange() and applied in patchSpeed()...
Definition: MSLCM_SL2015.h:362
const double SUMO_const_haltingSpeed
the speed threshold at which vehicles are considered as halting
Definition: StdDefs.h:61
#define CUT_IN_LEFT_SPEED_THRESHOLD
std::string getParameter(const std::string &key) const
try to retrieve the given parameter from this device. Throw exception for unsupported key ...
The action is due to a TraCI request.
The link is a 180 degree turn (left-hand network)
virtual double followSpeedTransient(double duration, const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel) const
Computes the vehicle&#39;s follow speed that avoids a collision for the given amount of time...
Definition: MSCFModel.cpp:300
#define HELP_DECEL_FACTOR
double getLastStepDist() const
Get the distance the vehicle covered in the previous timestep.
Definition: MSVehicle.h:404
double mySpeedGainProbabilityRight
a value for tracking the probability that a change to the right is beneficial
Definition: MSLCM_SL2015.h:344
bool gDebugFlag2
Definition: StdDefs.cpp:34
#define LATGAP_SPEED_THRESHOLD2
static bool gSemiImplicitEulerUpdate
Definition: MSGlobals.h:56
#define LOOK_FORWARD
double myLookAheadSpeed
Definition: MSLCM_SL2015.h:358
bool myCFRelatedReady
Definition: MSLCM_SL2015.h:382
T MIN3(T a, T b, T c)
Definition: StdDefs.h:87
double myCommittedSpeed
the speed when committing to a change maneuver
bool currentDistAllows(double dist, int laneOffset, double lookForwardDist)
Definition: MSLCM_SL2015.h:210
#define NUMERICAL_EPS
Definition: config.h:145
int numSublanes() const
Definition: MSLeaderInfo.h:88
MSLane * getShadowLane() const
Returns the lane the vehicle&#39;s shadow is on during continuous/sublane lane change.
bool ignoreOverlap() const
Definition: MSVehicle.h:1597
bool myDontBrake
flag to prevent speed adaptation by slowing down
Definition: MSLCM_SL2015.h:371
#define LATGAP_SPEED_THRESHOLD
const MSLinkCont & getLinkCont() const
returns the container with all links !!!
Definition: MSLane.cpp:2099
double getRightSideOnEdge(const MSLane *lane=0) const
Get the vehicle&#39;s lateral position on the edge of the given lane (or its current edge if lane == 0) ...
Definition: MSVehicle.cpp:5256
const std::vector< LaneQ > & getBestLanes() const
Returns the description of best lanes to use in order to continue the route.
Definition: MSVehicle.cpp:4621
virtual double getArrivalPos() const
Returns this vehicle&#39;s desired arrivalPos for its current route (may change on reroute) ...
double getSpeed() const
Returns the vehicle&#39;s current speed.
Definition: MSVehicle.h:477
The vehicle is blocked by right leader.
int influenceChangeDecision(int state)
allow TraCI to influence a lane change decision
Definition: MSVehicle.cpp:5841
double myLastLateralGapLeft
the minimum lateral gaps to other vehicles that were found when last changing to the left and right ...
double getLatDist() const
Definition: MSVehicle.h:1589
double myImpatience
Definition: MSLCM_SL2015.h:396
public emergency vehicles
double mySafeLatDistLeft
Definition: MSLCM_SL2015.h:378
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 is a wrapper a...
blocker by follower
virtual double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0) const =0
Computes the vehicle&#39;s follow speed (no dawdling)
const std::string & getID() const
Returns the name of the vehicle.
#define TURN_LANE_DIST
Representation of a lane in the micro simulation.
Definition: MSLane.h:83
const MSCFModel & myCarFollowModel
The vehicle&#39;s car following model.
double getVehicleMaxSpeed(const SUMOTrafficObject *const veh) const
Returns the lane&#39;s maximum speed, given a vehicle&#39;s speed limit adaptation.
Definition: MSLane.h:519
The vehicle is blocked by right follower.
int computeSublaneShift(const MSEdge *prevEdge, const MSEdge *curEdge)
compute shift so that prevSublane + shift = newSublane
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) ...
SUMOVehicleClass getVehicleClass() const
Get this vehicle type&#39;s vehicle class.
void updateExpectedSublaneSpeeds(const MSLeaderDistanceInfo &ahead, int sublaneOffset, int laneIndex)
update expected speeds for each sublane of the current edge
double mySublaneParam
Definition: MSLCM_SL2015.h:390
double nextStopDist() const
return the distance to the next stop or doubleMax if there is none.
Definition: MSVehicle.h:1047
void updateGaps(const MSLeaderDistanceInfo &others, double foeOffset, double oldCenter, double gapFactor, double &surplusGapRight, double &surplusGapLeft, bool saveMinGap=false, double netOverlap=0, double latDist=0, std::vector< CLeaderDist > *collectBlockers=0)
check remaining lateral gaps for the given foe vehicles and optionally update minimum lateral gaps ...
The link has no direction (is a dead end link)
#define SPEED_GAIN_MIN_SECONDS
bool amBlockingFollowerPlusNB()
Definition: MSLCM_SL2015.h:204
#define HELP_OVERTAKE
bool congested() const
Definition: MSVehicle.h:718
#define KEEP_RIGHT_TIME
double myAssertive
Definition: MSLCM_SL2015.h:394
double getWidth() const
Returns the edges&#39;s width (sum over all lanes)
Definition: MSEdge.h:553