Eclipse SUMO - Simulation of Urban MObility
MSLaneChangerSublane.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-2020 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials are made available under the
5 // terms of the Eclipse Public License 2.0 which is available at
6 // https://www.eclipse.org/legal/epl-2.0/
7 // This Source Code may also be made available under the following Secondary
8 // Licenses when the conditions for such availability set forth in the Eclipse
9 // Public License 2.0 are satisfied: GNU General Public License, version 2
10 // or later which is available at
11 // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 /****************************************************************************/
19 // Performs sub-lane changing of vehicles
20 /****************************************************************************/
21 #include <config.h>
22 
23 #include "MSLaneChangerSublane.h"
24 #include "MSNet.h"
25 #include "MSVehicle.h"
26 #include "MSVehicleType.h"
27 #include "MSVehicleTransfer.h"
28 #include "MSGlobals.h"
29 #include <cassert>
30 #include <iterator>
31 #include <cstdlib>
32 #include <cmath>
35 #include <utils/geom/GeomHelper.h>
36 
37 
38 // ===========================================================================
39 // DEBUG constants
40 // ===========================================================================
41 #define DEBUG_COND (vehicle->getLaneChangeModel().debugVehicle())
42 //#define DEBUG_COND (vehicle->getID() == "disabled")
43 //#define DEBUG_DECISION
44 //#define DEBUG_ACTIONSTEPS
45 //#define DEBUG_STATE
46 //#define DEBUG_MANEUVER
47 //#define DEBUG_SURROUNDING
48 
49 // ===========================================================================
50 // member method definitions
51 // ===========================================================================
52 MSLaneChangerSublane::MSLaneChangerSublane(const std::vector<MSLane*>* lanes, bool allowChanging) :
53  MSLaneChanger(lanes, allowChanging) {
54  // initialize siblings
55  if (myChanger.front().lane->isInternal()) {
56  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
57  for (ChangerIt ce2 = myChanger.begin(); ce2 != myChanger.end(); ++ce2) {
58  if (ce != ce2 && ce->lane->getIncomingLanes().front().lane == ce2->lane->getIncomingLanes().front().lane) {
59  //std::cout << "addSibling lane=" << ce->lane->getID() << " offset=" << ce2->lane->getIndex() - ce->lane->getIndex() << "\n";
60  ce->siblings.push_back(ce2->lane->getIndex() - ce->lane->getIndex());
61  }
62  }
63  }
64  }
65 }
66 
67 
69 
70 void
73  // Prepare myChanger with a safe state.
74  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
75  ce->ahead = ce->lane->getPartialBeyond();
76 // std::cout << SIMTIME << " initChanger lane=" << ce->lane->getID() << " vehicles=" << toString(ce->lane->myVehicles) << "\n";
77 // std::cout << SIMTIME << " initChanger lane=" << ce->lane->getID() << " partial vehicles=" << toString(ce->lane->myPartialVehicles) << "\n";
78 // std::cout << SIMTIME << " initChanger lane=" << ce->lane->getID() << " partial vehicles beyond=" << toString(ce->ahead.toString()) << "\n";
79  }
80 }
81 
82 
83 
84 void
86  MSLaneChanger::updateChanger(vehHasChanged);
87  if (!vehHasChanged) {
88  MSVehicle* lead = myCandi->lead;
89  //std::cout << SIMTIME << " updateChanger lane=" << myCandi->lane->getID() << " lead=" << Named::getIDSecure(lead) << "\n";
90  myCandi->ahead.addLeader(lead, false, 0);
91  MSLane* shadowLane = lead->getLaneChangeModel().getShadowLane();
92  if (shadowLane != nullptr) {
93  const double latOffset = lead->getLane()->getRightSideOnEdge() - shadowLane->getRightSideOnEdge();
94  //std::cout << SIMTIME << " updateChanger shadowLane=" << shadowLane->getID() << " lead=" << Named::getIDSecure(lead) << "\n";
95  (myChanger.begin() + shadowLane->getIndex())->ahead.addLeader(lead, false, latOffset);
96  }
97  }
98  //std::cout << SIMTIME << " updateChanger: lane=" << myCandi->lane->getID() << " lead=" << Named::getIDSecure(myCandi->lead) << " ahead=" << myCandi->ahead.toString() << " vehHasChanged=" << vehHasChanged << "\n";
99  //for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
100  // std::cout << " lane=" << ce->lane->getID() << " vehicles=" << toString(ce->lane->myVehicles) << "\n";
101  //}
102 }
103 
104 
105 bool
107  // variant of change() for the sublane case
109  MSVehicle* vehicle = veh(myCandi);
110  vehicle->getLaneChangeModel().clearNeighbors();
111 #ifdef DEBUG_ACTIONSTEPS
112  if (DEBUG_COND) {
113  std::cout << "\nCHANGE" << std::endl;
114  }
115 #endif
116  assert(vehicle->getLane() == (*myCandi).lane);
117  assert(!vehicle->getLaneChangeModel().isChangingLanes());
118  if ( vehicle->isStoppedOnLane()) {
119  registerUnchanged(vehicle);
120  return false;
121  }
122  if (!vehicle->isActive()) {
123 #ifdef DEBUG_ACTIONSTEPS
124  if (DEBUG_COND) {
125  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' skips regular change checks." << std::endl;
126  }
127 #endif
128 
129  bool changed;
130  // let TraCI influence the wish to change lanes during non-actionsteps
131  checkTraCICommands(vehicle);
132 
133  // Resume change
134  changed = continueChangeSublane(vehicle, myCandi);
135 #ifdef DEBUG_ACTIONSTEPS
136  if (DEBUG_COND) {
137  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' lcm->maneuverDist=" << vehicle->getLaneChangeModel().getManeuverDist()
138  << " lcm->speedLat=" << vehicle->getLaneChangeModel().getSpeedLat() << std::endl;
139  }
140 #endif
141  return changed;
142  }
143 
144 #ifdef DEBUG_ACTIONSTEPS
145  if (DEBUG_COND) {
146  std::cout << "\n" << SIMTIME << " veh '" << vehicle->getID() << "' plans lanechange maneuver." << std::endl;
147  }
148 #endif
149  vehicle->updateBestLanes(); // needed?
150  for (int i = 0; i < (int) myChanger.size(); ++i) {
151  vehicle->adaptBestLanesOccupation(i, myChanger[i].dens);
152  }
153  // update leaders beyond the current edge for all lanes
154  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
155  ce->aheadNext = getLeaders(ce, vehicle);
156  }
157 
158  // update expected speeds
159  int sublaneIndex = 0;
160  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
161  vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(ce->aheadNext, sublaneIndex, ce->lane->getIndex());
162  for (int offset : ce->siblings) {
163  // treat sibling lanes (internal lanes with the same origin lane) as if they have the same geometry
164  ChangerIt ceSib = ce + offset;
165  vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(ceSib->aheadNext, sublaneIndex, ceSib->lane->getIndex());
166  }
167  sublaneIndex += ce->ahead.numSublanes();
168  }
169 
171  | (mayChange(1) ? LCA_LEFT : LCA_NONE));
172 
173  StateAndDist right = checkChangeHelper(vehicle, -1, alternatives);
174  StateAndDist left = checkChangeHelper(vehicle, 1, alternatives);
175  StateAndDist current = checkChangeHelper(vehicle, 0, alternatives);
176 
177  StateAndDist decision = vehicle->getLaneChangeModel().decideDirection(current,
178  vehicle->getLaneChangeModel().decideDirection(right, left));
179 #ifdef DEBUG_DECISION
180  if (vehicle->getLaneChangeModel().debugVehicle()) {
181  std::cout << "\n" << SIMTIME << " decision=" << toString((LaneChangeAction)decision.state) << " dir=" << decision.dir << " latDist=" << decision.latDist << " maneuverDist=" << decision.maneuverDist << "\n";
182  }
183 #endif
184  vehicle->getLaneChangeModel().setOwnState(decision.state);
185  if ((decision.state & LCA_WANTS_LANECHANGE) != 0 && (decision.state & LCA_BLOCKED) == 0) {
186  // change if the vehicle wants to and is allowed to change
187 #ifdef DEBUG_MANEUVER
188  if (DEBUG_COND) {
189  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' performing sublane change..." << std::endl;
190  }
191 #endif
192  return startChangeSublane(vehicle, myCandi, decision.latDist, decision.maneuverDist);
193  }
194  // @note this assumes vehicles can instantly abort any maneuvre in case of emergency
195  abortLCManeuver(vehicle);
196 
197  if ((right.state & (LCA_URGENT)) != 0 && (left.state & (LCA_URGENT)) != 0) {
198  // ... wants to go to the left AND to the right
199  // just let them go to the right lane...
200  left.state = 0;
201  }
202  return false;
203 }
204 
205 
206 void
208 #ifdef DEBUG_MANEUVER
209  if (DEBUG_COND) {
210  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' aborts LC-continuation."
211  << std::endl;
212  }
213 #endif
214  const int priorReason = vehicle->getLaneChangeModel().getPrevState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
215  const int reason = vehicle->getLaneChangeModel().getOwnState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
216  if (priorReason != 0 && priorReason != reason && vehicle->getLaneChangeModel().getPreviousManeuverDist() != 0) {
217  // original from cannot be reconstructed
218  const int priorDirection = vehicle->getLaneChangeModel().getPreviousManeuverDist() > 0 ? 1 : -1;
219 #ifdef DEBUG_MANEUVER
220  if (DEBUG_COND) {
221  std::cout << SIMTIME << " abortLCManeuver priorReason=" << toString((LaneChangeAction)priorReason)
222  << " reason=" << toString((LaneChangeAction)reason) << " veh=" << vehicle->getID() << "\n";
223  }
224 #endif
225  outputLCEnded(vehicle, myCandi, myCandi, priorDirection);
226  }
227  vehicle->getLaneChangeModel().setSpeedLat(0);
228  vehicle->getLaneChangeModel().setManeuverDist(0.);
229  registerUnchanged(vehicle);
230 }
231 
232 
234 MSLaneChangerSublane::checkChangeHelper(MSVehicle* vehicle, int laneOffset, LaneChangeAction alternatives) {
235  StateAndDist result = StateAndDist(0, 0, 0, 0);
236  if (mayChange(laneOffset)) {
237  const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes();
238  result.state = checkChangeSublane(laneOffset, alternatives, preb, result.latDist, result.maneuverDist);
239  result.dir = laneOffset;
240  if ((result.state & LCA_WANTS_LANECHANGE) != 0 && (result.state & LCA_URGENT) != 0 && (result.state & LCA_BLOCKED) != 0) {
241  (myCandi + laneOffset)->lastBlocked = vehicle;
242  if ((myCandi + laneOffset)->firstBlocked == nullptr) {
243  (myCandi + laneOffset)->firstBlocked = vehicle;
244  }
245  }
246  }
247  return result;
248 }
249 
250 
252 // (used to continue sublane changing in non-action steps).
253 bool
255  // lateral distance to complete maneuver
256  double remLatDist = vehicle->getLaneChangeModel().getManeuverDist();
257  if (remLatDist == 0) {
258  registerUnchanged(vehicle);
259  return false;
260  }
261  const double nextLatDist = SPEED2DIST(vehicle->getLaneChangeModel().computeSpeedLat(remLatDist, remLatDist));
262 #ifdef DEBUG_MANEUVER
263  if (DEBUG_COND) {
264  std::cout << SIMTIME << " vehicle '" << vehicle->getID() << "' continueChangeSublane()"
265  << " remLatDist=" << remLatDist << " nextLatDist=" << nextLatDist
266  << std::endl;
267  }
268 #endif
269 
270  const bool changed = startChangeSublane(vehicle, from, nextLatDist, remLatDist);
271  return changed;
272 }
273 
274 
275 bool
276 MSLaneChangerSublane::startChangeSublane(MSVehicle* vehicle, ChangerIt& from, double latDist, double maneuverDist) {
277  if (vehicle->isRemoteControlled()) {
278  registerUnchanged(vehicle);
279  return false;
280  }
281  // Prevent continuation of LC beyond lane borders if change is not allowed
282  const double distToRightLaneBorder = latDist < 0 ? vehicle->getLane()->getWidth() * 0.5 + vehicle->getLateralPositionOnLane() - vehicle->getWidth() * 0.5 : 0.;
283  const double distToLeftLaneBorder = latDist > 0 ? vehicle->getLane()->getWidth() * 0.5 - vehicle->getLateralPositionOnLane() - vehicle->getWidth() * 0.5 : 0.;
284  // determine direction of LC
285  const int direction = (latDist >= -distToRightLaneBorder && latDist <= distToLeftLaneBorder) ? 0 : (latDist < 0 ? -1 : 1);
286  ChangerIt to = from;
287  if (mayChange(direction)) {
288  to = from + direction;
289  } else {
290  // This may occur during maneuver continuation in non-actionsteps.
291  // TODO: Understand better why and test later if additional sublane actionstep debugging resolves this
292  // (XXX: perhaps one should try to extrapolate check for this case before to avoid maneuver initialization
293  // similar as for continuous LC in MSLaneChanger::checkChange())
294  //assert(false);
295  abortLCManeuver(vehicle);
296  return false;
297  }
298 
299  // The following does:
300  // 1) update vehicles lateral position according to latDist and target lane
301  // 2) distinguish several cases
302  // a) vehicle moves completely within the same lane
303  // b) vehicle intersects another lane
304  // - vehicle must be moved to the lane where it's midpoint is (either old or new)
305  // - shadow vehicle must be created/moved to the other lane if the vehicle intersects it
306  // 3) updated dens of all lanes that hold the vehicle or its shadow
307 
308  vehicle->myState.myPosLat += latDist;
310  vehicle->getLaneChangeModel().setSpeedLat(DIST2SPEED(latDist));
311 #ifdef DEBUG_MANEUVER
312  if (DEBUG_COND) {
313  std::cout << SIMTIME << " vehicle '" << vehicle->getID() << "' with maneuverDist=" << maneuverDist
314  << " and committedSpeed=" << vehicle->getLaneChangeModel().getCommittedSpeed()
315  << " increments lateral position by latDist=" << latDist << std::endl;
316  }
317 #endif
318 #ifdef DEBUG_SURROUNDING
319  if (DEBUG_COND) {
320  std::cout << SIMTIME << " vehicle '" << vehicle->getID() << "'\n to->ahead=" << to->ahead.toString()
321  << "'\n to->aheadNext=" << to->aheadNext.toString()
322  << std::endl;
323  }
324 #endif
325  const bool completedPriorManeuver = fabs(vehicle->getLaneChangeModel().getPreviousManeuverDist()) < NUMERICAL_EPS;
326  const bool completedManeuver = fabs(maneuverDist - latDist) < NUMERICAL_EPS;
327  vehicle->getLaneChangeModel().updateSafeLatDist(latDist);
328 
329  // current maneuver is aborted when direction or reason changes
330  const int priorReason = vehicle->getLaneChangeModel().getPrevState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
331  const int reason = vehicle->getLaneChangeModel().getOwnState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
332 #ifdef DEBUG_MANEUVER
333  if (DEBUG_COND) {
334  std::cout << SIMTIME << " vehicle '" << vehicle->getID()
335  << "' completedPriorManeuver=" << completedPriorManeuver
336  << " completedManeuver=" << completedManeuver
337  << " priorReason=" << toString((LaneChangeAction)priorReason)
338  << " reason=" << toString((LaneChangeAction)reason)
339  << " priorManeuverDist=" << vehicle->getLaneChangeModel().getPreviousManeuverDist()
340  << " maneuverDist=" << maneuverDist
341  << " latDist=" << latDist
342  << std::endl;
343  }
344 #endif
345  if (!completedManeuver && !completedPriorManeuver && priorReason != 0 &&
346  (vehicle->getLaneChangeModel().getPreviousManeuverDist() * latDist < 0
347  || priorReason != reason)) {
348  const int priorDirection = vehicle->getLaneChangeModel().getPreviousManeuverDist() > 0 ? 1 : -1;
349  // original from cannot be reconstructed
350 #ifdef DEBUG_MANEUVER
351  if (DEBUG_COND) {
352  std::cout << SIMTIME << " startChangeSublane abort priorReason=" << toString((LaneChangeAction)priorReason)
353  << " reason=" << toString((LaneChangeAction)reason) << " veh=" << vehicle->getID() << "\n";
354  }
355 #endif
356  outputLCEnded(vehicle, from, from, priorDirection);
357  }
358 
359  outputLCStarted(vehicle, from, to, direction, maneuverDist);
360  vehicle->getLaneChangeModel().setManeuverDist(maneuverDist - latDist);
361  const bool changedToNewLane = checkChangeToNewLane(vehicle, direction, from, to);
362 
363  MSLane* oldShadowLane = vehicle->getLaneChangeModel().getShadowLane();
365  MSLane* shadowLane = vehicle->getLaneChangeModel().getShadowLane();
366  if (shadowLane != nullptr && shadowLane != oldShadowLane) {
367  assert(to != from || oldShadowLane == 0);
368  const double latOffset = vehicle->getLane()->getRightSideOnEdge() - shadowLane->getRightSideOnEdge();
369  (myChanger.begin() + shadowLane->getIndex())->ahead.addLeader(vehicle, false, latOffset);
370  }
371  if (completedManeuver) {
372  outputLCEnded(vehicle, from, to, direction);
373  }
374 
375  // Update maneuver reservations on target lanes
376  MSLane* targetLane = vehicle->getLaneChangeModel().updateTargetLane();
377  if (!changedToNewLane && targetLane != nullptr
378  && vehicle->getActionStepLength() > DELTA_T) {
379  const int dir = (vehicle->getLaneChangeModel().getManeuverDist() > 0 ? 1 : -1);
380  ChangerIt target = from + dir;
381  const double actionStepDist = dir * vehicle->getVehicleType().getMaxSpeedLat() * vehicle->getActionStepLengthSecs();
382  const double latOffset = vehicle->getLatOffset(targetLane) + actionStepDist;
383  target->ahead.addLeader(vehicle, false, latOffset);
384  //std::cout << SIMTIME << " veh=" << vehicle->getID() << " target=" << targetLane->getID()
385  // << " actionStepDist=" << actionStepDist << " latOffset=" << latOffset
386  // << " targetAhead=" << target->ahead.toString() << "\n";
387  }
388 
389  // compute new angle of the vehicle from the x- and y-distances travelled within last time step
390  // (should happen last because primaryLaneChanged() also triggers angle computation)
391  // this part of the angle comes from the orientation of our current lane
392  double laneAngle = vehicle->getLane()->getShape().rotationAtOffset(vehicle->getLane()->interpolateLanePosToGeometryPos(vehicle->getPositionOnLane())) ;
393  if (vehicle->getLane()->getShape().length2D() == 0) {
394  if (vehicle->getFurtherLanes().size() == 0) {
395  laneAngle = vehicle->getAngle();
396  } else {
397  laneAngle = vehicle->getFurtherLanes().front()->getShape().rotationAtOffset(-NUMERICAL_EPS);
398  }
399  }
400  // this part of the angle comes from the vehicle's lateral movement
401  double changeAngle = 0;
402  // avoid flicker
403  if (fabs(latDist) > NUMERICAL_EPS) {
404  // angle is between vehicle front and vehicle back (and depending on travelled distance)
405  changeAngle = atan2(latDist, vehicle->getVehicleType().getLength() + SPEED2DIST(vehicle->getSpeed()));
406  if (MSGlobals::gLefthand) {
407  changeAngle *= -1;
408  }
409  }
410 #ifdef DEBUG_MANEUVER
411  if (vehicle->getLaneChangeModel().debugVehicle()) {
412  std::cout << SIMTIME << " startChangeSublane()"
413  << " shadowLane=" << (shadowLane != nullptr ? shadowLane->getID() : "NULL")
414  << " targetLane=" << (targetLane != nullptr ? targetLane->getID() : "NULL")
415  << " maneuverDist=" << vehicle->getLaneChangeModel().getManeuverDist()
416  << " latDist=" << latDist
417  << " old=" << Named::getIDSecure(oldShadowLane)
418  << " new=" << Named::getIDSecure(vehicle->getLaneChangeModel().getShadowLane())
419  << " laneA=" << RAD2DEG(laneAngle)
420  << " changeA=" << RAD2DEG(changeAngle)
421  << " oldA=" << RAD2DEG(vehicle->getAngle())
422  << " newA=" << RAD2DEG(laneAngle + changeAngle)
423  << "\n";
424  }
425 #endif
426  vehicle->setAngle(laneAngle + changeAngle, completedManeuver);
427 
428  // check if a traci maneuver must continue
429  if ((vehicle->getLaneChangeModel().getOwnState() & LCA_TRACI) != 0) {
430  if (vehicle->getLaneChangeModel().debugVehicle()) {
431  std::cout << SIMTIME << " continue TraCI-maneuver remainingLatDist=" << vehicle->getLaneChangeModel().getManeuverDist() << "\n";
432  }
434  }
435  from->lane->requireCollisionCheck();
436  to->lane->requireCollisionCheck();
437  return changedToNewLane;
438 }
439 
440 bool
442  const bool changedToNewLane = to != from && fabs(vehicle->getLateralPositionOnLane()) > 0.5 * vehicle->getLane()->getWidth() && mayChange(direction);
443  if (changedToNewLane) {
444  vehicle->myState.myPosLat -= direction * 0.5 * (from->lane->getWidth() + to->lane->getWidth());
445  to->lane->myTmpVehicles.insert(to->lane->myTmpVehicles.begin(), vehicle);
446  to->dens += vehicle->getVehicleType().getLengthWithGap();
448  if (!vehicle->isActive()) {
449  // update leaders beyond the current edge for all lanes
450  // @note to->aheadNext and from->aheadNext are only needed for output in non-action steps.
451  to->aheadNext = getLeaders(to, vehicle);
452  from->aheadNext = getLeaders(from, vehicle);
453  }
454  vehicle->getLaneChangeModel().setLeaderGaps(to->aheadNext);
455  vehicle->getLaneChangeModel().setFollowerGaps(to->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true));
456  vehicle->getLaneChangeModel().setOrigLeaderGaps(from->aheadNext);
457  }
458  vehicle->getLaneChangeModel().startLaneChangeManeuver(from->lane, to->lane, direction);
459  to->ahead.addLeader(vehicle, false, 0);
460  } else {
461  registerUnchanged(vehicle);
462  from->ahead.addLeader(vehicle, false, 0);
463  }
464  return changedToNewLane;
465 }
466 
467 void
468 MSLaneChangerSublane::outputLCStarted(MSVehicle* vehicle, ChangerIt& from, ChangerIt& to, int direction, double maneuverDist) {
470  // non-sublane change started
471  && ((vehicle->getLaneChangeModel().getOwnState() & (LCA_CHANGE_REASONS & ~LCA_SUBLANE)) != 0)
472  && ((vehicle->getLaneChangeModel().getOwnState() & LCA_STAY) == 0)
473  // no changing for the same reason in previous step (either not wanted or blocked)
476  || ((vehicle->getLaneChangeModel().getPrevState() & LCA_STAY) != 0)
477  || ((vehicle->getLaneChangeModel().getPrevState() & LCA_BLOCKED) != 0))
478  ) {
479 #ifdef DEBUG_STATE
480  if (DEBUG_COND) {
481  std::cout << SIMTIME << " veh=" << vehicle->getID() << " laneChangeStarted state=" << toString((LaneChangeAction)vehicle->getLaneChangeModel().getOwnState())
482  << " prevState=" << toString((LaneChangeAction)vehicle->getLaneChangeModel().getPrevState())
484  << " filtered=" << toString((LaneChangeAction)(vehicle->getLaneChangeModel().getOwnState() & (LCA_CHANGE_REASONS & ~LCA_SUBLANE)))
485  << "\n";
486  }
487 #endif
488  vehicle->getLaneChangeModel().setLeaderGaps(to->aheadNext);
489  vehicle->getLaneChangeModel().setFollowerGaps(to->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true));
490  vehicle->getLaneChangeModel().setOrigLeaderGaps(from->aheadNext);
491  vehicle->getLaneChangeModel().laneChangeOutput("changeStarted", from->lane, to->lane, direction, maneuverDist);
492  }
493 }
494 
495 void
496 MSLaneChangerSublane::outputLCEnded(MSVehicle* vehicle, ChangerIt& from, ChangerIt& to, int direction) {
498  // non-sublane change ended
499  && ((vehicle->getLaneChangeModel().getOwnState() & (LCA_CHANGE_REASONS & ~LCA_SUBLANE)) != 0)) {
500  vehicle->getLaneChangeModel().setLeaderGaps(to->aheadNext);
501  vehicle->getLaneChangeModel().setFollowerGaps(to->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true));
502  vehicle->getLaneChangeModel().setOrigLeaderGaps(from->aheadNext);
503  vehicle->getLaneChangeModel().laneChangeOutput("changeEnded", from->lane, to->lane, direction);
504  }
505 }
506 
507 
509 MSLaneChangerSublane::getLeaders(const ChangerIt& target, const MSVehicle* vehicle) const {
510  // get the leading vehicle on the lane to change to
511 #ifdef DEBUG_SURROUNDING
512  if (DEBUG_COND) {
513  std::cout << SIMTIME << " getLeaders lane=" << target->lane->getID() << " ego=" << vehicle->getID() << " ahead=" << target->ahead.toString() << "\n";
514  }
515 #endif
516  MSLeaderDistanceInfo result(target->lane, nullptr, 0);
517  for (int i = 0; i < target->ahead.numSublanes(); ++i) {
518  const MSVehicle* veh = target->ahead[i];
519  if (veh != nullptr) {
520  const double gap = veh->getBackPositionOnLane(target->lane) - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap();
521 #ifdef DEBUG_SURROUNDING
522  if (DEBUG_COND) {
523  std::cout << " ahead lead=" << veh->getID() << " leadBack=" << veh->getBackPositionOnLane() << " gap=" << gap << "\n";
524  }
525 #endif
526  result.addLeader(veh, gap, 0, i);
527  }
528  }
529  // if there are vehicles on the target lane with the same position as ego,
530  // they may not have been added to 'ahead' yet
531  const MSLeaderInfo& aheadSamePos = target->lane->getLastVehicleInformation(nullptr, 0, vehicle->getPositionOnLane(), false);
532  for (int i = 0; i < aheadSamePos.numSublanes(); ++i) {
533  const MSVehicle* veh = aheadSamePos[i];
534  if (veh != nullptr && veh != vehicle) {
535  const double gap = veh->getBackPositionOnLane(target->lane) - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap();
536 #ifdef DEBUG_SURROUNDING
537  if (DEBUG_COND) {
538  std::cout << " further lead=" << veh->getID() << " leadBack=" << veh->getBackPositionOnLane(target->lane) << " gap=" << gap << "\n";
539  }
540 #endif
541  result.addLeader(veh, gap, 0, i);
542  }
543  }
544 
545  if (result.numFreeSublanes() > 0) {
546  MSLane* targetLane = target->lane;
547 
548  double seen = vehicle->getLane()->getLength() - vehicle->getPositionOnLane();
549  double speed = vehicle->getSpeed();
550  // leader vehicle could be link leader on the next junction
551  double dist = MAX2(vehicle->getCarFollowModel().brakeGap(speed), 10.0) + vehicle->getVehicleType().getMinGap();
552  if (seen > dist) {
553 #ifdef DEBUG_SURROUNDING
554  if (DEBUG_COND) {
555  std::cout << " aborting forward search. dist=" << dist << " seen=" << seen << "\n";
556  }
557 #endif
558  return result;
559  }
560  const std::vector<MSLane*>& bestLaneConts = veh(myCandi)->getBestLanesContinuation(targetLane);
561 #ifdef DEBUG_SURROUNDING
562  if (DEBUG_COND) {
563  std::cout << " add consecutive before=" << result.toString() << " dist=" << dist;
564  }
565 #endif
566  target->lane->getLeadersOnConsecutive(dist, seen, speed, vehicle, bestLaneConts, result);
567 #ifdef DEBUG_SURROUNDING
568  if (DEBUG_COND) {
569  std::cout << " after=" << result.toString() << "\n";
570  }
571 #endif
572  }
573  return result;
574 }
575 
576 
577 int
579  int laneOffset,
580  LaneChangeAction alternatives,
581  const std::vector<MSVehicle::LaneQ>& preb,
582  double& latDist,
583  double& maneuverDist) const {
584 
585  ChangerIt target = myCandi + laneOffset;
586  MSVehicle* vehicle = veh(myCandi);
587  const MSLane& neighLane = *(target->lane);
588  int blocked = 0;
589 
590  MSLeaderDistanceInfo neighLeaders = target->aheadNext;
591  MSLeaderDistanceInfo neighFollowers = target->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
592  MSLeaderDistanceInfo neighBlockers(&neighLane, vehicle, vehicle->getLane()->getRightSideOnEdge() - neighLane.getRightSideOnEdge());
593  MSLeaderDistanceInfo leaders = myCandi->aheadNext;
594  MSLeaderDistanceInfo followers = myCandi->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
595  MSLeaderDistanceInfo blockers(vehicle->getLane(), vehicle, 0);
596 
597 #ifdef DEBUG_SURROUNDING
598  if (DEBUG_COND) std::cout << SIMTIME
599  << " checkChangeSublane: veh=" << vehicle->getID()
600  << " laneOffset=" << laneOffset
601  << "\n leaders=" << leaders.toString()
602  << "\n neighLeaders=" << neighLeaders.toString()
603  << "\n followers=" << followers.toString()
604  << "\n neighFollowers=" << neighFollowers.toString()
605  << "\n";
606 #endif
607 
608 
609  const int wish = vehicle->getLaneChangeModel().wantsChangeSublane(
610  laneOffset, alternatives,
611  leaders, followers, blockers,
612  neighLeaders, neighFollowers, neighBlockers,
613  neighLane, preb,
614  &(myCandi->lastBlocked), &(myCandi->firstBlocked), latDist, maneuverDist, blocked);
615  int state = blocked | wish;
616 
617  // XXX
618  // do are more careful (but expensive) check to ensure that a
619  // safety-critical leader is not being overlooked
620 
621  // XXX
622  // ensure that a continuous lane change manoeuvre can be completed
623  // before the next turning movement
624 
625  // let TraCI influence the wish to change lanes and the security to take
626  const int oldstate = state;
627  state = vehicle->influenceChangeDecision(state);
628 #ifdef DEBUG_STATE
629  if (DEBUG_COND && state != oldstate) {
630  std::cout << SIMTIME << " veh=" << vehicle->getID() << " stateAfterTraCI=" << toString((LaneChangeAction)state) << " original=" << toString((LaneChangeAction)oldstate) << "\n";
631  }
632 #endif
633  vehicle->getLaneChangeModel().saveLCState(laneOffset, oldstate, state);
634  if (laneOffset != 0) {
635  vehicle->getLaneChangeModel().saveNeighbors(laneOffset, neighFollowers, neighLeaders);
636  }
637  return state;
638 }
639 
640 
641 /****************************************************************************/
#define RAD2DEG(x)
Definition: GeomHelper.h:36
#define DEBUG_COND
SUMOTime DELTA_T
Definition: SUMOTime.cpp:37
#define SPEED2DIST(x)
Definition: SUMOTime.h:43
#define SIMTIME
Definition: SUMOTime.h:60
#define DIST2SPEED(x)
Definition: SUMOTime.h:45
LaneChangeAction
The state of a vehicle's lane-change behavior.
@ LCA_BLOCKED
blocked in all directions
@ LCA_NONE
@ LCA_URGENT
The action is urgent (to be defined by lc-model)
@ LCA_STAY
Needs to stay on the current lane.
@ LCA_SUBLANE
used by the sublane model
@ LCA_LEFT
Wants go to the left.
@ LCA_CHANGE_REASONS
reasons of lane change
@ LCA_TRACI
The action is due to a TraCI request.
@ LCA_WANTS_LANECHANGE
lane can change
@ LCA_RIGHT
Wants go to the right.
T MAX2(T a, T b)
Definition: StdDefs.h:79
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:44
void saveLCState(const int dir, const int stateWithoutTraCI, const int state)
void setFollowerGaps(CLeaderDist follower, double secGap)
virtual void setOwnState(const int state)
static bool outputLCEnded()
whether start of maneuvers shall be recorede
double getManeuverDist() const
Returns the remaining unblocked distance for the current maneuver. (only used by sublane model)
void laneChangeOutput(const std::string &tag, MSLane *source, MSLane *target, int direction, double maneuverDist=0)
called once the vehicle ends a lane change manoeuvre (non-instant)
MSLane * getShadowLane() const
Returns the lane the vehicle's shadow is on during continuous/sublane lane change.
bool startLaneChangeManeuver(MSLane *source, MSLane *target, int direction)
start the lane change maneuver and return whether it continues
static bool haveLCOutput()
whether lanechange-output is active
virtual void updateExpectedSublaneSpeeds(const MSLeaderDistanceInfo &ahead, int sublaneOffset, int laneIndex)
update expected speeds for each sublane of the current edge
void setLeaderGaps(CLeaderDist, double secGap)
virtual double computeSpeedLat(double latDist, double &maneuverDist) const
decides the next lateral speed depending on the remaining lane change distance to be covered and upda...
void setOrigLeaderGaps(CLeaderDist, double secGap)
virtual int wantsChangeSublane(int laneOffset, LaneChangeAction alternatives, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &followers, const MSLeaderDistanceInfo &blockers, const MSLeaderDistanceInfo &neighLeaders, const MSLeaderDistanceInfo &neighFollowers, const MSLeaderDistanceInfo &neighBlockers, const MSLane &neighLane, const std::vector< MSVehicle::LaneQ > &preb, MSVehicle **lastBlocked, MSVehicle **firstBlocked, double &latDist, double &targetDistLat, int &blocked)
void setManeuverDist(const double dist)
Updates the remaining distance for the current maneuver while it is continued within non-action steps...
void setSpeedLat(double speedLat)
set the lateral speed and update lateral acceleraton
virtual void updateSafeLatDist(const double travelledLatDist)
Updates the value of safe lateral distances (in SL2015) during maneuver continuation in non-action st...
double getSpeedLat() const
return the lateral speed of the current lane change maneuver
virtual StateAndDist decideDirection(StateAndDist sd1, StateAndDist sd2) const
decide in which direction to move in case both directions are desirable
static bool outputLCStarted()
whether start of maneuvers shall be recorede
virtual bool debugVehicle() const
whether the current vehicles shall be debugged
bool isChangingLanes() const
return true if the vehicle currently performs a lane change maneuver
void clearNeighbors()
Clear info on neighboring vehicle from previous step.
void saveNeighbors(const int dir, const MSLeaderDistanceInfo &followers, const MSLeaderDistanceInfo &leaders)
Saves the lane change relevant vehicles, which are currently on neighboring lanes in the given direct...
double getWidth() const
Returns the vehicle's width.
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver's reaction time tau (i....
Definition: MSCFModel.h:311
static bool gLefthand
Whether lefthand-drive is being simulated.
Definition: MSGlobals.h:136
Performs lane changing of vehicles.
Definition: MSLaneChanger.h:45
void checkTraCICommands(MSVehicle *vehicle)
Take into account traci LC-commands.
virtual void initChanger()
Initialize the changer before looping over all vehicles.
MSVehicle * veh(ConstChangerIt ce) const
Changer myChanger
Container for ChangeElemements, one for every lane in the edge.
ChangerIt findCandidate()
Find current candidate. If there is none, myChanger.end() is returned.
bool mayChange(int direction) const
whether changing to the lane in the given direction should be considered
void registerUnchanged(MSVehicle *vehicle)
ChangerIt myCandi
Changer::iterator ChangerIt
the iterator moving over the ChangeElems
virtual void updateChanger(bool vehHasChanged)
bool startChangeSublane(MSVehicle *vehicle, ChangerIt &from, double latDist, double maneuverDist)
change by the specified amount and return whether a new lane was entered
bool checkChangeToNewLane(MSVehicle *vehicle, const int direction, ChangerIt from, ChangerIt to)
check whether the given vehicle has entered the new lane 'to->lane' during a sublane LC-step
void outputLCStarted(MSVehicle *vehicle, ChangerIt &from, ChangerIt &to, int direction, double maneuverDist)
optional output for start of lane-change maneuvre
MSLeaderDistanceInfo getLeaders(const ChangerIt &target, const MSVehicle *ego) const
get leaders for ego on the given lane
StateAndDist checkChangeHelper(MSVehicle *vehicle, int laneOffset, LaneChangeAction alternatives)
helper function that calls checkChangeSublane and sets blocker information
virtual void initChanger()
Initialize the changer before looping over all vehicles.
virtual void updateChanger(bool vehHasChanged)
void abortLCManeuver(MSVehicle *vehicle)
immediately stop lane-changing and register vehicle as unchanged
MSLaneChangerSublane()
Default constructor.
void outputLCEnded(MSVehicle *vehicle, ChangerIt &from, ChangerIt &to, int direction)
optional output for end of lane-change maneuvre
MSAbstractLaneChangeModel::StateAndDist StateAndDist
int checkChangeSublane(int laneOffset, LaneChangeAction alternatives, const std::vector< MSVehicle::LaneQ > &preb, double &latDist, double &maneuverDist) const
check whether sub-lane changing in the given direction is desirable and possible
bool continueChangeSublane(MSVehicle *vehicle, ChangerIt &from)
Continue a sublane-lane change maneuver and return whether the midpoint was passed in this step.
Representation of a lane in the micro simulation.
Definition: MSLane.h:82
double getLength() const
Returns the lane's length.
Definition: MSLane.h:539
double getRightSideOnEdge() const
Definition: MSLane.h:1082
int getIndex() const
Returns the lane's index.
Definition: MSLane.h:562
const PositionVector & getShape() const
Returns this lane's shape.
Definition: MSLane.h:476
double interpolateLanePosToGeometryPos(double lanePos) const
Definition: MSLane.h:497
double getWidth() const
Returns the lane's width.
Definition: MSLane.h:555
saves leader/follower vehicles and their distances relative to an ego vehicle
Definition: MSLeaderInfo.h:130
virtual std::string toString() const
print a debugging representation
virtual int addLeader(const MSVehicle *veh, double gap, double latOffset=0, int sublane=-1)
int numFreeSublanes() const
Definition: MSLeaderInfo.h:89
int numSublanes() const
Definition: MSLeaderInfo.h:85
void setSublaneChange(double latDist)
Sets a new sublane-change request.
Definition: MSVehicle.cpp:426
double myPosLat
the stored lateral position
Definition: MSVehicle.h:140
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:77
void updateBestLanes(bool forceRebuild=false, const MSLane *startLane=0)
computes the best lanes to use in order to continue the route
Definition: MSVehicle.cpp:4717
const std::vector< MSLane * > & getFurtherLanes() const
Definition: MSVehicle.h:811
bool isStoppedOnLane() const
Definition: MSVehicle.cpp:1442
bool isActive() const
Returns whether the current simulation step is an action point for the vehicle.
Definition: MSVehicle.h:603
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:4699
double getActionStepLengthSecs() const
Returns the vehicle's action step length in secs, i.e. the interval between two action points.
Definition: MSVehicle.h:502
int influenceChangeDecision(int state)
allow TraCI to influence a lane change decision
Definition: MSVehicle.cpp:6093
bool isRemoteControlled() const
Returns the information whether the vehicle is fully controlled via TraCI.
Definition: MSVehicle.cpp:6112
void adaptBestLanesOccupation(int laneIndex, double density)
update occupation from MSLaneChanger
Definition: MSVehicle.cpp:5262
void setAngle(double angle, bool straightenFurther=false)
Set a custom vehicle angle in rad, optionally updates furtherLanePosLat.
Definition: MSVehicle.cpp:1255
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
Definition: MSVehicle.cpp:5106
double getBackPositionOnLane(const MSLane *lane) const
Get the vehicle's position relative to the given lane.
Definition: MSVehicle.cpp:4046
double getLatOffset(const MSLane *lane) const
Get the offset that that must be added to interpret myState.myPosLat for the given lane.
Definition: MSVehicle.cpp:5523
SUMOTime getActionStepLength() const
Returns the vehicle's action step length in millisecs, i.e. the interval between two action points.
Definition: MSVehicle.h:494
Influencer & getInfluencer()
Definition: MSVehicle.cpp:6059
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
Definition: MSVehicle.h:411
double getSpeed() const
Returns the vehicle's current speed.
Definition: MSVehicle.h:458
Position myCachedPosition
Definition: MSVehicle.h:1893
const std::vector< LaneQ > & getBestLanes() const
Returns the description of best lanes to use in order to continue the route.
Definition: MSVehicle.cpp:4711
double getPositionOnLane() const
Get the vehicle's position along the lane.
Definition: MSVehicle.h:374
const MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:550
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
Definition: MSVehicle.h:930
double getAngle() const
Returns the vehicle's direction in radians.
Definition: MSVehicle.h:700
State myState
This Vehicles driving state (pos and speed)
Definition: MSVehicle.h:1820
double getLengthWithGap() const
Get vehicle's length including the minimum gap [m].
double getMinGap() const
Get the free space in front of vehicles of this class.
double getMaxSpeedLat() const
Get vehicle's maximum lateral speed [m/s].
double getLength() const
Get vehicle's length [m].
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:66
const std::string & getID() const
Returns the id.
Definition: Named.h:73
static const Position INVALID
used to indicate that a position is valid
Definition: Position.h:282
double length2D() const
Returns the length.
double rotationAtOffset(double pos) const
Returns the rotation at the given length.