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