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