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  // VARIANT_6 : counterNoHelp
1294  && ((myOwnState & myLca) != 0))
1295  ||
1296  // continue previous cooperative change
1297  ((myPreviousState & LCA_COOPERATIVE) != 0
1298  && !myCanChangeFully
1299  // change is in the right direction
1300  && (laneOffset * myManeuverDist > 0)))
1301  && (inconvenience < myCooperativeParam)
1302  && (changeToBest || currentDistAllows(neighDist, abs(bestLaneOffset) + 1, laDist))) {
1303 
1304  // VARIANT_2 (nbWhenChangingToHelp)
1305 #ifdef DEBUG_COOPERATE
1306  if (gDebugFlag2) {
1307  std::cout << STEPS2TIME(currentTime)
1308  << " veh=" << myVehicle.getID()
1309  << " amBlocking=" << amBlockingFollowerPlusNB()
1310  << " prevState=" << toString((LaneChangeAction)myPreviousState)
1311  << " origLatDist=" << myManeuverDist
1312  << " wantsChangeToHelp=" << (right ? "right" : "left")
1313  << " state=" << myOwnState
1314  //<< (((myOwnState & myLca) == 0) ? " (counter)" : "")
1315  << "\n";
1316  }
1317 #endif
1318 
1319  ret |= LCA_COOPERATIVE | LCA_URGENT ;//| LCA_CHANGE_TO_HELP;
1320  if (!cancelRequest(ret, laneOffset)) {
1321  latDist = amBlockingFollowerPlusNB() ? latLaneDist : myManeuverDist;
1322  blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1323  leaders, followers, blockers,
1324  neighLeaders, neighFollowers, neighBlockers);
1325  return ret;
1326  }
1327  }
1328 
1329  // --------
1330 
1331 
1334  //if ((blocked & LCA_BLOCKED) != 0) {
1335  // return ret;
1336  //}
1338 
1339  // -------- higher speed
1340  //if ((congested(neighLead.first) && neighLead.second < 20) || predInteraction(leader.first)) { //!!!
1341  // return ret;
1342  //}
1343 
1344  // iterate over all possible combinations of sublanes this vehicle might cover and check the potential speed
1345  const MSEdge& edge = myVehicle.getLane()->getEdge();
1346  const std::vector<double>& sublaneSides = edge.getSubLaneSides();
1347  assert(sublaneSides.size() == myExpectedSublaneSpeeds.size());
1348  const double vehWidth = getWidth();
1349  const double rightVehSide = myVehicle.getCenterOnEdge() - 0.5 * vehWidth;
1350  const double leftVehSide = rightVehSide + vehWidth;
1351  // figure out next speed when staying where we are
1352  double defaultNextSpeed = std::numeric_limits<double>::max();
1354  int leftmostOnEdge = (int)sublaneSides.size() - 1;
1355  while (leftmostOnEdge > 0 && sublaneSides[leftmostOnEdge] > leftVehSide) {
1356  leftmostOnEdge--;
1357  }
1358  int rightmostOnEdge = leftmostOnEdge;
1359  while (rightmostOnEdge > 0 && sublaneSides[rightmostOnEdge] > rightVehSide + NUMERICAL_EPS) {
1360  defaultNextSpeed = MIN2(defaultNextSpeed, myExpectedSublaneSpeeds[rightmostOnEdge]);
1361 #ifdef DEBUG_WANTSCHANGE
1362  if (gDebugFlag2) {
1363  std::cout << " adapted to current sublane=" << rightmostOnEdge << " defaultNextSpeed=" << defaultNextSpeed << "\n";
1364  std::cout << " sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] << " rightVehSide=" << rightVehSide << "\n";
1365  }
1366 #endif
1367  rightmostOnEdge--;
1368  }
1369  defaultNextSpeed = MIN2(defaultNextSpeed, myExpectedSublaneSpeeds[rightmostOnEdge]);
1370 #ifdef DEBUG_WANTSCHANGE
1371  if (gDebugFlag2) {
1372  std::cout << " adapted to current sublane=" << rightmostOnEdge << " defaultNextSpeed=" << defaultNextSpeed << "\n";
1373  std::cout << " sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] << " rightVehSide=" << rightVehSide << "\n";
1374  }
1375 #endif
1376  double maxGain = -std::numeric_limits<double>::max();
1377  double maxGainRight = -std::numeric_limits<double>::max();
1378  double maxGainLeft = -std::numeric_limits<double>::max();
1379  double latDistNice = std::numeric_limits<double>::max();
1380 
1381  const int iMin = MIN2(myVehicle.getLane()->getRightmostSublane(), neighLane.getRightmostSublane());
1382  const double leftMax = MAX2(
1383  myVehicle.getLane()->getRightSideOnEdge() + myVehicle.getLane()->getWidth(),
1384  neighLane.getRightSideOnEdge() + neighLane.getWidth());
1385  assert(leftMax <= edge.getWidth());
1386  int sublaneCompact = MAX2(iMin, rightmostOnEdge - 1); // try to compactify to the right by default
1387 
1388 #ifdef DEBUG_WANTSCHANGE
1389  if (gDebugFlag2) std::cout
1390  << " checking sublanes rightmostOnEdge=" << rightmostOnEdge
1391  << " leftmostOnEdge=" << leftmostOnEdge
1392  << " iMin=" << iMin
1393  << " leftMax=" << leftMax
1394  << " sublaneCompact=" << sublaneCompact
1395  << "\n";
1396 #endif
1397  for (int i = iMin; i < (int)sublaneSides.size(); ++i) {
1398  if (sublaneSides[i] + vehWidth < leftMax) {
1399  // i is the rightmost sublane and the left side of vehicles still fits on the edge,
1400  // compute min speed of all sublanes covered by the vehicle in this case
1401  double vMin = myExpectedSublaneSpeeds[i];
1402  //std::cout << " i=" << i << "\n";
1403  int j = i;
1404  while (vMin > 0 && j < (int)sublaneSides.size() && sublaneSides[j] < sublaneSides[i] + vehWidth) {
1405  vMin = MIN2(vMin, myExpectedSublaneSpeeds[j]);
1406  //std::cout << " j=" << j << " vMin=" << vMin << " sublaneSides[j]=" << sublaneSides[j] << " leftVehSide=" << leftVehSide << " rightVehSide=" << rightVehSide << "\n";
1407  ++j;
1408  }
1409  const double relativeGain = (vMin - defaultNextSpeed) / MAX2(vMin, RELGAIN_NORMALIZATION_MIN_SPEED);
1410  const double currentLatDist = sublaneSides[i] - rightVehSide;
1411  // @note this is biased for changing to the left since we compare the sublanes in ascending order
1412  if (relativeGain > maxGain) {
1413  maxGain = relativeGain;
1414  if (maxGain > GAIN_PERCEPTION_THRESHOLD) {
1415  sublaneCompact = i;
1416  latDist = currentLatDist;
1417 #ifdef DEBUG_WANTSCHANGE
1418  if (gDebugFlag2) {
1419  std::cout << " i=" << i << " newLatDist=" << latDist << " relGain=" << relativeGain << "\n";
1420  }
1421 #endif
1422  }
1423  } else {
1424  // if anticipated gains to the left are higher, prefer this
1425  if (currentLatDist > 0
1426  //&& latDist < 0 @todo check why this causes some timeloss in tests
1427  && mySpeedGainProbabilityLeft > mySpeedGainProbabilityRight
1428  && relativeGain > GAIN_PERCEPTION_THRESHOLD
1429  && maxGain - relativeGain < NUMERICAL_EPS) {
1430  latDist = currentLatDist;
1431  }
1432  }
1433 #ifdef DEBUG_WANTSCHANGE
1434  if (gDebugFlag2) {
1435  std::cout << " i=" << i << " rightmostOnEdge=" << rightmostOnEdge << " vMin=" << vMin << " relGain=" << relativeGain << " sublaneCompact=" << sublaneCompact << " curLatDist=" << currentLatDist << "\n";
1436  }
1437 #endif
1438  if (currentLatDist < -NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) {
1439  maxGainRight = MAX2(maxGainRight, relativeGain);
1440  } else if (currentLatDist > NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) {
1441  maxGainLeft = MAX2(maxGainLeft, relativeGain);
1442  }
1443  const double subAlignDist = sublaneSides[i] - rightVehSide;
1444  if (fabs(subAlignDist) < fabs(latDistNice)) {
1445  latDistNice = subAlignDist;
1446 #ifdef DEBUG_WANTSCHANGE
1447  if (gDebugFlag2) std::cout
1448  << " nicest sublane=" << i
1449  << " side=" << sublaneSides[i]
1450  << " rightSide=" << rightVehSide
1451  << " latDistNice=" << latDistNice
1452  << " maxGainR=" << maxGainRight
1453  << " maxGainL=" << maxGainLeft
1454  << "\n";
1455 #endif
1456  }
1457  }
1458  }
1459  // updated change probabilities
1460  if (maxGainRight != -std::numeric_limits<double>::max()) {
1461 #ifdef DEBUG_WANTSCHANGE
1462  if (gDebugFlag2) {
1463  std::cout << " speedGainR_old=" << mySpeedGainProbabilityRight;
1464  }
1465 #endif
1466  mySpeedGainProbabilityRight += myVehicle.getActionStepLengthSecs() * maxGainRight;
1467 #ifdef DEBUG_WANTSCHANGE
1468  if (gDebugFlag2) {
1469  std::cout << " speedGainR_new=" << mySpeedGainProbabilityRight << "\n";
1470  }
1471 #endif
1472  }
1473  if (maxGainLeft != -std::numeric_limits<double>::max()) {
1474 #ifdef DEBUG_WANTSCHANGE
1475  if (gDebugFlag2) {
1476  std::cout << " speedGainL_old=" << mySpeedGainProbabilityLeft;
1477  }
1478 #endif
1479  mySpeedGainProbabilityLeft += myVehicle.getActionStepLengthSecs() * maxGainLeft;
1480 #ifdef DEBUG_WANTSCHANGE
1481  if (gDebugFlag2) {
1482  std::cout << " speedGainL_new=" << mySpeedGainProbabilityLeft << "\n";
1483  }
1484 #endif
1485  }
1486  // decay if there is no reason for or against changing (only if we have enough information)
1487  if ((fabs(maxGainRight) < NUMERICAL_EPS || maxGainRight == -std::numeric_limits<double>::max())
1488  && (right || (alternatives & LCA_RIGHT) == 0)) {
1489  mySpeedGainProbabilityRight *= pow(SPEEDGAIN_DECAY_FACTOR, myVehicle.getActionStepLengthSecs());
1490  }
1491  if ((fabs(maxGainLeft) < NUMERICAL_EPS || maxGainLeft == -std::numeric_limits<double>::max())
1492  && (left || (alternatives & LCA_LEFT) == 0)) {
1493  mySpeedGainProbabilityLeft *= pow(SPEEDGAIN_DECAY_FACTOR, myVehicle.getActionStepLengthSecs());
1494  }
1495 
1496 
1497 #ifdef DEBUG_WANTSCHANGE
1498  if (gDebugFlag2) std::cout << SIMTIME
1499  << " veh=" << myVehicle.getID()
1500  << " defaultNextSpeed=" << defaultNextSpeed
1501  << " maxGain=" << maxGain
1502  << " maxGainRight=" << maxGainRight
1503  << " maxGainLeft=" << maxGainLeft
1504  << " latDist=" << latDist
1505  << " latDistNice=" << latDistNice
1506  << " sublaneCompact=" << sublaneCompact
1507  << "\n";
1508 #endif
1509 
1510  if (!left) {
1511  // ONLY FOR CHANGING TO THE RIGHT
1512  if (right && maxGain >= 0 && latDist <= 0) {
1513  // honor the obligation to keep right (Rechtsfahrgebot)
1514  // XXX consider fast approaching followers on the current lane
1515  //const double vMax = myLookAheadSpeed;
1516  const double vMax = myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle);
1517  const double acceptanceTime = KEEP_RIGHT_ACCEPTANCE * vMax * MAX2(1., myVehicle.getSpeed()) / myVehicle.getLane()->getSpeedLimit();
1518  double fullSpeedGap = MAX2(0., neighDist - myVehicle.getCarFollowModel().brakeGap(vMax));
1519  double fullSpeedDrivingSeconds = MIN2(acceptanceTime, fullSpeedGap / vMax);
1520  CLeaderDist neighLead = getSlowest(neighLeaders);
1521  if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
1522  fullSpeedGap = MAX2(0., MIN2(fullSpeedGap,
1523  neighLead.second - myVehicle.getCarFollowModel().getSecureGap(
1524  vMax, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
1525  fullSpeedDrivingSeconds = MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
1526  }
1527  const double deltaProb = (myChangeProbThresholdRight * (fullSpeedDrivingSeconds / acceptanceTime) / KEEP_RIGHT_TIME);
1528  myKeepRightProbability += myVehicle.getActionStepLengthSecs() * deltaProb;
1529 
1530 #ifdef DEBUG_WANTSCHANGE
1531  if (gDebugFlag2) {
1532  std::cout << STEPS2TIME(currentTime)
1533  << " considering keepRight:"
1534  << " vMax=" << vMax
1535  << " neighDist=" << neighDist
1536  << " brakeGap=" << myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed())
1537  << " leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
1538  << " secGap=" << (neighLead.first == 0 ? -1 : myVehicle.getCarFollowModel().getSecureGap(
1539  myVehicle.getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
1540  << " acceptanceTime=" << acceptanceTime
1541  << " fullSpeedGap=" << fullSpeedGap
1542  << " fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
1543  << " dProb=" << deltaProb
1544  << " keepRight=" << myKeepRightProbability
1545  << " speedGainL=" << mySpeedGainProbabilityLeft
1546  << "\n";
1547  }
1548 #endif
1549  if (myKeepRightProbability * myKeepRightParam > MAX2(myChangeProbThresholdRight, mySpeedGainProbabilityLeft)
1550  /*&& latLaneDist <= -NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()*/) {
1551  ret |= LCA_KEEPRIGHT;
1552  assert(myVehicle.getLane()->getIndex() > neighLane.getIndex());
1553  if (!cancelRequest(ret, laneOffset)) {
1554  latDist = latLaneDist;
1555  blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1556  leaders, followers, blockers,
1557  neighLeaders, neighFollowers, neighBlockers);
1558  return ret;
1559  }
1560  }
1561  }
1562 
1563 #ifdef DEBUG_WANTSCHANGE
1564  if (gDebugFlag2) {
1565  std::cout << STEPS2TIME(currentTime)
1566  << " speedGainR=" << mySpeedGainProbabilityRight
1567  << " speedGainL=" << mySpeedGainProbabilityLeft
1568  << " neighDist=" << neighDist
1569  << " neighTime=" << neighDist / MAX2(.1, myVehicle.getSpeed())
1570  << " rThresh=" << myChangeProbThresholdRight
1571  << " latDist=" << latDist
1572  << "\n";
1573  }
1574 #endif
1575 
1576  if (latDist < 0 && mySpeedGainProbabilityRight >= MAX2(myChangeProbThresholdRight, mySpeedGainProbabilityLeft)
1577  && neighDist / MAX2(.1, myVehicle.getSpeed()) > 20.) {
1578  ret |= LCA_SPEEDGAIN;
1579  if (!cancelRequest(ret, laneOffset)) {
1580  int blockedFully = 0;
1581  blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1582  leaders, followers, blockers,
1583  neighLeaders, neighFollowers, neighBlockers,
1584  0, 0, false, 0, &blockedFully);
1585  //commitManoeuvre(blocked, blockedFully, leaders, neighLeaders, neighLane);
1586  return ret;
1587  }
1588  }
1589  }
1590  if (!right) {
1591 
1592  const bool stayInLane = myVehicle.getLateralPositionOnLane() + latDist < 0.5 * myVehicle.getLane()->getWidth();
1593 #ifdef DEBUG_WANTSCHANGE
1594  if (gDebugFlag2) {
1595  std::cout << STEPS2TIME(currentTime)
1596  << " speedGainL=" << mySpeedGainProbabilityLeft
1597  << " speedGainR=" << mySpeedGainProbabilityRight
1598  << " latDist=" << latDist
1599  << " neighDist=" << neighDist
1600  << " neighTime=" << neighDist / MAX2(.1, myVehicle.getSpeed())
1601  << " lThresh=" << myChangeProbThresholdLeft
1602  << " stayInLane=" << stayInLane
1603  << "\n";
1604  }
1605 #endif
1606 
1607  if (latDist > 0 && mySpeedGainProbabilityLeft > myChangeProbThresholdLeft &&
1608  // if we leave our lane, we should be able to stay in the new
1609  // lane for some time
1610  (stayInLane || neighDist / MAX2(.1, myVehicle.getSpeed()) > SPEED_GAIN_MIN_SECONDS)) {
1611  ret |= LCA_SPEEDGAIN;
1612  if (!cancelRequest(ret, laneOffset)) {
1613  int blockedFully = 0;
1614  blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1615  leaders, followers, blockers,
1616  neighLeaders, neighFollowers, neighBlockers,
1617  0, 0, false, 0, &blockedFully);
1618  //commitManoeuvre(blocked, blockedFully, leaders, neighLeaders, neighLane);
1619  return ret;
1620  }
1621  }
1622  }
1623 
1624  // only factor in preferred lateral alignment if there is no speedGain motivation
1625  if (fabs(latDist) <= NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) {
1626  double latDistSublane = 0.;
1627  const double halfLaneWidth = myVehicle.getLane()->getWidth() * 0.5;
1628  const double halfVehWidth = getWidth() * 0.5;
1629  if (myVehicle.getParameter().arrivalPosLatProcedure != ARRIVAL_POSLAT_DEFAULT
1630  && myVehicle.getRoute().getLastEdge() == &myVehicle.getLane()->getEdge()
1631  && bestLaneOffset == 0
1632  && (myVehicle.getArrivalPos() - myVehicle.getPositionOnLane()) < ARRIVALPOS_LAT_THRESHOLD) {
1633  // vehicle is on its final edge, on the correct lane and close to
1634  // its arrival position. Change to the desired lateral position
1635  switch (myVehicle.getParameter().arrivalPosLatProcedure) {
1636  case ARRIVAL_POSLAT_GIVEN:
1637  latDistSublane = myVehicle.getParameter().arrivalPosLat - myVehicle.getLateralPositionOnLane();
1638  break;
1639  case ARRIVAL_POSLAT_RIGHT:
1640  latDistSublane = -halfLaneWidth + halfVehWidth - myVehicle.getLateralPositionOnLane();
1641  break;
1642  case ARRIVAL_POSLAT_CENTER:
1643  latDistSublane = -myVehicle.getLateralPositionOnLane();
1644  break;
1645  case ARRIVAL_POSLAT_LEFT:
1646  latDistSublane = halfLaneWidth - halfVehWidth - myVehicle.getLateralPositionOnLane();
1647  break;
1648  default:
1649  assert(false);
1650  }
1651 #ifdef DEBUG_WANTSCHANGE
1652  if (gDebugFlag2) std::cout << SIMTIME
1653  << " arrivalPosLatProcedure=" << myVehicle.getParameter().arrivalPosLatProcedure
1654  << " arrivalPosLat=" << myVehicle.getParameter().arrivalPosLat << "\n";
1655 #endif
1656 
1657  } else {
1658 
1659  LateralAlignment align = myVehicle.getVehicleType().getPreferredLateralAlignment();
1660  // Check whether the vehicle should adapt its alignment to an upcoming turn
1661  if (myTurnAlignmentDist > 0) {
1662  const std::pair<double, LinkDirection>& turnInfo = myVehicle.getNextTurn();
1663  if (turnInfo.first < myTurnAlignmentDist) {
1664  // Vehicle is close enough to the link to change its default alignment
1665  switch (turnInfo.second) {
1666  case LINKDIR_TURN:
1667  case LINKDIR_LEFT:
1668  case LINKDIR_PARTLEFT:
1669  align = LATALIGN_LEFT;
1670  break;
1671  case LINKDIR_TURN_LEFTHAND:
1672  case LINKDIR_RIGHT:
1673  case LINKDIR_PARTRIGHT:
1674  align = LATALIGN_RIGHT;
1675  break;
1676  case LINKDIR_STRAIGHT:
1677  case LINKDIR_NODIR:
1678  default:
1679  break;
1680  }
1681  }
1682  }
1683  switch (align) {
1684  case LATALIGN_RIGHT:
1685  latDistSublane = -halfLaneWidth + halfVehWidth - myVehicle.getLateralPositionOnLane();
1686  break;
1687  case LATALIGN_LEFT:
1688  latDistSublane = halfLaneWidth - halfVehWidth - myVehicle.getLateralPositionOnLane();
1689  break;
1690  case LATALIGN_CENTER:
1691  latDistSublane = -myVehicle.getLateralPositionOnLane();
1692  break;
1693  case LATALIGN_NICE:
1694  latDistSublane = latDistNice;
1695  break;
1696  case LATALIGN_COMPACT:
1697  latDistSublane = sublaneSides[sublaneCompact] - rightVehSide;
1698  break;
1699  case LATALIGN_ARBITRARY:
1700  latDistSublane = 0;
1701  break;
1702  }
1703  }
1704 
1705 #if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE) || defined(DEBUG_MANEUVER)
1706  if (gDebugFlag2) std::cout << SIMTIME
1707  << " alignment=" << toString(myVehicle.getVehicleType().getPreferredLateralAlignment())
1708  << " mySpeedGainR=" << mySpeedGainProbabilityRight
1709  << " mySpeedGainL=" << mySpeedGainProbabilityLeft
1710  << " latDist=" << latDist
1711  << " latDistSublane=" << latDistSublane
1712  << " relGainSublane=" << computeSpeedGain(latDistSublane, defaultNextSpeed)
1713  << " maneuverDist=" << maneuverDist
1714  << " myCanChangeFully=" << myCanChangeFully
1715  << " prevState=" << toString((LaneChangeAction)myPreviousState)
1716  << "\n";
1717 #endif
1718 
1719  if ((latDistSublane < 0 && mySpeedGainProbabilityRight < mySpeedLossProbThreshold)
1720  || (latDistSublane > 0 && mySpeedGainProbabilityLeft < mySpeedLossProbThreshold)
1721  || computeSpeedGain(latDistSublane, defaultNextSpeed) < -mySublaneParam) {
1722  // do not risk losing speed
1723  latDistSublane = 0;
1724  }
1725  // Ignore preferred lateral alignment if we are in the middle of an unfinished non-alignment maneuver into the opposite direction
1726  if (!myCanChangeFully
1727  && (myPreviousState & (LCA_STRATEGIC | LCA_COOPERATIVE | LCA_KEEPRIGHT | LCA_SPEEDGAIN)) != 0
1728  && ((myManeuverDist < 0 && latDistSublane > 0) || (myManeuverDist > 0 && latDistSublane < 0))) {
1729  latDistSublane = 0;
1730  }
1731  latDist = latDistSublane;
1732  // XXX first compute preferred adaptation and then override with speed
1733  // (this way adaptation is still done if changing for speedgain is
1734  // blocked)
1735  if (fabs(latDist) >= NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) {
1736 #ifdef DEBUG_WANTSCHANGE
1737  if (gDebugFlag2) std::cout << SIMTIME
1738  << " adapting to preferred alignment=" << toString(myVehicle.getVehicleType().getPreferredLateralAlignment())
1739  << " latDist=" << latDist
1740  << "\n";
1741 #endif
1742  ret |= LCA_SUBLANE;
1743  if (!cancelRequest(ret, laneOffset)) {
1744  blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1745  leaders, followers, blockers,
1746  neighLeaders, neighFollowers, neighBlockers);
1747  return ret;
1748  }
1749  } else {
1750  return ret | LCA_SUBLANE | LCA_STAY;
1751  }
1752  }
1753  latDist = 0;
1754 
1755 
1756  // --------
1757  /*
1758  if (changeToBest && bestLaneOffset == curr.bestLaneOffset && laneOffset != 0
1759  && (right
1760  ? mySpeedGainProbabilityRight > MAX2(0., mySpeedGainProbabilityLeft)
1761  : mySpeedGainProbabilityLeft > MAX2(0., mySpeedGainProbabilityRight))) {
1762  // change towards the correct lane, speedwise it does not hurt
1763  ret |= LCA_STRATEGIC;
1764  if (!cancelRequest(ret, laneOffset)) {
1765  latDist = latLaneDist;
1766  blocked = checkBlocking(neighLane, latDist, laneOffset,
1767  leaders, followers, blockers,
1768  neighLeaders, neighFollowers, neighBlockers);
1769  return ret;
1770  }
1771  }
1772  */
1773 #ifdef DEBUG_WANTSCHANGE
1774  if (gDebugFlag2) {
1775  std::cout << STEPS2TIME(currentTime)
1776  << " veh=" << myVehicle.getID()
1777  << " mySpeedGainR=" << mySpeedGainProbabilityRight
1778  << " mySpeedGainL=" << mySpeedGainProbabilityLeft
1779  << " myKeepRight=" << myKeepRightProbability
1780  << "\n";
1781  }
1782 #endif
1783  return ret;
1784 }
1785 
1786 
1787 int
1788 MSLCM_SL2015::slowDownForBlocked(MSVehicle** blocked, int state) {
1789  // if this vehicle is blocking someone in front, we maybe decelerate to let him in
1790  if ((*blocked) != 0) {
1791  double gap = (*blocked)->getPositionOnLane() - (*blocked)->getVehicleType().getLength() - myVehicle.getPositionOnLane() - myVehicle.getVehicleType().getMinGap();
1792 #ifdef DEBUG_SLOWDOWN
1793  if (gDebugFlag2) {
1794  std::cout << SIMTIME
1795  << " veh=" << myVehicle.getID()
1796  << " blocked=" << Named::getIDSecure(*blocked)
1797  << " gap=" << gap
1798  << "\n";
1799  }
1800 #endif
1801  if (gap > POSITION_EPS) {
1802  //const bool blockedWantsUrgentRight = (((*blocked)->getLaneChangeModel().getOwnState() & LCA_RIGHT != 0)
1803  // && ((*blocked)->getLaneChangeModel().getOwnState() & LCA_URGENT != 0));
1804 
1805  if (myVehicle.getSpeed() < ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxDecel())
1806  //|| blockedWantsUrgentRight // VARIANT_10 (helpblockedRight)
1807  ) {
1808  if ((*blocked)->getSpeed() < SUMO_const_haltingSpeed) {
1809  state |= LCA_AMBACKBLOCKER_STANDING;
1810  } else {
1811  state |= LCA_AMBACKBLOCKER;
1812  }
1813  addLCSpeedAdvice(myCarFollowModel.followSpeed(
1814  &myVehicle, myVehicle.getSpeed(),
1815  (double)(gap - POSITION_EPS), (*blocked)->getSpeed(),
1816  (*blocked)->getCarFollowModel().getMaxDecel()));
1817  //(*blocked) = 0; // VARIANT_14 (furtherBlock)
1818  }
1819  }
1820  }
1821  return state;
1822 }
1823 
1824 
1825 void
1826 MSLCM_SL2015::saveBlockerLength(const MSVehicle* blocker, int lcaCounter) {
1827 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
1828  if (gDebugFlag2) {
1829  std::cout << SIMTIME
1830  << " veh=" << myVehicle.getID()
1831  << " saveBlockerLength blocker=" << Named::getIDSecure(blocker)
1832  << " bState=" << (blocker == 0 ? "None" : toString(blocker->getLaneChangeModel().getOwnState()))
1833  << "\n";
1834  }
1835 #endif
1836  if (blocker != 0 && (blocker->getLaneChangeModel().getOwnState() & lcaCounter) != 0) {
1837  // is there enough space in front of us for the blocker?
1838  const double potential = myLeftSpace - myVehicle.getCarFollowModel().brakeGap(
1839  myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel(), 0);
1840  if (blocker->getVehicleType().getLengthWithGap() <= potential) {
1841  // save at least his length in myLeadingBlockerLength
1842  myLeadingBlockerLength = MAX2(blocker->getVehicleType().getLengthWithGap(), myLeadingBlockerLength);
1843 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
1844  if (gDebugFlag2) {
1845  std::cout << SIMTIME
1846  << " veh=" << myVehicle.getID()
1847  << " blocker=" << Named::getIDSecure(blocker)
1848  << " saving myLeadingBlockerLength=" << myLeadingBlockerLength
1849  << "\n";
1850  }
1851 #endif
1852  } else {
1853  // we cannot save enough space for the blocker. It needs to save
1854  // space for ego instead
1855 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
1856  if (gDebugFlag2) {
1857  std::cout << SIMTIME
1858  << " veh=" << myVehicle.getID()
1859  << " blocker=" << Named::getIDSecure(blocker)
1860  << " cannot save space=" << blocker->getVehicleType().getLengthWithGap()
1861  << " potential=" << potential
1862  << "\n";
1863  }
1864 #endif
1865  ((MSVehicle*)blocker)->getLaneChangeModel().saveBlockerLength(myVehicle.getVehicleType().getLengthWithGap());
1866  }
1867  }
1868 }
1869 
1870 
1871 void MSLCM_SL2015::addLCSpeedAdvice(const double vSafe) {
1872  const double accel = SPEED2ACCEL(vSafe - myVehicle.getSpeed());
1873  myLCAccelerationAdvices.push_back(accel);
1874 #ifdef DEBUG_INFORM
1875  if (DEBUG_COND) {
1876  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " accepted LC speed advice "
1877  << "vSafe=" << vSafe << " -> accel=" << accel << "\n";
1878  }
1879 #endif
1880 }
1881 
1882 
1883 void
1884 MSLCM_SL2015::updateExpectedSublaneSpeeds(const MSLeaderDistanceInfo& ahead, int sublaneOffset, int laneIndex) {
1885  const std::vector<MSLane*>& lanes = myVehicle.getLane()->getEdge().getLanes();
1886  const std::vector<MSVehicle::LaneQ>& preb = myVehicle.getBestLanes();
1887  const MSLane* lane = lanes[laneIndex];
1888  const double vMax = lane->getVehicleMaxSpeed(&myVehicle);
1889  assert(preb.size() == lanes.size());
1890 
1891  for (int sublane = 0; sublane < (int)ahead.numSublanes(); ++sublane) {
1892  const int edgeSublane = sublane + sublaneOffset;
1893  if (edgeSublane >= (int)myExpectedSublaneSpeeds.size()) {
1894  // this may happen if a sibling lane is wider than the changer lane
1895  continue;
1896  }
1897  if (lane->allowsVehicleClass(myVehicle.getVehicleType().getVehicleClass())) {
1898  // lane allowed, find potential leaders and compute safe speeds
1899  // XXX anticipate future braking if leader has a lower speed than myVehicle
1900  const MSVehicle* leader = ahead[sublane].first;
1901  const double gap = ahead[sublane].second;
1902  double vSafe;
1903  if (leader == 0) {
1904  vSafe = myCarFollowModel.followSpeed(&myVehicle, vMax, preb[laneIndex].length, 0, 0);
1905  } else {
1906  if (leader->getAcceleration() > 0.5 * leader->getCarFollowModel().getMaxAccel()) {
1907  // assume that the leader will continue accelerating to its maximum speed
1908  vSafe = leader->getLane()->getVehicleMaxSpeed(leader);
1909  } else {
1910  vSafe = myCarFollowModel.followSpeed(
1911  &myVehicle, vMax, gap, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel());
1912 #ifdef DEBUG_EXPECTED_SLSPEED
1913  if (DEBUG_COND) {
1914  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " updateExpectedSublaneSpeeds edgeSublane=" << edgeSublane << " leader=" << leader->getID() << " gap=" << gap << " vSafe=" << vSafe << "\n";
1915  }
1916 #endif
1917  const double deltaV = vMax - leader->getSpeed();
1918  if (deltaV > 0 && gap / deltaV < 5) {
1919  // anticipate future braking
1920  vSafe = MIN2(vSafe, leader->getSpeed());
1921  }
1922  }
1923  }
1924  // take pedestrians into account
1925  if (lane->getEdge().getPersons().size() > 0 && MSPModel::getModel()->hasPedestrians(lane)) {
1927  double foeRight, foeLeft;
1928  ahead.getSublaneBorders(sublane, 0, foeRight, foeLeft);
1929  // get all leaders ahead or overlapping
1930  PersonDist leader = MSPModel::getModel()->nextBlocking(lane, myVehicle.getPositionOnLane() - myVehicle.getVehicleType().getLength() , foeRight, foeLeft);
1931  if (leader.first != 0) {
1932  const double gap = leader.second - myVehicle.getVehicleType().getMinGap() - myVehicle.getVehicleType().getLength();
1933  const double vSafePed = myCarFollowModel.stopSpeed(&myVehicle, vMax, gap);
1934  vSafe = MIN2(vSafe, vSafePed);
1935  }
1936  }
1937  vSafe = MIN2(vMax, vSafe);
1938  const double memoryFactor = pow(SPEEDGAIN_MEMORY_FACTOR, myVehicle.getActionStepLengthSecs());
1939  myExpectedSublaneSpeeds[edgeSublane] = memoryFactor * myExpectedSublaneSpeeds[edgeSublane] + (1 - memoryFactor) * vSafe;
1940  } else {
1941  // lane forbidden
1942  myExpectedSublaneSpeeds[edgeSublane] = -1;
1943  }
1944  }
1945  // XXX deal with leaders on subsequent lanes based on preb
1946 }
1947 
1948 
1949 double
1950 MSLCM_SL2015::computeSpeedGain(double latDistSublane, double defaultNextSpeed) const {
1951  double result = std::numeric_limits<double>::max();
1952  const double res = MSGlobals::gLateralResolution > 0 ? MSGlobals::gLateralResolution : myVehicle.getLane()->getWidth();
1953  const std::vector<double>& sublaneSides = myVehicle.getLane()->getEdge().getSubLaneSides();
1954  const double vehWidth = getWidth();
1955  const double rightVehSide = myVehicle.getCenterOnEdge() - vehWidth * 0.5 + latDistSublane;
1956  const double leftVehSide = rightVehSide + vehWidth;
1957  for (int i = 0; i < (int)sublaneSides.size(); ++i) {
1958  if (overlap(rightVehSide, leftVehSide, sublaneSides[i], sublaneSides[i] + res)) {
1959  result = MIN2(result, myExpectedSublaneSpeeds[i]);
1960  }
1961  //std::cout << " i=" << i << " rightVehSide=" << rightVehSide << " leftVehSide=" << leftVehSide << " sublaneR=" << sublaneSides[i] << " sublaneL=" << sublaneSides[i] + res
1962  // << " overlap=" << overlap(rightVehSide, leftVehSide, sublaneSides[i], sublaneSides[i] + res) << " speed=" << myExpectedSublaneSpeeds[i] << " result=" << result << "\n";
1963  }
1964  return result - defaultNextSpeed;
1965 }
1966 
1967 
1968 CLeaderDist
1969 MSLCM_SL2015::getLongest(const MSLeaderDistanceInfo& ldi) {
1970  int iMax = 0;
1971  double maxLength = -1;
1972  for (int i = 0; i < ldi.numSublanes(); ++i) {
1973  if (ldi[i].first != 0) {
1974  const double length = ldi[i].first->getVehicleType().getLength();
1975  if (length > maxLength) {
1976  maxLength = length;
1977  iMax = i;
1978  }
1979  }
1980  }
1981  return ldi[iMax];
1982 }
1983 
1984 
1985 CLeaderDist
1986 MSLCM_SL2015::getSlowest(const MSLeaderDistanceInfo& ldi) {
1987  int iMax = 0;
1988  double minSpeed = std::numeric_limits<double>::max();
1989  for (int i = 0; i < ldi.numSublanes(); ++i) {
1990  if (ldi[i].first != 0) {
1991  const double speed = ldi[i].first->getSpeed();
1992  if (speed < minSpeed) {
1993  minSpeed = speed;
1994  iMax = i;
1995  }
1996  }
1997  }
1998  return ldi[iMax];
1999 }
2000 
2001 
2002 int
2003 MSLCM_SL2015::checkBlocking(const MSLane& neighLane, double& latDist, double& maneuverDist, int laneOffset,
2004  const MSLeaderDistanceInfo& leaders,
2005  const MSLeaderDistanceInfo& followers,
2006  const MSLeaderDistanceInfo& /*blockers */,
2007  const MSLeaderDistanceInfo& neighLeaders,
2008  const MSLeaderDistanceInfo& neighFollowers,
2009  const MSLeaderDistanceInfo& /* neighBlockers */,
2010  std::vector<CLeaderDist>* collectLeadBlockers,
2011  std::vector<CLeaderDist>* collectFollowBlockers,
2012  bool keepLatGapManeuver,
2013  double gapFactor,
2014  int* retBlockedFully) {
2015  // truncate latDist according to maxSpeedLat
2016  if (!keepLatGapManeuver) {
2017  myManeuverDist = latDist;
2018  maneuverDist = latDist;
2019  }
2020  const double maxDist = SPEED2DIST(myVehicle.getVehicleType().getMaxSpeedLat());
2021  latDist = MAX2(MIN2(latDist, maxDist), -maxDist);
2022 
2023  if (!myCFRelatedReady) {
2024  updateCFRelated(leaders, myVehicle.getLane()->getRightSideOnEdge(), true);
2025  updateCFRelated(followers, myVehicle.getLane()->getRightSideOnEdge(), false);
2026  if (laneOffset != 0) {
2027  updateCFRelated(neighLeaders, neighLane.getRightSideOnEdge(), true);
2028  updateCFRelated(neighFollowers, neighLane.getRightSideOnEdge(), false);
2029  }
2030  myCFRelatedReady = true;
2031  }
2032 
2033  // reduce latDist to avoid blockage with overlapping vehicles (no minGapLat constraints)
2034  const double center = myVehicle.getCenterOnEdge();
2035  updateGaps(leaders, myVehicle.getLane()->getRightSideOnEdge(), center, gapFactor, mySafeLatDistRight, mySafeLatDistLeft, false, 0, latDist, collectLeadBlockers);
2036  updateGaps(followers, myVehicle.getLane()->getRightSideOnEdge(), center, gapFactor, mySafeLatDistRight, mySafeLatDistLeft, false, 0, latDist, collectFollowBlockers);
2037  if (laneOffset != 0) {
2038  updateGaps(neighLeaders, neighLane.getRightSideOnEdge(), center, gapFactor, mySafeLatDistRight, mySafeLatDistLeft, false, 0, latDist, collectLeadBlockers);
2039  updateGaps(neighFollowers, neighLane.getRightSideOnEdge(), center, gapFactor, mySafeLatDistRight, mySafeLatDistLeft, false, 0, latDist, collectFollowBlockers);
2040  }
2041 #ifdef DEBUG_BLOCKING
2042  if (gDebugFlag2) {
2043  std::cout << " checkBlocking latDist=" << latDist << " mySafeLatDistRight=" << mySafeLatDistRight << " mySafeLatDistLeft=" << mySafeLatDistLeft << "\n";
2044  }
2045 #endif
2046  // if we can move at least a little bit in the desired direction, do so (rather than block)
2047  if (latDist < 0) {
2048  if (mySafeLatDistRight <= NUMERICAL_EPS) {
2049  return LCA_BLOCKED_RIGHT | LCA_OVERLAPPING;
2050  } else {
2051  latDist = MAX2(latDist, -mySafeLatDistRight);
2052  }
2053  } else {
2054  if (mySafeLatDistLeft <= NUMERICAL_EPS) {
2055  return LCA_BLOCKED_LEFT | LCA_OVERLAPPING;
2056  } else {
2057  latDist = MIN2(latDist, mySafeLatDistLeft);
2058  }
2059  }
2060 
2061  myCanChangeFully = (myManeuverDist == 0 || latDist == myManeuverDist);
2062 #ifdef DEBUG_BLOCKING
2063  if (gDebugFlag2) {
2064  std::cout << " checkBlocking fully=" << myCanChangeFully << " latDist=" << latDist << " myManeuverDist=" << myManeuverDist << "\n";
2065  }
2066 #endif
2067  // destination sublanes must be safe
2068  // intermediate sublanes must not be blocked by overlapping vehicles
2069 
2070  // XXX avoid checking the same leader multiple times
2071  // XXX ensure that only changes within the same lane are undertaken if laneOffset = 0
2072 
2073  int blocked = 0;
2074  blocked |= checkBlockingVehicles(&myVehicle, leaders, latDist, myVehicle.getLane()->getRightSideOnEdge(), true, LCA_BLOCKED_BY_LEADER,
2075  mySafeLatDistRight, mySafeLatDistLeft, collectLeadBlockers);
2076  blocked |= checkBlockingVehicles(&myVehicle, followers, latDist, myVehicle.getLane()->getRightSideOnEdge(), false, LCA_BLOCKED_BY_FOLLOWER,
2077  mySafeLatDistRight, mySafeLatDistLeft, collectFollowBlockers);
2078  if (laneOffset != 0) {
2079  blocked |= checkBlockingVehicles(&myVehicle, neighLeaders, latDist, neighLane.getRightSideOnEdge(), true,
2080  (laneOffset == -1 ? LCA_BLOCKED_BY_RIGHT_LEADER : LCA_BLOCKED_BY_LEFT_LEADER),
2081  mySafeLatDistRight, mySafeLatDistLeft, collectLeadBlockers);
2082  blocked |= checkBlockingVehicles(&myVehicle, neighFollowers, latDist, neighLane.getRightSideOnEdge(), false,
2083  (laneOffset == -1 ? LCA_BLOCKED_BY_RIGHT_FOLLOWER : LCA_BLOCKED_BY_LEFT_FOLLOWER),
2084  mySafeLatDistRight, mySafeLatDistLeft, collectFollowBlockers);
2085  }
2086 
2087  int blockedFully = 0;
2088  blockedFully |= checkBlockingVehicles(&myVehicle, leaders, myManeuverDist, myVehicle.getLane()->getRightSideOnEdge(), true, LCA_BLOCKED_BY_LEADER,
2089  mySafeLatDistRight, mySafeLatDistLeft, collectLeadBlockers);
2090  blockedFully |= checkBlockingVehicles(&myVehicle, followers, myManeuverDist, myVehicle.getLane()->getRightSideOnEdge(), false, LCA_BLOCKED_BY_FOLLOWER,
2091  mySafeLatDistRight, mySafeLatDistLeft, collectFollowBlockers);
2092  if (laneOffset != 0) {
2093  blockedFully |= checkBlockingVehicles(&myVehicle, neighLeaders, myManeuverDist, neighLane.getRightSideOnEdge(), true,
2094  (laneOffset == -1 ? LCA_BLOCKED_BY_RIGHT_LEADER : LCA_BLOCKED_BY_LEFT_LEADER),
2095  mySafeLatDistRight, mySafeLatDistLeft, collectLeadBlockers);
2096  blockedFully |= checkBlockingVehicles(&myVehicle, neighFollowers, myManeuverDist, neighLane.getRightSideOnEdge(), false,
2097  (laneOffset == -1 ? LCA_BLOCKED_BY_RIGHT_FOLLOWER : LCA_BLOCKED_BY_LEFT_FOLLOWER),
2098  mySafeLatDistRight, mySafeLatDistLeft, collectFollowBlockers);
2099  }
2100  if (retBlockedFully != 0) {
2101  *retBlockedFully = blockedFully;
2102  }
2103  if (blocked == 0 && !myCanChangeFully && myPushy == 0 && !keepLatGapManeuver) {
2104  // aggressive drivers immediately start moving towards potential
2105  // blockers and only check that the start of their maneuver (latDist) is safe. In
2106  // contrast, cautious drivers need to check latDist and origLatDist to
2107  // ensure that the maneuver can be finished without encroaching on other vehicles.
2108  blocked |= blockedFully;
2109  } else {
2110  // XXX: in case of action step length > simulation step length, pushing may lead to collisions,
2111  // because maneuver is continued until myManeuverDist is reached (perhaps set myManeuverDist=latDist)
2112  }
2113  if (collectFollowBlockers != 0 && collectLeadBlockers != 0) {
2114  // prevent vehicles from being classified as leader and follower simultaneously
2115  for (std::vector<CLeaderDist>::const_iterator it2 = collectLeadBlockers->begin(); it2 != collectLeadBlockers->end(); ++it2) {
2116  for (std::vector<CLeaderDist>::iterator it = collectFollowBlockers->begin(); it != collectFollowBlockers->end();) {
2117  if ((*it2).first == (*it).first) {
2118 #ifdef DEBUG_BLOCKING
2119  if (gDebugFlag2) {
2120  std::cout << " removed follower " << (*it).first->getID() << " because it is already a leader\n";
2121  }
2122 #endif
2123  it = collectFollowBlockers->erase(it);
2124  } else {
2125  ++it;
2126  }
2127  }
2128  }
2129  }
2130  return blocked;
2131 }
2132 
2133 
2134 int
2135 MSLCM_SL2015::checkBlockingVehicles(
2136  const MSVehicle* ego, const MSLeaderDistanceInfo& vehicles,
2137  double latDist, double foeOffset, bool leaders, LaneChangeAction blockType,
2138  double& safeLatGapRight, double& safeLatGapLeft,
2139  std::vector<CLeaderDist>* collectBlockers) const {
2140  // determine borders where safety/no-overlap conditions must hold
2141  const double vehWidth = getWidth();
2142  const double rightVehSide = ego->getRightSideOnEdge();
2143  const double leftVehSide = rightVehSide + vehWidth;
2144  const double rightVehSideDest = rightVehSide + latDist;
2145  const double leftVehSideDest = leftVehSide + latDist;
2146  const double rightNoOverlap = MIN2(rightVehSideDest, rightVehSide);
2147  const double leftNoOverlap = MAX2(leftVehSideDest, leftVehSide);
2148 #ifdef DEBUG_BLOCKING
2149  if (gDebugFlag2) {
2150  std::cout << " checkBlockingVehicles"
2151  << " latDist=" << latDist
2152  << " foeOffset=" << foeOffset
2153  << " vehRight=" << rightVehSide
2154  << " vehLeft=" << leftVehSide
2155  << " rightNoOverlap=" << rightNoOverlap
2156  << " leftNoOverlap=" << leftNoOverlap
2157  << " destRight=" << rightVehSideDest
2158  << " destLeft=" << leftVehSideDest
2159  << " leaders=" << leaders
2160  << " blockType=" << toString((LaneChangeAction) blockType)
2161  << "\n";
2162  }
2163 #endif
2164  int result = 0;
2165  for (int i = 0; i < vehicles.numSublanes(); ++i) {
2166  CLeaderDist vehDist = vehicles[i];
2167  if (vehDist.first != 0 && myCFRelated.count(vehDist.first) == 0) {
2168  const MSVehicle* leader = vehDist.first;
2169  const MSVehicle* follower = ego;
2170  if (!leaders) {
2171  std::swap(leader, follower);
2172  }
2173  // only check the current stripe occupied by foe (transform into edge-coordinates)
2174  double foeRight, foeLeft;
2175  vehicles.getSublaneBorders(i, foeOffset, foeRight, foeLeft);
2176  const bool overlapBefore = overlap(rightVehSide, leftVehSide, foeRight, foeLeft);
2177  const bool overlapDest = overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft);
2178  const bool overlapAny = overlap(rightNoOverlap, leftNoOverlap, foeRight, foeLeft);
2179 #ifdef DEBUG_BLOCKING
2180  if (gDebugFlag2) {
2181  std::cout << " foe=" << vehDist.first->getID()
2182  << " gap=" << vehDist.second
2183  << " secGap=" << follower->getCarFollowModel().getSecureGap(follower->getSpeed(), leader->getSpeed(), leader->getCarFollowModel().getMaxDecel())
2184  << " foeRight=" << foeRight
2185  << " foeLeft=" << foeLeft
2186  << " overlapBefore=" << overlapBefore
2187  << " overlap=" << overlapAny
2188  << " overlapDest=" << overlapDest
2189  << "\n";
2190  }
2191 #endif
2192  if (overlapAny) {
2193  if (vehDist.second < 0) {
2194  if (overlapBefore && !overlapDest) {
2195 #ifdef DEBUG_BLOCKING
2196  if (gDebugFlag2) {
2197  std::cout << " ignoring current overlap to come clear\n";
2198  }
2199 #endif
2200  } else {
2201 #ifdef DEBUG_BLOCKING
2202  if (gDebugFlag2) {
2203  std::cout << " overlap (" << toString((LaneChangeAction)blockType) << ")\n";
2204  }
2205 #endif
2206  result |= (blockType | LCA_OVERLAPPING);
2207  if (collectBlockers == 0) {
2208  return result;
2209  } else {
2210  collectBlockers->push_back(vehDist);
2211  }
2212  }
2213  } else if (overlapDest || !myCanChangeFully) {
2214  // Estimate state after actionstep (follower may be accelerating!)
2215  // A comparison between secure gap depending on the expected speeds and the extrapolated gap
2216  // determines whether the s is blocking the lane change.
2217  // (Note that the longitudinal state update has already taken effect before LC dynamics (thus "-TS" below), would be affected by #3665)
2218 
2219  // Use conservative estimate for time until next action step
2220  // (XXX: how can the ego know the foe's action step length?)
2221  const double timeTillAction = MAX2(follower->getActionStepLengthSecs(), leader->getActionStepLengthSecs()) - TS;
2222  // Ignore decel for follower
2223  const double followerAccel = MAX2(0., follower->getAcceleration());
2224  const double leaderAccel = leader->getAcceleration();
2225  // Expected gap after next actionsteps
2226  const double expectedGap = MSCFModel::gapExtrapolation(timeTillAction, vehDist.second, leader->getSpeed(), follower->getSpeed(), leaderAccel, followerAccel, std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
2227 
2228  // Determine expected speeds and corresponding secure gap at the extrapolated timepoint
2229  const double followerExpectedSpeed = follower->getSpeed() + timeTillAction * followerAccel;
2230  const double leaderExpectedSpeed = MAX2(0., leader->getSpeed() + timeTillAction * leaderAccel);
2231  const double expectedSecureGap = follower->getCarFollowModel().getSecureGap(followerExpectedSpeed, leaderExpectedSpeed, leader->getCarFollowModel().getMaxDecel());
2232 
2233 #if defined(DEBUG_ACTIONSTEPS) && defined(DEBUG_BLOCKING)
2234  if (gDebugFlag2) {
2235  std::cout << " timeTillAction=" << timeTillAction
2236  << " followerAccel=" << followerAccel
2237  << " followerExpectedSpeed=" << followerExpectedSpeed
2238  << " leaderAccel=" << leaderAccel
2239  << " leaderExpectedSpeed=" << leaderExpectedSpeed
2240  << "\n gap=" << vehDist.second
2241  << " gapChange=" << (expectedGap - vehDist.second)
2242  << " expectedGap=" << expectedGap
2243  << " expectedSecureGap=" << expectedSecureGap
2244  << " safeLatGapLeft=" << safeLatGapLeft
2245  << " safeLatGapRight=" << safeLatGapRight
2246  << std::endl;
2247  }
2248 #endif
2249 
2250  // @note for euler-update, a different value for secureGap2 may be obtained when applying safetyFactor to followerDecel rather than secureGap
2251  const double secureGap2 = expectedSecureGap * getSafetyFactor();
2252  if (expectedGap < secureGap2) {
2253  // Foe is a blocker. Update lateral safe gaps accordingly.
2254  if (foeRight > leftVehSide) {
2255  safeLatGapLeft = MIN2(safeLatGapLeft, foeRight - leftVehSide);
2256  } else if (foeLeft < rightVehSide) {
2257  safeLatGapRight = MIN2(safeLatGapRight, rightVehSide - foeLeft);
2258  }
2259 
2260 #ifdef DEBUG_BLOCKING
2261  if (gDebugFlag2) {
2262  std::cout << " blocked by " << vehDist.first->getID() << " gap=" << vehDist.second << " expectedGap=" << expectedGap
2263  << " expectedSecureGap=" << expectedSecureGap << " secGap2=" << secureGap2 << " safetyFactor=" << getSafetyFactor()
2264  << " safeLatGapLeft=" << safeLatGapLeft << " safeLatGapRight=" << safeLatGapRight
2265  << "\n";
2266  }
2267 #endif
2268  result |= blockType;
2269  if (collectBlockers == 0) {
2270  return result;
2271  } else {
2272  collectBlockers->push_back(vehDist);
2273  }
2274 #ifdef DEBUG_BLOCKING
2275  } else if (gDebugFlag2 && expectedGap < expectedSecureGap) {
2276  std::cout << " ignore blocker " << vehDist.first->getID() << " gap=" << vehDist.second << " expectedGap=" << expectedGap
2277  << " expectedSecureGap=" << expectedSecureGap << " secGap2=" << secureGap2 << " safetyFactor=" << getSafetyFactor() << "\n";
2278 #endif
2279  }
2280  }
2281  }
2282  }
2283  }
2284  return result;
2285 
2286 }
2287 
2288 
2289 void
2290 MSLCM_SL2015::updateCFRelated(const MSLeaderDistanceInfo& vehicles, double foeOffset, bool leaders) {
2291  // to ensure that we do not ignore the wrong vehicles due to numerical
2292  // instability we slightly reduce the width
2293  const double vehWidth = myVehicle.getVehicleType().getWidth() - NUMERICAL_EPS;
2294  const double rightVehSide = myVehicle.getCenterOnEdge() - 0.5 * vehWidth;
2295  const double leftVehSide = rightVehSide + vehWidth;
2296 #ifdef DEBUG_BLOCKING
2297  if (gDebugFlag2) {
2298  std::cout << " updateCFRelated foeOffset=" << foeOffset << " vehicles=" << vehicles.toString() << "\n";
2299  }
2300 #endif
2301  for (int i = 0; i < vehicles.numSublanes(); ++i) {
2302  CLeaderDist vehDist = vehicles[i];
2303  if (vehDist.first != 0 && myCFRelated.count(vehDist.first) == 0) {
2304  double foeRight, foeLeft;
2305  vehicles.getSublaneBorders(i, foeOffset, foeRight, foeLeft);
2306  if (overlap(rightVehSide, leftVehSide, foeRight, foeLeft) && (vehDist.second >= 0
2307  // avoid deadlock due to #3729
2308  || (!leaders
2309  && myVehicle.getPositionOnLane() >= myVehicle.getVehicleType().getLength()
2310  && myVehicle.getSpeed() < SUMO_const_haltingSpeed
2311  && vehDist.first->getSpeed() < SUMO_const_haltingSpeed
2312  && -vehDist.second < vehDist.first->getVehicleType().getMinGap()
2313  && &(myVehicle.getLane()->getEdge()) != &(vehDist.first->getLane()->getEdge()))
2314  )) {
2315 #ifdef DEBUG_BLOCKING
2316  if (gDebugFlag2) {
2317  std::cout << " ignoring cfrelated foe=" << vehDist.first->getID() << " gap=" << vehDist.second
2318  << " sublane=" << i
2319  << " foeOffset=" << foeOffset
2320  << " egoR=" << rightVehSide << " egoL=" << leftVehSide
2321  << " iR=" << foeRight << " iL=" << foeLeft
2322  << " egoV=" << myVehicle.getSpeed() << " foeV=" << vehDist.first->getSpeed()
2323  << " egoE=" << myVehicle.getLane()->getEdge().getID() << " egoE=" << vehDist.first->getLane()->getEdge().getID()
2324  << "\n";
2325  }
2326 #endif
2327  myCFRelated.insert(vehDist.first);
2328  }
2329  }
2330  }
2331 }
2332 
2333 
2334 bool
2335 MSLCM_SL2015::overlap(double right, double left, double right2, double left2) {
2336  assert(right <= left);
2337  assert(right2 <= left2);
2338  return left2 >= right + NUMERICAL_EPS && left >= right2 + NUMERICAL_EPS;
2339 }
2340 
2341 
2342 MSLCM_SL2015::StateAndDist
2343 MSLCM_SL2015::decideDirection(StateAndDist sd1, StateAndDist sd2) const {
2344  // ignore dummy decisions (returned if mayChange() failes)
2345  if (sd1.state == 0) {
2346  return sd2;
2347  } else if (sd2.state == 0) {
2348  return sd1;
2349  }
2350  // LCA_SUBLANE is special because LCA_STAY|LCA_SUBLANE may override another LCA_SUBLANE command
2351  const bool want1 = ((sd1.state & LCA_WANTS_LANECHANGE) != 0) || ((sd1.state & LCA_SUBLANE) != 0 && (sd1.state & LCA_STAY) != 0);
2352  const bool want2 = ((sd2.state & LCA_WANTS_LANECHANGE) != 0) || ((sd2.state & LCA_SUBLANE) != 0 && (sd2.state & LCA_STAY) != 0);
2353  const bool can1 = ((sd1.state & LCA_BLOCKED) == 0);
2354  const bool can2 = ((sd2.state & LCA_BLOCKED) == 0);
2355 #ifdef DEBUG_WANTSCHANGE
2356  if (DEBUG_COND) std::cout << SIMTIME
2357  << " veh=" << myVehicle.getID()
2358  << " state1=" << toString((LaneChangeAction)sd1.state)
2359  << " want1=" << (sd1.state & LCA_WANTS_LANECHANGE)
2360  << " dist1=" << sd1.latDist
2361  << " dir1=" << sd1.dir
2362  << " state2=" << toString((LaneChangeAction)sd2.state)
2363  << " want2=" << (sd2.state & LCA_WANTS_LANECHANGE)
2364  << " dist2=" << sd2.latDist
2365  << " dir2=" << sd2.dir
2366  << "\n";
2367 #endif
2368  if (want1) {
2369  if (want2) {
2370  // decide whether right or left has higher priority (lower value in enum LaneChangeAction)
2371  if ((sd1.state & LCA_CHANGE_REASONS) < (sd2.state & LCA_CHANGE_REASONS)) {
2372  //if (DEBUG_COND) std::cout << " " << (sd1.state & LCA_CHANGE_REASONS) << " < " << (sd2.state & LCA_CHANGE_REASONS) << "\n";
2373  return (!can1 && can2 && sd1.sameDirection(sd2)) ? sd2 : sd1;
2374  //return sd1;
2375  } else if ((sd1.state & LCA_CHANGE_REASONS) > (sd2.state & LCA_CHANGE_REASONS)) {
2376  //if (DEBUG_COND) std::cout << " " << (sd1.state & LCA_CHANGE_REASONS) << " > " << (sd2.state & LCA_CHANGE_REASONS) << "\n";
2377  return (!can2 && can1 && sd1.sameDirection(sd2)) ? sd1 : sd2;
2378  //return sd2;
2379  } else {
2380  // same priority.
2381  if ((sd1.state & LCA_SUBLANE) != 0) {
2382  // special treatment: prefer action with dir != 0
2383  if (sd1.dir == 0) {
2384  return sd2;
2385  } else if (sd2.dir == 0) {
2386  return sd1;
2387  } else {
2388  // prefer action that knows more about the desired direction
2389  // @note when deciding between right and left, right is always given as sd1
2390  assert(sd1.dir == -1);
2391  assert(sd2.dir == 1);
2392  if (sd1.latDist <= 0) {
2393  return sd1;
2394  } else if (sd2.latDist >= 0) {
2395  return sd2;
2396  }
2397  // when in doubt, prefer moving to the right
2398  return sd1.latDist <= sd2.latDist ? sd1 : sd2;
2399  }
2400  } else {
2401  // see which one is allowed
2402  return can1 ? sd1 : sd2;
2403  }
2404  }
2405  } else {
2406  return sd1;
2407  }
2408  } else {
2409  return sd2;
2410  }
2411 
2412 }
2413 
2414 
2415 LaneChangeAction
2416 MSLCM_SL2015::getLCA(int state, double latDist) {
2417  return ((latDist == 0 || (state & LCA_CHANGE_REASONS) == 0)
2418  ? LCA_NONE : (latDist < 0 ? LCA_RIGHT : LCA_LEFT));
2419 }
2420 
2421 
2422 int
2423 MSLCM_SL2015::checkStrategicChange(int ret,
2424  int laneOffset,
2425  const std::vector<MSVehicle::LaneQ>& preb,
2426  const MSLeaderDistanceInfo& leaders,
2427  const MSLeaderDistanceInfo& neighLeaders,
2428  int currIdx,
2429  int bestLaneOffset,
2430  bool changeToBest,
2431  double currentDist,
2432  double neighDist,
2433  double laDist,
2434  int roundaboutEdgesAhead,
2435  double latLaneDist,
2436  double& latDist
2437  ) {
2438  const bool right = (laneOffset == -1);
2439  const bool left = (laneOffset == 1);
2440  const MSVehicle::LaneQ& curr = preb[currIdx];
2441  const MSVehicle::LaneQ& neigh = preb[currIdx + laneOffset];
2442  const MSVehicle::LaneQ& best = preb[currIdx + bestLaneOffset];
2443 
2444  myLeftSpace = currentDist - myVehicle.getPositionOnLane();
2445  const double usableDist = (currentDist - myVehicle.getPositionOnLane() - best.occupation * JAM_FACTOR);
2446  //- (best.lane->getVehicleNumber() * neighSpeed)); // VARIANT 9 jfSpeed
2447  const double maxJam = MAX2(neigh.occupation, curr.occupation);
2448  const double neighLeftPlace = MAX2(0., neighDist - myVehicle.getPositionOnLane() - maxJam);
2449  // save the left space
2450 
2451 #ifdef DEBUG_STRATEGIC_CHANGE
2452  if (gDebugFlag2) {
2453  std::cout << SIMTIME
2454  << " veh=" << myVehicle.getID()
2455  << " laSpeed=" << myLookAheadSpeed
2456  << " laDist=" << laDist
2457  << " currentDist=" << currentDist
2458  << " usableDist=" << usableDist
2459  << " bestLaneOffset=" << bestLaneOffset
2460  << " best.length=" << best.length
2461  << " maxJam=" << maxJam
2462  << " neighLeftPlace=" << neighLeftPlace
2463  << "\n";
2464  }
2465 #endif
2466 
2467  if (laneOffset != 0 && changeToBest && bestLaneOffset == curr.bestLaneOffset
2468  && currentDistDisallows(usableDist, bestLaneOffset, laDist)) {
2470  latDist = latLaneDist;
2471  ret |= LCA_STRATEGIC | LCA_URGENT;
2472  } else {
2473  // VARIANT_20 (noOvertakeRight)
2474  if (!myAllowOvertakingRight && left && !myVehicle.congested() && neighLeaders.hasVehicles() && myVehicle.getVehicleType().getVehicleClass() != SVC_EMERGENCY) {
2475  // check for slower leader on the left. we should not overtake but
2476  // rather move left ourselves (unless congested)
2477  // XXX only adapt as much as possible to get a lateral gap
2478  CLeaderDist cld = getSlowest(neighLeaders);
2479  const MSVehicle* nv = cld.first;
2480  if (nv->getSpeed() < myVehicle.getSpeed()) {
2481  const double vSafe = myCarFollowModel.followSpeed(
2482  &myVehicle, myVehicle.getSpeed(), cld.second, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel());
2483  addLCSpeedAdvice(vSafe);
2484  if (vSafe < myVehicle.getSpeed()) {
2485  mySpeedGainProbabilityRight += myVehicle.getActionStepLengthSecs() * myChangeProbThresholdLeft / 3;
2486  }
2487 #ifdef DEBUG_STRATEGIC_CHANGE
2488  if (gDebugFlag2) {
2489  std::cout << SIMTIME
2490  << " avoid overtaking on the right nv=" << nv->getID()
2491  << " nvSpeed=" << nv->getSpeed()
2492  << " mySpeedGainProbabilityR=" << mySpeedGainProbabilityRight
2493  << " plannedSpeed=" << myVehicle.getSpeed() + ACCEL2SPEED(myLCAccelerationAdvices.back())
2494  << "\n";
2495  }
2496 #endif
2497  }
2498  }
2499 
2500  if (!changeToBest && (currentDistDisallows(neighLeftPlace, abs(bestLaneOffset) + 2, laDist))) {
2501  // the opposite lane-changing direction should be done than the one examined herein
2502  // we'll check whether we assume we could change anyhow and get back in time...
2503  //
2504  // this rule prevents the vehicle from moving in opposite direction of the best lane
2505  // unless the way till the end where the vehicle has to be on the best lane
2506  // is long enough
2507 #ifdef DEBUG_STRATEGIC_CHANGE
2508  if (gDebugFlag2) {
2509  std::cout << " veh=" << myVehicle.getID() << " could not change back and forth in time (1) neighLeftPlace=" << neighLeftPlace << "\n";
2510  }
2511 #endif
2512  ret |= LCA_STAY | LCA_STRATEGIC;
2513  } else if (
2514  laneOffset != 0
2515  && bestLaneOffset == 0
2516  && !leaders.hasStoppedVehicle()
2517  && neigh.bestContinuations.back()->getLinkCont().size() != 0
2518  && roundaboutEdgesAhead == 0
2519  && neighDist < TURN_LANE_DIST) {
2520  // VARIANT_21 (stayOnBest)
2521  // we do not want to leave the best lane for a lane which leads elsewhere
2522  // unless our leader is stopped or we are approaching a roundabout
2523 #ifdef DEBUG_STRATEGIC_CHANGE
2524  if (gDebugFlag2) {
2525  std::cout << " veh=" << myVehicle.getID() << " does not want to leave the bestLane (neighDist=" << neighDist << ")\n";
2526  }
2527 #endif
2528  ret |= LCA_STAY | LCA_STRATEGIC;
2529  } else if (right
2530  && bestLaneOffset == 0
2531  && myVehicle.getLane()->getSpeedLimit() > 80. / 3.6
2532  && myLookAheadSpeed > SUMO_const_haltingSpeed
2533  ) {
2534  // let's also regard the case where the vehicle is driving on a highway...
2535  // in this case, we do not want to get to the dead-end of an on-ramp
2536 #ifdef DEBUG_STRATEGIC_CHANGE
2537  if (gDebugFlag2) {
2538  std::cout << " veh=" << myVehicle.getID() << " does not want to get stranded on the on-ramp of a highway\n";
2539  }
2540 #endif
2541  ret |= LCA_STAY | LCA_STRATEGIC;
2542  }
2543  }
2544  if ((ret & LCA_URGENT) == 0 && getShadowLane() != 0 &&
2545  // ignore overlap if it goes in the correct direction
2546  bestLaneOffset * myVehicle.getLateralPositionOnLane() <= 0) {
2547  // no decision or decision to stay
2548  // make sure to stay within lane bounds in case the shadow lane ends
2549  //const double requiredDist = MAX2(2 * myVehicle.getLateralOverlap(), getSublaneWidth()) / SUMO_const_laneWidth * laDist;
2550  const double requiredDist = 2 * myVehicle.getLateralOverlap() / SUMO_const_laneWidth * laDist;
2551  double currentShadowDist = -myVehicle.getPositionOnLane();
2552  for (std::vector<MSLane*>::const_iterator it = curr.bestContinuations.begin(); it != curr.bestContinuations.end(); ++it) {
2553  if (*it == 0) {
2554  continue;
2555  }
2556  MSLane* shadow = getShadowLane(*it);
2557  if (shadow == 0 || currentShadowDist >= requiredDist) {
2558  break;
2559  }
2560  currentShadowDist += shadow->getLength();
2561  }
2562 #ifdef DEBUG_STRATEGIC_CHANGE
2563  if (gDebugFlag2) {
2564  std::cout << " veh=" << myVehicle.getID() << " currentShadowDist=" << currentShadowDist << " requiredDist=" << requiredDist << " overlap=" << myVehicle.getLateralOverlap() << "\n";
2565  }
2566 #endif
2567  if (currentShadowDist < requiredDist && currentShadowDist < usableDist) {
2568  myLeftSpace = currentShadowDist;
2569  latDist = myVehicle.getLateralPositionOnLane() < 0 ? myVehicle.getLateralOverlap() : - myVehicle.getLateralOverlap();
2570 #ifdef DEBUG_STRATEGIC_CHANGE
2571  if (gDebugFlag2) {
2572  std::cout << " must change for shadowLane end latDist=" << latDist << " myLeftSpace=" << myLeftSpace << "\n";
2573  }
2574 #endif
2575  ret |= LCA_STRATEGIC | LCA_URGENT | LCA_STAY ;
2576  }
2577  }
2578 
2579  // check for overriding TraCI requests
2580 #if defined(DEBUG_STRATEGIC_CHANGE) || defined(DEBUG_TRACI)
2581  if (gDebugFlag2) {
2582  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " ret=" << ret;
2583  }
2584 #endif
2585  // store state before canceling
2586  myCanceledStates[laneOffset] |= ret;
2587  int retTraCI = myVehicle.influenceChangeDecision(ret);
2588  if ((retTraCI & LCA_TRACI) != 0) {
2589  if ((retTraCI & LCA_STAY) != 0) {
2590  ret = retTraCI;
2591  latDist = 0;
2592  } else if (((retTraCI & LCA_RIGHT) != 0 && laneOffset < 0)
2593  || ((retTraCI & LCA_LEFT) != 0 && laneOffset > 0)) {
2594  ret = retTraCI;
2595  latDist = latLaneDist;
2596  }
2597  }
2598 #if defined(DEBUG_STRATEGIC_CHANGE) || defined(DEBUG_TRACI)
2599  if (gDebugFlag2) {
2600  std::cout << " reqAfterInfluence=" << ret << " ret=" << ret << "\n";
2601  }
2602 #endif
2603  return ret;
2604 }
2605 
2606 
2607 double
2608 MSLCM_SL2015::computeGapFactor(int state) const {
2609  return (state & LCA_STRATEGIC) != 0 ? MAX2(0.0, (1.0 - myPushy * (1 + 0.5 * myImpatience))) : 1.0;
2610 }
2611 
2612 
2613 int
2614 MSLCM_SL2015::keepLatGap(int state,
2615  const MSLeaderDistanceInfo& leaders,
2616  const MSLeaderDistanceInfo& followers,
2617  const MSLeaderDistanceInfo& blockers,
2618  const MSLeaderDistanceInfo& neighLeaders,
2619  const MSLeaderDistanceInfo& neighFollowers,
2620  const MSLeaderDistanceInfo& neighBlockers,
2621  const MSLane& neighLane,
2622  int laneOffset,
2623  double& latDist,
2624  double& maneuverDist,
2625  int& blocked) {
2626 
2627  /* @notes
2628  * vehicles may need to compromise between fulfilling lane change objectives
2629  * (LCA_STRATEGIC, LCA_SPEED etc) and maintaining lateral gap. The minimum
2630  * acceptable lateral gap depends on
2631  * - the cultural context (China vs Europe)
2632  * - the driver agressiveness (willingness to encroach on other vehicles to force them to move laterally as well)
2633  * - see @note in checkBlocking
2634  * - the vehicle type (car vs motorcycle)
2635  * - the current speed
2636  * - the speed difference
2637  * - the importance / urgency of the desired maneuver
2638  *
2639  * the object of this method is to evaluate the above circumstances and
2640  * either:
2641  * - allow the current maneuver (state, latDist)
2642  * - to override the current maneuver with a distance-keeping maneuver
2643  *
2644  *
2645  * laneChangeModel/driver parameters
2646  * - bool pushy (willingness to encroach)
2647  * - float minGap at 100km/h (to be interpolated for lower speeds (assume 0 at speed 0)
2648  * - gapFactors (a factor for each of the change reasons
2649  *
2650  * further assumptions
2651  * - the maximum of egoSpeed and deltaSpeed can be used when interpolating minGap
2652  * - distance keeping to the edges of the road can be ignored (for now)
2653  *
2654  * currentMinGap = minGap * min(1.0, max(v, abs(v - vOther)) / 100) * gapFactor[lc_reason]
2655  *
2656  * */
2657 
2659  double gapFactor = computeGapFactor(state);
2660  const bool stayInLane = laneOffset == 0 || ((state & LCA_STRATEGIC) != 0 && (state & LCA_STAY) != 0);
2661  const double oldLatDist = latDist;
2662 
2663  // compute gaps after maneuver
2664  const double halfWidth = getWidth() * 0.5;
2665  // if the current maneuver is blocked we will stay where we are
2666  const double oldCenter = myVehicle.getCenterOnEdge();
2667  // surplus gaps. these are used to collect various constraints
2668  // if they do not permit the desired maneuvre, should override it to better maintain distance
2669  // stay within the current edge
2670  double surplusGapRight = oldCenter - halfWidth;
2671  double surplusGapLeft = myVehicle.getLane()->getEdge().getWidth() - oldCenter - halfWidth;
2672 #ifdef DEBUG_KEEP_LATGAP
2673  if (gDebugFlag2) {
2674  std::cout << "\n " << SIMTIME << " keepLatGap() laneOffset=" << laneOffset
2675  << " latDist=" << latDist
2676  << " state=" << toString((LaneChangeAction)state)
2677  << " blocked=" << toString((LaneChangeAction)blocked)
2678  << " gapFactor=" << gapFactor
2679  << " stayInLane=" << stayInLane << "\n"
2680  << " stayInEdge: surplusGapRight=" << surplusGapRight << " surplusGapLeft=" << surplusGapLeft << "\n";
2681  }
2682 #endif
2683  // staying within the edge overrides all minGap considerations
2684  if (surplusGapLeft < 0 || surplusGapRight < 0) {
2685  gapFactor = 0;
2686  }
2687 
2688  // maintain gaps to vehicles on the current lane
2689  // ignore vehicles that are too far behind
2690  const double netOverlap = -myVehicle.getVehicleType().getLength() * 0.5;
2691  updateGaps(leaders, myVehicle.getLane()->getRightSideOnEdge(), oldCenter, gapFactor, surplusGapRight, surplusGapLeft, true);
2692  updateGaps(followers, myVehicle.getLane()->getRightSideOnEdge(), oldCenter, gapFactor, surplusGapRight, surplusGapLeft, true, netOverlap);
2693 
2694  if (laneOffset != 0) {
2695  // maintain gaps to vehicles on the target lane
2696  updateGaps(neighLeaders, neighLane.getRightSideOnEdge(), oldCenter, gapFactor, surplusGapRight, surplusGapLeft, true);
2697  updateGaps(neighFollowers, neighLane.getRightSideOnEdge(), oldCenter, gapFactor, surplusGapRight, surplusGapLeft, true, netOverlap);
2698  }
2699 #ifdef DEBUG_KEEP_LATGAP
2700  if (gDebugFlag2) {
2701  std::cout << " minGapLat: surplusGapRight=" << surplusGapRight << " surplusGapLeft=" << surplusGapLeft << "\n"
2702  << " lastGaps: right=" << myLastLateralGapRight << " left=" << myLastLateralGapLeft << "\n";
2703  }
2704 #endif
2705  // we also need to track the physical gap, in addition to the psychological gap
2706  double physicalGapLeft = myLastLateralGapLeft == NO_NEIGHBOR ? surplusGapLeft : myLastLateralGapLeft;
2707  double physicalGapRight = myLastLateralGapRight == NO_NEIGHBOR ? surplusGapRight : myLastLateralGapRight;
2708 
2709  const double halfLaneWidth = myVehicle.getLane()->getWidth() * 0.5;
2710  if (stayInLane || laneOffset == 1) {
2711  // do not move past the right boundary of the current lane (traffic wasn't checked there)
2712  // but assume it's ok to be where we are in case we are already beyond
2713  surplusGapRight = MIN2(surplusGapRight, MAX2(0.0, halfLaneWidth + myVehicle.getLateralPositionOnLane() - halfWidth));
2714  physicalGapRight = MIN2(physicalGapRight, MAX2(0.0, halfLaneWidth + myVehicle.getLateralPositionOnLane() - halfWidth));
2715  }
2716  if (stayInLane || laneOffset == -1) {
2717  // do not move past the left boundary of the current lane (traffic wasn't checked there)
2718  // but assume it's ok to be where we are in case we are already beyond
2719  surplusGapLeft = MIN2(surplusGapLeft, MAX2(0.0, halfLaneWidth - myVehicle.getLateralPositionOnLane() - halfWidth));
2720  physicalGapLeft = MIN2(physicalGapLeft, MAX2(0.0, halfLaneWidth - myVehicle.getLateralPositionOnLane() - halfWidth));
2721  }
2722 #ifdef DEBUG_KEEP_LATGAP
2723  if (gDebugFlag2) {
2724  std::cout << " stayInLane: surplusGapRight=" << surplusGapRight << " surplusGapLeft=" << surplusGapLeft << "\n";
2725  }
2726 #endif
2727 
2728  if (surplusGapRight + surplusGapLeft < 0) {
2729  // insufficient lateral space to fulfill all requirements. apportion space proportionally
2730  if ((state & LCA_CHANGE_REASONS) == 0) {
2731  state |= LCA_SUBLANE;
2732  }
2733  const double equalDeficit = 0.5 * (surplusGapLeft + surplusGapRight);
2734  if (surplusGapRight < surplusGapLeft) {
2735  // shift further to the left but no further than there is physical space
2736  const double delta = MIN2(equalDeficit - surplusGapRight, physicalGapLeft);
2737  latDist = delta;
2738  maneuverDist = delta;
2739 #ifdef DEBUG_KEEP_LATGAP
2740  if (gDebugFlag2) {
2741  std::cout << " insufficient latSpace, move left: delta=" << delta << "\n";
2742  }
2743 #endif
2744  } else {
2745  // shift further to the right but no further than there is physical space
2746  const double delta = MIN2(equalDeficit - surplusGapLeft, physicalGapRight);
2747  latDist = -delta;
2748  maneuverDist = -delta;
2749 #ifdef DEBUG_KEEP_LATGAP
2750  if (gDebugFlag2) {
2751  std::cout << " insufficient latSpace, move right: delta=" << delta << "\n";
2752  }
2753 #endif
2754  }
2755  } else {
2756  // sufficient space. move as far as the gaps permit
2757  latDist = MAX2(MIN2(latDist, surplusGapLeft), -surplusGapRight);
2758  maneuverDist = MAX2(MIN2(maneuverDist, surplusGapLeft), -surplusGapRight);
2759 #ifdef DEBUG_KEEP_LATGAP
2760  if (gDebugFlag2) {
2761  std::cout << " adapted latDist=" << latDist << " maneuverDist=" << maneuverDist << " (old=" << oldLatDist << ")\n";
2762  }
2763 #endif
2764  }
2765  // take into account overriding traci sublane-request
2766  if (myVehicle.hasInfluencer() && myVehicle.getInfluencer().getLatDist() != 0) {
2767  // @note: the influence is reset in MSAbstractLaneChangeModel::setOwnState at the end of the lane-changing code for this vehicle
2768  latDist = myVehicle.getInfluencer().getLatDist();
2769  maneuverDist = myVehicle.getInfluencer().getLatDist();
2770  state |= LCA_TRACI;
2771 #ifdef DEBUG_KEEP_LATGAP
2772  if (gDebugFlag2) {
2773  std::cout << " traci influenced latDist=" << latDist << "\n";
2774  }
2775 #endif
2776  }
2777  // if we cannot move in the desired direction, consider the maneuver blocked anyway
2778  bool nonSublaneChange = (state & (LCA_STRATEGIC | LCA_COOPERATIVE | LCA_SPEEDGAIN | LCA_KEEPRIGHT)) != 0;
2779  if (nonSublaneChange) {
2780  if ((latDist < NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) && (oldLatDist > 0)) {
2781 #ifdef DEBUG_KEEP_LATGAP
2782  if (gDebugFlag2) {
2783  std::cout << " wanted changeToLeft oldLatDist=" << oldLatDist << ", blocked latGap changeToRight\n";
2784  }
2785 #endif
2786  latDist = oldLatDist; // restore old request for usage in decideDirection()
2787  blocked = LCA_OVERLAPPING | LCA_BLOCKED_LEFT;
2788  } else if ((latDist > -NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) && (oldLatDist < 0)) {
2789 #ifdef DEBUG_KEEP_LATGAP
2790  if (gDebugFlag2) {
2791  std::cout << " wanted changeToRight oldLatDist=" << oldLatDist << ", blocked latGap changeToLeft\n";
2792  }
2793 #endif
2794  latDist = oldLatDist; // restore old request for usage in decideDirection()
2795  blocked = LCA_OVERLAPPING | LCA_BLOCKED_RIGHT;
2796  }
2797  }
2798  // if we move, even though we wish to stay, update the change reason (except for TraCI)
2799  if (fabs(latDist) > NUMERICAL_EPS * myVehicle.getActionStepLengthSecs() && oldLatDist == 0) {
2800  state &= (~(LCA_CHANGE_REASONS | LCA_STAY) | LCA_TRACI);
2801  }
2802  // update blocked status
2803  if (fabs(latDist - oldLatDist) > NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) {
2804 #ifdef DEBUG_KEEP_LATGAP
2805  if (gDebugFlag2) {
2806  std::cout << " latDistUpdated=" << latDist << " oldLatDist=" << oldLatDist << "\n";
2807  }
2808 #endif
2809  blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset, leaders, followers, blockers, neighLeaders, neighFollowers, neighBlockers, 0, 0, nonSublaneChange);
2810  }
2811  if (fabs(latDist) > NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) {
2812  state = (state & ~LCA_STAY);
2813  if ((state & LCA_CHANGE_REASONS) == 0) {
2814  state |= LCA_SUBLANE;
2815  }
2816  } else {
2817  if ((state & LCA_SUBLANE) != 0) {
2818  state |= LCA_STAY;
2819  }
2820  // avoid setting blinker due to numerical issues
2821  latDist = 0;
2822  }
2823 #if defined(DEBUG_KEEP_LATGAP) || defined(DEBUG_STATE)
2824  if (gDebugFlag2) {
2825  std::cout << " latDist2=" << latDist
2826  << " state2=" << toString((LaneChangeAction)state)
2827  << " lastGapLeft=" << myLastLateralGapLeft
2828  << " lastGapRight=" << myLastLateralGapRight
2829  << " blockedAfter=" << toString((LaneChangeAction)blocked)
2830  << "\n";
2831  }
2832 #endif
2833  return state;
2834 }
2835 
2836 
2837 void
2838 MSLCM_SL2015::updateGaps(const MSLeaderDistanceInfo& others, double foeOffset, double oldCenter, double gapFactor,
2839  double& surplusGapRight, double& surplusGapLeft,
2840  bool saveMinGap, double netOverlap,
2841  double latDist,
2842  std::vector<CLeaderDist>* collectBlockers) {
2843  if (others.hasVehicles()) {
2844  const double halfWidth = getWidth() * 0.5 + NUMERICAL_EPS;
2845  const double baseMinGap = myVehicle.getVehicleType().getMinGapLat();
2846  for (int i = 0; i < others.numSublanes(); ++i) {
2847  if (others[i].first != 0 && others[i].second <= 0
2848  && myCFRelated.count(others[i].first) == 0
2849  && (netOverlap == 0 || others[i].second + others[i].first->getVehicleType().getMinGap() < netOverlap)) {
2851  const MSVehicle* foe = others[i].first;
2852  const double res = MSGlobals::gLateralResolution > 0 ? MSGlobals::gLateralResolution : others[i].first->getLane()->getWidth();
2853  double foeRight, foeLeft;
2854  others.getSublaneBorders(i, foeOffset, foeRight, foeLeft);
2855  const double foeCenter = foeRight + 0.5 * res;
2856  const double gap = MIN2(fabs(foeRight - oldCenter), fabs(foeLeft - oldCenter)) - halfWidth;
2857  const double deltaV = MIN2(LATGAP_SPEED_THRESHOLD, MAX3(LATGAP_SPEED_THRESHOLD2, myVehicle.getSpeed(), (double)fabs(myVehicle.getSpeed() - foe->getSpeed())));
2858  const double desiredMinGap = baseMinGap * deltaV / LATGAP_SPEED_THRESHOLD;
2859  const double currentMinGap = desiredMinGap * gapFactor; // pushy vehicles may accept a lower lateral gap temporarily
2860  /*
2861  if (netOverlap != 0) {
2862  // foe vehicle is follower with its front ahead of the ego midpoint
2863  // scale gap requirements so it gets lower for foe which are further behind ego
2864  //
2865  // relOverlap approaches 0 as the foe gets closer to the midpoint and it equals 1 if the foe is driving head-to-head
2866  const double relOverlap = 1 - (others[i].second + others[i].first->getVehicleType().getMinGap()) / netOverlap;
2867  currentMinGap *= currOverlap * relOverlap;
2868  }
2869  */
2870 #if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
2871  if (debugVehicle()) {
2872  std::cout << " updateGaps"
2873  << " i=" << i
2874  << " foe=" << foe->getID()
2875  << " foeRight=" << foeRight
2876  << " foeLeft=" << foeLeft
2877  << " oldCenter=" << oldCenter
2878  << " gap=" << others[i].second
2879  << " latgap=" << gap
2880  << " currentMinGap=" << currentMinGap
2881  << " surplusGapRight=" << surplusGapRight
2882  << " surplusGapLeft=" << surplusGapLeft
2883  << "\n";
2884  }
2885 #endif
2886 
2887  // If foe is maneuvering towards ego, reserve some additional distance.
2888  // But don't expect the foe to come closer than currentMinGap if it isn't already there.
2889  // (XXX: How can the ego know the foe's maneuver dist?)
2890  if (foeCenter < oldCenter) {
2891  const double foeManeuverDist = MAX2(0., foe->getLaneChangeModel().getManeuverDist());
2892  surplusGapRight = MIN3(surplusGapRight, gap - currentMinGap, MAX2(currentMinGap, gap - foeManeuverDist));
2893  } else {
2894  const double foeManeuverDist = -MIN2(0., foe->getLaneChangeModel().getManeuverDist());
2895  surplusGapLeft = MIN3(surplusGapLeft, gap - currentMinGap, MAX2(currentMinGap, gap - foeManeuverDist));
2896  }
2897  if (saveMinGap) {
2898  if (foeCenter < oldCenter) {
2899 #if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
2900  if (gDebugFlag2 && gap < myLastLateralGapRight) {
2901  std::cout << " new minimum rightGap=" << gap << "\n";
2902  }
2903 #endif
2904  myLastLateralGapRight = MIN2(myLastLateralGapRight, gap);
2905  } else {
2906 #if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
2907  if (gDebugFlag2 && gap < myLastLateralGapLeft) {
2908  std::cout << " new minimum leftGap=" << gap << "\n";
2909  }
2910 #endif
2911  myLastLateralGapLeft = MIN2(myLastLateralGapLeft, gap);
2912  }
2913  }
2914  if (collectBlockers != 0) {
2915  // check if the vehicle is blocking a desire lane change
2916  if ((foeCenter < oldCenter && latDist < 0 && gap < (desiredMinGap - latDist))
2917  || (foeCenter > oldCenter && latDist > 0 && gap < (desiredMinGap + latDist))) {
2918  collectBlockers->push_back(others[i]);
2919  }
2920  }
2921  }
2922  }
2923  }
2924 }
2925 
2926 
2927 double
2928 MSLCM_SL2015::getWidth() const {
2929  return myVehicle.getVehicleType().getWidth() + NUMERICAL_EPS;
2930 }
2931 
2932 
2933 double
2934 MSLCM_SL2015::computeSpeedLat(double latDist, double& maneuverDist) {
2935  int currentDirection = mySpeedLat >= 0 ? 1 : -1;
2936  int directionWish = latDist >= 0 ? 1 : -1;
2937  const double maxSpeedLat = MIN2(myVehicle.getVehicleType().getMaxSpeedLat(),
2938  myMaxSpeedLatStanding + myMaxSpeedLatFactor * myVehicle.getSpeed());
2939 
2940 #ifdef DEBUG_MANEUVER
2941  if (debugVehicle()) {
2942  std::cout << SIMTIME
2943  << " veh=" << myVehicle.getID()
2944  << " computeSpeedLat()"
2945  << " currentDirection=" << currentDirection
2946  << " directionWish=" << directionWish
2947  << std::endl;
2948  }
2949 #endif
2950  // reduced lateral speed (in the desired direction). Don't change direction against desired.
2951  double speedDecel;
2952  if (directionWish == 1) {
2953  speedDecel = MAX2(mySpeedLat - ACCEL2SPEED(myAccelLat), 0.);
2954  } else {
2955  speedDecel = MIN2(mySpeedLat + ACCEL2SPEED(myAccelLat), 0.);
2956  }
2957  // Eventually reduce lateral speed even more to ensure safety
2958  double speedDecelSafe = MAX2(MIN2(speedDecel, DIST2SPEED(mySafeLatDistLeft)), DIST2SPEED(-mySafeLatDistRight));
2959 
2960  // increased lateral speed (in the desired direction)
2961  double speedAccel = MAX2(MIN2(mySpeedLat + directionWish * ACCEL2SPEED(myAccelLat), maxSpeedLat), -maxSpeedLat);
2962  // increase lateral speed more strongly to ensure safety (when moving in the wrong direction)
2963  double speedAccelSafe = latDist * speedAccel >= 0 ? speedAccel : 0;
2964 
2965  // can we reach the target distance in a single step? (XXX: assumes "Euler" update)
2966  double speedBound = DIST2SPEED(latDist);
2967  // for lat-gap keeping maneuvres myOrigLatDist may be 0
2968  const double fullLatDist = latDist > 0 ? MIN2(mySafeLatDistLeft, MAX2(maneuverDist, latDist)) : MAX2(-mySafeLatDistRight, MIN2(maneuverDist, latDist));
2969 
2970  // update maneuverDist, if safety constraints apply in its direction
2971  if (maneuverDist * latDist > 0) {
2972  maneuverDist = fullLatDist;
2973  }
2974 
2975 #ifdef DEBUG_MANEUVER
2976  if (debugVehicle()) {
2977  std::cout << SIMTIME
2978  << " veh=" << myVehicle.getID()
2979  << " speedLat=" << mySpeedLat
2980  << " latDist=" << latDist
2981  << " maneuverDist=" << maneuverDist
2982  << " mySafeLatDistRight=" << mySafeLatDistRight
2983  << " mySafeLatDistLeft=" << mySafeLatDistLeft
2984  << " fullLatDist=" << fullLatDist
2985  << " speedAccel=" << speedAccel
2986  << " speedDecel=" << speedDecel
2987  << " speedBound=" << speedBound
2988  << std::endl;
2989  }
2990 #endif
2991  if (speedDecel * speedAccel <= 0 && (
2992  // speedAccel and speedDecel bracket speed 0. This means we can end the maneuver
2993  (latDist >= 0 && speedAccel >= speedBound && speedBound >= speedDecel)
2994  || (latDist <= 0 && speedAccel <= speedBound && speedBound <= speedDecel))) {
2995  // we can reach the desired value in this step
2996 #ifdef DEBUG_MANEUVER
2997  if (debugVehicle()) {
2998  std::cout << " computeSpeedLat a)\n";
2999  }
3000 #endif
3001  return speedBound;
3002  }
3003  // are we currently moving in the wrong direction?
3004  if (latDist * mySpeedLat < 0) {
3005 #ifdef DEBUG_MANEUVER
3006  if (debugVehicle()) {
3007  std::cout << " computeSpeedLat b)\n";
3008  }
3009 #endif
3010  return speedAccelSafe;
3011  }
3012  // check if the remaining distance allows to accelerate laterally
3013  double minDistAccel = SPEED2DIST(speedAccel) + currentDirection * MSCFModel::brakeGapEuler(fabs(speedAccel), myAccelLat, 0); // most we can move in the target direction
3014  if ((fabs(minDistAccel) < fabs(fullLatDist)) || (fabs(minDistAccel - fullLatDist) < NUMERICAL_EPS)) {
3015 #ifdef DEBUG_MANEUVER
3016  if (debugVehicle()) {
3017  std::cout << " computeSpeedLat c)\n";
3018  }
3019 #endif
3020  return speedAccel;
3021  } else {
3022 #ifdef DEBUG_MANEUVER
3023  if (debugVehicle()) {
3024  std::cout << " minDistAccel=" << minDistAccel << "\n";
3025  }
3026 #endif
3027  // check if the remaining distance allows to maintain current lateral speed
3028  double minDistCurrent = SPEED2DIST(mySpeedLat) + currentDirection * MSCFModel::brakeGapEuler(fabs(mySpeedLat), myAccelLat, 0);
3029  if ((fabs(minDistCurrent) < fabs(fullLatDist)) || (fabs(minDistCurrent - fullLatDist) < NUMERICAL_EPS)) {
3030 #ifdef DEBUG_MANEUVER
3031  if (debugVehicle()) {
3032  std::cout << " computeSpeedLat d)\n";
3033  }
3034 #endif
3035  return mySpeedLat;
3036  }
3037  }
3038  // reduce lateral speed
3039 #ifdef DEBUG_MANEUVER
3040  if (debugVehicle()) {
3041  std::cout << " computeSpeedLat e)\n";
3042  }
3043 #endif
3044  return speedDecelSafe;
3045 }
3046 
3047 
3048 void
3049 MSLCM_SL2015::commitManoeuvre(int blocked, int blockedFully,
3050  const MSLeaderDistanceInfo& leaders,
3051  const MSLeaderDistanceInfo& neighLeaders,
3052  const MSLane& neighLane,
3053  double maneuverDist) {
3054  if (!blocked && !blockedFully && !myCanChangeFully) {
3055  // round to full action steps
3056  double secondsToLeaveLane;
3057  if (MSGlobals::gSemiImplicitEulerUpdate) {
3058  secondsToLeaveLane = ceil(fabs(maneuverDist) / myVehicle.getVehicleType().getMaxSpeedLat() / myVehicle.getActionStepLengthSecs()) * myVehicle.getActionStepLengthSecs();
3059  // XXX myAccelLat must be taken into account (refs #3601, see ballistic case for solution)
3060 
3061  // XXX This also causes probs: if the difference between the current speed and the committed is higher than the maximal decel,
3062  // the vehicle may pass myLeftSpace before completing the maneuver.
3063  myCommittedSpeed = MIN3(myLeftSpace / secondsToLeaveLane,
3064  myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle),
3065  myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle));
3066  } else {
3067 
3068  // Calculate seconds needed for leaving lane assuming start from lateral speed zero, and lat.accel == -lat.decel
3069  secondsToLeaveLane = MSCFModel::estimateArrivalTime(fabs(maneuverDist), 0., 0., myVehicle.getVehicleType().getMaxSpeedLat(), myAccelLat, myAccelLat);
3070  // round to full action steps
3071  secondsToLeaveLane = ceil(secondsToLeaveLane / myVehicle.getActionStepLengthSecs()) * myVehicle.getActionStepLengthSecs();
3072 
3073  // committed speed will eventually be pushed into a drive item during the next planMove() step. This item
3074  // will not be read before the next action step at current time + actionStepLength-TS, so we need to schedule the corresponding speed.
3075  const double timeTillActionStep = myVehicle.getActionStepLengthSecs() - TS;
3076  const double nextActionStepSpeed = MAX2(0., myVehicle.getSpeed() + timeTillActionStep * myVehicle.getAcceleration());
3077  double nextLeftSpace;
3078  if (nextActionStepSpeed > 0.) {
3079  nextLeftSpace = myLeftSpace - timeTillActionStep * (myVehicle.getSpeed() + nextActionStepSpeed) * 0.5;
3080  } else if (myVehicle.getAcceleration() == 0) {
3081  nextLeftSpace = myLeftSpace;
3082  } else {
3083  assert(myVehicle.getAcceleration() < 0.);
3084  nextLeftSpace = myLeftSpace + (myVehicle.getSpeed() * myVehicle.getSpeed() / myVehicle.getAcceleration()) * 0.5;
3085  }
3086  const double avoidArrivalSpeed = nextActionStepSpeed + TS * MSCFModel::avoidArrivalAccel(nextLeftSpace, secondsToLeaveLane - timeTillActionStep, nextActionStepSpeed);
3087 
3088  myCommittedSpeed = MIN3(avoidArrivalSpeed,
3089  myVehicle.getSpeed() + myVehicle.getCarFollowModel().getMaxAccel() * myVehicle.getActionStepLengthSecs(),
3090  myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle));
3091 
3092 #ifdef DEBUG_MANEUVER
3093  if (gDebugFlag2) {
3094  std::cout << SIMTIME
3095  << " veh=" << myVehicle.getID()
3096  << " avoidArrivalSpeed=" << avoidArrivalSpeed
3097  << " currentSpeed=" << myVehicle.getSpeed()
3098  << " myLeftSpace=" << myLeftSpace
3099  << "\n nextLeftSpace=" << nextLeftSpace
3100  << " nextLeftSpace=" << nextActionStepSpeed
3101  << " nextActionStepRemainingSeconds=" << secondsToLeaveLane - timeTillActionStep
3102  << "\n";
3103  }
3104 #endif
3105  }
3106  myCommittedSpeed = commitFollowSpeed(myCommittedSpeed, maneuverDist, secondsToLeaveLane, leaders, myVehicle.getLane()->getRightSideOnEdge());
3107  myCommittedSpeed = commitFollowSpeed(myCommittedSpeed, maneuverDist, secondsToLeaveLane, neighLeaders, neighLane.getRightSideOnEdge());
3108  if (myCommittedSpeed < myVehicle.getCarFollowModel().minNextSpeed(myVehicle.getSpeed(), &myVehicle)) {
3109  myCommittedSpeed = 0;
3110  }
3111 #ifdef DEBUG_MANEUVER
3112  if (gDebugFlag2) {
3113  std::cout << SIMTIME
3114  << " veh=" << myVehicle.getID()
3115  << " secondsToLeave=" << secondsToLeaveLane
3116  << " maxNext=" << myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle)
3117  << " committed=" << myCommittedSpeed
3118  << "\n";
3119  }
3120 #endif
3121  }
3122 }
3123 
3124 double
3125 MSLCM_SL2015::commitFollowSpeed(double speed, double latDist, double secondsToLeaveLane, const MSLeaderDistanceInfo& leaders, double foeOffset) const {
3126  if (leaders.hasVehicles()) {
3127  // we distinguish 3 cases
3128  // - vehicles with lateral overlap at the end of the maneuver: try to follow safely
3129  // - vehicles with overlap at the start of the maneuver: avoid collision within secondsToLeaveLane
3130  // - vehicles without overlap: ignore
3131 
3132  const double maxDecel = myVehicle.getCarFollowModel().getMaxDecel();
3133  // temporarily use another decel value
3134  MSCFModel& cfmodel = const_cast<MSCFModel&>(myVehicle.getCarFollowModel());
3135  cfmodel.setMaxDecel(maxDecel / getSafetyFactor());
3136 
3137  const double vehWidth = getWidth();
3138  const double rightVehSide = myVehicle.getCenterOnEdge() - 0.5 * vehWidth;
3139  const double leftVehSide = rightVehSide + vehWidth;
3140  const double rightVehSideDest = rightVehSide + latDist;
3141  const double leftVehSideDest = leftVehSide + latDist;
3142 #ifdef DEBUG_MANEUVER
3143  if (gDebugFlag2) {
3144  std::cout << " commitFollowSpeed"
3145  << " latDist=" << latDist
3146  << " foeOffset=" << foeOffset
3147  << " vehRight=" << rightVehSide
3148  << " vehLeft=" << leftVehSide
3149  << " destRight=" << rightVehSideDest
3150  << " destLeft=" << leftVehSideDest
3151  << "\n";
3152  }
3153 #endif
3154  for (int i = 0; i < leaders.numSublanes(); ++i) {
3155  CLeaderDist vehDist = leaders[i];
3156  if (vehDist.first != 0) {
3157  const MSVehicle* leader = vehDist.first;
3158  // only check the current stripe occuped by foe (transform into edge-coordinates)
3159  double foeRight, foeLeft;
3160  leaders.getSublaneBorders(i, foeOffset, foeRight, foeLeft);
3161 #ifdef DEBUG_MANEUVER
3162  if (gDebugFlag2) {
3163  std::cout << " foe=" << vehDist.first->getID()
3164  << " gap=" << vehDist.second
3165  << " secGap=" << myVehicle.getCarFollowModel().getSecureGap(myVehicle.getSpeed(), leader->getSpeed(), leader->getCarFollowModel().getMaxDecel())
3166  << " foeRight=" << foeRight
3167  << " foeLeft=" << foeLeft
3168  << " overlapBefore=" << overlap(rightVehSide, leftVehSide, foeRight, foeLeft)
3169  << " overlapDest=" << overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)
3170  << "\n";
3171  }
3172 #endif
3173  if (overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)) {
3174  // case 1
3175  assert(vehDist.second >= 0);
3176  const double vSafe = myVehicle.getCarFollowModel().followSpeed(
3177  &myVehicle, speed, vehDist.second, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel());
3178  speed = MIN2(speed, vSafe);
3179 #ifdef DEBUG_MANEUVER
3180  if (gDebugFlag2) {
3181  std::cout << " case1 vsafe=" << vSafe << " speed=" << speed << "\n";
3182  }
3183 #endif
3184  } else if (overlap(rightVehSide, leftVehSide, foeRight, foeLeft)) {
3185  // case 2
3186  const double vSafe = myVehicle.getCarFollowModel().followSpeedTransient(
3187  secondsToLeaveLane,
3188  &myVehicle, speed, vehDist.second, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel());
3189  speed = MIN2(speed, vSafe);
3190 #ifdef DEBUG_MANEUVER
3191  if (gDebugFlag2) {
3192  std::cout << " case2 vsafe=" << vSafe << " speed=" << speed << "\n";
3193  }
3194 #endif
3195  }
3196  }
3197  }
3198  // restore original deceleration
3199  cfmodel.setMaxDecel(maxDecel);
3200 
3201  }
3202  return speed;
3203 }
3204 
3205 double
3206 MSLCM_SL2015::getSafetyFactor() const {
3207  return 1 / ((1 + 0.5 * myImpatience) * myAssertive);
3208 }
3209 
3210 
3211 std::string
3212 MSLCM_SL2015::getParameter(const std::string& key) const {
3213  if (key == toString(SUMO_ATTR_LCA_STRATEGIC_PARAM)) {
3214  return toString(myStrategicParam);
3215  } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_PARAM)) {
3216  return toString(myCooperativeParam);
3217  } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_PARAM)) {
3218  return toString(mySpeedGainParam);
3219  } else if (key == toString(SUMO_ATTR_LCA_KEEPRIGHT_PARAM)) {
3220  return toString(myKeepRightParam);
3221  } else if (key == toString(SUMO_ATTR_LCA_SUBLANE_PARAM)) {
3222  return toString(mySublaneParam);
3223  } else if (key == toString(SUMO_ATTR_LCA_PUSHY)) {
3224  return toString(myPushy);
3225  } else if (key == toString(SUMO_ATTR_LCA_ASSERTIVE)) {
3226  return toString(myAssertive);
3227  } else if (key == toString(SUMO_ATTR_LCA_IMPATIENCE)) {
3228  return toString(myImpatience);
3229  } else if (key == toString(SUMO_ATTR_LCA_TIME_TO_IMPATIENCE)) {
3230  return toString(myTimeToImpatience);
3231  } else if (key == toString(SUMO_ATTR_LCA_ACCEL_LAT)) {
3232  return toString(myAccelLat);
3233  } else if (key == toString(SUMO_ATTR_LCA_LOOKAHEADLEFT)) {
3234  return toString(myLookaheadLeft);
3235  } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAINRIGHT)) {
3236  return toString(mySpeedGainRight);
3237  }
3238  throw InvalidArgument("Parameter '" + key + "' is not supported for laneChangeModel of type '" + toString(myModel) + "'");
3239 }
3240 
3241 void
3242 MSLCM_SL2015::setParameter(const std::string& key, const std::string& value) {
3243  double doubleValue;
3244  try {
3245  doubleValue = TplConvert::_2double(value.c_str());
3246  } catch (NumberFormatException&) {
3247  throw InvalidArgument("Setting parameter '" + key + "' requires a number for laneChangeModel of type '" + toString(myModel) + "'");
3248  }
3249  if (key == toString(SUMO_ATTR_LCA_STRATEGIC_PARAM)) {
3250  myStrategicParam = doubleValue;
3251  } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_PARAM)) {
3252  myCooperativeParam = doubleValue;
3253  } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_PARAM)) {
3254  mySpeedGainParam = doubleValue;
3255  } else if (key == toString(LCA_KEEPRIGHT)) {
3256  myKeepRightParam = doubleValue;
3257  } else if (key == toString(SUMO_ATTR_LCA_SUBLANE_PARAM)) {
3258  mySublaneParam = doubleValue;
3259  } else if (key == toString(SUMO_ATTR_LCA_PUSHY)) {
3260  myPushy = doubleValue;
3261  } else if (key == toString(SUMO_ATTR_LCA_ASSERTIVE)) {
3262  myAssertive = doubleValue;
3263  } else if (key == toString(SUMO_ATTR_LCA_IMPATIENCE)) {
3264  myImpatience = doubleValue;
3265  myMinImpatience = doubleValue;
3266  } else if (key == toString(SUMO_ATTR_LCA_TIME_TO_IMPATIENCE)) {
3267  myTimeToImpatience = doubleValue;
3268  } else if (key == toString(SUMO_ATTR_LCA_ACCEL_LAT)) {
3269  myAccelLat = doubleValue;
3270  } else if (key == toString(SUMO_ATTR_LCA_TURN_ALIGNMENT_DISTANCE)) {
3271  myTurnAlignmentDist = doubleValue;
3272  } else if (key == toString(SUMO_ATTR_LCA_LOOKAHEADLEFT)) {
3273  myLookaheadLeft = doubleValue;
3274  } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAINRIGHT)) {
3275  mySpeedGainRight = doubleValue;
3276  } else {
3277  throw InvalidArgument("Setting parameter '" + key + "' is not supported for laneChangeModel of type '" + toString(myModel) + "'");
3278  }
3279  initDerivedParameters();
3280 }
3281 
3282 
3283 int
3284 MSLCM_SL2015::wantsChange(
3285  int laneOffset,
3286  MSAbstractLaneChangeModel::MSLCMessager& /* msgPass */,
3287  int blocked,
3288  const std::pair<MSVehicle*, double>& leader,
3289  const std::pair<MSVehicle*, double>& neighLead,
3290  const std::pair<MSVehicle*, double>& neighFollow,
3291  const MSLane& neighLane,
3292  const std::vector<MSVehicle::LaneQ>& preb,
3293  MSVehicle** lastBlocked,
3294  MSVehicle** firstBlocked) {
3295 
3296  const LaneChangeAction alternatives = LCA_NONE; // @todo pas this data
3297 
3298 #ifdef DEBUG_WANTSCHANGE
3299  if (DEBUG_COND) {
3300  std::cout << "\nWANTS_CHANGE\n" << SIMTIME
3301  //<< std::setprecision(10)
3302  << " veh=" << myVehicle.getID()
3303  << " lane=" << myVehicle.getLane()->getID()
3304  << " pos=" << myVehicle.getPositionOnLane()
3305  << " posLat=" << myVehicle.getLateralPositionOnLane()
3306  << " speed=" << myVehicle.getSpeed()
3307  << " considerChangeTo=" << (laneOffset == -1 ? "right" : "left")
3308  << "\n";
3309  }
3310 #endif
3311 
3312  double latDist = 0;
3313  const MSLane* dummy = myVehicle.getLane();
3314  MSLeaderDistanceInfo leaders(leader, dummy);
3315  MSLeaderDistanceInfo followers(std::make_pair((MSVehicle*)0, -1), dummy);
3316  MSLeaderDistanceInfo blockers(std::make_pair((MSVehicle*)0, -1), dummy);
3317  MSLeaderDistanceInfo neighLeaders(neighLead, dummy);
3318  MSLeaderDistanceInfo neighFollowers(neighFollow, dummy);
3319  MSLeaderDistanceInfo neighBlockers(std::make_pair((MSVehicle*)0, -1), dummy);
3320 
3321  double maneuverDist;
3322  int result = _wantsChangeSublane(laneOffset,
3323  alternatives,
3324  leaders, followers, blockers,
3325  neighLeaders, neighFollowers, neighBlockers,
3326  neighLane, preb,
3327  lastBlocked, firstBlocked, latDist, maneuverDist, blocked);
3328 
3329  myManeuverDist = 0;
3330  myCanChangeFully = true;
3331  // ignore sublane motivation
3332  result &= ~LCA_SUBLANE;
3333  result |= getLCA(result, latDist);
3334 
3335 #if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE)
3336  if (DEBUG_COND) {
3337  if (result & LCA_WANTS_LANECHANGE) {
3338  std::cout << SIMTIME
3339  << " veh=" << myVehicle.getID()
3340  << " wantsChangeTo=" << (laneOffset == -1 ? "right" : "left")
3341  << ((result & LCA_URGENT) ? " (urgent)" : "")
3342  << ((result & LCA_CHANGE_TO_HELP) ? " (toHelp)" : "")
3343  << ((result & LCA_STRATEGIC) ? " (strat)" : "")
3344  << ((result & LCA_COOPERATIVE) ? " (coop)" : "")
3345  << ((result & LCA_SPEEDGAIN) ? " (speed)" : "")
3346  << ((result & LCA_KEEPRIGHT) ? " (keepright)" : "")
3347  << ((result & LCA_TRACI) ? " (traci)" : "")
3348  << ((blocked & LCA_BLOCKED) ? " (blocked)" : "")
3349  << ((blocked & LCA_OVERLAPPING) ? " (overlap)" : "")
3350  << "\n\n\n";
3351  }
3352  }
3353 #endif
3354 
3355  return result;
3356 }
3357 
3358 /****************************************************************************/
MSLCM_SL2015::inform
void * inform(void *info, MSVehicle *sender)
Definition: MSLCM_SL2015.cpp:479
LCA_BLOCKED_RIGHT
blocked right
Definition: SUMOXMLDefinitions.h:1104
SUMO_ATTR_LCA_SUBLANE_PARAM
Definition: SUMOXMLDefinitions.h:532
LINKDIR_PARTLEFT
The link is a partial left direction.
Definition: SUMOXMLDefinitions.h:1024
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:313
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:1122
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:3242
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:1070
SPEED2DIST
#define SPEED2DIST(x)
Definition: SUMOTime.h:48
LCA_COOPERATIVE
The action is done to help someone else.
Definition: SUMOXMLDefinitions.h:1066
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:3206
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:1130
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:2608
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:240
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:1084
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:2135
SUMO_const_laneWidth
const double SUMO_const_laneWidth
Definition: StdDefs.h:51
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:2934
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:2003
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:1127
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:1986
MSVehicle::getBestLaneOffset
int getBestLaneOffset() const
Definition: MSVehicle.cpp:4409
MSNet.h
LINKDIR_TURN
The link is a 180 degree turn.
Definition: SUMOXMLDefinitions.h:1016
LCA_RIGHT
Wants go to the right.
Definition: SUMOXMLDefinitions.h:1062
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:4757
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:234
MSLCM_SL2015::myTimeToImpatience
double myTimeToImpatience
Definition: MSLCM_SL2015.h:397
LCA_BLOCKED_LEFT
blocked left
Definition: SUMOXMLDefinitions.h:1102
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:76
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:537
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:1157
LATALIGN_RIGHT
drive on the right side
Definition: SUMOXMLDefinitions.h:1151
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:455
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:1060
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:1068
LCA_AMBACKBLOCKER_STANDING
Definition: SUMOXMLDefinitions.h:1131
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:529
MAX3
T MAX3(T a, T b, T c)
Definition: StdDefs.h:90
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:1094
LCA_RIGHT_IMPATIENCE
#define LCA_RIGHT_IMPATIENCE
Definition: MSLCM_SL2015.cpp:43
LINKDIR_LEFT
The link is a (hard) left direction.
Definition: SUMOXMLDefinitions.h:1020
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:1149
MSLCM_SL2015::myChangeProbThresholdRight
double myChangeProbThresholdRight
Definition: MSLCM_SL2015.h:411
MSVehicle::getLaneChangeModel
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:4028
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:2614
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:1014
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:1058
LCA_NONE
Definition: SUMOXMLDefinitions.h:1056
MSLCM_SL2015::saveBlockerLength
void saveBlockerLength(const MSVehicle *blocker, int lcaCounter)
save space for vehicles which need to counter-lane-change
Definition: MSLCM_SL2015.cpp:1826
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:1161
LCA_WANTS_LANECHANGE
lane can change
Definition: SUMOXMLDefinitions.h:1098
MSLCM_SL2015::getWidth
double getWidth() const
return the widht of this vehicle (padded for numerical stability)
Definition: MSLCM_SL2015.cpp:2928
MSLCM_SL2015::overlap
static bool overlap(double right, double left, double right2, double left2)
return whether the given intervals overlap
Definition: MSLCM_SL2015.cpp:2335
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:1112
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:2343
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:3049
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:3125
SUMO_ATTR_LCA_PUSHY
Definition: SUMOXMLDefinitions.h:533
SUMO_ATTR_LCA_KEEPRIGHT_PARAM
Definition: SUMOXMLDefinitions.h:531
SUMO_ATTR_LCA_ASSERTIVE
Definition: SUMOXMLDefinitions.h:535
LCA_BLOCKED
blocked in all directions
Definition: SUMOXMLDefinitions.h:1110
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:2290
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:4807
LCA_URGENT
The action is urgent (to be defined by lc-model)
Definition: SUMOXMLDefinitions.h:1074
MSCFModel::getSecureGap
virtual double getSecureGap(const double speed, const double leaderSpeed, const double leaderMaxDecel) const
Returns the minimum gap to reserve if the leader is braking at maximum (>=0)
Definition: MSCFModel.h:328
MAGIC_OFFSET
#define MAGIC_OFFSET
Definition: MSLCM_SL2015.cpp:37
SUMO_ATTR_LCA_SPEEDGAINRIGHT
Definition: SUMOXMLDefinitions.h:540
MSCFModel::brakeGapEuler
static double brakeGapEuler(const double speed, const double decel, const double headwayTime)
Definition: MSCFModel.cpp:89
ARRIVAL_POSLAT_CENTER
At the center of the lane.
Definition: SUMOVehicleParameter.h:241
SUMO_ATTR_LCA_STRATEGIC_PARAM
Definition: SUMOXMLDefinitions.h:528
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:1871
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:2416
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:4655
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:1120
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:530
MIN2
T MIN2(T a, T b)
Definition: StdDefs.h:70
LINKDIR_RIGHT
The link is a (hard) right direction.
Definition: SUMOXMLDefinitions.h:1022
MSLane.h
TplConvert.h
LCA_STRATEGIC
The action is needed to follow the route (navigational lc)
Definition: SUMOXMLDefinitions.h:1064
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:1125
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:1106
MSAbstractLaneChangeModel::myOwnState
int myOwnState
The current state of the vehicle.
Definition: MSAbstractLaneChangeModel.h:566
LATALIGN_ARBITRARY
maintain the current alignment
Definition: SUMOXMLDefinitions.h:1155
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:1969
LINKDIR_PARTRIGHT
The link is a partial right direction.
Definition: SUMOXMLDefinitions.h:1026
MSAbstractLaneChangeModel::cancelRequest
bool cancelRequest(int state, int laneOffset)
whether the influencer cancels the given request
Definition: MSAbstractLaneChangeModel.cpp:389
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:498
SUMO_ATTR_LCA_IMPATIENCE
Definition: SUMOXMLDefinitions.h:536
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:1950
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:1153
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:2423
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:543
LCM_SL2015
Definition: SUMOXMLDefinitions.h:1140
MSVehicle::getInfluencer
Influencer & getInfluencer()
Returns the velocity/lane influencer.
Definition: MSVehicle.cpp:5197
LaneChangeAction
LaneChangeAction
The state of a vehicle&#39;s lane-change behavior.
Definition: SUMOXMLDefinitions.h:1052
LCA_AMBLOCKINGFOLLOWER
Definition: SUMOXMLDefinitions.h:1121
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:474
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:1090
MSLCM_SL2015::slowDownForBlocked
int slowDownForBlocked(MSVehicle **blocked, int state)
compute useful slowdowns for blocked vehicles
Definition: MSLCM_SL2015.cpp:1788
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:384
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:1159
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:60
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:3212
LCA_TRACI
The action is due to a TraCI request.
Definition: SUMOXMLDefinitions.h:1072
LINKDIR_TURN_LEFTHAND
The link is a 180 degree turn (left-hand network)
Definition: SUMOXMLDefinitions.h:1018
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:295
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:83
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
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:1952
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:4649
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:4040
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:1086
MSVehicle::influenceChangeDecision
int influenceChangeDecision(int state)
allow TraCI to influence a lane change decision
Definition: MSVehicle.cpp:5221
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:538
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:3284
SUMO_ATTR_LCA_LOOKAHEADLEFT
Definition: SUMOXMLDefinitions.h:539
RandHelper.h
LCA_BLOCKED_BY_FOLLOWER
blocker by follower
Definition: SUMOXMLDefinitions.h:1108
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:1088
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:1123
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:1884
MSLCM_SL2015::mySublaneParam
double mySublaneParam
Definition: MSLCM_SL2015.h:388
LCA_BLOCKED_BY_LEFT_LEADER
Definition: SUMOXMLDefinitions.h:1082
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:2838
LINKDIR_NODIR
The link has no direction (is a dead end link)
Definition: SUMOXMLDefinitions.h:1028
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:534
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