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