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