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