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