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