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