SUMO - Simulation of Urban MObility
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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-sim.org/
14 // Copyright (C) 2001-2014 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 "MSEdge.h"
38 #include "MSLane.h"
39 #include "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 //#define DEBUG_VEHICLE_GUI_SELECTION 1
47 #ifdef DEBUG_VEHICLE_GUI_SELECTION
49 #include <guisim/GUIVehicle.h>
50 #include <guisim/GUILane.h>
51 #endif
52 
53 
54 
55 // ===========================================================================
56 // variable definitions
57 // ===========================================================================
58 // 80km/h will be the threshold for dividing between long/short foresight
59 #define LOOK_FORWARD_SPEED_DIVIDER (SUMOReal)14.
60 
61 #define LOOK_FORWARD_RIGHT (SUMOReal)10.
62 #define LOOK_FORWARD_LEFT (SUMOReal)20.
63 
64 #define JAM_FACTOR (SUMOReal)1.
65 
66 #define LCA_RIGHT_IMPATIENCE (SUMOReal)-1.
67 #define CUT_IN_LEFT_SPEED_THRESHOLD (SUMOReal)27.
68 
69 #define LOOK_AHEAD_MIN_SPEED (SUMOReal)0.0
70 #define LOOK_AHEAD_SPEED_MEMORY (SUMOReal)0.9
71 #define LOOK_AHEAD_SPEED_DECREMENT 6.
72 
73 #define HELP_DECEL_FACTOR (SUMOReal)1.0
74 
75 #define HELP_OVERTAKE (SUMOReal)(10.0 / 3.6)
76 #define MIN_FALLBEHIND (SUMOReal)(14.0 / 3.6)
77 
78 #define KEEP_RIGHT_HEADWAY (SUMOReal)2.0
79 
80 #define URGENCY (SUMOReal)2.0
81 
82 #define ROUNDABOUT_DIST_BONUS (SUMOReal)80.0
83 
84 #define CHANGE_PROB_THRESHOLD_RIGHT (SUMOReal)2.0
85 #define CHANGE_PROB_THRESHOLD_LEFT (SUMOReal)0.2
86 #define KEEP_RIGHT_TIME (SUMOReal)5.0 // the number of seconds after which a vehicle should move to the right lane
87 #define KEEP_RIGHT_ACCEPTANCE (SUMOReal)2.0 // calibration factor for determining the desire to keep right
88 
89 #define RELGAIN_NORMALIZATION_MIN_SPEED (SUMOReal)10.0
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 {}
102 
104  changed();
105 }
106 
107 
108 int
110  int laneOffset,
112  int blocked,
113  const std::pair<MSVehicle*, SUMOReal>& leader,
114  const std::pair<MSVehicle*, SUMOReal>& neighLead,
115  const std::pair<MSVehicle*, SUMOReal>& neighFollow,
116  const MSLane& neighLane,
117  const std::vector<MSVehicle::LaneQ>& preb,
118  MSVehicle** lastBlocked,
119  MSVehicle** firstBlocked) {
120  const int result = _wantsChange(laneOffset, msgPass, blocked, leader, neighLead, neighFollow, neighLane, preb, lastBlocked, firstBlocked);
121  return result;
122 }
123 
124 
125 SUMOReal
126 MSLCM_LC2013::patchSpeed(const SUMOReal min, const SUMOReal wanted, const SUMOReal max, const MSCFModel& cfModel) {
127  const SUMOReal newSpeed = _patchSpeed(min, wanted, max, cfModel);
128  return newSpeed;
129 }
130 
131 
132 SUMOReal
133 MSLCM_LC2013::_patchSpeed(const SUMOReal min, const SUMOReal wanted, const SUMOReal max, const MSCFModel& cfModel) {
134  int state = myOwnState;
135 
136  // letting vehicles merge in at the end of the lane in case of counter-lane change, step#2
137  SUMOReal MAGIC_offset = 1.;
138  // if we want to change and have a blocking leader and there is enough room for him in front of us
139  if (myLeadingBlockerLength != 0) {
141  if (space > 0) {
142  // compute speed for decelerating towards a place which allows the blocking leader to merge in in front
143  SUMOReal safe = cfModel.stopSpeed(&myVehicle, myVehicle.getSpeed(), space);
144  // if we are approaching this place
145  if (safe < wanted) {
146  // return this speed as the speed to use
147  return MAX2(min, safe);
148  }
149  }
150  }
151 
152  SUMOReal nVSafe = wanted;
153  bool gotOne = false;
154  for (std::vector<SUMOReal>::const_iterator i = myVSafes.begin(); i != myVSafes.end(); ++i) {
155  SUMOReal v = (*i);
156  if (v >= min && v <= max) {
157  nVSafe = MIN2(v, nVSafe);
158  gotOne = true;
159  } else {
160  }
161  }
162 
163  if (gotOne && !myDontBrake) {
164  return nVSafe;
165  }
166 
167  // check whether the vehicle is blocked
168  if ((state & LCA_WANTS_LANECHANGE) != 0 && (state & LCA_BLOCKED) != 0) {
169  if ((state & LCA_STRATEGIC) != 0) {
170  // necessary decelerations are controlled via vSafe. If there are
171  // none it means we should speed up
172  return (max + wanted) / (SUMOReal) 2.0;
173  } else if ((state & LCA_COOPERATIVE) != 0) {
174  // only minor adjustments in speed should be done
175  if ((state & LCA_BLOCKED_BY_LEADER) != 0) {
176  return (min + wanted) / (SUMOReal) 2.0;
177  }
178  if ((state & LCA_BLOCKED_BY_FOLLOWER) != 0) {
179  return (max + wanted) / (SUMOReal) 2.0;
180  }
181  }
182  }
183 
184  // accelerate if being a blocking leader or blocking follower not able to brake
185  // (and does not have to change lanes)
186  if ((state & LCA_AMBLOCKINGLEADER) != 0) {
187  return (max + wanted) / (SUMOReal) 2.0;
188  }
189 
190  if ((state & LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0) {
191  }
192  if (myVehicle.getLane()->getEdge().getLanes().size() == 1) {
193  // remove chaning information if on a road with a single lane
194  changed();
195  }
196  return wanted;
197 }
198 
199 
200 void*
201 MSLCM_LC2013::inform(void* info, MSVehicle* /* sender */) {
202  Info* pinfo = (Info*) info;
203  myVSafes.push_back(pinfo->first);
204  myOwnState |= pinfo->second;
205  delete pinfo;
206  return (void*) true;
207 }
208 
209 
210 SUMOReal
212  int blocked,
213  int dir,
214  const std::pair<MSVehicle*, SUMOReal>& neighLead,
215  SUMOReal remainingSeconds) {
216  SUMOReal plannedSpeed = MIN2(myVehicle.getSpeed(),
218  for (std::vector<SUMOReal>::const_iterator i = myVSafes.begin(); i != myVSafes.end(); ++i) {
219  SUMOReal v = (*i);
221  plannedSpeed = MIN2(plannedSpeed, v);
222  }
223  }
224  if ((blocked & LCA_BLOCKED_BY_LEADER) != 0) {
225  assert(neighLead.first != 0);
226  MSVehicle* nv = neighLead.first;
227  // decide whether we want to overtake the leader or follow it
228  const SUMOReal dv = plannedSpeed - nv->getSpeed();
229  const SUMOReal overtakeDist = (neighLead.second // drive to back of follower
230  + nv->getVehicleType().getLengthWithGap() // drive to front of follower
231  + myVehicle.getVehicleType().getLength() // ego back reaches follower front
232  + nv->getCarFollowModel().getSecureGap( // save gap to follower
234 
235  if (dv < 0
236  // overtaking on the right on an uncongested highway is forbidden (noOvertakeLCLeft)
237  || (dir == LCA_MLEFT && !myVehicle.congested())
238  // not enough space to overtake?
239  || myLeftSpace < overtakeDist
240  // not enough time to overtake?
241  || dv * remainingSeconds < overtakeDist) {
242  // cannot overtake
243  msgPass.informNeighLeader(new Info(-1, dir | LCA_AMBLOCKINGLEADER), &myVehicle);
244  // slow down smoothly to follow leader
245  const SUMOReal targetSpeed = myCarFollowModel.followSpeed(
246  &myVehicle, myVehicle.getSpeed(), neighLead.second, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel());
247  if (targetSpeed < myVehicle.getSpeed()) {
248  // slow down smoothly to follow leader
250  MAX2(MIN_FALLBEHIND, (myVehicle.getSpeed() - targetSpeed) / remainingSeconds)));
251  const SUMOReal nextSpeed = MIN2(plannedSpeed, myVehicle.getSpeed() - decel);
252  myVSafes.push_back(nextSpeed);
253  return nextSpeed;
254  } else {
255  // leader is fast enough anyway
256  myVSafes.push_back(targetSpeed);
257  return plannedSpeed;
258  }
259  } else {
260  // overtaking, leader should not accelerate
261  msgPass.informNeighLeader(new Info(nv->getSpeed(), dir | LCA_AMBLOCKINGLEADER), &myVehicle);
262  return -1;
263  }
264  } else if (neighLead.first != 0) { // (remainUnblocked)
265  // we are not blocked now. make sure we stay far enough from the leader
266  MSVehicle* nv = neighLead.first;
267  const SUMOReal nextNVSpeed = nv->getSpeed() - HELP_OVERTAKE; // conservative
268  const SUMOReal dv = SPEED2DIST(myVehicle.getSpeed() - nextNVSpeed);
269  const SUMOReal targetSpeed = myCarFollowModel.followSpeed(
270  &myVehicle, myVehicle.getSpeed(), neighLead.second - dv, nextNVSpeed, nv->getCarFollowModel().getMaxDecel());
271  myVSafes.push_back(targetSpeed);
272  return MIN2(targetSpeed, plannedSpeed);
273  } else {
274  // not overtaking
275  return plannedSpeed;
276  }
277 }
278 
279 
280 void
282  int blocked,
283  int dir,
284  const std::pair<MSVehicle*, SUMOReal>& neighFollow,
285  SUMOReal remainingSeconds,
286  SUMOReal plannedSpeed) {
287  if ((blocked & LCA_BLOCKED_BY_FOLLOWER) != 0) {
288  assert(neighFollow.first != 0);
289  MSVehicle* nv = neighFollow.first;
290 
291  // are we fast enough to cut in without any help?
292  if (plannedSpeed - nv->getSpeed() >= HELP_OVERTAKE) {
293  const SUMOReal neededGap = nv->getCarFollowModel().getSecureGap(nv->getSpeed(), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel());
294  if ((neededGap - neighFollow.second) / remainingSeconds < (plannedSpeed - nv->getSpeed())) {
295  // follower might even accelerate but not to much
296  msgPass.informNeighFollower(new Info(plannedSpeed - HELP_OVERTAKE, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
297  return;
298  }
299  }
300  // decide whether we will request help to cut in before the follower or allow to be overtaken
301 
302  // PARAMETERS
303  // assume other vehicle will assume the equivalent of 1 second of
304  // maximum deceleration to help us (will probably be spread over
305  // multiple seconds)
306  // -----------
307  const SUMOReal helpDecel = nv->getCarFollowModel().getMaxDecel() * HELP_DECEL_FACTOR ;
308 
309  // change in the gap between ego and blocker over 1 second (not STEP!)
310  const SUMOReal neighNewSpeed = MAX2((SUMOReal)0, nv->getSpeed() - ACCEL2SPEED(helpDecel));
311  const SUMOReal neighNewSpeed1s = MAX2((SUMOReal)0, nv->getSpeed() - helpDecel);
312  const SUMOReal dv = plannedSpeed - neighNewSpeed1s;
313  // new gap between follower and self in case the follower does brake for 1s
314  const SUMOReal decelGap = neighFollow.second + dv;
315  const SUMOReal secureGap = nv->getCarFollowModel().getSecureGap(neighNewSpeed1s, plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel());
316  if (decelGap > 0 && decelGap >= secureGap) {
317  // if the blocking neighbor brakes it could actually help
318  // how hard does it actually need to be?
319  const SUMOReal vsafe = MAX2(neighNewSpeed, nv->getCarFollowModel().followSpeed(
320  nv, nv->getSpeed(), neighFollow.second, plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel()));
321  msgPass.informNeighFollower(new Info(vsafe, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
322  } else if (dv > 0 && dv * remainingSeconds > (secureGap - decelGap + POSITION_EPS)) {
323  // decelerating once is sufficient to open up a large enough gap in time
324  msgPass.informNeighFollower(new Info(neighNewSpeed, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
325  } else {
327  //if (dir == LCA_MRIGHT && myVehicle.getWaitingSeconds() > LCA_RIGHT_IMPATIENCE &&
328  // nv->getSpeed() > myVehicle.getSpeed()) {
329  if (nv->getSpeed() > myVehicle.getSpeed() &&
331  || (dir == LCA_MLEFT && plannedSpeed > CUT_IN_LEFT_SPEED_THRESHOLD) // VARIANT_22 (slowDownLeft)
332  )) {
333  // let the follower slow down to increase the likelyhood that later vehicles will be slow enough to help
334  // follower should still be fast enough to open a gap
335  vhelp = MAX2(neighNewSpeed, myVehicle.getSpeed() + HELP_OVERTAKE);
336  if ((nv->getSpeed() - myVehicle.getSpeed()) / helpDecel < remainingSeconds) {
337  msgPass.informNeighFollower(new Info(vhelp, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
338  return;
339  }
340  }
341  msgPass.informNeighFollower(new Info(vhelp, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
342  // this follower is supposed to overtake us. slow down smoothly to allow this
343  const SUMOReal overtakeDist = (neighFollow.second // follower reaches ego back
344  + myVehicle.getVehicleType().getLengthWithGap() // follower reaches ego front
345  + nv->getVehicleType().getLength() // follower back at ego front
346  + myVehicle.getCarFollowModel().getSecureGap( // follower has safe dist to ego
347  plannedSpeed, vhelp, nv->getCarFollowModel().getMaxDecel()));
348  // speed difference to create a sufficiently large gap
349  const SUMOReal needDV = overtakeDist / remainingSeconds;
350  // make sure the deceleration is not to strong
351  myVSafes.push_back(MAX2(vhelp - needDV, myVehicle.getSpeed() - ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxDecel())));
352  }
353  }
354 }
355 
356 
357 void
359  myOwnState = 0;
361  myLeftSpace = 0;
362  myVSafes.clear();
363  myDontBrake = false;
364  // truncate to work around numerical instability between different builds
365  mySpeedGainProbability = ceil(mySpeedGainProbability * 100000.0) * 0.00001;
366  myKeepRightProbability = ceil(myKeepRightProbability * 100000.0) * 0.00001;
367 }
368 
369 
370 void
372  myOwnState = 0;
376  if (myVehicle.getBestLaneOffset() == 0) {
377  // if we are not yet on our best lane there might still be unseen blockers
378  // (during patchSpeed)
380  myLeftSpace = 0;
381  }
383  myVSafes.clear();
384  myDontBrake = false;
385 }
386 
387 
388 int
390  int laneOffset,
392  int blocked,
393  const std::pair<MSVehicle*, SUMOReal>& leader,
394  const std::pair<MSVehicle*, SUMOReal>& neighLead,
395  const std::pair<MSVehicle*, SUMOReal>& neighFollow,
396  const MSLane& neighLane,
397  const std::vector<MSVehicle::LaneQ>& preb,
398  MSVehicle** lastBlocked,
399  MSVehicle** firstBlocked) {
400  assert(laneOffset == 1 || laneOffset == -1);
401  const SUMOTime currentTime = MSNet::getInstance()->getCurrentTimeStep();
402  // compute bestLaneOffset
403  MSVehicle::LaneQ curr, neigh, best;
404  int bestLaneOffset = 0;
405  SUMOReal currentDist = 0;
406  SUMOReal neighDist = 0;
407  int currIdx = 0;
408  MSLane* prebLane = myVehicle.getLane();
409  if (prebLane->getEdge().getPurpose() == MSEdge::EDGEFUNCTION_INTERNAL) {
410  // internal edges are not kept inside the bestLanes structure
411  prebLane = prebLane->getLinkCont()[0]->getLane();
412  }
413  for (int p = 0; p < (int) preb.size(); ++p) {
414  if (preb[p].lane == prebLane && p + laneOffset >= 0) {
415  assert(p + laneOffset < (int)preb.size());
416  curr = preb[p];
417  neigh = preb[p + laneOffset];
418  currentDist = curr.length;
419  neighDist = neigh.length;
420  bestLaneOffset = curr.bestLaneOffset;
421  if (bestLaneOffset == 0 && preb[p + laneOffset].bestLaneOffset == 0) {
422  bestLaneOffset = laneOffset;
423  }
424  best = preb[p + bestLaneOffset];
425  currIdx = p;
426  break;
427  }
428  }
429  // direction specific constants
430  const bool right = (laneOffset == -1);
431  const int lca = (right ? LCA_RIGHT : LCA_LEFT);
432  const int myLca = (right ? LCA_MRIGHT : LCA_MLEFT);
433  const int lcaCounter = (right ? LCA_LEFT : LCA_RIGHT);
434  const bool changeToBest = (right && bestLaneOffset < 0) || (!right && bestLaneOffset > 0);
435  // keep information about being a leader/follower
436  int ret = (myOwnState & 0xffff0000);
437  int req = 0; // the request to change or stay
438 
439  ret = slowDownForBlocked(lastBlocked, ret);
440  if (lastBlocked != firstBlocked) {
441  ret = slowDownForBlocked(firstBlocked, ret);
442  }
443 
444 
445  // we try to estimate the distance which is necessary to get on a lane
446  // we have to get on in order to keep our route
447  // we assume we need something that depends on our velocity
448  // and compare this with the free space on our wished lane
449  //
450  // if the free space is somehow less than the space we need, we should
451  // definitely try to get to the desired lane
452  //
453  // this rule forces our vehicle to change the lane if a lane changing is necessary soon
454  // lookAheadDistance:
455  // we do not want the lookahead distance to change all the time so we discrectize the speed a bit
456 
459  } else {
462  }
464  laDist += myVehicle.getVehicleType().getLengthWithGap() * (SUMOReal) 2.;
465  // free space that is available for changing
466  //const SUMOReal neighSpeed = (neighLead.first != 0 ? neighLead.first->getSpeed() :
467  // neighFollow.first != 0 ? neighFollow.first->getSpeed() :
468  // best.lane->getSpeedLimit());
469  // @note: while this lets vehicles change earlier into the correct direction
470  // it also makes the vehicles more "selfish" and prevents changes which are necessary to help others
471 
472  int roundaboutEdgesAhead = 0;
473  for (std::vector<MSLane*>::iterator it = curr.bestContinuations.begin(); it != curr.bestContinuations.end(); ++it) {
474  if ((*it) != 0 && (*it)->getEdge().isRoundabout()) {
475  roundaboutEdgesAhead += 1;
476  } else if (roundaboutEdgesAhead > 0) {
477  // only check the next roundabout
478  break;
479  }
480  }
481  int roundaboutEdgesAheadNeigh = 0;
482  for (std::vector<MSLane*>::iterator it = neigh.bestContinuations.begin(); it != neigh.bestContinuations.end(); ++it) {
483  if ((*it) != 0 && (*it)->getEdge().isRoundabout()) {
484  roundaboutEdgesAheadNeigh += 1;
485  } else if (roundaboutEdgesAheadNeigh > 0) {
486  // only check the next roundabout
487  break;
488  }
489  }
490  if (roundaboutEdgesAhead > 1) {
491  currentDist += roundaboutEdgesAhead * ROUNDABOUT_DIST_BONUS;
492  neighDist += roundaboutEdgesAheadNeigh * ROUNDABOUT_DIST_BONUS;
493  }
494 
495  const SUMOReal usableDist = (currentDist - myVehicle.getPositionOnLane() - best.occupation * JAM_FACTOR);
496  const SUMOReal maxJam = MAX2(preb[currIdx + laneOffset].occupation, preb[currIdx].occupation);
497  const SUMOReal neighLeftPlace = MAX2((SUMOReal) 0, neighDist - myVehicle.getPositionOnLane() - maxJam);
498 
499  if (changeToBest && bestLaneOffset == curr.bestLaneOffset
500  && currentDistDisallows(usableDist, bestLaneOffset, laDist)) {
502  ret = ret | lca | LCA_STRATEGIC | LCA_URGENT;
503  } else {
504 
505  if (!myAllowOvertakingRight && !right && !myVehicle.congested() && neighLead.first != 0) {
506  // check for slower leader on the left. we should not overtake but
507  // rather move left ourselves (unless congested)
508  MSVehicle* nv = neighLead.first;
509  if (nv->getSpeed() < myVehicle.getSpeed()) {
511  &myVehicle, myVehicle.getSpeed(), neighLead.second, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel()));
512  if (nv->getSpeed() + 5 / 3.6 < myVehicle.getSpeed()) {
514  }
515  }
516  }
517 
518  if (!changeToBest && (currentDistDisallows(neighLeftPlace, abs(bestLaneOffset) + 2, laDist))) {
519  // the opposite lane-changing direction should be done than the one examined herein
520  // we'll check whether we assume we could change anyhow and get back in time...
521  //
522  // this rule prevents the vehicle from moving in opposite direction of the best lane
523  // unless the way till the end where the vehicle has to be on the best lane
524  // is long enough
525  ret = ret | LCA_STAY | LCA_STRATEGIC;
526  } else if (bestLaneOffset == 0 && (neighLeftPlace * 2. < laDist)) {
527  // the current lane is the best and a lane-changing would cause a situation
528  // of which we assume we will not be able to return to the lane we have to be on.
529  // this rule prevents the vehicle from leaving the current, best lane when it is
530  // close to this lane's end
531  ret = ret | LCA_STAY | LCA_STRATEGIC;
532  }
533  }
534  // check for overriding TraCI requests
536  if ((ret & lcaCounter) != 0) {
537  // we are not interested in traci requests for the opposite direction here
538  ret &= ~(LCA_TRACI | lcaCounter | LCA_URGENT);
539  }
540 
541  if ((ret & LCA_STAY) != 0) {
542  return ret;
543  }
544  if ((ret & LCA_URGENT) != 0) {
545  // prepare urgent lane change maneuver
546  // save the left space
547  myLeftSpace = currentDist - myVehicle.getPositionOnLane();
548  if (changeToBest && abs(bestLaneOffset) > 1) {
549  // there might be a vehicle which needs to counter-lane-change one lane further and we cannot see it yet
550  myLeadingBlockerLength = MAX2((SUMOReal)(right ? 20.0 : 40.0), myLeadingBlockerLength);
551  }
552 
553  // letting vehicles merge in at the end of the lane in case of counter-lane change, step#1
554  // if there is a leader and he wants to change to the opposite direction
555  saveBlockerLength(neighLead.first, lcaCounter);
556  if (*firstBlocked != neighLead.first) {
557  saveBlockerLength(*firstBlocked, lcaCounter);
558  }
559 
560  const SUMOReal remainingSeconds = ((ret & LCA_TRACI) == 0 ?
563  const SUMOReal plannedSpeed = informLeader(msgPass, blocked, myLca, neighLead, remainingSeconds);
564  if (plannedSpeed >= 0) {
565  // maybe we need to deal with a blocking follower
566  informFollower(msgPass, blocked, myLca, neighFollow, remainingSeconds, plannedSpeed);
567  }
568 
569  return ret;
570  }
571 
572  if (roundaboutEdgesAhead > 1) {
573  // try to use the inner lanes of a roundabout to increase throughput
574  // unless we are approaching the exit
575  if (lca == LCA_LEFT) {
576  req = ret | lca | LCA_COOPERATIVE;
577  } else {
578  req = ret | LCA_STAY | LCA_COOPERATIVE;
579  }
580  if (!cancelRequest(req)) {
581  return ret | req;
582  }
583  }
584 
585  // let's also regard the case where the vehicle is driving on a highway...
586  // in this case, we do not want to get to the dead-end of an on-ramp
587  if (right) {
588  if (bestLaneOffset == 0 && myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle) > 80. / 3.6 && myLookAheadSpeed > SUMO_const_haltingSpeed) {
589  req = ret | LCA_STAY | LCA_STRATEGIC;
590  if (!cancelRequest(req)) {
591  return ret | req;
592  }
593  }
594  }
595  // --------
596 
597  // -------- make place on current lane if blocking follower
598  //if (amBlockingFollowerPlusNB()) {
599  // std::cout << myVehicle.getID() << ", " << currentDistAllows(neighDist, bestLaneOffset, laDist)
600  // << " neighDist=" << neighDist
601  // << " currentDist=" << currentDist
602  // << "\n";
603  //}
605  && (changeToBest || currentDistAllows(neighDist, abs(bestLaneOffset) + 1, laDist))) {
606 
607  req = ret | lca | LCA_COOPERATIVE | LCA_URGENT ;//| LCA_CHANGE_TO_HELP;
608  if (!cancelRequest(req)) {
609  return ret | req;
610  }
611  }
612 
613  // --------
614 
615 
618  //if ((blocked & LCA_BLOCKED) != 0) {
619  // return ret;
620  //}
622 
623  // -------- higher speed
624  //if ((congested(neighLead.first) && neighLead.second < 20) || predInteraction(leader.first)) { //!!!
625  // return ret;
626  //}
628  SUMOReal neighLaneVSafe = neighLane.getVehicleMaxSpeed(&myVehicle);
629  if (neighLead.first == 0) {
630  neighLaneVSafe = MIN2(neighLaneVSafe, myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), neighDist, 0, 0));
631  } else {
632  // @todo: what if leader is below safe gap?!!!
633  neighLaneVSafe = MIN2(neighLaneVSafe, myCarFollowModel.followSpeed(
634  &myVehicle, myVehicle.getSpeed(), neighLead.second, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()));
635  }
636  if (leader.first == 0) {
637  thisLaneVSafe = MIN2(thisLaneVSafe, myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), currentDist, 0, 0));
638  } else {
639  // @todo: what if leader is below safe gap?!!!
640  thisLaneVSafe = MIN2(thisLaneVSafe, myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), leader.second, leader.first->getSpeed(), leader.first->getCarFollowModel().getMaxDecel()));
641  }
642 
644  thisLaneVSafe = MIN2(thisLaneVSafe, vMax);
645  neighLaneVSafe = MIN2(neighLaneVSafe, vMax);
646  const SUMOReal relativeGain = (neighLaneVSafe - thisLaneVSafe) / MAX2(neighLaneVSafe,
648 
649  if (right) {
650  // ONLY FOR CHANGING TO THE RIGHT
651  if (thisLaneVSafe - 5 / 3.6 > neighLaneVSafe) {
652  // ok, the current lane is faster than the right one...
653  if (mySpeedGainProbability < 0) {
654  mySpeedGainProbability /= 2.0;
655  //myKeepRightProbability /= 2.0;
656  }
657  } else {
658  // ok, the current lane is not faster than the right one
659  mySpeedGainProbability -= relativeGain;
660 
661  // honor the obligation to keep right (Rechtsfahrgebot)
662  // XXX consider fast approaching followers on the current lane
663  //const SUMOReal vMax = myLookAheadSpeed;
664  const SUMOReal acceptanceTime = KEEP_RIGHT_ACCEPTANCE * vMax * MAX2((SUMOReal)1, myVehicle.getSpeed()) / myVehicle.getLane()->getSpeedLimit();
665  SUMOReal fullSpeedGap = MAX2((SUMOReal)0, neighDist - myVehicle.getCarFollowModel().brakeGap(vMax));
666  SUMOReal fullSpeedDrivingSeconds = MIN2(acceptanceTime, fullSpeedGap / vMax);
667  if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
668  fullSpeedGap = MAX2((SUMOReal)0, MIN2(fullSpeedGap,
669  neighLead.second - myVehicle.getCarFollowModel().getSecureGap(
670  vMax, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
671  fullSpeedDrivingSeconds = MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
672  }
673  const SUMOReal deltaProb = (CHANGE_PROB_THRESHOLD_RIGHT
675  * (fullSpeedDrivingSeconds / acceptanceTime) / KEEP_RIGHT_TIME);
676  myKeepRightProbability -= deltaProb;
677 
678  if (gDebugFlag2) {
679  std::cout << STEPS2TIME(currentTime)
680  << " veh=" << myVehicle.getID()
681  << " vMax=" << vMax
682  << " neighDist=" << neighDist
683  << " brakeGap=" << myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed())
684  << " leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
685  << " secGap=" << (neighLead.first == 0 ? -1 : myVehicle.getCarFollowModel().getSecureGap(
686  myVehicle.getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
687  << " acceptanceTime=" << acceptanceTime
688  << " fullSpeedGap=" << fullSpeedGap
689  << " fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
690  << " dProb=" << deltaProb
691  << "\n";
692  }
694  req = ret | lca | LCA_KEEPRIGHT;
695  if (!cancelRequest(req)) {
696  return ret | req;
697  }
698  }
699  }
700 
702  && neighDist / MAX2((SUMOReal) .1, myVehicle.getSpeed()) > 20.) { //./MAX2((SUMOReal) .1, myVehicle.getSpeed())) { // -.1
703  req = ret | lca | LCA_SPEEDGAIN;
704  if (!cancelRequest(req)) {
705  return ret | req;
706  }
707  }
708  } else {
709  // ONLY FOR CHANGING TO THE LEFT
710  if (thisLaneVSafe > neighLaneVSafe) {
711  // this lane is better
712  if (mySpeedGainProbability > 0) {
713  mySpeedGainProbability /= 2.0;
714  }
715  } else {
716  // left lane is better
717  mySpeedGainProbability += relativeGain;
718  }
719  if (mySpeedGainProbability > CHANGE_PROB_THRESHOLD_LEFT && neighDist / MAX2((SUMOReal) .1, myVehicle.getSpeed()) > 20.) { // .1
720  req = ret | lca | LCA_SPEEDGAIN;
721  if (!cancelRequest(req)) {
722  return ret | req;
723  }
724  }
725  }
726  // --------
727  if (changeToBest && bestLaneOffset == curr.bestLaneOffset
728  && (right ? mySpeedGainProbability < 0 : mySpeedGainProbability > 0)) {
729  // change towards the correct lane, speedwise it does not hurt
730  req = ret | lca | LCA_STRATEGIC;
731  if (!cancelRequest(req)) {
732  return ret | req;
733  }
734  }
735 
736  return ret;
737 }
738 
739 
740 int
742  // if this vehicle is blocking someone in front, we maybe decelerate to let him in
743  if ((*blocked) != 0) {
744  SUMOReal gap = (*blocked)->getPositionOnLane() - (*blocked)->getVehicleType().getLength() - myVehicle.getPositionOnLane() - myVehicle.getVehicleType().getMinGap();
745  if (gap > POSITION_EPS) {
747  if ((*blocked)->getSpeed() < SUMO_const_haltingSpeed) {
749  } else {
750  state |= LCA_AMBACKBLOCKER;
751  }
754  (SUMOReal)(gap - POSITION_EPS), (*blocked)->getSpeed(),
755  (*blocked)->getCarFollowModel().getMaxDecel()));
756  }
757  }
758  }
759  return state;
760 }
761 
762 
763 void
764 MSLCM_LC2013::saveBlockerLength(MSVehicle* blocker, int lcaCounter) {
765  if (blocker != 0 && (blocker->getLaneChangeModel().getOwnState() & lcaCounter) != 0) {
766  // is there enough space in front of us for the blocker?
769  if (blocker->getVehicleType().getLengthWithGap() <= potential) {
770  // save at least his length in myLeadingBlockerLength
772  } else {
773  // we cannot save enough space for the blocker. It needs to save
774  // space for ego instead
776  }
777  }
778 }
779 /****************************************************************************/
780