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