SUMO - Simulation of Urban MObility
MSLCM_LC2013.cpp
Go to the documentation of this file.
1 /****************************************************************************/
11 // A lane change model developed by D. Krajzewicz, J. Erdmann et al. between 2004 and 2013
12 /****************************************************************************/
13 // SUMO, Simulation of Urban MObility; see http://sumo.dlr.de/
14 // Copyright (C) 2001-2016 DLR (http://www.dlr.de/) and contributors
15 /****************************************************************************/
16 //
17 // This file is part of SUMO.
18 // SUMO is free software: you can redistribute it and/or modify
19 // it under the terms of the GNU General Public License as published by
20 // the Free Software Foundation, either version 3 of the License, or
21 // (at your option) any later version.
22 //
23 /****************************************************************************/
24 
25 
26 // ===========================================================================
27 // included modules
28 // ===========================================================================
29 #ifdef _MSC_VER
30 #include <windows_config.h>
31 #else
32 #include <config.h>
33 #endif
34 
35 #include <iostream>
37 #include <microsim/MSEdge.h>
38 #include <microsim/MSLane.h>
39 #include <microsim/MSNet.h>
40 #include "MSLCM_LC2013.h"
41 
42 #ifdef CHECK_MEMORY_LEAKS
43 #include <foreign/nvwa/debug_new.h>
44 #endif // CHECK_MEMORY_LEAKS
45 
46 
47 // ===========================================================================
48 // variable definitions
49 // ===========================================================================
50 // 80km/h will be the threshold for dividing between long/short foresight
51 #define LOOK_FORWARD_SPEED_DIVIDER (SUMOReal)14.
52 
53 #define LOOK_FORWARD_RIGHT (SUMOReal)10.
54 #define LOOK_FORWARD_LEFT (SUMOReal)20.
55 
56 #define JAM_FACTOR (SUMOReal)1.
57 
58 #define LCA_RIGHT_IMPATIENCE (SUMOReal)-1.
59 #define CUT_IN_LEFT_SPEED_THRESHOLD (SUMOReal)27.
60 
61 #define LOOK_AHEAD_MIN_SPEED (SUMOReal)0.0
62 #define LOOK_AHEAD_SPEED_MEMORY (SUMOReal)0.9
63 #define LOOK_AHEAD_SPEED_DECREMENT 6.
64 
65 #define HELP_DECEL_FACTOR (SUMOReal)1.0
66 
67 #define HELP_OVERTAKE (SUMOReal)(10.0 / 3.6)
68 #define MIN_FALLBEHIND (SUMOReal)(7.0 / 3.6)
69 
70 #define RELGAIN_NORMALIZATION_MIN_SPEED (SUMOReal)10.0
71 #define URGENCY (SUMOReal)2.0
72 
73 #define KEEP_RIGHT_TIME (SUMOReal)5.0 // the number of seconds after which a vehicle should move to the right lane
74 #define KEEP_RIGHT_ACCEPTANCE (SUMOReal)2.0 // calibration factor for determining the desire to keep right
75 #define ROUNDABOUT_DIST_BONUS (SUMOReal)80.0
76 
77 
78 // ===========================================================================
79 // debug defines
80 // ===========================================================================
81 //#define DEBUG_PATCH_SPEED
82 //#define DEBUG_INFORMED
83 //#define DEBUG_INFORMER
84 //#define DEBUG_CONSTRUCTOR
85 //#define DEBUG_WANTS_CHANGE
86 //#define DEBUG_SLOW_DOWN
87 //#define DEBUG_SAVE_BLOCKER_LENGTH
88 
89 #define DEBUG_COND (myVehicle.getID() == "disabled")
90 
91 // ===========================================================================
92 // member method definitions
93 // ===========================================================================
96  mySpeedGainProbability(0),
97  myKeepRightProbability(0),
98  myLeadingBlockerLength(0),
99  myLeftSpace(0),
100  myLookAheadSpeed(LOOK_AHEAD_MIN_SPEED),
101  myStrategicParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_STRATEGIC_PARAM, 1)),
102  myCooperativeParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_PARAM, 1)),
103  mySpeedGainParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAIN_PARAM, 1)),
104  myKeepRightParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_KEEPRIGHT_PARAM, 1)),
105  myChangeProbThresholdRight(2.0 * myKeepRightParam / MAX2(NUMERICAL_EPS, mySpeedGainParam)),
106  myChangeProbThresholdLeft(0.2 / MAX2(NUMERICAL_EPS, mySpeedGainParam)) {
107 #ifdef DEBUG_CONSTRUCTOR
108  if (DEBUG_COND) {
109  std::cout << SIMTIME
110  << " create lcModel veh=" << myVehicle.getID()
111  << " lcStrategic=" << myStrategicParam
112  << " lcCooperative=" << myCooperativeParam
113  << " lcSpeedGain=" << mySpeedGainParam
114  << " lcKeepRight=" << myKeepRightParam
115  << "\n";
116  }
117 #endif
118 }
119 
121  changed();
122 }
123 
124 
125 bool
127  return DEBUG_COND;
128 }
129 
130 
131 int
133  int laneOffset,
135  int blocked,
136  const std::pair<MSVehicle*, SUMOReal>& leader,
137  const std::pair<MSVehicle*, SUMOReal>& neighLead,
138  const std::pair<MSVehicle*, SUMOReal>& neighFollow,
139  const MSLane& neighLane,
140  const std::vector<MSVehicle::LaneQ>& preb,
141  MSVehicle** lastBlocked,
142  MSVehicle** firstBlocked) {
143 
144 #ifdef DEBUG_WANTS_CHANGE
145  if (DEBUG_COND) {
146  std::cout << "\n" << STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep())
147  //<< std::setprecision(10)
148  << " veh=" << myVehicle.getID()
149  << " lane=" << myVehicle.getLane()->getID()
150  << " pos=" << myVehicle.getPositionOnLane()
151  << " posLat=" << myVehicle.getLateralPositionOnLane()
152  << " speed=" << myVehicle.getSpeed()
153  << " considerChangeTo=" << (laneOffset == -1 ? "right" : "left")
154  << "\n";
155  }
156 #endif
157 
158  const int result = _wantsChange(laneOffset, msgPass, blocked, leader, neighLead, neighFollow, neighLane, preb, lastBlocked, firstBlocked);
159 
160 #ifdef DEBUG_WANTS_CHANGE
161  if (DEBUG_COND) {
162  if (result & LCA_WANTS_LANECHANGE) {
163  std::cout << STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep())
164  << " veh=" << myVehicle.getID()
165  << " wantsChangeTo=" << (laneOffset == -1 ? "right" : "left")
166  << ((result & LCA_URGENT) ? " (urgent)" : "")
167  << ((result & LCA_CHANGE_TO_HELP) ? " (toHelp)" : "")
168  << ((result & LCA_STRATEGIC) ? " (strat)" : "")
169  << ((result & LCA_COOPERATIVE) ? " (coop)" : "")
170  << ((result & LCA_SPEEDGAIN) ? " (speed)" : "")
171  << ((result & LCA_KEEPRIGHT) ? " (keepright)" : "")
172  << ((result & LCA_TRACI) ? " (traci)" : "")
173  << ((blocked & LCA_BLOCKED) ? " (blocked)" : "")
174  << ((blocked & LCA_OVERLAPPING) ? " (overlap)" : "")
175  << "\n\n\n";
176  }
177  }
178 #endif
179 
180  return result;
181 }
182 
183 
184 SUMOReal
185 MSLCM_LC2013::patchSpeed(const SUMOReal min, const SUMOReal wanted, const SUMOReal max, const MSCFModel& cfModel) {
186  const SUMOReal newSpeed = _patchSpeed(min, wanted, max, cfModel);
187 
188 #ifdef DEBUG_PATCH_SPEED
189  if (DEBUG_COND) {
190  const std::string patched = (wanted != newSpeed ? " patched=" + toString(newSpeed) : "");
191  std::cout << STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep())
192  << " veh=" << myVehicle.getID()
193  << " lane=" << myVehicle.getLane()->getID()
194  << " pos=" << myVehicle.getPositionOnLane()
195  << " v=" << myVehicle.getSpeed()
196  << " wanted=" << wanted
197  << patched
198  << "\n\n";
199  }
200 #endif
201 
202  return newSpeed;
203 }
204 
205 
206 SUMOReal
207 MSLCM_LC2013::_patchSpeed(const SUMOReal min, const SUMOReal wanted, const SUMOReal max, const MSCFModel& cfModel) {
208  int state = myOwnState;
209 #ifdef DEBUG_PATCH_SPEED
210  if (DEBUG_COND) {
211  std::cout << SIMTIME << " patchSpeed state=" << state << " myVSafes=" << toString(myVSafes) << "\n";
212  }
213 #endif
214  // letting vehicles merge in at the end of the lane in case of counter-lane change, step#2
215  SUMOReal MAGIC_offset = 1.;
216  // if we want to change and have a blocking leader and there is enough room for him in front of us
217  if (myLeadingBlockerLength != 0) {
219 #ifdef DEBUG_PATCH_SPEED
220  if (DEBUG_COND) {
221  std::cout << SIMTIME << " veh=" << myVehicle.getID() << " myLeadingBlockerLength=" << myLeadingBlockerLength << " space=" << space << "\n";
222  }
223 #endif
224  if (space > 0) {
225  // compute speed for decelerating towards a place which allows the blocking leader to merge in in front
226  SUMOReal safe = cfModel.stopSpeed(&myVehicle, myVehicle.getSpeed(), space);
227  // if we are approaching this place
228  if (safe < wanted) {
229  // return this speed as the speed to use
230 #ifdef DEBUG_PATCH_SPEED
231  if (DEBUG_COND) {
232  std::cout << time << " veh=" << myVehicle.getID() << " slowing down for leading blocker, safe=" << safe << (safe + NUMERICAL_EPS < min ? " (not enough)" : "") << "\n";
233  }
234 #endif
235  return MAX2(min, safe);
236  }
237  }
238  }
239 
240  SUMOReal nVSafe = wanted;
241  bool gotOne = false;
242  for (std::vector<SUMOReal>::const_iterator i = myVSafes.begin(); i != myVSafes.end(); ++i) {
243  SUMOReal v = (*i);
244  if (v >= min && v <= max) {
245  nVSafe = MIN2(v * myCooperativeParam + (1 - myCooperativeParam) * wanted, nVSafe);
246  gotOne = true;
247 #ifdef DEBUG_PATCH_SPEED
248  if (DEBUG_COND) {
249  std::cout << time << " veh=" << myVehicle.getID() << " got nVSafe=" << nVSafe << "\n";
250  }
251 #endif
252  } else {
253  }
254  }
255 
256  if (gotOne && !myDontBrake) {
257 #ifdef DEBUG_PATCH_SPEED
258  if (DEBUG_COND) {
259  std::cout << time << " veh=" << myVehicle.getID() << " got vSafe\n";
260  }
261 #endif
262  return nVSafe;
263  }
264 
265  // check whether the vehicle is blocked
266  if ((state & LCA_WANTS_LANECHANGE) != 0 && (state & LCA_BLOCKED) != 0) {
267  if ((state & LCA_STRATEGIC) != 0) {
268  // necessary decelerations are controlled via vSafe. If there are
269  // none it means we should speed up
270 #ifdef DEBUG_PATCH_SPEED
271  if (DEBUG_COND) {
272  std::cout << time << " veh=" << myVehicle.getID() << " LCA_WANTS_LANECHANGE (strat, no vSafe)\n";
273  }
274 #endif
275  return (max + wanted) / (SUMOReal) 2.0;
276  } else if ((state & LCA_COOPERATIVE) != 0) {
277  // only minor adjustments in speed should be done
278  if ((state & LCA_BLOCKED_BY_LEADER) != 0) {
279 #ifdef DEBUG_PATCH_SPEED
280  if (DEBUG_COND) {
281  std::cout << time << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_LEADER (coop)\n";
282  }
283 #endif
284  return (min + wanted) / (SUMOReal) 2.0;
285  }
286  if ((state & LCA_BLOCKED_BY_FOLLOWER) != 0) {
287 #ifdef DEBUG_PATCH_SPEED
288  if (DEBUG_COND) {
289  std::cout << time << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_FOLLOWER (coop)\n";
290  }
291 #endif
292  return (max + wanted) / (SUMOReal) 2.0;
293  }
294  }
295  }
296 
297  // accelerate if being a blocking leader or blocking follower not able to brake
298  // (and does not have to change lanes)
299  if ((state & LCA_AMBLOCKINGLEADER) != 0) {
300 #ifdef DEBUG_PATCH_SPEED
301  if (DEBUG_COND) {
302  std::cout << time << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGLEADER\n";
303  }
304 #endif
305  return (max + wanted) / (SUMOReal) 2.0;
306  }
307 
308  if ((state & LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0) {
309 #ifdef DEBUG_PATCH_SPEED
310  if (DEBUG_COND) {
311  std::cout << time << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER_DONTBRAKE\n";
312  }
313 #endif
314  }
315  if (myVehicle.getLane()->getEdge().getLanes().size() == 1) {
316  // remove chaning information if on a road with a single lane
317  changed();
318  }
319  return wanted;
320 }
321 
322 
323 void*
324 MSLCM_LC2013::inform(void* info, MSVehicle* sender) {
325  UNUSED_PARAMETER(sender);
326  Info* pinfo = (Info*)info;
327  myVSafes.push_back(pinfo->first);
328  myOwnState |= pinfo->second;
329 #ifdef DEBUG_INFORMED
330  if (DEBUG_COND) {
331  std::cout << STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep())
332  << " veh=" << myVehicle.getID()
333  << " informedBy=" << sender->getID()
334  << " info=" << pinfo->second
335  << " vSafe=" << pinfo->first
336  << "\n";
337  }
338 #endif
339  delete pinfo;
340  return (void*) true;
341 }
342 
343 
344 SUMOReal
346  int blocked,
347  int dir,
348  const std::pair<MSVehicle*, SUMOReal>& neighLead,
349  SUMOReal remainingSeconds) {
350  SUMOReal plannedSpeed = MIN2(myVehicle.getSpeed(),
352  for (std::vector<SUMOReal>::const_iterator i = myVSafes.begin(); i != myVSafes.end(); ++i) {
353  SUMOReal v = (*i);
355  plannedSpeed = MIN2(plannedSpeed, v);
356  }
357  }
358 #ifdef DEBUG_INFORMER
359  if (DEBUG_COND) {
360  std::cout << " informLeader speed=" << myVehicle.getSpeed() << " planned=" << plannedSpeed << "\n";
361  }
362 #endif
363  if ((blocked & LCA_BLOCKED_BY_LEADER) != 0) {
364  assert(neighLead.first != 0);
365  MSVehicle* nv = neighLead.first;
366 #ifdef DEBUG_INFORMER
367  if (DEBUG_COND) {
368  std::cout << " blocked by leader nv=" << nv->getID() << " nvSpeed=" << nv->getSpeed() << " needGap="
370  }
371 #endif
372  // decide whether we want to overtake the leader or follow it
373  const SUMOReal dv = plannedSpeed - nv->getSpeed();
374  const SUMOReal overtakeDist = (neighLead.second // drive to back of follower
375  + nv->getVehicleType().getLengthWithGap() // drive to front of follower
376  + myVehicle.getVehicleType().getLength() // ego back reaches follower front
377  + nv->getCarFollowModel().getSecureGap( // save gap to follower
379 
380  if (dv < 0
381  // overtaking on the right on an uncongested highway is forbidden (noOvertakeLCLeft)
383  // not enough space to overtake?
384  || myLeftSpace - myLeadingBlockerLength < overtakeDist
385  // not enough time to overtake?
386  || dv * remainingSeconds < overtakeDist) {
387  // cannot overtake
388  msgPass.informNeighLeader(new Info(-1, dir | LCA_AMBLOCKINGLEADER), &myVehicle);
389  // slow down smoothly to follow leader
390  const SUMOReal targetSpeed = myCarFollowModel.followSpeed(
391  &myVehicle, myVehicle.getSpeed(), neighLead.second, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel());
392  if (targetSpeed < myVehicle.getSpeed()) {
393  // slow down smoothly to follow leader
395  MAX2(MIN_FALLBEHIND, (myVehicle.getSpeed() - targetSpeed) / remainingSeconds));
396  const SUMOReal nextSpeed = MIN2(plannedSpeed, myVehicle.getSpeed() - ACCEL2SPEED(decel));
397 #ifdef DEBUG_INFORMER
398  if (DEBUG_COND) {
399  std::cout << STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep())
400  << " cannot overtake leader nv=" << nv->getID()
401  << " dv=" << dv
402  << " remainingSeconds=" << remainingSeconds
403  << " targetSpeed=" << targetSpeed
404  << " nextSpeed=" << nextSpeed
405  << "\n";
406  }
407 #endif
408  myVSafes.push_back(nextSpeed);
409  return nextSpeed;
410  } else {
411  // leader is fast enough anyway
412 #ifdef DEBUG_INFORMER
413  if (DEBUG_COND) {
414  std::cout << STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep())
415  << " cannot overtake fast leader nv=" << nv->getID()
416  << " dv=" << dv
417  << " remainingSeconds=" << remainingSeconds
418  << " targetSpeed=" << targetSpeed
419  << "\n";
420  }
421 #endif
422  myVSafes.push_back(targetSpeed);
423  return plannedSpeed;
424  }
425  } else {
426  // overtaking, leader should not accelerate
427 #ifdef DEBUG_INFORMER
428  if (DEBUG_COND) {
429  std::cout << STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep())
430  << " wants to overtake leader nv=" << nv->getID()
431  << " dv=" << dv
432  << " remainingSeconds=" << remainingSeconds
433  << " currentGap=" << neighLead.second
435  << " overtakeDist=" << overtakeDist
436  << "\n";
437  }
438 #endif
439  msgPass.informNeighLeader(new Info(nv->getSpeed(), dir | LCA_AMBLOCKINGLEADER), &myVehicle);
440  return -1;
441  }
442  } else if (neighLead.first != 0) { // (remainUnblocked)
443  // we are not blocked now. make sure we stay far enough from the leader
444  MSVehicle* nv = neighLead.first;
445  const SUMOReal nextNVSpeed = nv->getSpeed() - HELP_OVERTAKE; // conservative
446  const SUMOReal dv = SPEED2DIST(myVehicle.getSpeed() - nextNVSpeed);
447  const SUMOReal targetSpeed = myCarFollowModel.followSpeed(
448  &myVehicle, myVehicle.getSpeed(), neighLead.second - dv, nextNVSpeed, nv->getCarFollowModel().getMaxDecel());
449  myVSafes.push_back(targetSpeed);
450 #ifdef DEBUG_INFORMER
451  if (DEBUG_COND) {
452  std::cout << " not blocked by leader nv=" << nv->getID()
453  << " nvSpeed=" << nv->getSpeed()
454  << " gap=" << neighLead.second
455  << " nextGap=" << neighLead.second - dv
457  << " targetSpeed=" << targetSpeed
458  << "\n";
459  }
460 #endif
461  return MIN2(targetSpeed, plannedSpeed);
462  } else {
463  // not overtaking
464  return plannedSpeed;
465  }
466 }
467 
468 
469 void
471  int blocked,
472  int dir,
473  const std::pair<MSVehicle*, SUMOReal>& neighFollow,
474  SUMOReal remainingSeconds,
475  SUMOReal plannedSpeed) {
476  if ((blocked & LCA_BLOCKED_BY_FOLLOWER) != 0) {
477  assert(neighFollow.first != 0);
478  MSVehicle* nv = neighFollow.first;
479 #ifdef DEBUG_INFORMER
480  if (DEBUG_COND) {
481  std::cout << " blocked by follower nv=" << nv->getID() << " nvSpeed=" << nv->getSpeed() << " needGap="
482  << nv->getCarFollowModel().getSecureGap(nv->getSpeed(), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel()) << " planned=" << plannedSpeed << "\n";
483  }
484 #endif
485 
486  // are we fast enough to cut in without any help?
487  if (plannedSpeed - nv->getSpeed() >= HELP_OVERTAKE) {
488  const SUMOReal neededGap = nv->getCarFollowModel().getSecureGap(nv->getSpeed(), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel());
489  if ((neededGap - neighFollow.second) / remainingSeconds < (plannedSpeed - nv->getSpeed())) {
490 #ifdef DEBUG_INFORMER
491  if (DEBUG_COND) {
492  std::cout << " wants to cut in before nv=" << nv->getID() << " without any help." << "\nneededGap = " << neededGap << "\n";
493  }
494 #endif
495  // follower might even accelerate but not to much
496  msgPass.informNeighFollower(new Info(plannedSpeed - HELP_OVERTAKE, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
497  return;
498  }
499  }
500  // decide whether we will request help to cut in before the follower or allow to be overtaken
501 
502  // PARAMETERS
503  // assume other vehicle will assume the equivalent of 1 second of
504  // maximum deceleration to help us (will probably be spread over
505  // multiple seconds)
506  // -----------
507  const SUMOReal helpDecel = nv->getCarFollowModel().getMaxDecel() * HELP_DECEL_FACTOR ;
508 
509  // change in the gap between ego and blocker over 1 second (not STEP!)
510  const SUMOReal neighNewSpeed = MAX2((SUMOReal)0, nv->getSpeed() - ACCEL2SPEED(helpDecel));
511  const SUMOReal neighNewSpeed1s = MAX2((SUMOReal)0, nv->getSpeed() - helpDecel);
512  const SUMOReal dv = plannedSpeed - neighNewSpeed1s;
513  // new gap between follower and self in case the follower does brake for 1s
514  const SUMOReal decelGap = neighFollow.second + dv;
515  const SUMOReal secureGap = nv->getCarFollowModel().getSecureGap(neighNewSpeed1s, plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel());
516 #ifdef DEBUG_INFORMER
517  if (DEBUG_COND) {
518  std::cout << STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep())
519  << " egoV=" << myVehicle.getSpeed()
520  << " egoNV=" << plannedSpeed
521  << " nvNewSpeed=" << neighNewSpeed
522  << " nvNewSpeed1s=" << neighNewSpeed1s
523  << " deltaGap=" << dv
524  << " decelGap=" << decelGap
525  << " secGap=" << secureGap
526  << "\n";
527  }
528 #endif
529  if (decelGap > 0 && decelGap >= secureGap) {
530  // if the blocking neighbor brakes it could actually help
531  // how hard does it actually need to be?
532  // to be safe in the next step the following equation has to hold:
533  // vsafe <= followSpeed(gap=currentGap - SPEED2DIST(vsafe), ...)
534  // we compute an upper bound on vsafe by doing the computation twice
535  const SUMOReal vsafe1 = MAX2(neighNewSpeed, nv->getCarFollowModel().followSpeed(
536  nv, nv->getSpeed(), neighFollow.second + SPEED2DIST(plannedSpeed), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel()));
537  const SUMOReal vsafe = MAX2(neighNewSpeed, nv->getCarFollowModel().followSpeed(
538  nv, nv->getSpeed(), neighFollow.second + SPEED2DIST(plannedSpeed - vsafe1), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel()));
539  // the following assertion cannot be guaranteed because the CFModel handles small gaps differently, see MSCFModel::maximumSafeStopSpeed
540  // assert(vsafe <= vsafe1);
541  msgPass.informNeighFollower(new Info(vsafe, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
542 #ifdef DEBUG_INFORMER
543  if (DEBUG_COND) {
544  std::cout << " wants to cut in before nv=" << nv->getID()
545  << " vsafe1=" << vsafe1
546  << " vsafe=" << vsafe
547  << " newSecGap=" << nv->getCarFollowModel().getSecureGap(vsafe, plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel())
548  << "\n";
549  }
550 #endif
551  } else if (dv > 0 && dv * remainingSeconds > (secureGap - decelGap + POSITION_EPS)) {
552  // decelerating once is sufficient to open up a large enough gap in time
553  msgPass.informNeighFollower(new Info(neighNewSpeed, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
554 #ifdef DEBUG_INFORMER
555  if (DEBUG_COND) {
556  std::cout << " wants to cut in before nv=" << nv->getID() << " (eventually)\n";
557  }
558 #endif
559  } else {
561  //if (dir == LCA_MRIGHT && myVehicle.getWaitingSeconds() > LCA_RIGHT_IMPATIENCE &&
562  // nv->getSpeed() > myVehicle.getSpeed()) {
563  if (nv->getSpeed() > myVehicle.getSpeed() &&
565  || (dir == LCA_MLEFT && plannedSpeed > CUT_IN_LEFT_SPEED_THRESHOLD) // VARIANT_22 (slowDownLeft)
566  )) {
567  // let the follower slow down to increase the likelyhood that later vehicles will be slow enough to help
568  // follower should still be fast enough to open a gap
569  vhelp = MAX2(neighNewSpeed, myVehicle.getSpeed() + HELP_OVERTAKE);
570 #ifdef DEBUG_INFORMER
571  if (DEBUG_COND) {
572  std::cout << " wants right follower to slow down a bit\n";
573  }
574 #endif
575  if ((nv->getSpeed() - myVehicle.getSpeed()) / helpDecel < remainingSeconds) {
576 #ifdef DEBUG_INFORMER
577  if (DEBUG_COND) {
578  std::cout << " wants to cut in before right follower nv=" << nv->getID() << " (eventually)\n";
579  }
580 #endif
581  msgPass.informNeighFollower(new Info(vhelp, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
582  return;
583  }
584  }
585  msgPass.informNeighFollower(new Info(vhelp, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
586  // this follower is supposed to overtake us. slow down smoothly to allow this
587  const SUMOReal overtakeDist = (neighFollow.second // follower reaches ego back
588  + myVehicle.getVehicleType().getLengthWithGap() // follower reaches ego front
589  + nv->getVehicleType().getLength() // follower back at ego front
590  + myVehicle.getCarFollowModel().getSecureGap( // follower has safe dist to ego
591  plannedSpeed, vhelp, nv->getCarFollowModel().getMaxDecel()));
592  // speed difference to create a sufficiently large gap
593  const SUMOReal needDV = overtakeDist / remainingSeconds;
594  // make sure the deceleration is not to strong
595  myVSafes.push_back(MAX2(vhelp - needDV, myVehicle.getSpeed() - ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxDecel())));
596 #ifdef DEBUG_INFORMER
597  if (DEBUG_COND) {
598  std::cout << STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep())
599  << " veh=" << myVehicle.getID()
600  << " wants to be overtaken by=" << nv->getID()
601  << " overtakeDist=" << overtakeDist
602  << " vneigh=" << nv->getSpeed()
603  << " vhelp=" << vhelp
604  << " needDV=" << needDV
605  << " vsafe=" << myVSafes.back()
606  << "\n";
607  }
608 #endif
609  }
610  }
611 }
612 
613 
614 void
616  // keep information about strategic change direction
619  myLeftSpace = 0;
620  myVSafes.clear();
621  myDontBrake = false;
622  // truncate to work around numerical instability between different builds
623  mySpeedGainProbability = ceil(mySpeedGainProbability * 100000.0) * 0.00001;
624  myKeepRightProbability = ceil(myKeepRightProbability * 100000.0) * 0.00001;
625 }
626 
627 
628 void
630  myOwnState = 0;
633  if (myVehicle.getBestLaneOffset() == 0) {
634  // if we are not yet on our best lane there might still be unseen blockers
635  // (during patchSpeed)
637  myLeftSpace = 0;
638  }
640  myVSafes.clear();
641  myDontBrake = false;
642 }
643 
644 
645 int
647  int laneOffset,
649  int blocked,
650  const std::pair<MSVehicle*, SUMOReal>& leader,
651  const std::pair<MSVehicle*, SUMOReal>& neighLead,
652  const std::pair<MSVehicle*, SUMOReal>& neighFollow,
653  const MSLane& neighLane,
654  const std::vector<MSVehicle::LaneQ>& preb,
655  MSVehicle** lastBlocked,
656  MSVehicle** firstBlocked) {
657  assert(laneOffset == 1 || laneOffset == -1);
658  const SUMOTime currentTime = MSNet::getInstance()->getCurrentTimeStep();
659  // compute bestLaneOffset
660  MSVehicle::LaneQ curr, neigh, best;
661  int bestLaneOffset = 0;
662  SUMOReal currentDist = 0;
663  SUMOReal neighDist = 0;
664  int currIdx = 0;
665  MSLane* prebLane = myVehicle.getLane();
666  if (prebLane->getEdge().getPurpose() == MSEdge::EDGEFUNCTION_INTERNAL) {
667  // internal edges are not kept inside the bestLanes structure
668  prebLane = prebLane->getLinkCont()[0]->getLane();
669  }
670  const bool checkOpposite = &neighLane.getEdge() != &myVehicle.getLane()->getEdge();
671  const int prebOffset = (checkOpposite ? 0 : laneOffset);
672  for (int p = 0; p < (int) preb.size(); ++p) {
673  if (preb[p].lane == prebLane && p + laneOffset >= 0) {
674  assert(p + prebOffset < (int)preb.size());
675  curr = preb[p];
676  neigh = preb[p + prebOffset];
677  currentDist = curr.length;
678  neighDist = neigh.length;
679  bestLaneOffset = curr.bestLaneOffset;
680  if (bestLaneOffset == 0 && preb[p + prebOffset].bestLaneOffset == 0) {
681 #ifdef DEBUG_WANTS_CHANGE
682  if (DEBUG_COND) {
683  std::cout << STEPS2TIME(currentTime)
684  << " veh=" << myVehicle.getID()
685  << " bestLaneOffsetOld=" << bestLaneOffset
686  << " bestLaneOffsetNew=" << laneOffset
687  << "\n";
688  }
689 #endif
690  bestLaneOffset = prebOffset;
691  }
692  best = preb[p + bestLaneOffset];
693  currIdx = p;
694  break;
695  }
696  }
697  // direction specific constants
698  const bool right = (laneOffset == -1);
699  if (isOpposite() && right) {
700  neigh = preb[preb.size() - 1];
701  curr = neigh;
702  best = neigh;
703  bestLaneOffset = -1;
704  curr.bestLaneOffset = -1;
705  neighDist = neigh.length;
706  currentDist = curr.length;
707  }
709  const int lca = (right ? LCA_RIGHT : LCA_LEFT);
710  const int myLca = (right ? LCA_MRIGHT : LCA_MLEFT);
711  const int lcaCounter = (right ? LCA_LEFT : LCA_RIGHT);
712  const bool changeToBest = (right && bestLaneOffset < 0) || (!right && bestLaneOffset > 0);
713  // keep information about being a leader/follower
714  int ret = (myOwnState & 0xffff0000);
715  int req = 0; // the request to change or stay
716 
717  ret = slowDownForBlocked(lastBlocked, ret);
718  if (lastBlocked != firstBlocked) {
719  ret = slowDownForBlocked(firstBlocked, ret);
720  }
721 
722 #ifdef DEBUG_WANTS_CHANGE
723  if (DEBUG_COND) {
724  std::cout << SIMTIME
725  << " veh=" << myVehicle.getID()
726  << " _wantsChange state=" << myOwnState
727  << " myVSafes=" << toString(myVSafes)
728  << " firstBlocked=" << Named::getIDSecure(*firstBlocked)
729  << " lastBlocked=" << Named::getIDSecure(*lastBlocked)
730  << " leader=" << Named::getIDSecure(leader.first)
731  << " leaderGap=" << leader.second
732  << " neighLead=" << Named::getIDSecure(neighLead.first)
733  << " neighLeadGap=" << neighLead.second
734  << " neighFollow=" << Named::getIDSecure(neighFollow.first)
735  << " neighFollowGap=" << neighFollow.second
736  << "\n";
737  }
738 #endif
739 
740  // we try to estimate the distance which is necessary to get on a lane
741  // we have to get on in order to keep our route
742  // we assume we need something that depends on our velocity
743  // and compare this with the free space on our wished lane
744  //
745  // if the free space is somehow less than the space we need, we should
746  // definitely try to get to the desired lane
747  //
748  // this rule forces our vehicle to change the lane if a lane changing is necessary soon
749  // lookAheadDistance:
750  // we do not want the lookahead distance to change all the time so we discrectize the speed a bit
751 
754  } else {
755  // memory decay factor for this time step
756  const SUMOReal memoryFactor = 1. - (1. - LOOK_AHEAD_SPEED_MEMORY) * TS;
757  assert(memoryFactor > 0.);
759  (memoryFactor * myLookAheadSpeed + (1 - memoryFactor) * myVehicle.getSpeed()));
760  }
762  laDist += myVehicle.getVehicleType().getLengthWithGap() * (SUMOReal) 2.;
763 
764  // react to a stopped leader on the current lane
765  if (bestLaneOffset == 0 && leader.first != 0 && leader.first->isStopped()) {
766  // value is doubled for the check since we change back and forth
767  laDist = 0.5 * (myVehicle.getVehicleType().getLengthWithGap()
768  + leader.first->getVehicleType().getLengthWithGap());
769  }
770 
771  // free space that is available for changing
772  //const SUMOReal neighSpeed = (neighLead.first != 0 ? neighLead.first->getSpeed() :
773  // neighFollow.first != 0 ? neighFollow.first->getSpeed() :
774  // best.lane->getSpeedLimit());
775  // @note: while this lets vehicles change earlier into the correct direction
776  // it also makes the vehicles more "selfish" and prevents changes which are necessary to help others
777 
778  int roundaboutEdgesAhead = 0;
779  for (std::vector<MSLane*>::iterator it = curr.bestContinuations.begin(); it != curr.bestContinuations.end(); ++it) {
780  if ((*it) != 0 && (*it)->getEdge().isRoundabout()) {
781  roundaboutEdgesAhead += 1;
782  } else if (roundaboutEdgesAhead > 0) {
783  // only check the next roundabout
784  break;
785  }
786  }
787  int roundaboutEdgesAheadNeigh = 0;
788  for (std::vector<MSLane*>::iterator it = neigh.bestContinuations.begin(); it != neigh.bestContinuations.end(); ++it) {
789  if ((*it) != 0 && (*it)->getEdge().isRoundabout()) {
790  roundaboutEdgesAheadNeigh += 1;
791  } else if (roundaboutEdgesAheadNeigh > 0) {
792  // only check the next roundabout
793  break;
794  }
795  }
796  if (roundaboutEdgesAhead > 1) {
797  currentDist += roundaboutEdgesAhead * ROUNDABOUT_DIST_BONUS * myCooperativeParam;
798  neighDist += roundaboutEdgesAheadNeigh * ROUNDABOUT_DIST_BONUS * myCooperativeParam;
799  }
800 #ifdef DEBUG_WANTS_CHANGE
801  if (DEBUG_COND) {
802  if (roundaboutEdgesAhead > 0) {
803  std::cout << " roundaboutEdgesAhead=" << roundaboutEdgesAhead << " roundaboutEdgesAheadNeigh=" << roundaboutEdgesAheadNeigh << "\n";
804  }
805  }
806 #endif
807 
808  const SUMOReal usableDist = (currentDist - posOnLane - best.occupation * JAM_FACTOR);
809  const SUMOReal maxJam = MAX2(preb[currIdx + prebOffset].occupation, preb[currIdx].occupation);
810  const SUMOReal neighLeftPlace = MAX2((SUMOReal) 0, neighDist - posOnLane - maxJam);
811 
812 #ifdef DEBUG_WANTS_CHANGE
813  if (DEBUG_COND) {
814  std::cout << STEPS2TIME(currentTime)
815  << " veh=" << myVehicle.getID()
816  << " laSpeed=" << myLookAheadSpeed
817  << " laDist=" << laDist
818  << " currentDist=" << currentDist
819  << " usableDist=" << usableDist
820  << " bestLaneOffset=" << bestLaneOffset
821  << " best.occupation=" << best.occupation
822  << " best.length=" << best.length
823  << " maxJam=" << maxJam
824  << " neighLeftPlace=" << neighLeftPlace
825  << "\n";
826  }
827 #endif
828 
829  if (changeToBest && bestLaneOffset == curr.bestLaneOffset
830  && currentDistDisallows(usableDist, bestLaneOffset, laDist)) {
832  ret = ret | lca | LCA_STRATEGIC | LCA_URGENT;
833  } else {
834 
835  if (!myAllowOvertakingRight && !right && !myVehicle.congested() && neighLead.first != 0) {
836  // check for slower leader on the left. we should not overtake but
837  // rather move left ourselves (unless congested)
838  MSVehicle* nv = neighLead.first;
839  if (nv->getSpeed() < myVehicle.getSpeed()) {
840  const SUMOReal vSafe = myCarFollowModel.followSpeed(
841  &myVehicle, myVehicle.getSpeed(), neighLead.second, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel());
842  myVSafes.push_back(vSafe);
843  if (vSafe < myVehicle.getSpeed()) {
845  }
846 #ifdef DEBUG_WANTS_CHANGE
847  if (DEBUG_COND) {
848  std::cout << STEPS2TIME(currentTime)
849  << " avoid overtaking on the right nv=" << nv->getID()
850  << " nvSpeed=" << nv->getSpeed()
851  << " mySpeedGainProbability=" << mySpeedGainProbability
852  << " plannedSpeed=" << myVSafes.back()
853  << "\n";
854  }
855 #endif
856  }
857  }
858 
859  if (!changeToBest && (currentDistDisallows(neighLeftPlace, abs(bestLaneOffset) + 2, laDist))) {
860  // the opposite lane-changing direction should be done than the one examined herein
861  // we'll check whether we assume we could change anyhow and get back in time...
862  //
863  // this rule prevents the vehicle from moving in opposite direction of the best lane
864  // unless the way till the end where the vehicle has to be on the best lane
865  // is long enough
866 #ifdef DEBUG_WANTS_CHANGE
867  if (DEBUG_COND) {
868  std::cout << " veh=" << myVehicle.getID() << " could not change back and forth in time (1) neighLeftPlace=" << neighLeftPlace << "\n";
869  }
870 #endif
871  ret = ret | LCA_STAY | LCA_STRATEGIC;
872  } else if (bestLaneOffset == 0 && (neighLeftPlace * 2. < laDist)) {
873  // the current lane is the best and a lane-changing would cause a situation
874  // of which we assume we will not be able to return to the lane we have to be on.
875  // this rule prevents the vehicle from leaving the current, best lane when it is
876  // close to this lane's end
877 #ifdef DEBUG_WANTS_CHANGE
878  if (DEBUG_COND) {
879  std::cout << " veh=" << myVehicle.getID() << " could not change back and forth in time (2) neighLeftPlace=" << neighLeftPlace << "\n";
880  }
881 #endif
882  ret = ret | LCA_STAY | LCA_STRATEGIC;
883  }
884  }
885  // check for overriding TraCI requests
886 #ifdef DEBUG_WANTS_CHANGE
887  if (DEBUG_COND) {
888  std::cout << STEPS2TIME(currentTime) << " veh=" << myVehicle.getID() << " ret=" << ret;
889  }
890 #endif
892  if ((ret & lcaCounter) != 0) {
893  // we are not interested in traci requests for the opposite direction here
894  ret &= ~(LCA_TRACI | lcaCounter | LCA_URGENT);
895  }
896 #ifdef DEBUG_WANTS_CHANGE
897  if (DEBUG_COND) {
898  std::cout << " retAfterInfluence=" << ret << "\n";
899  }
900 #endif
901 
902  if ((ret & LCA_STAY) != 0) {
903  return ret;
904  }
905  if ((ret & LCA_URGENT) != 0) {
906  // prepare urgent lane change maneuver
907  // save the left space
908  myLeftSpace = currentDist - posOnLane;
909  if (changeToBest && abs(bestLaneOffset) > 1) {
910  // there might be a vehicle which needs to counter-lane-change one lane further and we cannot see it yet
911 #ifdef DEBUG_WANTS_CHANGE
912  if (DEBUG_COND) {
913  std::cout << " reserving space for unseen blockers\n";
914  }
915 #endif
916  myLeadingBlockerLength = MAX2((SUMOReal)(right ? 20.0 : 40.0), myLeadingBlockerLength);
917  }
918 
919  // letting vehicles merge in at the end of the lane in case of counter-lane change, step#1
920  // if there is a leader and he wants to change to the opposite direction
921  saveBlockerLength(neighLead.first, lcaCounter);
922  if (*firstBlocked != neighLead.first) {
923  saveBlockerLength(*firstBlocked, lcaCounter);
924  }
925 
926  const SUMOReal remainingSeconds = ((ret & LCA_TRACI) == 0 ?
929  const SUMOReal plannedSpeed = informLeader(msgPass, blocked, myLca, neighLead, remainingSeconds);
930  if (plannedSpeed >= 0) {
931  // maybe we need to deal with a blocking follower
932  informFollower(msgPass, blocked, myLca, neighFollow, remainingSeconds, plannedSpeed);
933  }
934 #ifdef DEBUG_WANTS_CHANGE
935  if (DEBUG_COND) {
936  std::cout << STEPS2TIME(currentTime)
937  << " veh=" << myVehicle.getID()
938  << " myLeftSpace=" << myLeftSpace
939  << " remainingSeconds=" << remainingSeconds
940  << " plannedSpeed=" << plannedSpeed
941  << "\n";
942  }
943 #endif
944 
945  return ret;
946  }
947 
948  if (roundaboutEdgesAhead > 1) {
949  // try to use the inner lanes of a roundabout to increase throughput
950  // unless we are approaching the exit
951  if (lca == LCA_LEFT) {
952  req = ret | lca | LCA_COOPERATIVE;
953  } else {
954  req = ret | LCA_STAY | LCA_COOPERATIVE;
955  }
956  if (!cancelRequest(req)) {
957  return ret | req;
958  }
959  }
960 
961  // let's also regard the case where the vehicle is driving on a highway...
962  // in this case, we do not want to get to the dead-end of an on-ramp
963  if (right) {
964  if (bestLaneOffset == 0 && myVehicle.getLane()->getSpeedLimit() > 80. / 3.6 && myLookAheadSpeed > SUMO_const_haltingSpeed) {
965 #ifdef DEBUG_WANTS_CHANGE
966  if (DEBUG_COND) {
967  std::cout << " veh=" << myVehicle.getID() << " does not want to get stranded on the on-ramp of a highway\n";
968  }
969 #endif
970  req = ret | LCA_STAY | LCA_STRATEGIC;
971  if (!cancelRequest(req)) {
972  return ret | req;
973  }
974  }
975  }
976  // --------
977 
978  // -------- make place on current lane if blocking follower
979  //if (amBlockingFollowerPlusNB()) {
980  // std::cout << myVehicle.getID() << ", " << currentDistAllows(neighDist, bestLaneOffset, laDist)
981  // << " neighDist=" << neighDist
982  // << " currentDist=" << currentDist
983  // << "\n";
984  //}
985 
986  const SUMOReal inconvenience = MIN2((SUMOReal)1.0, (laneOffset < 0
990  && (inconvenience <= myCooperativeParam)
991  //&& ((myOwnState & lcaCounter) == 0) // VARIANT_6 : counterNoHelp
992  && (changeToBest || currentDistAllows(neighDist, abs(bestLaneOffset) + 1, laDist))) {
993 #ifdef DEBUG_WANTS_CHANGE
994  if (DEBUG_COND) {
995  std::cout << STEPS2TIME(currentTime)
996  << " veh=" << myVehicle.getID()
997  << " wantsChangeToHelp=" << (right ? "right" : "left")
998  << " state=" << myOwnState
999  // << (((myOwnState & lcaCounter) != 0) ? " (counter)" : "")
1000  << "\n";
1001  }
1002 #endif
1003  req = ret | lca | LCA_COOPERATIVE | LCA_URGENT ;//| LCA_CHANGE_TO_HELP;
1004  if (!cancelRequest(req)) {
1005  return ret | req;
1006  }
1007  }
1008 
1009  // --------
1010 
1011 
1014  //if ((blocked & LCA_BLOCKED) != 0) {
1015  // return ret;
1016  //}
1018 
1019  // -------- higher speed
1020  //if ((congested(neighLead.first) && neighLead.second < 20) || predInteraction(leader.first)) { //!!!
1021  // return ret;
1022  //}
1023  SUMOReal thisLaneVSafe = myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle);
1024  SUMOReal neighLaneVSafe = neighLane.getVehicleMaxSpeed(&myVehicle);
1025  if (neighLead.first == 0) {
1026  neighLaneVSafe = MIN2(neighLaneVSafe, myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), neighDist, 0, 0));
1027  } else {
1028  // @todo: what if leader is below safe gap?!!!
1029  neighLaneVSafe = MIN2(neighLaneVSafe, myCarFollowModel.followSpeed(
1030  &myVehicle, myVehicle.getSpeed(), neighLead.second, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()));
1031  }
1032  if (leader.first == 0) {
1033  thisLaneVSafe = MIN2(thisLaneVSafe, myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), currentDist, 0, 0));
1034  } else {
1035  // @todo: what if leader is below safe gap?!!!
1036  thisLaneVSafe = MIN2(thisLaneVSafe, myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), leader.second, leader.first->getSpeed(), leader.first->getCarFollowModel().getMaxDecel()));
1037  }
1038 #ifdef DEBUG_WANTS_CHANGE
1039  if (DEBUG_COND) {
1040  std::cout << STEPS2TIME(currentTime)
1041  << " veh=" << myVehicle.getID()
1042  << " currentDist=" << currentDist
1043  << " neighDist=" << neighDist
1044  << "\n";
1045  }
1046 #endif
1047 
1049  thisLaneVSafe = MIN2(thisLaneVSafe, vMax);
1050  neighLaneVSafe = MIN2(neighLaneVSafe, vMax);
1051  const SUMOReal relativeGain = (neighLaneVSafe - thisLaneVSafe) / MAX2(neighLaneVSafe,
1053 
1054  if (right) {
1055  // ONLY FOR CHANGING TO THE RIGHT
1056  if (thisLaneVSafe - 5 / 3.6 > neighLaneVSafe) {
1057  // ok, the current lane is faster than the right one...
1058  if (mySpeedGainProbability < 0) {
1059  mySpeedGainProbability *= pow(0.5, TS);
1060  //myKeepRightProbability /= 2.0;
1061  }
1062  } else {
1063  // ok, the current lane is not (much) faster than the right one
1064  // @todo recheck the 5 km/h discount on thisLaneVSafe
1065 
1066  // do not promote changing to the left just because changing to the
1067  // right is bad
1068  if (mySpeedGainProbability < 0 || relativeGain > 0) {
1069  mySpeedGainProbability -= TS * relativeGain;
1070  }
1071 
1072  // honor the obligation to keep right (Rechtsfahrgebot)
1073  // XXX consider fast approaching followers on the current lane
1074  //const SUMOReal vMax = myLookAheadSpeed;
1075  const SUMOReal acceptanceTime = KEEP_RIGHT_ACCEPTANCE * vMax * MAX2((SUMOReal)1, myVehicle.getSpeed()) / myVehicle.getLane()->getSpeedLimit();
1076  SUMOReal fullSpeedGap = MAX2((SUMOReal)0, neighDist - myVehicle.getCarFollowModel().brakeGap(vMax));
1077  SUMOReal fullSpeedDrivingSeconds = MIN2(acceptanceTime, fullSpeedGap / vMax);
1078  if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
1079  fullSpeedGap = MAX2((SUMOReal)0, MIN2(fullSpeedGap,
1080  neighLead.second - myVehicle.getCarFollowModel().getSecureGap(
1081  vMax, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
1082  fullSpeedDrivingSeconds = MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
1083  }
1084  const SUMOReal deltaProb = (myChangeProbThresholdRight
1085  * STEPS2TIME(DELTA_T)
1086  * (fullSpeedDrivingSeconds / acceptanceTime) / KEEP_RIGHT_TIME);
1087  myKeepRightProbability -= TS * deltaProb;
1088 
1089 #ifdef DEBUG_WANTS_CHANGE
1090  if (DEBUG_COND) {
1091  std::cout << STEPS2TIME(currentTime)
1092  << " veh=" << myVehicle.getID()
1093  << " vMax=" << vMax
1094  << " neighDist=" << neighDist
1095  << " brakeGap=" << myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed())
1096  << " leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
1097  << " secGap=" << (neighLead.first == 0 ? -1 : myVehicle.getCarFollowModel().getSecureGap(
1098  myVehicle.getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
1099  << " acceptanceTime=" << acceptanceTime
1100  << " fullSpeedGap=" << fullSpeedGap
1101  << " fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
1102  << " dProb=" << deltaProb
1103  << " myKeepRightProbability=" << myKeepRightProbability
1104  << "\n";
1105  }
1106 #endif
1108  req = ret | lca | LCA_KEEPRIGHT;
1109  if (!cancelRequest(req)) {
1110  return ret | req;
1111  }
1112  }
1113  }
1114 
1115 #ifdef DEBUG_WANTS_CHANGE
1116  if (DEBUG_COND) {
1117  std::cout << STEPS2TIME(currentTime)
1118  << " veh=" << myVehicle.getID()
1119  << " speed=" << myVehicle.getSpeed()
1120  << " mySpeedGainProbability=" << mySpeedGainProbability
1121  << " thisLaneVSafe=" << thisLaneVSafe
1122  << " neighLaneVSafe=" << neighLaneVSafe
1123  << " relativeGain=" << relativeGain
1124  << " blocked=" << blocked
1125  << "\n";
1126  }
1127 #endif
1128 
1130  && neighDist / MAX2((SUMOReal) .1, myVehicle.getSpeed()) > 20.) { //./MAX2((SUMOReal) .1, myVehicle.getSpeed())) { // -.1
1131  req = ret | lca | LCA_SPEEDGAIN;
1132  if (!cancelRequest(req)) {
1133  return ret | req;
1134  }
1135  }
1136  } else {
1137  // ONLY FOR CHANGING TO THE LEFT
1138  if (thisLaneVSafe > neighLaneVSafe) {
1139  // this lane is better
1140  if (mySpeedGainProbability > 0) {
1141  mySpeedGainProbability *= pow(0.5, TS);
1142  }
1143  } else {
1144  // left lane is better
1145  mySpeedGainProbability += TS * relativeGain;
1146  }
1147 
1148 #ifdef DEBUG_WANTS_CHANGE
1149  if (DEBUG_COND) {
1150  std::cout << STEPS2TIME(currentTime)
1151  << " veh=" << myVehicle.getID()
1152  << " speed=" << myVehicle.getSpeed()
1153  << " mySpeedGainProbability=" << mySpeedGainProbability
1154  << " thisLaneVSafe=" << thisLaneVSafe
1155  << " neighLaneVSafe=" << neighLaneVSafe
1156  << " relativeGain=" << relativeGain
1157  << " blocked=" << blocked
1158  << "\n";
1159  }
1160 #endif
1161 
1162  if (mySpeedGainProbability > myChangeProbThresholdLeft && neighDist / MAX2((SUMOReal) .1, myVehicle.getSpeed()) > 20.) { // .1
1163  req = ret | lca | LCA_SPEEDGAIN;
1164  if (!cancelRequest(req)) {
1165  return ret | req;
1166  }
1167  }
1168  }
1169  // --------
1170  if (changeToBest && bestLaneOffset == curr.bestLaneOffset
1171  && (right ? mySpeedGainProbability < 0 : mySpeedGainProbability > 0)) {
1172  // change towards the correct lane, speedwise it does not hurt
1173  req = ret | lca | LCA_STRATEGIC;
1174  if (!cancelRequest(req)) {
1175  return ret | req;
1176  }
1177  }
1178 #ifdef DEBUG_WANTS_CHANGE
1179  if (DEBUG_COND) {
1180  std::cout << STEPS2TIME(currentTime)
1181  << " veh=" << myVehicle.getID()
1182  << " mySpeedGainProbability=" << mySpeedGainProbability
1183  << " myKeepRightProbability=" << myKeepRightProbability
1184  << " thisLaneVSafe=" << thisLaneVSafe
1185  << " neighLaneVSafe=" << neighLaneVSafe
1186  << "\n";
1187  }
1188 #endif
1189 
1190  return ret;
1191 }
1192 
1193 
1194 int
1196  // if this vehicle is blocking someone in front, we maybe decelerate to let him in
1197  if ((*blocked) != 0) {
1198  // XXX: don't we need to take care of possibly different lane-geometries, here? (Leo)
1199  SUMOReal gap = (*blocked)->getPositionOnLane() - (*blocked)->getVehicleType().getLength() - myVehicle.getPositionOnLane() - myVehicle.getVehicleType().getMinGap();
1200 #ifdef DEBUG_SLOW_DOWN
1201  if (DEBUG_COND) {
1202  std::cout << STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep())
1203  << " veh=" << myVehicle.getID()
1204  << " blocked=" << Named::getIDSecure(*blocked)
1205  << " gap=" << gap
1206  << "\n";
1207  }
1208 #endif
1209  if (gap > POSITION_EPS) {
1211  if ((*blocked)->getSpeed() < SUMO_const_haltingSpeed) {
1212  state |= LCA_AMBACKBLOCKER_STANDING;
1213  } else {
1214  state |= LCA_AMBACKBLOCKER;
1215  }
1218  (SUMOReal)(gap - POSITION_EPS), (*blocked)->getSpeed(),
1219  (*blocked)->getCarFollowModel().getMaxDecel()));
1220  }
1221  }
1222  }
1223  return state;
1224 }
1225 
1226 
1227 void
1228 MSLCM_LC2013::saveBlockerLength(MSVehicle* blocker, int lcaCounter) {
1229 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
1230  if (DEBUG_COND) {
1231  std::cout << STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep())
1232  << " veh=" << myVehicle.getID()
1233  << " saveBlockerLength blocker=" << Named::getIDSecure(blocker)
1234  << " bState=" << (blocker == 0 ? "None" : toString(blocker->getLaneChangeModel().getOwnState()))
1235  << "\n";
1236  }
1237 #endif
1238  if (blocker != 0 && (blocker->getLaneChangeModel().getOwnState() & lcaCounter) != 0) {
1239  // is there enough space in front of us for the blocker?
1242  if (blocker->getVehicleType().getLengthWithGap() <= potential) {
1243  // save at least his length in myLeadingBlockerLength
1245 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
1246  if (DEBUG_COND) {
1247  std::cout << STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep())
1248  << " veh=" << myVehicle.getID()
1249  << " blocker=" << Named::getIDSecure(blocker)
1250  << " saving myLeadingBlockerLength=" << myLeadingBlockerLength
1251  << "\n";
1252  }
1253 #endif
1254  } else {
1255  // we cannot save enough space for the blocker. It needs to save
1256  // space for ego instead
1257 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
1258  if (DEBUG_COND) {
1259  std::cout << STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep())
1260  << " veh=" << myVehicle.getID()
1261  << " blocker=" << Named::getIDSecure(blocker)
1262  << " cannot save space=" << blocker->getVehicleType().getLengthWithGap()
1263  << " potential=" << potential
1264  << "\n";
1265  }
1266 #endif
1268  }
1269  }
1270 }
1271 /****************************************************************************/
1272 
bool debugVehicle() const
whether the current vehicles shall be debugged
void saveBlockerLength(MSVehicle *blocker, int lcaCounter)
save space for vehicles which need to counter-lane-change
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
SUMOReal getMaxSpeed() const
Get vehicle&#39;s maximum speed [m/s].
long long int SUMOTime
Definition: SUMOTime.h:43
MSLCM_LC2013(MSVehicle &v)
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.
int wantsChange(int laneOffset, MSAbstractLaneChangeModel::MSLCMessager &msgPass, int blocked, const std::pair< MSVehicle *, SUMOReal > &leader, const std::pair< MSVehicle *, SUMOReal > &neighLead, const std::pair< MSVehicle *, SUMOReal > &neighFollow, const MSLane &neighLane, const std::vector< MSVehicle::LaneQ > &preb, MSVehicle **lastBlocked, MSVehicle **firstBlocked)
Called to examine whether the vehicle wants to change using the given laneOffset. This method gets th...
void prepareStep()
#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
#define MIN_FALLBEHIND
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.
#define ACCEL2SPEED(x)
Definition: SUMOTime.h:61
#define LOOK_FORWARD_RIGHT
const std::vector< MSLane * > & getLanes() const
Returns this edge&#39;s lanes.
Definition: MSEdge.h:192
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
const SUMOReal myChangeProbThresholdRight
Definition: MSLCM_LC2013.h:206
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)
SUMOReal getLength() const
Returns the lane&#39;s length.
Definition: MSLane.h:475
#define KEEP_RIGHT_ACCEPTANCE
The car-following model abstraction.
Definition: MSCFModel.h:59
SUMOReal myKeepRightProbability
Definition: MSLCM_LC2013.h:183
void * informNeighFollower(void *info, MSVehicle *sender)
Informs the follower on the desired lane.
Wants go to the right.
SUMOReal getLength() const
Get vehicle&#39;s length [m].
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
const SUMOReal myKeepRightParam
Definition: MSLCM_LC2013.h:200
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
Definition: Named.h:59
const SUMOReal myChangeProbThresholdLeft
Definition: MSLCM_LC2013.h:207
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
std::vector< SUMOReal > myVSafes
Definition: MSLCM_LC2013.h:192
#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
const SUMOReal myCooperativeParam
Definition: MSLCM_LC2013.h:198
#define UNUSED_PARAMETER(x)
Definition: StdDefs.h:39
#define abs(a)
Definition: polyfonts.c:67
void informFollower(MSAbstractLaneChangeModel::MSLCMessager &msgPass, int blocked, int dir, const std::pair< MSVehicle *, SUMOReal > &neighFollow, SUMOReal remainingSeconds, SUMOReal plannedSpeed)
decide whether we will try cut in before the follower or allow to be overtaken
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:2462
#define SIMTIME
Definition: SUMOTime.h:70
SUMOReal getLateralPositionOnLane() const
Get the vehicle&#39;s lateral position on the lane.
Definition: MSVehicle.h:375
Needs to stay on the current lane.
const SUMOReal myStrategicParam
Definition: MSLCM_LC2013.h:197
#define ROUNDABOUT_DIST_BONUS
#define DEBUG_COND
static bool myAllowOvertakingRight
whether overtaking on the right is permitted
A class responsible for exchanging messages between cars involved in lane-change interaction.
const std::string & getID() const
Returns the id.
Definition: Named.h:66
bool currentDistDisallows(SUMOReal dist, int laneOffset, SUMOReal lookForwardDist)
Definition: MSLCM_LC2013.h:165
bool currentDistAllows(SUMOReal dist, int laneOffset, SUMOReal lookForwardDist)
Definition: MSLCM_LC2013.h:168
#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
The action is urgent (to be defined by lc-model)
SUMOReal myLeadingBlockerLength
Definition: MSLCM_LC2013.h:185
SUMOReal getMinGap() const
Get the free space in front of vehicles of this class.
#define CUT_IN_LEFT_SPEED_THRESHOLD
SUMOReal myLookAheadSpeed
Definition: MSLCM_LC2013.h:190
#define LOOK_AHEAD_SPEED_MEMORY
#define STEPS2TIME(x)
Definition: SUMOTime.h:65
int slowDownForBlocked(MSVehicle **blocked, int state)
compute useful slowdowns for blocked vehicles
T MIN2(T a, T b)
Definition: StdDefs.h:69
#define RELGAIN_NORMALIZATION_MIN_SPEED
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
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
int myOwnState
The current state of the vehicle.
std::string toString(const T &t, std::streamsize accuracy=OUTPUT_ACCURACY)
Definition: ToString.h:55
SUMOReal informLeader(MSAbstractLaneChangeModel::MSLCMessager &msgPass, int blocked, int dir, const std::pair< MSVehicle *, SUMOReal > &neighLead, SUMOReal remainingSeconds)
virtual void saveBlockerLength(SUMOReal length)
reserve space at the end of the lane to avoid dead locks
std::pair< SUMOReal, int > Info
information regarding save velocity (unused) and state flags of the ego vehicle
Definition: MSLCM_LC2013.h:173
int _wantsChange(int laneOffset, MSAbstractLaneChangeModel::MSLCMessager &msgPass, int blocked, const std::pair< MSVehicle *, SUMOReal > &leader, const std::pair< MSVehicle *, SUMOReal > &neighLead, const std::pair< MSVehicle *, SUMOReal > &neighFollow, const MSLane &neighLane, const std::vector< MSVehicle::LaneQ > &preb, MSVehicle **lastBlocked, MSVehicle **firstBlocked)
helper function for doing the actual work
MSVehicle & myVehicle
The vehicle this lane-changer belongs to.
void * informNeighLeader(void *info, MSVehicle *sender)
Informs the leader on the desired lane.
EdgeBasicFunction getPurpose() const
Returns the edge type (EdgeBasicFunction)
Definition: MSEdge.h:249
Influencer & getInfluencer()
Returns the velocity/lane influencer.
Definition: MSVehicle.cpp:3448
#define JAM_FACTOR
SUMOReal occupation
The overall vehicle sum on consecutive lanes which can be passed without a lane change.
Definition: MSVehicle.h:622
std::vector< MSLane * > bestContinuations
Consecutive lane that can be followed without a lane change (contribute to length and occupation) ...
Definition: MSVehicle.h:630
const MSVehicleType & getVehicleType() const
Returns the vehicle&#39;s type definition.
Definition: MSBaseVehicle.h:97
#define URGENCY
const SUMOReal SUMO_const_haltingSpeed
the speed threshold at which vehicles are considered as halting
Definition: StdDefs.h:57
virtual ~MSLCM_LC2013()
SUMOReal getSpeed() const
Returns the vehicle&#39;s current speed.
Definition: MSVehicle.h:410
SUMOReal getWaitingSeconds() const
Returns the number of seconds waited (speed was lesser than 0.1m/s)
Definition: MSVehicle.h:502
#define LOOK_FORWARD_LEFT
SUMOReal mySpeedGainProbability
a value for tracking the probability that a change to the offset with the same sign is beneficial ...
Definition: MSLCM_LC2013.h:179
The action is due to a TraCI request.
SUMOReal myLeftSpace
Definition: MSLCM_LC2013.h:186
#define SUMOReal
Definition: config.h:213
#define NUMERICAL_EPS
Definition: config.h:160
bool amBlockingFollowerPlusNB()
Definition: MSLCM_LC2013.h:162
#define HELP_OVERTAKE
const MSLinkCont & getLinkCont() const
returns the container with all links !!!
Definition: MSLane.cpp:1445
SUMOReal _patchSpeed(const SUMOReal min, const SUMOReal wanted, const SUMOReal max, const MSCFModel &cfModel)
#define LOOK_AHEAD_MIN_SPEED
#define LCA_RIGHT_IMPATIENCE
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
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
The edge is an internal edge.
Definition: MSEdge.h:97
void * inform(void *info, MSVehicle *sender)
Representation of a lane in the micro simulation.
Definition: MSLane.h:79
const MSCFModel & myCarFollowModel
The vehicle&#39;s car following model.
const SUMOReal mySpeedGainParam
Definition: MSLCM_LC2013.h:199
Interface for lane-change models.
int getBestLaneOffset() const
returns the current offset from the best lane
Definition: MSVehicle.cpp:2836
#define HELP_DECEL_FACTOR
const std::string & getID() const
Returns the name of the vehicle.
#define KEEP_RIGHT_TIME