SUMO - Simulation of Urban MObility
MSLaneChanger.cpp
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2002-2018 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
20 // Performs lane changing of vehicles
21 /****************************************************************************/
22 
23 // ===========================================================================
24 // included modules
25 // ===========================================================================
26 #include <config.h>
27 
28 #include "MSLaneChanger.h"
29 #include "MSNet.h"
30 #include "MSVehicle.h"
31 #include "MSVehicleType.h"
32 #include "MSVehicleTransfer.h"
33 #include "MSGlobals.h"
34 #include <cassert>
35 #include <iterator>
36 #include <cstdlib>
37 #include <cmath>
41 
42 #define OPPOSITE_OVERTAKING_SAFE_TIMEGAP 0.0
43 #define OPPOSITE_OVERTAKING_SAFETYGAP_HEADWAY_FACTOR 0.0
44 #define OPPOSITE_OVERTAKING_SAFETY_FACTOR 1.2
45 // XXX maxLookAhead should be higher if all leaders are stopped and lower when they are jammed/queued
46 #define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD 150.0 // just a guess
47 #define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD_EMERGENCY 1000.0 // just a guess
48 // this is used for finding oncoming vehicles while driving in the opposite direction
49 #define OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD 200.0 // just a guess
50 
51 // ===========================================================================
52 // debug defines
53 // ===========================================================================
54 
55 //#define DEBUG_CONTINUE_CHANGE
56 //#define DEBUG_CHECK_CHANGE
57 //#define DEBUG_SURROUNDING_VEHICLES // debug getRealFollower() and getRealLeader()
58 //#define DEBUG_CHANGE_OPPOSITE
59 //#define DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
60 //#define DEBUG_ACTIONSTEPS
61 //#define DEBUG_STATE
62 //#define DEBUG_CANDIDATE
63 //#define DEBUG_COND (vehicle->getLaneChangeModel().debugVehicle())
64 #define DEBUG_COND (vehicle->isSelected())
65 
66 
67 
68 // ===========================================================================
69 // ChangeElem member method definitions
70 // ===========================================================================
72  lead(nullptr),
73  lane(_lane),
74  hoppedVeh(nullptr),
75  lastBlocked(nullptr),
76  firstBlocked(nullptr),
77  ahead(lane),
78  aheadNext(lane, nullptr, 0) {
79 }
80 
81 void
83  lane->myTmpVehicles.insert(lane->myTmpVehicles.begin(), vehicle);
84  dens += vehicle->getVehicleType().getLengthWithGap();
85  hoppedVeh = vehicle;
86 }
87 
88 
89 // ===========================================================================
90 // member method definitions
91 // ===========================================================================
92 MSLaneChanger::MSLaneChanger(const std::vector<MSLane*>* lanes, bool allowChanging) :
93  myAllowsChanging(allowChanging),
94  myChangeToOpposite(lanes->front()->getEdge().canChangeToOpposite()) {
95 
96  // Fill the changer with the lane-data.
97  myChanger.reserve(lanes->size());
98  for (std::vector<MSLane*>::const_iterator lane = lanes->begin(); lane != lanes->end(); ++lane) {
99  myChanger.push_back(ChangeElem(*lane));
100  myChanger.back().mayChangeRight = lane != lanes->begin();
101  myChanger.back().mayChangeLeft = (lane + 1) != lanes->end();
102  // avoid changing on internal sibling lane
103  if ((*lane)->isInternal()) {
104  if (myChanger.back().mayChangeRight && (*lane)->getLogicalPredecessorLane() == (*(lane - 1))->getLogicalPredecessorLane()) {
105  myChanger.back().mayChangeRight = false;
106  }
107  if (myChanger.back().mayChangeLeft && (*lane)->getLogicalPredecessorLane() == (*(lane + 1))->getLogicalPredecessorLane()) {
108  myChanger.back().mayChangeLeft = false;
109  }
110  }
111  }
112 }
113 
114 
116 }
117 
118 
119 void
121  // This is what happens in one timestep. After initialization of the
122  // changer, each vehicle will try to change. After that the changer
123  // needs an update to prevent multiple changes of one vehicle.
124  // Finally, the change-result has to be given back to the lanes.
125  initChanger();
126  try {
127  while (vehInChanger()) {
128  const bool haveChanged = change();
129  updateChanger(haveChanged);
130  }
131  updateLanes(t);
132  } catch (const ProcessError&) {
133  // clean up locks or the gui may hang
134  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
135  ce->lane->releaseVehicles();
136  }
137  throw;
138  }
139 }
140 
141 
142 void
144  // Prepare myChanger with a safe state.
145  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
146  ce->lead = nullptr;
147  ce->hoppedVeh = nullptr;
148  ce->lastBlocked = nullptr;
149  ce->firstBlocked = nullptr;
150  ce->dens = 0;
151  ce->lane->getVehiclesSecure();
152 
153  //std::cout << SIMTIME << " initChanger lane=" << ce->lane->getID() << " vehicles=" << toString(ce->lane->myVehicles) << "\n";
154  }
155 }
156 
157 
158 void
159 MSLaneChanger::updateChanger(bool vehHasChanged) {
160  assert(veh(myCandi) != 0);
161 
162  // "Push" the vehicles to the back, i.e. follower becomes vehicle,
163  // vehicle becomes leader, and leader becomes predecessor of vehicle,
164  // if it exists.
165  if (!vehHasChanged) {
166  //std::cout << SIMTIME << " updateChanger: lane=" << myCandi->lane->getID() << " has new lead=" << veh(myCandi)->getID() << "\n";
167  myCandi->lead = veh(myCandi);
168  }
169 
170  MSLane::VehCont& vehicles = myCandi->lane->myVehicles;
171  vehicles.pop_back();
172  //std::cout << SIMTIME << " updateChanger lane=" << myCandi->lane->getID() << " vehicles=" << toString(myCandi->lane->myVehicles) << "\n";
173 }
174 
175 
176 void
178 
179  // Update the lane's vehicle-container.
180  // First: it is bad style to change other classes members, but for
181  // this release, other attempts were too time-consuming. In a next
182  // release we will change from this lane-centered design to a vehicle-
183  // centered. This will solve many problems.
184  // Second: this swap would be faster if vehicle-containers would have
185  // been pointers, but then I had to change too much of the MSLane code.
186  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
187  //std::cout << SIMTIME << " updateLanes lane=" << ce->lane->getID() << " myVehicles=" << toString(ce->lane->myVehicles) << " myTmpVehicles=" << toString(ce->lane->myTmpVehicles) << "\n";
188  ce->lane->swapAfterLaneChange(t);
189  ce->lane->releaseVehicles();
190  }
191 }
192 
193 
196  // Find the vehicle in myChanger with the largest position. If there
197  // is no vehicle in myChanger (shouldn't happen) , return myChanger.end().
198  ChangerIt max = myChanger.end();
199 #ifdef DEBUG_CANDIDATE
200  std::cout << SIMTIME << " findCandidate() on edge " << myChanger.begin()->lane->getEdge().getID() << std::endl;
201 #endif
202 
203  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
204  if (veh(ce) == nullptr) {
205  continue;
206  }
207 #ifdef DEBUG_CANDIDATE
208  std::cout << " lane = " << ce->lane->getID() << "\n";
209  std::cout << " check vehicle=" << veh(ce)->getID() << " pos=" << veh(ce)->getPositionOnLane() << " lane=" << ce->lane->getID() << " isFrontOnLane=" << veh(ce)->isFrontOnLane(ce->lane) << "\n";
210 #endif
211  if (max == myChanger.end()) {
212 #ifdef DEBUG_CANDIDATE
213  std::cout << " new max vehicle=" << veh(ce)->getID() << " pos=" << veh(ce)->getPositionOnLane() << " lane=" << ce->lane->getID() << " isFrontOnLane=" << veh(ce)->isFrontOnLane(ce->lane) << "\n";
214 #endif
215  max = ce;
216  continue;
217  }
218  assert(veh(ce) != 0);
219  assert(veh(max) != 0);
220  if (veh(max)->getPositionOnLane() < veh(ce)->getPositionOnLane()) {
221 #ifdef DEBUG_CANDIDATE
222  std::cout << " new max vehicle=" << veh(ce)->getID() << " pos=" << veh(ce)->getPositionOnLane() << " lane=" << ce->lane->getID() << " isFrontOnLane=" << veh(ce)->isFrontOnLane(ce->lane) << " oldMaxPos=" << veh(max)->getPositionOnLane() << "\n";
223 #endif
224  max = ce;
225  }
226  }
227  assert(max != myChanger.end());
228  assert(veh(max) != 0);
229  return max;
230 }
231 
232 
233 bool
234 MSLaneChanger::mayChange(int direction) const {
235  if (direction == 0) {
236  return true;
237  }
238  if (!myAllowsChanging) {
239  return false;
240  }
241  if (direction == -1) {
242  return myCandi->mayChangeRight && (myCandi - 1)->lane->allowsVehicleClass(veh(myCandi)->getVehicleType().getVehicleClass());
243  } else if (direction == 1) {
244  return myCandi->mayChangeLeft && (myCandi + 1)->lane->allowsVehicleClass(veh(myCandi)->getVehicleType().getVehicleClass());
245  } else {
246  return false;
247  }
248 }
249 
250 
251 bool
253  // Find change-candidate. If it is on an allowed lane, try to change
254  // to the right (there is a rule in Germany that you have to change
255  // to the right, unless you are overtaking). If change to the right
256  // isn't possible, check if there is a possibility to overtake (on the
257  // left.
258  // If candidate isn't on an allowed lane, changing to an allowed has
259  // priority.
260 
261 #ifdef DEBUG_ACTIONSTEPS
262 // std::cout<< "\nCHANGE" << std::endl;
263 #endif
264 
265 
267  MSVehicle* vehicle = veh(myCandi);
268 
269  if (vehicle->getLaneChangeModel().isChangingLanes()) {
270  return continueChange(vehicle, myCandi);
271  }
272  if (!myAllowsChanging || vehicle->getLaneChangeModel().alreadyChanged() || vehicle->isStoppedOnLane()) {
273  registerUnchanged(vehicle);
274  return false;
275  }
276 
277  if (!vehicle->isActive()) {
278 #ifdef DEBUG_ACTIONSTEPS
279  if DEBUG_COND {
280  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' skips regular change checks." << std::endl;
281  }
282 #endif
283  bool changed = false;
284  const int oldstate = vehicle->getLaneChangeModel().getOwnState();
285  // let TraCI influence the wish to change lanes during non-actionsteps
286  checkTraCICommands(vehicle);
287  if (oldstate != vehicle->getLaneChangeModel().getOwnState()) {
288  changed = applyTraCICommands(vehicle);
289  }
290  if (!changed) {
291  registerUnchanged(vehicle);
292  }
293  return changed;
294  }
295 
296  // Check for changes to the opposite lane if vehicle is active
297  std::pair<MSVehicle* const, double> leader = getRealLeader(myCandi);
298  if (myChanger.size() == 1 || vehicle->getLaneChangeModel().isOpposite() || (!mayChange(-1) && !mayChange(1))) {
299  if (changeOpposite(leader)) {
300  return true;
301  }
302  registerUnchanged(vehicle);
303  return false;
304  }
305 
306  vehicle->updateBestLanes(); // needed?
307  for (int i = 0; i < (int) myChanger.size(); ++i) {
308  vehicle->adaptBestLanesOccupation(i, myChanger[i].dens);
309  }
310 
311  const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes();
312  // check whether the vehicle wants and is able to change to right lane
313  int stateRight = 0;
314  if (mayChange(-1)) {
315  stateRight = checkChangeWithinEdge(-1, leader, preb);
316  // change if the vehicle wants to and is allowed to change
317  if ((stateRight & LCA_RIGHT) != 0 && (stateRight & LCA_BLOCKED) == 0) {
318  vehicle->getLaneChangeModel().setOwnState(stateRight);
319  return startChange(vehicle, myCandi, -1);
320  }
321  if ((stateRight & LCA_RIGHT) != 0 && (stateRight & LCA_URGENT) != 0) {
322  (myCandi - 1)->lastBlocked = vehicle;
323  if ((myCandi - 1)->firstBlocked == nullptr) {
324  (myCandi - 1)->firstBlocked = vehicle;
325  }
326  }
327  }
328 
329  // check whether the vehicle wants and is able to change to left lane
330  int stateLeft = 0;
331  if (mayChange(1)) {
332  stateLeft = checkChangeWithinEdge(1, leader, preb);
333  // change if the vehicle wants to and is allowed to change
334  if ((stateLeft & LCA_LEFT) != 0 && (stateLeft & LCA_BLOCKED) == 0) {
335  vehicle->getLaneChangeModel().setOwnState(stateLeft);
336  return startChange(vehicle, myCandi, 1);
337  }
338  if ((stateLeft & LCA_LEFT) != 0 && (stateLeft & LCA_URGENT) != 0) {
339  (myCandi + 1)->lastBlocked = vehicle;
340  if ((myCandi + 1)->firstBlocked == nullptr) {
341  (myCandi + 1)->firstBlocked = vehicle;
342  }
343  }
344  }
345 
346  if ((stateRight & LCA_URGENT) != 0 && (stateLeft & LCA_URGENT) != 0) {
347  // ... wants to go to the left AND to the right
348  // just let them go to the right lane...
349  stateLeft = 0;
350  }
351  vehicle->getLaneChangeModel().setOwnState(stateRight | stateLeft);
352 
353  // only emergency vehicles should change to the opposite side on a
354  // multi-lane road
355  if (vehicle->getVehicleType().getVehicleClass() == SVC_EMERGENCY
356  && changeOpposite(leader)) {
357  return true;
358  }
359 
360  registerUnchanged(vehicle);
361  return false;
362 }
363 
364 
365 void
367  myCandi->lane->myTmpVehicles.insert(myCandi->lane->myTmpVehicles.begin(), veh(myCandi));
368  myCandi->dens += vehicle->getVehicleType().getLengthWithGap();
369  vehicle->getLaneChangeModel().unchanged();
370 }
371 
372 
373 
374 void
376 #ifdef DEBUG_STATE
377  const int oldstate = vehicle->getLaneChangeModel().getOwnState();
378 #endif
380 #ifdef DEBUG_STATE
381  if (DEBUG_COND) {
382  const int newstate = vehicle->getLaneChangeModel().getOwnState();
383  std::cout << SIMTIME
384  << " veh=" << vehicle->getID()
385  << " oldState=" << toString((LaneChangeAction) oldstate)
386  << " newState=" << toString((LaneChangeAction) newstate)
387  << ((newstate & LCA_BLOCKED) != 0 ? " (blocked)" : "")
388  << ((newstate & LCA_OVERLAPPING) != 0 ? " (overlap)" : "")
389  << "\n";
390  }
391 #endif
392 }
393 
394 
395 bool
397  // Execute request if not blocked
398  bool changed = false;
399  const int state = vehicle->getLaneChangeModel().getOwnState();
400  const int dir = (state & LCA_RIGHT) != 0 ? -1 : ((state & LCA_LEFT) != 0 ? 1 : 0);
401  const bool execute = dir != 0 && ((state & LCA_BLOCKED) == 0);
402  if (execute) {
403  ChangerIt to = myCandi + dir;
404  bool continuous = vehicle->getLaneChangeModel().startLaneChangeManeuver(myCandi->lane, to->lane, dir);
405  if (continuous) {
406  changed = continueChange(vehicle, myCandi);
407  } else {
408  // insert vehicle into target lane
409  to->registerHop(vehicle);
410  changed = true;
411  }
412  }
413  return changed;
414 }
415 
416 
417 bool
418 MSLaneChanger::startChange(MSVehicle* vehicle, ChangerIt& from, int direction) {
419  if (vehicle->isRemoteControlled()) {
420  registerUnchanged(vehicle);
421  return false;
422  }
423  ChangerIt to = from + direction;
424  // @todo delay entering the target lane until the vehicle intersects it
425  // physically (considering lane width and vehicle width)
426  //if (to->lane->getID() == "beg_1") std::cout << SIMTIME << " startChange to lane=" << to->lane->getID() << " myTmpVehiclesBefore=" << toString(to->lane->myTmpVehicles) << "\n";
427  const bool continuous = vehicle->getLaneChangeModel().startLaneChangeManeuver(from->lane, to->lane, direction);
428  if (continuous) {
429  return continueChange(vehicle, myCandi);
430  } else {
431  to->registerHop(vehicle);
432  to->lane->requireCollisionCheck();
433  return true;
434  }
435 }
436 
437 bool
440  const int direction = lcm.getLaneChangeDirection();
441  const bool pastMidpoint = lcm.updateCompletion();
442  vehicle->myState.myPosLat += SPEED2DIST(lcm.getSpeedLat());
444  if (pastMidpoint) {
445  ChangerIt to = from + direction;
446  MSLane* source = myCandi->lane;
447  MSLane* target = to->lane;
448  vehicle->myState.myPosLat -= direction * 0.5 * (source->getWidth() + target->getWidth());
449  lcm.primaryLaneChanged(source, target, direction);
450  to->registerHop(vehicle);
451  to->lane->requireCollisionCheck();
452  } else {
453  from->registerHop(vehicle);
454  from->lane->requireCollisionCheck();
455  }
456  if (!lcm.isChangingLanes()) {
457  vehicle->myState.myPosLat = 0;
458  lcm.endLaneChangeManeuver();
459  }
460  lcm.updateShadowLane();
461  if (lcm.getShadowLane() != nullptr) {
462  // set as hoppedVeh on the shadow lane so it is found as leader on both lanes
463  ChangerIt shadow = pastMidpoint ? from : from + lcm.getShadowDirection();
464  shadow->hoppedVeh = vehicle;
466  }
467  vehicle->myAngle = vehicle->computeAngle();
468 
469 #ifdef DEBUG_CONTINUE_CHANGE
470  if (DEBUG_COND) {
471  std::cout << SIMTIME
472  << " continueChange veh=" << vehicle->getID()
473  << " from=" << Named::getIDSecure(from->lane)
474  << " dir=" << direction
475  << " pastMidpoint=" << pastMidpoint
476  << " posLat=" << vehicle->getLateralPositionOnLane()
477  //<< " completion=" << lcm.getLaneChangeCompletion()
478  << " shadowLane=" << Named::getIDSecure(lcm.getShadowLane())
479  //<< " shadowHopped=" << Named::getIDSecure(shadow->lane)
480  << "\n";
481  }
482 #endif
483  return pastMidpoint && lcm.getShadowLane() == nullptr;
484 }
485 
486 
487 std::pair<MSVehicle* const, double>
489  assert(veh(myCandi) != 0);
490  MSVehicle* vehicle = veh(myCandi);
491 #ifdef DEBUG_SURROUNDING_VEHICLES
492  if (DEBUG_COND) {
493  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' looks for leader on lc-target lane '" << target->lane->getID() << "'." << std::endl;
494  }
495 #endif
496  // get the leading vehicle on the lane to change to
497  MSVehicle* neighLead = target->lead;
498 
499 #ifdef DEBUG_SURROUNDING_VEHICLES
500  if (DEBUG_COND) {
501  if (neighLead != 0) {
502  std::cout << "Considering '" << neighLead->getID() << "' at position " << neighLead->getPositionOnLane() << std::endl;
503  }
504  }
505 #endif
506 
507  //if (veh(myCandi)->getID() == "disabled") std::cout << SIMTIME
508  // << " target=" << target->lane->getID()
509  // << " neighLead=" << Named::getIDSecure(neighLead)
510  // << " hopped=" << Named::getIDSecure(target->hoppedVeh)
511  // << " (416)\n";
512  // check whether the hopped vehicle became the leader
513  if (target->hoppedVeh != nullptr) {
514  double hoppedPos = target->hoppedVeh->getPositionOnLane();
515 #ifdef DEBUG_SURROUNDING_VEHICLES
516  if (DEBUG_COND) {
517  std::cout << "Considering hopped vehicle '" << target->hoppedVeh->getID() << "' at position " << hoppedPos << std::endl;
518  }
519 #endif
520  if (hoppedPos > veh(myCandi)->getPositionOnLane() && (neighLead == nullptr || neighLead->getPositionOnLane() > hoppedPos)) {
521  neighLead = target->hoppedVeh;
522  //if (veh(myCandi)->getID() == "flow.21") std::cout << SIMTIME << " neighLead=" << Named::getIDSecure(neighLead) << " (422)\n";
523  }
524  }
525  if (neighLead == nullptr) {
526 #ifdef DEBUG_SURROUNDING_VEHICLES
527  if (DEBUG_COND) {
528  std::cout << "Looking for leader on consecutive lanes." << std::endl;
529  }
530 #endif
531  // There's no leader on the target lane. Look for leaders on consecutive lanes.
532  // (there might also be partial leaders due to continuous lane changing)
533  MSLane* targetLane = target->lane;
534  const double egoBack = vehicle->getBackPositionOnLane();
535  double leaderBack = targetLane->getLength();
536  for (MSVehicle* pl : targetLane->myPartialVehicles) {
537  double plBack = pl->getBackPositionOnLane(targetLane);
538  if (plBack < leaderBack &&
539  pl->getPositionOnLane(targetLane) + pl->getVehicleType().getMinGap() >= egoBack) {
540  neighLead = pl;
541  leaderBack = plBack;
542  }
543  }
544  if (neighLead != nullptr) {
545 #ifdef DEBUG_SURROUNDING_VEHICLES
546  if (DEBUG_COND) {
547  std::cout << " found leader=" << neighLead->getID() << " (partial)\n";
548  }
549 #endif
550  return std::pair<MSVehicle*, double>(neighLead, leaderBack - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap());
551  }
552  double seen = myCandi->lane->getLength() - veh(myCandi)->getPositionOnLane();
553  double speed = veh(myCandi)->getSpeed();
554  double dist = veh(myCandi)->getCarFollowModel().brakeGap(speed) + veh(myCandi)->getVehicleType().getMinGap();
555  // always check for link leaders while on an internal lane
556  if (seen > dist && !myCandi->lane->isInternal()) {
557 #ifdef DEBUG_SURROUNDING_VEHICLES
558  if (DEBUG_COND) {
559  std::cout << " found no leader within dist=" << dist << "\n";
560  }
561 #endif
562  return std::pair<MSVehicle* const, double>(static_cast<MSVehicle*>(nullptr), -1);
563  }
564  const std::vector<MSLane*>& bestLaneConts = veh(myCandi)->getBestLanesContinuation(targetLane);
565 
566  std::pair<MSVehicle* const, double> result = target->lane->getLeaderOnConsecutive(dist, seen, speed, *veh(myCandi), bestLaneConts);
567 #ifdef DEBUG_SURROUNDING_VEHICLES
568  if (DEBUG_COND) {
569  std::cout << " found consecutiveLeader=" << Named::getIDSecure(result.first) << "\n";
570  }
571 #endif
572  return result;
573  } else {
574  MSVehicle* candi = veh(myCandi);
575 #ifdef DEBUG_SURROUNDING_VEHICLES
576  if (DEBUG_COND) {
577  std::cout << " found leader=" << neighLead->getID() << "\n";
578  }
579 #endif
580  return std::pair<MSVehicle* const, double>(neighLead, neighLead->getBackPositionOnLane(target->lane) - candi->getPositionOnLane() - candi->getVehicleType().getMinGap());
581  }
582 }
583 
584 
585 std::pair<MSVehicle* const, double>
587  assert(veh(myCandi) != 0);
588 
589 #ifdef DEBUG_SURROUNDING_VEHICLES
590  MSVehicle* vehicle = veh(myCandi);
591  if (DEBUG_COND) {
592  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' looks for follower on lc-target lane '" << target->lane->getID() << "'." << std::endl;
593  }
594 #endif
595  MSVehicle* candi = veh(myCandi);
596  const double candiPos = candi->getPositionOnLane();
597  MSVehicle* neighFollow = veh(target);
598 
599 #ifdef DEBUG_SURROUNDING_VEHICLES
600  if (DEBUG_COND) {
601  if (neighFollow != 0) {
602  std::cout << "veh(target) returns '" << neighFollow->getID() << "' at position " << neighFollow->getPositionOnLane() << std::endl;
603  } else {
604  std::cout << "veh(target) returns none." << std::endl;
605  }
606  }
607 #endif
608 
609 
610 #ifdef DEBUG_SURROUNDING_VEHICLES
611  if (DEBUG_COND) {
612  if (getCloserFollower(candiPos, neighFollow, target->hoppedVeh) != neighFollow) {
613  std::cout << "Hopped vehicle '" << target->hoppedVeh->getID() << "' at position " << target->hoppedVeh->getPositionOnLane() << " is closer." << std::endl;
614  }
615  }
616 #endif
617 
618  // check whether the hopped vehicle became the follower
619  neighFollow = getCloserFollower(candiPos, neighFollow, target->hoppedVeh);
620 
621 
622 #ifdef DEBUG_SURROUNDING_VEHICLES
623  if (DEBUG_COND) {
624  MSVehicle* partialBehind = getCloserFollower(candiPos, neighFollow, target->lane->getPartialBehind(candi));
625  if (partialBehind != 0 && partialBehind != neighFollow) {
626  std::cout << "'Partial behind'-vehicle '" << target->lane->getPartialBehind(candi)->getID() << "' at position " << partialBehind->getPositionOnLane() << " is closer." << std::endl;
627  }
628  }
629 #endif
630  // or a follower which is partially lapping into the target lane
631  neighFollow = getCloserFollower(candiPos, neighFollow, target->lane->getPartialBehind(candi));
632 
633  if (neighFollow == nullptr) {
634  CLeaderDist consecutiveFollower = target->lane->getFollowersOnConsecutive(candi, candi->getBackPositionOnLane(), true)[0];
635 #ifdef DEBUG_SURROUNDING_VEHICLES
636  if (DEBUG_COND) {
637  if (consecutiveFollower.first == 0) {
638  std::cout << "no follower found." << std::endl;
639  } else {
640  std::cout << "found follower '" << consecutiveFollower.first->getID() << "' on consecutive lanes." << std::endl;
641  }
642  }
643 #endif
644  return std::make_pair(const_cast<MSVehicle*>(consecutiveFollower.first), consecutiveFollower.second);
645  } else {
646 #ifdef DEBUG_SURROUNDING_VEHICLES
647  if (DEBUG_COND) {
648  std::cout << "found follower '" << neighFollow->getID() << "'." << std::endl;
649  }
650 #endif
651  MSVehicle* candi = veh(myCandi);
652  return std::pair<MSVehicle* const, double>(neighFollow,
653  candi->getPositionOnLane() - candi->getVehicleType().getLength() - neighFollow->getPositionOnLane() - neighFollow->getVehicleType().getMinGap());
654  }
655 }
656 
657 
658 MSVehicle*
659 MSLaneChanger::getCloserFollower(const double maxPos, MSVehicle* follow1, MSVehicle* follow2) {
660  if (follow1 == nullptr || follow1->getPositionOnLane() > maxPos) {
661  return follow2;
662  } else if (follow2 == nullptr || follow2->getPositionOnLane() > maxPos) {
663  return follow1;
664  } else {
665  if (follow1->getPositionOnLane() > follow2->getPositionOnLane()) {
666  return follow1;
667  } else {
668  return follow2;
669  }
670  }
671 }
672 
673 int
675  int laneOffset,
676  const std::pair<MSVehicle* const, double>& leader,
677  const std::vector<MSVehicle::LaneQ>& preb) const {
678 
679  std::pair<MSVehicle* const, double> neighLead = getRealLeader(myCandi + laneOffset);
680  std::pair<MSVehicle*, double> neighFollow = getRealFollower(myCandi + laneOffset);
681  if (neighLead.first != nullptr && neighLead.first == neighFollow.first) {
682  // vehicles should not be leader and follower at the same time to avoid
683  // contradictory behavior
684  neighFollow.first = 0;
685  }
686  ChangerIt target = myCandi + laneOffset;
687  return checkChange(laneOffset, target->lane, leader, neighLead, neighFollow, preb);
688 }
689 
690 int
692  int laneOffset,
693  const MSLane* targetLane,
694  const std::pair<MSVehicle* const, double>& leader,
695  const std::pair<MSVehicle* const, double>& neighLead,
696  const std::pair<MSVehicle* const, double>& neighFollow,
697  const std::vector<MSVehicle::LaneQ>& preb) const {
698 
699  MSVehicle* vehicle = veh(myCandi);
700 
701 #ifdef DEBUG_CHECK_CHANGE
702  if (DEBUG_COND) {
703  std::cout
704  << "\n" << SIMTIME << " checkChange() for vehicle '" << vehicle->getID() << "'"
705  << std::endl;
706  }
707 #endif
708 
709  int blocked = 0;
710  int blockedByLeader = (laneOffset == -1 ? LCA_BLOCKED_BY_RIGHT_LEADER : LCA_BLOCKED_BY_LEFT_LEADER);
711  int blockedByFollower = (laneOffset == -1 ? LCA_BLOCKED_BY_RIGHT_FOLLOWER : LCA_BLOCKED_BY_LEFT_FOLLOWER);
712  // overlap
713  if (neighFollow.first != nullptr && neighFollow.second < 0) {
714  blocked |= (blockedByFollower | LCA_OVERLAPPING);
715 
716  // Debug (Leo)
717 #ifdef DEBUG_CHECK_CHANGE
718  if (DEBUG_COND) {
719  std::cout << SIMTIME
720  << " overlapping with follower..."
721  << std::endl;
722  }
723 #endif
724 
725  }
726  if (neighLead.first != nullptr && neighLead.second < 0) {
727  blocked |= (blockedByLeader | LCA_OVERLAPPING);
728 
729 #ifdef DEBUG_CHECK_CHANGE
730  if (DEBUG_COND) {
731  std::cout << SIMTIME
732  << " overlapping with leader..."
733  << std::endl;
734  }
735 #endif
736 
737  }
738  double secureFrontGap = MSAbstractLaneChangeModel::NO_NEIGHBOR;
739  double secureBackGap = MSAbstractLaneChangeModel::NO_NEIGHBOR;
740  double secureOrigFrontGap = MSAbstractLaneChangeModel::NO_NEIGHBOR;
741 
742  const double tauRemainder = vehicle->getActionStepLength() == DELTA_T ? 0 : MAX2(vehicle->getCarFollowModel().getHeadwayTime() - TS, 0.);
743  // safe back gap
744  if ((blocked & blockedByFollower) == 0 && neighFollow.first != nullptr) {
745  // Calculate secure gap conservatively with vNextFollower / vNextLeader as
746  // extrapolated speeds after the driver's expected reaction time (tau).
747  // NOTE: there exists a possible source for collisions if the follower and the leader
748  // have desynchronized action steps as the extrapolated speeds can be exceeded in this case
749 
750  // Expected reaction time (tau) for the follower-vehicle.
751  // (substracted TS since at this point the vehicles' states are already updated)
752  const double vNextFollower = neighFollow.first->getSpeed() + MAX2(0., tauRemainder * neighFollow.first->getAcceleration());
753  const double vNextLeader = vehicle->getSpeed() + MIN2(0., tauRemainder * vehicle->getAcceleration());
754  // !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren
755  secureBackGap = neighFollow.first->getCarFollowModel().getSecureGap(vNextFollower,
756  vNextLeader, vehicle->getCarFollowModel().getMaxDecel());
757  if (neighFollow.second < secureBackGap * vehicle->getLaneChangeModel().getSafetyFactor()) {
758  blocked |= blockedByFollower;
759 
760  // Debug (Leo)
761 #ifdef DEBUG_CHECK_CHANGE
762  if (DEBUG_COND) {
763  std::cout << SIMTIME
764  << " back gap unsafe: "
765  << "gap = " << neighFollow.second
766  << " vNextFollower=" << vNextFollower
767  << " vNextEgo=" << vNextLeader
768  << ", secureGap = "
769  << neighFollow.first->getCarFollowModel().getSecureGap(vNextFollower,
770  vNextLeader, vehicle->getCarFollowModel().getMaxDecel())
771  << std::endl;
772  }
773 #endif
774 
775  }
776  }
777 
778  // safe front gap
779  if ((blocked & blockedByLeader) == 0 && neighLead.first != nullptr) {
780  // Calculate secure gap conservatively with vNextFollower / vNextLeader as
781  // extrapolated speeds after the driver's expected reaction time (tau).
782  // NOTE: there exists a possible source for collisions if the follower and the leader
783  // have desynchronized action steps as the extrapolated speeds can be exceeded in this case
784 
785  // Expected reaction time (tau) for the follower-vehicle.
786  // (substracted TS since at this point the vehicles' states are already updated)
787  const double vNextFollower = vehicle->getSpeed() + MAX2(0., tauRemainder * vehicle->getAcceleration());
788  const double vNextLeader = neighLead.first->getSpeed() + MIN2(0., tauRemainder * neighLead.first->getAcceleration());
789  // !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren
790  secureFrontGap = vehicle->getCarFollowModel().getSecureGap(vNextFollower,
791  vNextLeader, neighLead.first->getCarFollowModel().getMaxDecel());
792  if (neighLead.second < secureFrontGap * vehicle->getLaneChangeModel().getSafetyFactor()) {
793  blocked |= blockedByLeader;
794 
795  // Debug (Leo)
796 #ifdef DEBUG_CHECK_CHANGE
797  if (DEBUG_COND) {
798  std::cout << SIMTIME
799  << " front gap unsafe: "
800  << "gap = " << neighLead.second
801  << " vNextLeader=" << vNextLeader
802  << " vNextEgo=" << vNextFollower
803  << ", secureGap = "
804  << vehicle->getCarFollowModel().getSecureGap(vNextFollower,
805  vNextLeader, neighLead.first->getCarFollowModel().getMaxDecel())
806  << std::endl;
807  }
808 #endif
809 
810  }
811  }
812  if (blocked == 0 && MSPModel::getModel()->hasPedestrians(targetLane)) {
813  PersonDist leader = MSPModel::getModel()->nextBlocking(targetLane, vehicle->getBackPositionOnLane(),
814  vehicle->getRightSideOnLane(), vehicle->getRightSideOnLane() + vehicle->getVehicleType().getWidth(),
815  ceil(vehicle->getSpeed() / vehicle->getCarFollowModel().getMaxDecel()));
816  if (leader.first != 0) {
817  const double brakeGap = vehicle->getCarFollowModel().brakeGap(vehicle->getSpeed());
818  // returned gap value is relative to backPosition
819  const double gap = leader.second - vehicle->getVehicleType().getLengthWithGap();
820 #ifdef DEBUG_CHECK_CHANGE
821  if (DEBUG_COND) {
822  std::cout << SIMTIME << " pedestrian on road " + leader.first->getID() << " gap=" << gap << " brakeGap=" << brakeGap << "\n";
823  }
824 #endif
825  if (brakeGap > gap) {
826  blocked |= blockedByLeader;
827 #ifdef DEBUG_CHECK_CHANGE
828  if (DEBUG_COND) {
829  std::cout << SIMTIME << " blocked by pedestrian " + leader.first->getID() << "\n";
830  }
831 #endif
832  }
833  }
834  }
835 
836  if (leader.first != nullptr) {
837  secureOrigFrontGap = vehicle->getCarFollowModel().getSecureGap(vehicle->getSpeed(), leader.first->getSpeed(), leader.first->getCarFollowModel().getMaxDecel());
838  }
839 
840  MSAbstractLaneChangeModel::MSLCMessager msg(leader.first, neighLead.first, neighFollow.first);
841  int state = blocked | vehicle->getLaneChangeModel().wantsChange(
842  laneOffset, msg, blocked, leader, neighLead, neighFollow, *targetLane, preb, &(myCandi->lastBlocked), &(myCandi->firstBlocked));
843 
844  if (blocked == 0 && (state & LCA_WANTS_LANECHANGE) != 0 && neighLead.first != nullptr) {
845  // do a more careful (but expensive) check to ensure that a
846  // safety-critical leader is not being overlooked
847  // while changing on an intersection, it is not sufficient to abort the
848  // search with a leader on the current lane because all linkLeaders must
849  // be considered as well
850  const double seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
851  const double speed = vehicle->getSpeed();
852  const double dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
853  if (seen < dist || myCandi->lane->isInternal()) {
854  std::pair<MSVehicle* const, double> neighLead2 = targetLane->getCriticalLeader(dist, seen, speed, *vehicle);
855  if (neighLead2.first != nullptr && neighLead2.first != neighLead.first) {
856  const double secureGap = vehicle->getCarFollowModel().getSecureGap(vehicle->getSpeed(),
857  neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel());
858  const double secureGap2 = secureGap * vehicle->getLaneChangeModel().getSafetyFactor();
859 #ifdef DEBUG_SURROUNDING_VEHICLES
860  if (DEBUG_COND) {
861  std::cout << SIMTIME << " found critical leader=" << neighLead2.first->getID()
862  << " gap=" << neighLead2.second << " secGap=" << secureGap << " secGap2=" << secureGap2 << "\n";
863  }
864 #endif
865  if (neighLead2.second < secureGap2) {
866  state |= blockedByLeader;
867  }
868  }
869  }
870  }
871  if (blocked == 0 && (state & LCA_WANTS_LANECHANGE)) {
872  // ensure that merging is safe for any upcoming zipper links after changing
873  if (vehicle->unsafeLinkAhead(targetLane)) {
874  state |= blockedByLeader;
875  }
876  }
877 
878  if ((state & LCA_BLOCKED) == 0 && (state & LCA_WANTS_LANECHANGE) != 0 && MSGlobals::gLaneChangeDuration > DELTA_T) {
879  // Ensure that a continuous lane change manoeuvre can be completed before the next turning movement.
880  // Assume lateral position == 0. (If this should change in the future add + laneOffset*vehicle->getLateralPositionOnLane() to distToNeighLane)
881  const double distToNeighLane = 0.5 * (vehicle->getLane()->getWidth() + targetLane->getWidth());
882  // Extrapolate the LC duration if operating with speed dependent lateral speed.
883  const MSAbstractLaneChangeModel& lcm = vehicle->getLaneChangeModel();
884  const double assumedDecel = lcm.getAssumedDecelForLaneChangeDuration();
885  const double estimatedLCDuration = lcm.estimateLCDuration(vehicle->getSpeed(), distToNeighLane, assumedDecel);
886  if (estimatedLCDuration == -1) {
887  // Can't guarantee that LC will succeed if vehicle is braking -> assert(lcm.myMaxSpeedLatStanding==0)
888 #ifdef DEBUG_CHECK_CHANGE
889  if DEBUG_COND {
890  std::cout << SIMTIME << " checkChange() too slow to guarantee completion of continuous lane change."
891  << "\nestimatedLCDuration=" << estimatedLCDuration
892  << "\ndistToNeighLane=" << distToNeighLane
893  << std::endl;
894  }
895 #endif
896  state |= LCA_INSUFFICIENT_SPEED;
897  } else {
898  // Compute covered distance, when braking for the whole lc duration
899  const double decel = vehicle->getCarFollowModel().getMaxDecel() * estimatedLCDuration;
900  const double avgSpeed = 0.5 * (
901  MAX2(0., vehicle->getSpeed() - ACCEL2SPEED(vehicle->getCarFollowModel().getMaxDecel())) +
902  MAX2(0., vehicle->getSpeed() - decel));
903  // Distance required for lane change.
904  const double space2change = avgSpeed * estimatedLCDuration;
905  // Available distance for LC maneuver (distance till next turn)
906  double seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
907 #ifdef DEBUG_CHECK_CHANGE
908  if DEBUG_COND {
909  std::cout << SIMTIME << " checkChange() checking continuous lane change..."
910  << "\ndistToNeighLane=" << distToNeighLane
911  << " estimatedLCDuration=" << estimatedLCDuration
912  << " space2change=" << space2change
913  << " avgSpeed=" << avgSpeed
914  << std::endl;
915  }
916 #endif
917 
918  // for finding turns it doesn't matter whether we look along the current lane or the target lane
919  const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation();
920  int view = 1;
921  MSLane* nextLane = vehicle->getLane();
922  MSLinkCont::const_iterator link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
923  while (!nextLane->isLinkEnd(link) && seen <= space2change) {
924  if ((*link)->getDirection() == LINKDIR_LEFT || (*link)->getDirection() == LINKDIR_RIGHT
925  // the lanes after an internal junction are on different
926  // edges and do not allow lane-changing
927  || (nextLane->getEdge().isInternal() && (*link)->getViaLaneOrLane()->getEdge().isInternal())
928  ) {
929  state |= LCA_INSUFFICIENT_SPACE;
930  break;
931  }
932  if ((*link)->getViaLane() == nullptr) {
933  view++;
934  }
935  nextLane = (*link)->getViaLaneOrLane();
936  seen += nextLane->getLength();
937  // get the next link used
938  link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
939  }
940 #ifdef DEBUG_CHECK_CHANGE
941  if (DEBUG_COND) {
942  std::cout << " available distance=" << seen << std::endl;
943  }
944 #endif
945  if (nextLane->isLinkEnd(link) && seen < space2change) {
946 #ifdef DEBUG_CHECK_CHANGE
947  if (DEBUG_COND) {
948  std::cout << SIMTIME << " checkChange insufficientSpace: seen=" << seen << " space2change=" << space2change << "\n";
949  }
950 #endif
951  state |= LCA_INSUFFICIENT_SPACE;
952  }
953 
954  if ((state & LCA_BLOCKED) == 0) {
955  // check for dangerous leaders in case the target lane changes laterally between
956  // now and the lane-changing midpoint
957  const double speed = vehicle->getSpeed();
958  seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
959  nextLane = vehicle->getLane();
960  view = 1;
961  const double dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
962  MSLinkCont::const_iterator link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
963  while (!nextLane->isLinkEnd(link) && seen <= space2change && seen <= dist) {
964  nextLane = (*link)->getViaLaneOrLane();
965  MSLane* targetLane = nextLane->getParallelLane(laneOffset);
966  if (targetLane == nullptr) {
967  state |= LCA_INSUFFICIENT_SPACE;
968  break;
969  } else {
970  std::pair<MSVehicle* const, double> neighLead2 = targetLane->getLeader(vehicle, -seen, std::vector<MSLane*>());
971  if (neighLead2.first != nullptr && neighLead2.first != neighLead.first
972  && (neighLead2.second < vehicle->getCarFollowModel().getSecureGap(
973  vehicle->getSpeed(), neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel()))) {
974  state |= blockedByLeader;
975  break;
976  }
977  }
978  if ((*link)->getViaLane() == nullptr) {
979  view++;
980  }
981  seen += nextLane->getLength();
982  // get the next link used
983  link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
984  }
985  }
986  }
987  }
988  const int oldstate = state;
989  // let TraCI influence the wish to change lanes and the security to take
990  state = vehicle->influenceChangeDecision(state);
991 #ifdef DEBUG_CHECK_CHANGE
992  if (DEBUG_COND) {
993  std::cout << SIMTIME
994  << " veh=" << vehicle->getID()
995  << " oldState=" << toString((LaneChangeAction)oldstate)
996  << " newState=" << toString((LaneChangeAction)state)
997  << ((blocked & LCA_BLOCKED) ? " (blocked)" : "")
998  << ((blocked & LCA_OVERLAPPING) ? " (overlap)" : "")
999  << "\n";
1000  }
1001 #endif
1002  vehicle->getLaneChangeModel().saveLCState(laneOffset, oldstate, state);
1003  if (blocked == 0 && (state & LCA_WANTS_LANECHANGE)) {
1004  // this lane change will be executed, save gaps
1005  vehicle->getLaneChangeModel().setFollowerGaps(neighFollow, secureBackGap);
1006  vehicle->getLaneChangeModel().setLeaderGaps(neighLead, secureFrontGap);
1007  vehicle->getLaneChangeModel().setOrigLeaderGaps(leader, secureOrigFrontGap);
1008  }
1009  return state;
1010 }
1011 
1012 
1013 bool
1014 MSLaneChanger::changeOpposite(std::pair<MSVehicle*, double> leader) {
1015  if (!myChangeToOpposite) {
1016  return false;
1017  }
1018  myCandi = findCandidate();
1019  MSVehicle* vehicle = veh(myCandi);
1020  MSLane* source = vehicle->getLane();
1021  if (vehicle->isStopped()) {
1022  // stopped vehicles obviously should not change lanes. Usually this is
1023  // prevent by appropriate bestLane distances
1024  return false;
1025  }
1026  int ret = 0;
1027  ret = vehicle->influenceChangeDecision(ret);
1028  bool oppositeChangeByTraci = false;
1029  // Check whether a lane change to the opposite direction was requested via TraCI
1030  if ((ret & (LCA_TRACI)) != 0) {
1031  oppositeChangeByTraci = true;
1032  }
1033  const bool isOpposite = vehicle->getLaneChangeModel().isOpposite();
1034  if (!isOpposite && leader.first == 0 && !oppositeChangeByTraci) {
1035  // no reason to change unless there is a leader
1036  // or we are changing back to the propper direction
1037  // XXX also check whether the leader is so far away as to be irrelevant
1038  return false;
1039  }
1040  if (!isOpposite && !oppositeChangeByTraci
1041  && vehicle->getVClass() != SVC_EMERGENCY
1042  && leader.first != 0
1043  && leader.first->signalSet(MSNet::getInstance()->lefthand()
1045  // do not try to overtake a vehicle that is about to turn left or wants
1046  // to change left itself
1047 #ifdef DEBUG_CHANGE_OPPOSITE
1048  if (DEBUG_COND) {
1049  std::cout << " not overtaking leader " << leader.first->getID() << " that has blinker set\n";
1050  }
1051 #endif
1052  return false;
1053  }
1054 
1055  MSLane* opposite = source->getOpposite();
1056  //There is no lane for opposite driving
1057  if (opposite == nullptr || !opposite->allowsVehicleClass(vehicle->getVClass())) {
1058  return false;
1059  }
1060  // changing into the opposite direction is always to the left (XXX except for left-hand networkds)
1061  int direction = isOpposite ? -1 : 1;
1062  std::pair<MSVehicle*, double> neighLead((MSVehicle*)nullptr, -1);
1063 
1064  // preliminary sanity checks for overtaking space
1065  double timeToOvertake;
1066  double spaceToOvertake;
1067 
1068  // we need to find two vehicles:
1069  // 1) the leader that shall be overtaken (not necessarily the current leader but one of its leaders that has enough space in front)
1070  // 2) the oncoming vehicle (we need to look past vehicles that are currentlyovertaking through the opposite direction themselves)
1071  //
1072  // if the vehicle is driving normally, then the search for 1) starts on the current lane and 2) on the opposite lane
1073  // if the vehicle is driving on the opposite side then 1) is found on the neighboring lane and 2) on the current lane
1074 
1075  std::pair<MSVehicle*, double> overtaken;
1076  std::pair<MSVehicle*, double> oncoming;
1077 
1078 
1079  if (!isOpposite && !oppositeChangeByTraci) {
1080  overtaken = getColumnleader(vehicle, leader);
1081  if (overtaken.first == 0) {
1082  return false;
1083  }
1084 #ifdef DEBUG_CHANGE_OPPOSITE
1085  if (DEBUG_COND) {
1086  std::cout << " compute time/space to overtake for columnLeader=" << overtaken.first->getID() << " egoGap=" << overtaken.second << "\n";
1087  }
1088 #endif
1089  computeOvertakingTime(vehicle, overtaken.first, overtaken.second, timeToOvertake, spaceToOvertake);
1090  // check for upcoming stops
1091  if (vehicle->nextStopDist() < spaceToOvertake) {
1092 #ifdef DEBUG_CHANGE_OPPOSITE
1093  if (DEBUG_COND) {
1094  std::cout << " cannot changeOpposite due to upcoming stop (dist=" << vehicle->nextStopDist() << " spaceToOvertake=" << spaceToOvertake << ")\n";
1095  }
1096 #endif
1097  return false;
1098  }
1099  neighLead = opposite->getOppositeLeader(vehicle, timeToOvertake * opposite->getSpeedLimit() * 2 + spaceToOvertake, true);
1100 
1101 #ifdef DEBUG_CHANGE_OPPOSITE
1102  if (DEBUG_COND) {
1103  std::cout << SIMTIME
1104  << " veh=" << vehicle->getID()
1105  << " changeOpposite opposite=" << opposite->getID()
1106  << " lead=" << Named::getIDSecure(leader.first)
1107  << " timeToOvertake=" << timeToOvertake
1108  << " spaceToOvertake=" << spaceToOvertake
1109  << "\n";
1110  }
1111 #endif
1112  // check for dangerous oncoming leader
1113  if (neighLead.first != 0) {
1114  const MSVehicle* oncoming = neighLead.first;
1115  // conservative: assume that the oncoming vehicle accelerates to its maximum speed
1116  const double oncomingSpeed = oncoming->isStopped() ? 0 : oncoming->getLane()->getVehicleMaxSpeed(oncoming);
1117  const double safetyGap = ((oncomingSpeed + vehicle->getLane()->getVehicleMaxSpeed(vehicle))
1118  * vehicle->getCarFollowModel().getHeadwayTime()
1120  const double surplusGap = neighLead.second - spaceToOvertake - timeToOvertake * oncomingSpeed - safetyGap;
1121 #ifdef DEBUG_CHANGE_OPPOSITE
1122  if (DEBUG_COND) {
1123  std::cout << SIMTIME
1124  << " oncoming=" << oncoming->getID()
1125  << " oncomingGap=" << neighLead.second
1126  << " leaderGap=" << leader.second
1127  << " safetyGap=" << safetyGap
1128  << " surplusGap=" << surplusGap
1129  << "\n";
1130  }
1131 #endif
1132  if (surplusGap < 0) {
1133 
1134 #ifdef DEBUG_CHANGE_OPPOSITE
1135  if (DEBUG_COND) {
1136  std::cout << " cannot changeOpposite due to dangerous oncoming (surplusGap=" << surplusGap << ")\n";
1137  }
1138 #endif
1139 
1140 #ifdef DEBUG_CHANGE_OPPOSITE
1141  if (DEBUG_COND) {
1142  if (oncoming->getLaneChangeModel().isOpposite()) {
1143  std::cout << SIMTIME << " ego=" << vehicle->getID() << " does not changeOpposite due to dangerous oncoming " << oncoming->getID() << " (but the leader is also opposite)\n";
1144  }
1145  }
1146 #endif
1147  return false;
1148  }
1149  }
1150 
1151  } else if (!oppositeChangeByTraci) {
1152  timeToOvertake = -1;
1153  // look forward as far as possible
1154  spaceToOvertake = std::numeric_limits<double>::max();
1156  leader = source->getOppositeLeader(vehicle, dist, true);
1157  double gap = leader.second;
1158  while (leader.first != nullptr && leader.first->getLaneChangeModel().isOpposite() && dist > 0) {
1159  // look beyond leaders that are also driving in the opposite direction until finding an oncoming leader or exhausting the look-ahead distance
1160 #ifdef DEBUG_CHANGE_OPPOSITE
1161  if (DEBUG_COND) {
1162  std::cout << SIMTIME << " ego=" << vehicle->getID() << " opposite leader=" << leader.first->getID() << " gap=" << gap << " is driving against the flow\n";
1163  }
1164 #endif
1165  const double gapToLeaderFront = leader.second + leader.first->getVehicleType().getLengthWithGap();
1166  dist -= gapToLeaderFront;
1167  leader = source->getOppositeLeader(leader.first, dist, true);
1168  if (leader.first != 0) {
1169  gap += gapToLeaderFront;
1170  }
1171  }
1172  leader.second = gap;
1173  // -1 will use getMaximumBrakeDist() as look-ahead distance
1174  neighLead = opposite->getOppositeLeader(vehicle, -1, false);
1175  } else {
1176  timeToOvertake = STEPS2TIME(vehicle->getInfluencer().getLaneTimeLineDuration());//todo discuss concept
1177  spaceToOvertake = timeToOvertake * vehicle->getLane()->getVehicleMaxSpeed(vehicle);
1178  }
1179  // compute remaining space on the opposite side
1180  // 1. the part that remains on the current lane
1181  double usableDist = isOpposite ? vehicle->getPositionOnLane() : source->getLength() - vehicle->getPositionOnLane();
1182 
1183  if (usableDist < spaceToOvertake) {
1184  // look forward along the next lanes
1185  const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation();
1186  assert(bestLaneConts.size() >= 1);
1187  std::vector<MSLane*>::const_iterator it = bestLaneConts.begin() + 1;
1188  while (usableDist < spaceToOvertake && it != bestLaneConts.end()) {
1189 #ifdef DEBUG_CHANGE_OPPOSITE
1190  if (DEBUG_COND) {
1191  std::cout << " usableDist=" << usableDist << " opposite=" << Named::getIDSecure((*it)->getOpposite()) << "\n";
1192  }
1193 #endif
1194  if ((*it)->getOpposite() == nullptr || !(*it)->getOpposite()->allowsVehicleClass(vehicle->getVClass())) {
1195  // opposite lane ends
1196  break;
1197  }
1198  // do not overtake past a minor link or turn
1199  if (*(it - 1) != nullptr) {
1200  MSLink* link = MSLinkContHelper::getConnectingLink(**(it - 1), **it);
1201  if (link == nullptr || link->getState() == LINKSTATE_ZIPPER
1202  || (link->getDirection() != LINKDIR_STRAIGHT && vehicle->getVehicleType().getVehicleClass() != SVC_EMERGENCY)
1203  || (!link->havePriority()
1204  // consider traci-influence
1205  && (!vehicle->hasInfluencer() || vehicle->getInfluencer().getRespectJunctionPriority())
1206  // consider junction model parameters
1207  && ((!link->haveRed() && !link->haveYellow()) || !vehicle->ignoreRed(link, true)))) {
1208 #ifdef DEBUG_CHANGE_OPPOSITE
1209  if (DEBUG_COND) {
1210  std::cout << " stop lookahead at link=" << (link == 0 ? "NULL" : link->getViaLaneOrLane()->getID()) << " state=" << (link == 0 ? "?" : toString(link->getState())) << " ignoreRed=" << vehicle->ignoreRed(link, true) << "\n";
1211  }
1212 #endif
1213  break;
1214  }
1215  }
1216  usableDist += (*it)->getLength();
1217  ++it;
1218  }
1219  }
1220  if (!isOpposite && usableDist < spaceToOvertake) {
1221 #ifdef DEBUG_CHANGE_OPPOSITE
1222  if (DEBUG_COND) {
1223  std::cout << " cannot changeOpposite due to insufficient space (seen=" << usableDist << " spaceToOvertake=" << spaceToOvertake << ")\n";
1224  }
1225 #endif
1226  return false;
1227  }
1228 #ifdef DEBUG_CHANGE_OPPOSITE
1229  if (DEBUG_COND) {
1230  std::cout << " usableDist=" << usableDist << " spaceToOvertake=" << spaceToOvertake << " timeToOvertake=" << timeToOvertake << "\n";
1231  }
1232 #endif
1233  // compute wish to change
1234  // Does "preb" mean "previousBestLanes" ??? If so *rename*
1235  std::vector<MSVehicle::LaneQ> preb = vehicle->getBestLanes();
1236  if (isOpposite) {
1237  // compute the remaining distance that can be drive on the opposite side
1238  // this value will put into LaneQ.length of the leftmost lane
1239  // @note: length counts from the start of the current lane
1240  // @note: see MSLCM_LC2013::_wantsChange @1092 (isOpposite()
1241  MSVehicle::LaneQ& laneQ = preb[preb.size() - 1];
1242  // position on the target lane
1243  const double forwardPos = source->getOppositePos(vehicle->getPositionOnLane());
1244 
1245  // consider usableDist (due to minor links or end of opposite lanes)
1246  laneQ.length = MIN2(laneQ.length, usableDist + forwardPos);
1247  // consider upcoming stops
1248  laneQ.length = MIN2(laneQ.length, vehicle->nextStopDist() + forwardPos);
1249  // consider oncoming leaders
1250  if (leader.first != 0) {
1251  if (!leader.first->getLaneChangeModel().isOpposite()) {
1252  MSVehicle* oncoming = leader.first;
1253  const double oncomingSpeed = oncoming->getAcceleration() > 0 ? oncoming->getLane()->getVehicleMaxSpeed(oncoming) : oncoming->getSpeed();
1254  const double safetyGap = ((oncomingSpeed + vehicle->getLane()->getVehicleMaxSpeed(vehicle))
1255  * vehicle->getCarFollowModel().getHeadwayTime()
1257  laneQ.length = MIN2(laneQ.length, leader.second / 2 + forwardPos - safetyGap);
1258 #ifdef DEBUG_CHANGE_OPPOSITE
1259  if (DEBUG_COND) {
1260  std::cout << SIMTIME << " found oncoming leader=" << oncoming->getID() << " gap=" << leader.second << "\n";
1261  }
1262 #endif
1263  } else {
1264 #ifdef DEBUG_CHANGE_OPPOSITE
1265  if (DEBUG_COND) {
1266  std::cout << SIMTIME << " opposite leader=" << leader.first->getID() << " gap=" << leader.second << " is driving against the flow\n";
1267  }
1268 #endif
1269  }
1270  if (neighLead.first != 0) {
1271  overtaken = getColumnleader(vehicle, neighLead);
1272  if (overtaken.first == 0) {
1273 #ifdef DEBUG_CHANGE_OPPOSITE
1274  if (DEBUG_COND) {
1275  std::cout << SIMTIME << " ego=" << vehicle->getID() << " did not find columnleader to overtake\n";
1276  }
1277 #endif
1278  } else {
1279  const double remainingDist = laneQ.length - forwardPos;
1280  computeOvertakingTime(vehicle, overtaken.first, overtaken.second, timeToOvertake, spaceToOvertake);
1281 #ifdef DEBUG_CHANGE_OPPOSITE
1282  if (DEBUG_COND) {
1283  std::cout << SIMTIME << " ego=" << vehicle->getID() << " is overtaking " << overtaken.first->getID()
1284  << " remainingDist=" << remainingDist << " spaceToOvertake=" << spaceToOvertake << " timeToOvertake=" << timeToOvertake << "\n";
1285  }
1286 #endif
1287  if (remainingDist > spaceToOvertake) {
1288  // exaggerate remaining dist so that the vehicle continues
1289  // overtaking (otherwise the lane change model might abort prematurely)
1290  laneQ.length += 1000;
1291  }
1292  }
1293  }
1294  leader.first = 0; // ignore leader after this
1295  }
1296 #ifdef DEBUG_CHANGE_OPPOSITE
1297  if (DEBUG_COND) {
1298  std::cout << SIMTIME << " veh=" << vehicle->getID() << " remaining dist=" << laneQ.length - forwardPos << " forwardPos=" << forwardPos << " laneQ.length=" << laneQ.length << "\n";
1299  }
1300 #endif
1301  }
1302  std::pair<MSVehicle* const, double> neighFollow = opposite->getOppositeFollower(vehicle);
1303  int state = checkChange(direction, opposite, leader, neighLead, neighFollow, preb);
1304  vehicle->getLaneChangeModel().setOwnState(state);
1305 
1306  bool changingAllowed = (state & LCA_BLOCKED) == 0;
1307  // change if the vehicle wants to and is allowed to change
1308  if ((state & LCA_WANTS_LANECHANGE) != 0 && changingAllowed
1309  // do not change to the opposite direction for cooperative reasons
1310  && (isOpposite || (state & LCA_COOPERATIVE) == 0)) {
1311  vehicle->getLaneChangeModel().startLaneChangeManeuver(source, opposite, direction);
1313  vehicle->myState.myPos = source->getOppositePos(vehicle->myState.myPos);
1316  if (!isOpposite) {
1317  vehicle->myState.myBackPos = source->getOppositePos(vehicle->myState.myBackPos);
1318  }
1319 #ifdef DEBUG_CHANGE_OPPOSITE
1320  if (DEBUG_COND) {
1321  std::cout << SIMTIME << " changing to opposite veh=" << vehicle->getID() << " dir=" << direction << " opposite=" << Named::getIDSecure(opposite) << " state=" << state << "\n";
1322  }
1323 #endif
1324  return true;
1325  }
1326 #ifdef DEBUG_CHANGE_OPPOSITE
1327  if (DEBUG_COND) {
1328  std::cout << SIMTIME << " not changing to opposite veh=" << vehicle->getID() << " dir=" << direction
1329  << " opposite=" << Named::getIDSecure(opposite) << " state=" << toString((LaneChangeAction)state) << "\n";
1330  }
1331 #endif
1332  return false;
1333 }
1334 
1335 
1336 void
1337 MSLaneChanger::computeOvertakingTime(const MSVehicle* vehicle, const MSVehicle* leader, double gap, double& timeToOvertake, double& spaceToOvertake) {
1338  // Assumptions:
1339  // - leader maintains the current speed
1340  // - vehicle merges with maxSpeed ahead of leader
1341  // XXX affected by ticket #860 (the formula is invalid for the current position update rule)
1342 
1343  // first compute these values for the case where vehicle is accelerating
1344  // without upper bound on speed
1345  const double vMax = vehicle->getLane()->getVehicleMaxSpeed(vehicle);
1346  const double v = vehicle->getSpeed();
1347  const double u = leader->getAcceleration() > 0 ? leader->getLane()->getVehicleMaxSpeed(leader) : leader->getSpeed();
1348  const double a = vehicle->getCarFollowModel().getMaxAccel();
1349  const double d = vehicle->getCarFollowModel().getMaxDecel();
1350  const double g = (
1351  // drive up to the rear of leader
1352  gap + vehicle->getVehicleType().getMinGap()
1353  // drive head-to-head with the leader
1354  + leader->getVehicleType().getLengthWithGap()
1355  // drive past the leader
1356  + vehicle->getVehicleType().getLength()
1357  // allow for safe gap between leader and vehicle
1358  + leader->getCarFollowModel().getSecureGap(u, vMax, d));
1359  const double sign = -1; // XXX recheck
1360  // v*t + t*t*a*0.5 = g + u*t
1361  // solve t
1362  // t = ((u - v - (((((2.0*(u - v))**2.0) + (8.0*a*g))**(1.0/2.0))*sign/2.0))/a)
1363  double t = (u - v - sqrt(4 * (u - v) * (u - v) + 8 * a * g) * sign * 0.5) / a;
1364 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1365  if (DEBUG_COND) {
1366  std::cout << " computeOvertakingTime v=" << v << " vMax=" << vMax << " u=" << u << " a=" << a << " d=" << d << " gap=" << gap << " g=" << g << " t=" << t
1367  << " distEgo=" << v* t + t* t* a * 0.5 << " distLead=" << g + u* t
1368  << "\n";
1369  }
1370 #endif
1371  assert(t >= 0);
1372 
1373  // allow for a safety time gap
1375  // round to multiples of step length (TS)
1376  t = ceil(t / TS) * TS;
1377 
1379  const double timeToMaxSpeed = (vMax - v) / a;
1380 
1381 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1382  if (DEBUG_COND) {
1383  std::cout << " t=" << t << " tvMax=" << timeToMaxSpeed << "\n";
1384  }
1385 #endif
1386  if (t <= timeToMaxSpeed) {
1387  timeToOvertake = t;
1388  spaceToOvertake = v * t + t * t * a * 0.5;
1389 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1390  if (DEBUG_COND) {
1391  std::cout << " sto=" << spaceToOvertake << "\n";
1392  }
1393 #endif
1394  } else {
1395  // space until max speed is reached
1396  const double s = v * timeToMaxSpeed + timeToMaxSpeed * timeToMaxSpeed * a * 0.5;
1397  const double m = timeToMaxSpeed;
1398  // s + (t-m) * vMax = g + u*t
1399  // solve t
1400  t = (g - s + m * vMax) / (vMax - u);
1401  if (t < 0) {
1402  // cannot overtake in time
1403 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1404  if (DEBUG_COND) {
1405  std::cout << " t2=" << t << "\n";
1406  }
1407 #endif
1408  timeToOvertake = std::numeric_limits<double>::max();
1409  spaceToOvertake = std::numeric_limits<double>::max();
1410  } else {
1411  // allow for a safety time gap
1413  // round to multiples of step length (TS)
1414  t = ceil(t / TS) * TS;
1415 
1416  timeToOvertake = t;
1417  spaceToOvertake = s + (t - m) * vMax;
1418 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1419  if (DEBUG_COND) {
1420  std::cout << " t2=" << t << " s=" << s << " sto=" << spaceToOvertake << " m=" << m << "\n";
1421  }
1422 #endif
1423  }
1424  }
1425  const double safetyFactor = OPPOSITE_OVERTAKING_SAFETY_FACTOR * vehicle->getLaneChangeModel().getOppositeSafetyFactor();
1426  timeToOvertake *= safetyFactor;
1427  spaceToOvertake *= safetyFactor;
1428 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1429  if (DEBUG_COND) {
1430  if (safetyFactor != 1) {
1431  std::cout << " applying safetyFactor=" << safetyFactor
1432  << " tto=" << timeToOvertake << " sto=" << spaceToOvertake << "\n";
1433  }
1434  }
1435 #endif
1436 
1437 }
1438 
1439 
1440 
1441 std::pair<MSVehicle*, double>
1442 MSLaneChanger::getColumnleader(MSVehicle* vehicle, std::pair<MSVehicle*, double> leader, double maxLookAhead) {
1443  assert(leader.first != 0);
1444  MSLane* source = vehicle->getLane();
1445  // find a leader vehicle with sufficient space ahead for merging back
1446  const double overtakingSpeed = source->getVehicleMaxSpeed(vehicle); // just a guess
1447  const double mergeBrakeGap = vehicle->getCarFollowModel().brakeGap(overtakingSpeed);
1448  std::pair<MSVehicle*, double> columnLeader = leader;
1449  double egoGap = leader.second;
1450  bool foundSpaceAhead = false;
1451  double seen = leader.second + leader.first->getVehicleType().getLengthWithGap();
1452  std::vector<MSLane*> conts = vehicle->getBestLanesContinuation();
1453  if (maxLookAhead == std::numeric_limits<double>::max()) {
1454  maxLookAhead = (vehicle->getVehicleType().getVehicleClass() == SVC_EMERGENCY
1457  }
1458 #ifdef DEBUG_CHANGE_OPPOSITE
1459  if (DEBUG_COND) {
1460  std::cout << " getColumnleader vehicle=" << vehicle->getID() << " leader=" << leader.first->getID() << " gap=" << leader.second << " maxLookAhead=" << maxLookAhead << "\n";
1461  }
1462 #endif
1463  const double safetyFactor = OPPOSITE_OVERTAKING_SAFETY_FACTOR * vehicle->getLaneChangeModel().getOppositeSafetyFactor();
1464  while (!foundSpaceAhead) {
1465  const double requiredSpaceAfterLeader = (columnLeader.first->getCarFollowModel().getSecureGap(
1466  columnLeader.first->getSpeed(), overtakingSpeed, vehicle->getCarFollowModel().getMaxDecel())
1467  + columnLeader.first->getVehicleType().getMinGap()
1468  + vehicle->getVehicleType().getLengthWithGap());
1469 
1470 
1471  // all leader vehicles on the current laneChanger edge are already moved into MSLane::myTmpVehicles
1472  const bool checkTmpVehicles = (&columnLeader.first->getLane()->getEdge() == &source->getEdge());
1473  std::pair<MSVehicle* const, double> leadLead = columnLeader.first->getLane()->getLeader(
1474  columnLeader.first, columnLeader.first->getPositionOnLane(), conts, requiredSpaceAfterLeader + mergeBrakeGap,
1475  checkTmpVehicles);
1476 
1477 #ifdef DEBUG_CHANGE_OPPOSITE
1478  if (DEBUG_COND) {
1479  std::cout << " leadLead=" << Named::getIDSecure(leadLead.first) << " gap=" << leadLead.second << "\n";
1480  }
1481 #endif
1482  if (leadLead.first == nullptr) {
1483  double availableSpace = columnLeader.first->getLane()->getLength() - columnLeader.first->getPositionOnLane();
1484  const double requiredSpace = safetyFactor * (requiredSpaceAfterLeader
1485  + vehicle->getCarFollowModel().brakeGap(overtakingSpeed));
1486 #ifdef DEBUG_CHANGE_OPPOSITE
1487  if (DEBUG_COND) {
1488  std::cout << " no direct leader found after columnLeader " << columnLeader.first->getID()
1489  << " availableSpace=" << availableSpace
1490  << " req1=" << requiredSpaceAfterLeader
1491  << " req2=" << requiredSpace / safetyFactor
1492  << " req3=" << requiredSpace
1493  << "\n";
1494  }
1495 #endif
1496  if (availableSpace > requiredSpace) {
1497  foundSpaceAhead = true;
1498  } else {
1499  // maybe the columnleader is stopped before a junction or takes a different turn.
1500  // try to find another columnleader on successive lanes
1501  MSLane* next = getLaneAfter(columnLeader.first->getLane(), conts);
1502 #ifdef DEBUG_CHANGE_OPPOSITE
1503  if (DEBUG_COND) {
1504  std::cout << " look for another leader on lane " << Named::getIDSecure(next) << "\n";
1505  }
1506 #endif
1507  while (next != nullptr) {
1508  seen += next->getLength();
1509  MSVehicle* cand = next->getLastAnyVehicle();
1510  if (cand == nullptr) {
1511  availableSpace += next->getLength();
1512  if (availableSpace > requiredSpace) {
1513  foundSpaceAhead = true;
1514  break;
1515  }
1516  } else {
1517  availableSpace += cand->getBackPositionOnLane();
1518  if (availableSpace > requiredSpace) {
1519  foundSpaceAhead = true;
1520  break;
1521  } else {
1522  return getColumnleader(vehicle, std::make_pair(cand, availableSpace + cand->getPositionOnLane()), maxLookAhead - seen);
1523  }
1524  }
1525  }
1526  if (!foundSpaceAhead) {
1527  return std::make_pair(nullptr, -1);
1528  }
1529  }
1530  } else {
1531  const double requiredSpace = safetyFactor * (requiredSpaceAfterLeader
1532  + vehicle->getCarFollowModel().getSecureGap(overtakingSpeed, leadLead.first->getSpeed(), leadLead.first->getCarFollowModel().getMaxDecel()));
1533 #ifdef DEBUG_CHANGE_OPPOSITE
1534  if (DEBUG_COND) {
1535  std::cout << " leader's leader " << leadLead.first->getID() << " space=" << leadLead.second
1536  << " req1=" << requiredSpaceAfterLeader
1537  << " req2=" << requiredSpace / safetyFactor
1538  << " req3=" << requiredSpace
1539  << "\n";
1540  }
1541 #endif
1542  if (leadLead.second > requiredSpace) {
1543  foundSpaceAhead = true;
1544  } else {
1545 #ifdef DEBUG_CHANGE_OPPOSITE
1546  if (DEBUG_COND) {
1547  std::cout << " not enough space after columnLeader=" << columnLeader.first->getID() << " required=" << requiredSpace << "\n";
1548  }
1549 #endif
1550  seen += MAX2(0., leadLead.second) + leadLead.first->getVehicleType().getLengthWithGap();
1551  if (seen > maxLookAhead) {
1552 #ifdef DEBUG_CHANGE_OPPOSITE
1553  if (DEBUG_COND) {
1554  std::cout << " cannot changeOpposite due to insufficient free space after columnLeader (seen=" << seen << " columnLeader=" << columnLeader.first->getID() << ")\n";
1555  }
1556 #endif
1557  return std::make_pair(nullptr, -1);
1558  }
1559  // see if merging after leadLead is possible
1560  egoGap += columnLeader.first->getVehicleType().getLengthWithGap() + leadLead.second;
1561  columnLeader = leadLead;
1562 #ifdef DEBUG_CHANGE_OPPOSITE
1563  if (DEBUG_COND) {
1564  std::cout << " new columnLeader=" << columnLeader.first->getID() << "\n";
1565  }
1566 #endif
1567  }
1568  }
1569  }
1570  columnLeader.second = egoGap;
1571  return columnLeader;
1572 }
1573 
1574 
1575 MSLane*
1576 MSLaneChanger::getLaneAfter(MSLane* lane, const std::vector<MSLane*>& conts) {
1577  for (auto it = conts.begin(); it != conts.end(); ++it) {
1578  if (*it == lane) {
1579  if (it + 1 != conts.end()) {
1580  return *(it + 1);
1581  } else {
1582  return nullptr;
1583  }
1584  }
1585  }
1586  return nullptr;
1587 }
1588 
1589 
1590 /****************************************************************************/
1591 
double myPos
the stored position
Definition: MSVehicle.h:136
void laneChange(SUMOTime t)
Start lane-change-process for all vehicles on the edge&#39;e lanes.
bool getRespectJunctionPriority() const
Returns whether junction priority rules shall be respected.
Definition: MSVehicle.h:1483
bool isChangingLanes() const
return true if the vehicle currently performs a lane change maneuver
double getLengthWithGap() const
Get vehicle&#39;s length including the minimum gap [m].
double computeAngle() const
compute the current vehicle angle
Definition: MSVehicle.cpp:1269
static MSVehicle * getCloserFollower(const double maxPos, MSVehicle *follow1, MSVehicle *follow2)
return the closer follower of ego
double getVehicleMaxSpeed(const SUMOVehicle *const veh) const
Returns the lane&#39;s maximum speed, given a vehicle&#39;s speed limit adaptation.
Definition: MSLane.h:492
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver&#39;s reaction time tau (i...
Definition: MSCFModel.h:313
bool ignoreRed(const MSLink *link, bool canBrake) const
decide whether a red (or yellow light) may be ignore
Definition: MSVehicle.cpp:5555
MSEdge & getEdge() const
Returns the lane&#39;s edge.
Definition: MSLane.h:640
double myAngle
the angle in radians (
Definition: MSVehicle.h:1852
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:79
long long int SUMOTime
Definition: SUMOTime.h:36
bool isRemoteControlled() const
Returns the information whether the vehicle is fully controlled via TraCI.
Definition: MSVehicle.cpp:5531
MSLane * lane
the lane corresponding to this ChangeElem (the current change candidate is on this lane) ...
Definition: MSLaneChanger.h:74
#define SPEED2DIST(x)
Definition: SUMOTime.h:48
The action is done to help someone else.
static std::pair< MSVehicle *, double > getColumnleader(MSVehicle *vehicle, std::pair< MSVehicle *, double > leader, double maxLookAhead=std::numeric_limits< double >::max())
virtual bool hasPedestrians(const MSLane *lane)
whether the given lane has pedestrians on it
Definition: MSPModel.h:85
std::pair< MSVehicle *const, double > getOppositeLeader(const MSVehicle *ego, double dist, bool oppositeDir) const
Definition: MSLane.cpp:3399
State myState
This Vehicles driving state (pos and speed)
Definition: MSVehicle.h:1786
#define ACCEL2SPEED(x)
Definition: SUMOTime.h:54
virtual double getAssumedDecelForLaneChangeDuration() const
Returns a deceleration value which is used for the estimation of the duration of a lane change...
MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:565
void setLeaderGaps(CLeaderDist, double secGap)
The vehicle is blocked by left follower.
int checkChange(int laneOffset, const MSLane *targetLane, const std::pair< MSVehicle *const, double > &leader, const std::pair< MSVehicle *const, double > &neighLead, const std::pair< MSVehicle *const, double > &neighFollow, const std::vector< MSVehicle::LaneQ > &preb) const
bool continueChange(MSVehicle *vehicle, ChangerIt &from)
continue a lane change maneuver and return whether the vehicle has completely moved onto the new lane...
virtual void initChanger()
Initialize the changer before looping over all vehicles.
int getShadowDirection() const
return the direction in which the current shadow lane lies
virtual bool changeOpposite(std::pair< MSVehicle *, double > leader)
const bool myAllowsChanging
double getPositionOnLane() const
Get the vehicle&#39;s position along the lane.
Definition: MSVehicle.h:403
double myPosLat
the stored lateral position
Definition: MSVehicle.h:142
MSVehicle * veh(ConstChangerIt ce) const
static MSLane * getLaneAfter(MSLane *lane, const std::vector< MSLane *> &conts)
return the next lane in conts beyond lane or nullptr
std::pair< MSVehicle *const, double > getRealLeader(const ChangerIt &target) const
Wants go to the right.
Vehicle is too slow to guarantee success of lane change (used for continuous lane changing in case th...
const bool myChangeToOpposite
whether this edge allows changing to the opposite direction edge
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:165
T MAX2(T a, T b)
Definition: StdDefs.h:76
#define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD
SUMOTime DELTA_T
Definition: SUMOTime.cpp:35
double getLength() const
Returns the lane&#39;s length.
Definition: MSLane.h:514
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:71
bool isLinkEnd(MSLinkCont::const_iterator &i) const
Definition: MSLane.cpp:1859
bool isStoppedOnLane() const
Definition: MSVehicle.cpp:1596
double getRightSideOnLane() const
Get the vehicle&#39;s lateral position on the lane:
Definition: MSVehicle.cpp:4931
void adaptBestLanesOccupation(int laneIndex, double density)
update occupation from MSLaneChanger
Definition: MSVehicle.cpp:4707
bool alreadyChanged() const
reset the flag whether a vehicle already moved to false
bool startChange(MSVehicle *vehicle, ChangerIt &from, int direction)
const std::string & getID() const
Returns the id.
Definition: Named.h:78
#define TS
Definition: SUMOTime.h:45
double length
The overall length which may be driven when using this lane without a lane change.
Definition: MSVehicle.h:812
VehCont myPartialVehicles
The lane&#39;s partial vehicles. This container holds all vehicles that are partially on this lane but wh...
Definition: MSLane.h:1232
Wants go to the left.
double getWidth() const
Returns the lane&#39;s width.
Definition: MSLane.h:530
ChangerIt findCandidate()
Find current candidate. If there is none, myChanger.end() is returned.
This is an uncontrolled, zipper-merge link.
The link is a (hard) left direction.
void checkTraCICommands()
Check for commands issued for the vehicle via TraCI and apply the appropriate state changes For the s...
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:4307
SUMOTime getLaneTimeLineDuration()
Definition: MSVehicle.cpp:356
void requireCollisionCheck()
require another collision check due to relevant changes in the simulation
Definition: MSLane.h:598
virtual double getOppositeSafetyFactor() const
return factor for modifying the safety constraints for opposite-diretction overtaking of the car-foll...
#define SIMTIME
Definition: SUMOTime.h:65
bool isFrontOnLane(const MSLane *lane) const
Returns the information whether the front of the vehicle is on the given lane.
Definition: MSVehicle.cpp:3790
bool vehInChanger() const
Check if there is a single change-candidate in the changer. Returns true if there is one...
Right blinker lights are switched on.
Definition: MSVehicle.h:1177
bool applyTraCICommands(MSVehicle *vehicle)
Execute TraCI LC-commands.
The link is a straight direction.
double getMaxAccel() const
Get the vehicle type&#39;s maximum acceleration [m/s^2].
Definition: MSCFModel.h:210
void setFollowerGaps(CLeaderDist follower, double secGap)
void checkTraCICommands(MSVehicle *vehicle)
Take into account traci LC-commands.
A class responsible for exchanging messages between cars involved in lane-change interaction.
virtual bool change()
The vehicle changes lanes (micro only)
virtual double estimateLCDuration(const double speed, const double remainingManeuverDist, const double decel) const
Calculates the maximal time needed to complete a lane change maneuver if lcMaxSpeedLatFactor and lcMa...
const MSCFModel & getCarFollowModel() const
Returns the vehicle&#39;s car following model definition.
Definition: MSVehicle.h:891
Left blinker lights are switched on.
Definition: MSVehicle.h:1179
virtual void updateChanger(bool vehHasChanged)
VehCont myTmpVehicles
Container for lane-changing vehicles. After completion of lane-change- process, the containers will b...
Definition: MSLane.h:1236
blocked in all directions
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:49
The action is urgent (to be defined by lc-model)
virtual double getSecureGap(const double speed, const double leaderSpeed, const double leaderMaxDecel) const
Returns the minimum gap to reserve if the leader is braking at maximum (>=0)
Definition: MSCFModel.h:328
static MSPModel * getModel()
Definition: MSPModel.cpp:59
SUMOTime getActionStepLength() const
Returns the vehicle&#39;s action step length in millisecs, i.e. the interval between two action points...
Definition: MSVehicle.h:509
void updateBestLanes(bool forceRebuild=false, const MSLane *startLane=0)
computes the best lanes to use in order to continue the route
Definition: MSVehicle.cpp:4325
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
Definition: MSVehicle.cpp:4668
#define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD_EMERGENCY
virtual int wantsChange(int laneOffset, MSAbstractLaneChangeModel::MSLCMessager &msgPass, int blocked, const std::pair< MSVehicle *, double > &leader, const std::pair< MSVehicle *, double > &neighLead, const std::pair< MSVehicle *, double > &neighFollow, const MSLane &neighLane, const std::vector< MSVehicle::LaneQ > &preb, MSVehicle **lastBlocked, MSVehicle **firstBlocked)
Called to examine whether the vehicle wants to change using the given laneOffset. This method gets th...
Position myCachedPosition
Definition: MSVehicle.h:1860
std::pair< MSVehicle *const, double > getOppositeFollower(const MSVehicle *ego) const
Definition: MSLane.cpp:3422
int getLaneChangeDirection() const
return the direction of the current lane change maneuver
double getSpeedLimit() const
Returns the lane&#39;s maximum allowed speed.
Definition: MSLane.h:506
void registerHop(MSVehicle *vehicle)
Register that vehicle belongs to Changer Item to after LC decisions.
#define STEPS2TIME(x)
Definition: SUMOTime.h:58
void forceVehicleInsertion(MSVehicle *veh, double pos, MSMoveReminder::Notification notification, double posLat=0)
Inserts the given vehicle at the given position.
Definition: MSLane.cpp:964
T MIN2(T a, T b)
Definition: StdDefs.h:70
The link is a (hard) right direction.
std::pair< MSVehicle *const, double > getLeader(const MSVehicle *veh, const double vehPos, const std::vector< MSLane *> &bestLaneConts, double dist=-1, bool checkTmpVehicles=false) const
Returns the immediate leader of veh and the distance to veh starting on this lane.
Definition: MSLane.cpp:2143
static void computeOvertakingTime(const MSVehicle *vehicle, const MSVehicle *leader, double gap, double &timeToOvertake, double &spaceToOvertake)
Compute the time and space required for overtaking the given leader.
A structure representing the best lanes for continuing the current route starting at &#39;lane&#39;...
Definition: MSVehicle.h:808
virtual PersonDist nextBlocking(const MSLane *lane, double minPos, double minRight, double maxLeft, double stopTime=0)
returns the next pedestrian beyond minPos that is laterally between minRight and maxLeft or 0 ...
Definition: MSPModel.h:91
bool hasInfluencer() const
Definition: MSVehicle.h:1640
double getMinGap() const
Get the free space in front of vehicles of this class.
double getMaxDecel() const
Get the vehicle type&#39;s maximal comfortable deceleration [m/s^2].
Definition: MSCFModel.h:218
MSLaneChanger()
Default constructor.
MSLane * getOpposite() const
return the opposite direction lane for lane changing or 0
Definition: MSLane.cpp:3359
bool isInternal() const
return whether this edge is an internal edge
Definition: MSEdge.h:225
void updateLanes(SUMOTime t)
std::vector< MSVehicle * > VehCont
Container for vehicles.
Definition: MSLane.h:90
static MSLinkCont::const_iterator succLinkSec(const SUMOVehicle &veh, int nRouteSuccs, const MSLane &succLinkSource, const std::vector< MSLane *> &conts)
Definition: MSLane.cpp:1926
int checkChangeWithinEdge(int laneOffset, const std::pair< MSVehicle *const, double > &leader, const std::vector< MSVehicle::LaneQ > &preb) const
std::pair< MSVehicle *const, double > getRealFollower(const ChangerIt &target) const
double getLateralPositionOnLane() const
Get the vehicle&#39;s lateral position on the lane.
Definition: MSVehicle.h:440
void setOrigLeaderGaps(CLeaderDist, double secGap)
bool startLaneChangeManeuver(MSLane *source, MSLane *target, int direction)
start the lane change maneuver and return whether it continues
#define OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD
std::pair< const MSPerson *, double > PersonDist
Definition: MSPModel.h:38
void primaryLaneChanged(MSLane *source, MSLane *target, int direction)
called once when the vehicles primary lane changes
double getWidth() const
Get the width which vehicles of this class shall have when being drawn.
#define OPPOSITE_OVERTAKING_SAFE_TIMEGAP
std::pair< const MSVehicle *, double > CLeaderDist
Definition: MSLeaderInfo.h:35
SUMOVehicleClass getVClass() const
Returns the vehicle&#39;s access class.
Influencer & getInfluencer()
Returns the velocity/lane influencer.
Definition: MSVehicle.cpp:5488
virtual double getSafetyFactor() const
return factor for modifying the safety constraints of the car-following model
LaneChangeAction
The state of a vehicle&#39;s lane-change behavior.
virtual void setOwnState(const int state)
double getOppositePos(double pos) const
return the corresponding position on the opposite lane
Definition: MSLane.cpp:3368
const MSVehicleType & getVehicleType() const
Returns the vehicle&#39;s type definition.
bool isActive() const
Returns whether the current simulation step is an action point for the vehicle.
Definition: MSVehicle.h:595
virtual ~MSLaneChanger()
Destructor.
virtual double getHeadwayTime() const
Get the driver&#39;s desired headway [s].
Definition: MSCFModel.h:259
The vehicle is blocked being overlapping.
#define DEBUG_COND
double getAcceleration() const
Returns the vehicle&#39;s acceleration in m/s (this is computed as the last step&#39;s mean acceleration in c...
Definition: MSVehicle.h:500
Changer::iterator ChangerIt
the iterator moving over the ChangeElems
MSVehicle * getLastAnyVehicle() const
returns the last vehicle that is fully or partially on this lane
Definition: MSLane.cpp:1898
std::pair< MSVehicle *const, double > getCriticalLeader(double dist, double seen, double speed, const MSVehicle &veh) const
Returns the most dangerous leader and the distance to him.
Definition: MSLane.cpp:2334
double getLength() const
Get vehicle&#39;s length [m].
bool allowsVehicleClass(SUMOVehicleClass vclass) const
Definition: MSLane.h:772
Changer myChanger
Container for ChangeElemements, one for every lane in the edge.
The action is due to a TraCI request.
double getBackPositionOnLane(const MSLane *lane) const
Get the vehicle&#39;s position relative to the given lane.
Definition: MSVehicle.cpp:3701
#define OPPOSITE_OVERTAKING_SAFETY_FACTOR
void registerUnchanged(MSVehicle *vehicle)
The vehicle does not have enough space to complete a continuous lane and change before the next turni...
bool unsafeLinkAhead(const MSLane *lane) const
whether the vehicle may safely move to the given lane with regard to upcoming links ...
Definition: MSVehicle.cpp:5112
MSLane * getShadowLane() const
Returns the lane the vehicle&#39;s shadow is on during continuous/sublane lane change.
bool isStopped() const
Returns whether the vehicle is at a stop.
Definition: MSVehicle.cpp:1585
MSVehicle * hoppedVeh
last vehicle that changed into this lane
Definition: MSLaneChanger.h:76
const std::vector< LaneQ > & getBestLanes() const
Returns the description of best lanes to use in order to continue the route.
Definition: MSVehicle.cpp:4319
double getSpeed() const
Returns the vehicle&#39;s current speed.
Definition: MSVehicle.h:483
The vehicle is blocked by right leader.
int influenceChangeDecision(int state)
allow TraCI to influence a lane change decision
Definition: MSVehicle.cpp:5512
public emergency vehicles
static SUMOTime gLaneChangeDuration
Definition: MSGlobals.h:82
const std::string & getID() const
Returns the name of the vehicle.
Representation of a lane in the micro simulation.
Definition: MSLane.h:78
double myBackPos
the stored back position
Definition: MSVehicle.h:147
The vehicle is blocked by right follower.
bool mayChange(int direction) const
whether changing to the lane in the given direction should be considered
void saveLCState(const int dir, const int stateWithoutTraCI, const int state)
double getSpeedLat() const
return the lateral speed of the current lane change maneuver
Interface for lane-change models.
SUMOVehicleClass getVehicleClass() const
Get this vehicle type&#39;s vehicle class.
#define OPPOSITE_OVERTAKING_SAFETYGAP_HEADWAY_FACTOR
ChangerIt myCandi
static const Position INVALID
used to indicate that a position is valid
Definition: Position.h:285
double nextStopDist() const
return the distance to the next stop or doubleMax if there is none.
Definition: MSVehicle.h:1043
MSLane * getParallelLane(int offset) const
Returns the lane with the given offset parallel to this one or 0 if it does not exist.
Definition: MSLane.cpp:2063
void endLaneChangeManeuver(const MSMoveReminder::Notification reason=MSMoveReminder::NOTIFICATION_LANE_CHANGE)