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