SUMO - Simulation of Urban MObility
MSLaneChangerSublane.cpp
Go to the documentation of this file.
1 /****************************************************************************/
7 // Performs sub-lane changing of vehicles
8 /****************************************************************************/
9 // SUMO, Simulation of Urban MObility; see http://sumo.dlr.de/
10 // Copyright (C) 2002-2016 DLR (http://www.dlr.de/) and contributors
11 /****************************************************************************/
12 //
13 // This file is part of SUMO.
14 // SUMO is free software: you can redistribute it and/or modify
15 // it under the terms of the GNU General Public License as published by
16 // the Free Software Foundation, either version 3 of the License, or
17 // (at your option) any later version.
18 //
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 
43 #ifdef CHECK_MEMORY_LEAKS
44 #include <foreign/nvwa/debug_new.h>
45 #endif // CHECK_MEMORY_LEAKS
46 
47 
48 // ===========================================================================
49 // member method definitions
50 // ===========================================================================
51 MSLaneChangerSublane::MSLaneChangerSublane(const std::vector<MSLane*>* lanes, bool allowChanging) :
52  MSLaneChanger(lanes, allowChanging) {
53 }
54 
55 
57 
58 
59 void
61  MSLaneChanger::updateChanger(vehHasChanged);
62  if (!vehHasChanged) {
63  MSVehicle* lead = myCandi->lead;
64  //std::cout << SIMTIME << " updateChanger lane=" << myCandi->lane->getID() << " lead=" << Named::getIDSecure(lead) << "\n";
65  myCandi->ahead.addLeader(lead, false, 0);
66  MSLane* shadowLane = lead->getLaneChangeModel().getShadowLane();
67  if (shadowLane != 0) {
68  const SUMOReal latOffset = lead->getLane()->getRightSideOnEdge() - shadowLane->getRightSideOnEdge();
69  //std::cout << SIMTIME << " updateChanger shadowLane=" << shadowLane->getID() << " lead=" << Named::getIDSecure(lead) << "\n";
70  (myChanger.begin() + shadowLane->getIndex())->ahead.addLeader(lead, false, latOffset);
71  }
72  }
73  //std::cout << SIMTIME << " updateChanger: lane=" << myCandi->lane->getID() << " lead=" << Named::getIDSecure(myCandi->lead) << " ahead=" << myCandi->ahead.toString() << " vehHasChanged=" << vehHasChanged << "\n";
74  //for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
75  // std::cout << " lane=" << ce->lane->getID() << " vehicles=" << toString(ce->lane->myVehicles) << "\n";
76  //}
77 }
78 
79 
80 bool
82  // variant of change() for the sublane case
84  MSVehicle* vehicle = veh(myCandi);
85 #ifdef DEBUG_VEHICLE_GUI_SELECTION
86  if (gDebugSelectedVehicle == vehicle->getID()) {
87  int bla = 0;
88  }
89 #endif
90  assert(vehicle->getLane() == (*myCandi).lane);
91  assert(!vehicle->getLaneChangeModel().isChangingLanes());
92 #ifndef NO_TRACI
93  if (vehicle->isRemoteControlled()) {
94  return false; // !!! temporary; just because it broke, here
95  }
96 #endif
97  vehicle->updateBestLanes(); // needed?
98  for (int i = 0; i < (int) myChanger.size(); ++i) {
99  vehicle->adaptBestLanesOccupation(i, myChanger[i].dens);
100  }
101 
102  // update expected speeds
103  int sublaneIndex = 0;
104  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
105  vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(ce->ahead, sublaneIndex, ce->lane->getIndex());
106  sublaneIndex += ce->ahead.numSublanes();
107  }
108 
109  StateAndDist right = checkChangeHelper(vehicle, -1);
110  StateAndDist left = checkChangeHelper(vehicle, 1);
111  StateAndDist current = checkChangeHelper(vehicle, 0);
112 
113  StateAndDist decision = vehicle->getLaneChangeModel().decideDirection(current,
114  vehicle->getLaneChangeModel().decideDirection(right, left));
115  if ((decision.state & LCA_WANTS_LANECHANGE) != 0 && (decision.state & LCA_BLOCKED) == 0) {
116  // change if the vehicle wants to and is allowed to change
117  if (vehicle->getLaneChangeModel().debugVehicle()) {
118  std::cout << SIMTIME << " decision=" << toString((LaneChangeAction)decision.state) << " latDist=" << decision.latDist << "\n";
119  }
120  vehicle->getLaneChangeModel().setOwnState(decision.state);
121  return startChangeSublane(vehicle, myCandi, decision.latDist);
122  }
123 
124  if ((right.state & (LCA_URGENT)) != 0 && (left.state & (LCA_URGENT)) != 0) {
125  // ... wants to go to the left AND to the right
126  // just let them go to the right lane...
127  left.state = 0;
128  }
129  vehicle->getLaneChangeModel().setOwnState(right.state | left.state | current.state);
130 
131  registerUnchanged(vehicle);
132  return false;
133 }
134 
135 
138  StateAndDist result = StateAndDist(0, 0, 0);
139  if (mayChange(laneOffset)) {
140  const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes();
141  result.state = checkChangeSublane(laneOffset, preb, result.latDist);
142  result.dir = laneOffset;
143  if ((result.state & LCA_WANTS_LANECHANGE) != 0 && (result.state & LCA_URGENT) != 0 && (result.state & LCA_BLOCKED) != 0) {
144  (myCandi + laneOffset)->lastBlocked = vehicle;
145  if ((myCandi + laneOffset)->firstBlocked == 0) {
146  (myCandi + laneOffset)->firstBlocked = vehicle;
147  }
148  }
149  }
150  return result;
151 }
152 
153 
154 bool
156  //gDebugFlag4 = vehicle->getID() == "Togliatti_80_26";
157  // 1) update vehicles lateral position according to latDist and target lane
158  vehicle->myState.myPosLat += latDist;
160 
161  // 2) distinguish several cases
162  // a) vehicle moves completely within the same lane
163  // b) vehicle intersects another lane
164  // - vehicle must be moved to the lane where it's midpoint is (either old or new)
165  // - shadow vehicle must be created/moved to the other lane if the vehicle intersects it
166  // 3) updated dens of all lanes that hold the vehicle or its shadow
167  const int direction = vehicle->getLateralPositionOnLane() < 0 ? -1 : 1;
168  ChangerIt to = from;
169  if (mayChange(direction)) {
170  to = from + direction;
171  } else {
173  }
174  const bool changedToNewLane = to != from && fabs(vehicle->getLateralPositionOnLane()) > 0.5 * vehicle->getLane()->getWidth() && mayChange(direction);
175  if (changedToNewLane) {
176  vehicle->myState.myPosLat -= direction * 0.5 * (from->lane->getWidth() + to->lane->getWidth());
177  to->lane->myTmpVehicles.insert(to->lane->myTmpVehicles.begin(), vehicle);
178  to->dens += vehicle->getVehicleType().getLengthWithGap();
179  vehicle->getLaneChangeModel().startLaneChangeManeuver(from->lane, to->lane, direction);
180  to->ahead.addLeader(vehicle, false, 0);
181  } else {
182  registerUnchanged(vehicle);
183  from->ahead.addLeader(vehicle, false, 0);
184  }
185 
186  MSLane* oldShadowLane = vehicle->getLaneChangeModel().getShadowLane();
188  MSLane* shadowLane = vehicle->getLaneChangeModel().getShadowLane();
189  if (shadowLane != 0 && shadowLane != oldShadowLane) {
190  assert(to != from);
191  const SUMOReal latOffset = vehicle->getLane()->getRightSideOnEdge() - shadowLane->getRightSideOnEdge();
192  (myChanger.begin() + shadowLane->getIndex())->ahead.addLeader(vehicle, false, latOffset);
193  }
194  if (gDebugFlag4) std::cout << SIMTIME << " startChangeSublane shadowLane"
195  << " old=" << Named::getIDSecure(oldShadowLane)
196  << " new=" << Named::getIDSecure(vehicle->getLaneChangeModel().getShadowLane()) << "\n";
197 
198  // compute new angle of the vehicle from the x- and y-distances travelled within last time step
199  // (should happen last because primaryLaneChanged() also triggers angle computation)
200  // this part of the angle comes from the orientation of our current lane
201  SUMOReal laneAngle = vehicle->getLane()->getShape().rotationAtOffset(vehicle->getLane()->interpolateLanePosToGeometryPos(vehicle->getPositionOnLane())) ;
202  // this part of the angle comes from the vehicle's lateral movement
203  SUMOReal changeAngle = atan2(latDist, SPEED2DIST(vehicle->getSpeed()));
204  vehicle->setAngle(laneAngle + changeAngle);
205 
206  return changedToNewLane;
207 }
208 
209 
211 MSLaneChangerSublane::getLeaders(const ChangerIt& target, const MSVehicle* ego) const {
212  //if (ego->getID() == "C" && SIMTIME == 17) {
213  // std::cout << "DEBUG\n";
214  //}
215  // get the leading vehicle on the lane to change to
216  if (gDebugFlag1) {
217  std::cout << SIMTIME << " getLeaders lane=" << target->lane->getID() << " ego=" << ego->getID() << " ahead=" << target->ahead.toString() << "\n";
218  }
219  MSLeaderDistanceInfo result(target->lane, 0, 0);
220  for (int i = 0; i < target->ahead.numSublanes(); ++i) {
221  const MSVehicle* veh = target->ahead[i];
222  if (veh != 0) {
223  assert(veh != 0);
224  const SUMOReal gap = veh->getBackPositionOnLane() - ego->getPositionOnLane() - ego->getVehicleType().getMinGap();
225  if (gDebugFlag1) {
226  std::cout << " ahead lead=" << veh->getID() << " leadBack=" << veh->getBackPositionOnLane() << " gap=" << gap << "\n";
227  }
228  result.addLeader(veh, gap, 0, i);
229  }
230  }
231  // if there are vehicles on the target lane with the same position as ego,
232  // they may not have been added to 'ahead' yet
233  const MSLeaderInfo& aheadSamePos = target->lane->getLastVehicleInformation(0, 0, ego->getPositionOnLane(), false);
234  for (int i = 0; i < aheadSamePos.numSublanes(); ++i) {
235  const MSVehicle* veh = aheadSamePos[i];
236  if (veh != 0 && veh != ego) {
237  const SUMOReal gap = veh->getBackPositionOnLane(target->lane) - ego->getPositionOnLane() - ego->getVehicleType().getMinGap();
238  if (gDebugFlag1) {
239  std::cout << " further lead=" << veh->getID() << " leadBack=" << veh->getBackPositionOnLane(target->lane) << " gap=" << gap << "\n";
240  }
241  result.addLeader(veh, gap, 0, i);
242  }
243  }
244 
245  if (result.numFreeSublanes() > 0) {
246  MSLane* targetLane = target->lane;
247 
248  SUMOReal seen = ego->getLane()->getLength() - ego->getPositionOnLane();
249  SUMOReal speed = ego->getSpeed();
250  SUMOReal dist = ego->getCarFollowModel().brakeGap(speed) + ego->getVehicleType().getMinGap();
251  if (seen > dist) {
252  return result;
253  }
254  const std::vector<MSLane*>& bestLaneConts = veh(myCandi)->getBestLanesContinuation(targetLane);
255  target->lane->getLeadersOnConsecutive(dist, seen, speed, ego, bestLaneConts, result);
256  }
257  return result;
258 }
259 
260 
261 int
263  int laneOffset,
264  const std::vector<MSVehicle::LaneQ>& preb,
265  SUMOReal& latDist) const {
266 
267  ChangerIt target = myCandi + laneOffset;
268  MSVehicle* vehicle = veh(myCandi);
269  const MSLane& neighLane = *(target->lane);
270  int blocked = 0;
271 
272  //gDebugFlag1 = vehicle->getLaneChangeModel().debugVehicle();
273 
274  MSLeaderDistanceInfo neighLeaders = getLeaders(target, vehicle);
275  MSLeaderDistanceInfo neighFollowers = target->lane->getFollowersOnConsecutive(vehicle, true);
276  MSLeaderDistanceInfo neighBlockers(&neighLane, vehicle, vehicle->getLane()->getRightSideOnEdge() - neighLane.getRightSideOnEdge());
277  MSLeaderDistanceInfo leaders = getLeaders(myCandi, vehicle);
278  MSLeaderDistanceInfo followers = myCandi->lane->getFollowersOnConsecutive(vehicle, true);
279  MSLeaderDistanceInfo blockers(vehicle->getLane(), vehicle, 0);
280 
281  if (gDebugFlag1) std::cout << SIMTIME
282  << " checkChangeSublane: veh=" << vehicle->getID()
283  << " laneOffset=" << laneOffset
284  << "\n leaders=" << leaders.toString()
285  << "\n neighLeaders=" << neighLeaders.toString()
286  << "\n";
287 
288 
289  const int wish = vehicle->getLaneChangeModel().wantsChangeSublane(
290  laneOffset,
291  leaders, followers, blockers,
292  neighLeaders, neighFollowers, neighBlockers,
293  neighLane, preb,
294  &(myCandi->lastBlocked), &(myCandi->firstBlocked), latDist, blocked);
295  int state = blocked | wish;
296 
297  // XXX
298  // do are more carefull (but expensive) check to ensure that a
299  // safety-critical leader is not being overloocked
300 
301  // XXX
302  // ensure that a continuous lane change manoeuvre can be completed
303  // before the next turning movement
304 
305 #ifndef NO_TRACI
306  // let TraCI influence the wish to change lanes and the security to take
307  //const int oldstate = state;
308  state = vehicle->influenceChangeDecision(state);
309  //if (vehicle->getID() == "150_2_36000000") {
310  // std::cout << STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep()) << " veh=" << vehicle->getID() << " oldstate=" << oldstate << " newstate=" << state << "\n";
311  //}
312 #endif
313  gDebugFlag1 = false;
314  return state;
315 }
316 
317 /****************************************************************************/
318 
bool gDebugFlag1
global utility flags for debugging
Definition: StdDefs.cpp:91
SUMOReal getWidth() const
Returns the lane&#39;s width.
Definition: MSLane.h:496
void setAngle(SUMOReal angle)
Set a custom vehicle angle in rad.
Definition: MSVehicle.cpp:817
bool isChangingLanes() const
return true if the vehicle currently performs a lane change maneuver
StateAndDist checkChangeHelper(MSVehicle *vehicle, int laneOffset)
helper function that calls checkChangeSublane and sets blocker information
SUMOReal getLength() const
Returns the lane&#39;s length.
Definition: MSLane.h:480
saves leader/follower vehicles and their distances relative to an ego vehicle
Definition: MSLeaderInfo.h:127
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:82
bool isRemoteControlled() const
Returns the information whether the vehicle is fully controlled via TraCI.
Definition: MSVehicle.cpp:3734
bool startChangeSublane(MSVehicle *vehicle, ChangerIt &from, SUMOReal latDist)
change by the specified amount and return whether a new lane was entered
#define SPEED2DIST(x)
Definition: SUMOTime.h:55
State myState
This Vehicles driving state (pos and speed)
Definition: MSVehicle.h:1437
virtual std::string toString() const
print a debugging representation
MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:487
int checkChangeSublane(int laneOffset, const std::vector< MSVehicle::LaneQ > &preb, SUMOReal &latDist) const
check whether sub-lane changing in the given direction is desirable and possible
MSVehicle * veh(ConstChangerIt ce) const
const PositionVector & getShape() const
Returns this lane&#39;s shape.
Definition: MSLane.h:427
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
Definition: Named.h:59
SUMOReal interpolateLanePosToGeometryPos(SUMOReal lanePos) const
Definition: MSLane.h:438
virtual StateAndDist decideDirection(StateAndDist sd1, StateAndDist sd2) const
decide in which direction to move in case both directions are desirable
virtual void updateChanger(bool vehHasChanged)
ChangerIt findCandidate()
Find current candidate. If there is none, myChanger.end() is returned.
SUMOReal rotationAtOffset(SUMOReal pos) const
Returns the rotation at the given length.
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:2702
#define SIMTIME
Definition: SUMOTime.h:70
std::string gDebugSelectedVehicle
Definition: StdDefs.cpp:95
virtual int wantsChangeSublane(int laneOffset, 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, SUMOReal &latDist, int &blocked)
Performs lane changing of vehicles.
Definition: MSLaneChanger.h:55
const MSCFModel & getCarFollowModel() const
Returns the vehicle&#39;s car following model definition.
Definition: MSVehicle.h:758
int getIndex() const
Returns the lane&#39;s index.
Definition: MSLane.h:503
MSLaneChangerSublane()
Default constructor.
virtual void updateChanger(bool vehHasChanged)
The action is urgent (to be defined by lc-model)
virtual void updateExpectedSublaneSpeeds(const MSLeaderInfo &ahead, int sublaneOffset, int laneIndex)
update expected speeds for each sublane of the current edge
void updateBestLanes(bool forceRebuild=false, const MSLane *startLane=0)
computes the best lanes to use in order to continue the route
Definition: MSVehicle.cpp:2720
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the subpart of best lanes that describes the vehicle&#39;s current lane and their successors...
Definition: MSVehicle.cpp:3048
Position myCachedPosition
Definition: MSVehicle.h:1488
SUMOReal getPositionOnLane() const
Get the vehicle&#39;s position along the lane.
Definition: MSVehicle.h:374
virtual void setOwnState(int state)
virtual int addLeader(const MSVehicle *veh, SUMOReal gap, SUMOReal latOffset=0, int sublane=-1)
std::string toString(const T &t, std::streamsize accuracy=OUTPUT_ACCURACY)
Definition: ToString.h:55
virtual bool debugVehicle() const
whether the current vehicles shall be debugged
bool gDebugFlag4
Definition: StdDefs.cpp:94
~MSLaneChangerSublane()
Destructor.
SUMOReal brakeGap(const SUMOReal speed) const
Returns the distance the vehicle needs to halt including driver&#39;s reaction time, assuming that during...
Definition: MSCFModel.h:263
bool startLaneChangeManeuver(MSLane *source, MSLane *target, int direction)
start the lane change maneuver and return whether it continues
SUMOReal getLateralPositionOnLane() const
Get the vehicle&#39;s lateral position on the lane.
Definition: MSVehicle.h:406
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.
const MSVehicleType & getVehicleType() const
Returns the vehicle&#39;s type definition.
Changer::iterator ChangerIt
the iterator moving over the ChangeElems
MSAbstractLaneChangeModel::StateAndDist StateAndDist
SUMOReal getMinGap() const
Get the free space in front of vehicles of this class.
Changer myChanger
Container for ChangeElemements, one for every lane in the edge.
void registerUnchanged(MSVehicle *vehicle)
SUMOReal myPosLat
the stored lateral position
Definition: MSVehicle.h:145
#define SUMOReal
Definition: config.h:213
void adaptBestLanesOccupation(int laneIndex, SUMOReal density)
update occupation from MSLaneChanger
Definition: MSVehicle.cpp:3087
int numSublanes() const
Definition: MSLeaderInfo.h:86
MSLane * getShadowLane() const
Returns the lane the vehicles shadow is on during continuous/sublane lane change. ...
int numFreeSublanes() const
Definition: MSLeaderInfo.h:90
const std::vector< LaneQ > & getBestLanes() const
Returns the description of best lanes to use in order to continue the route.
Definition: MSVehicle.cpp:2714
int influenceChangeDecision(int state)
allow TraCI to influence a lane change decision
Definition: MSVehicle.cpp:3714
SUMOReal getRightSideOnEdge() const
Definition: MSLane.h:897
const std::string & getID() const
Returns the name of the vehicle.
Representation of a lane in the micro simulation.
Definition: MSLane.h:79
SUMOReal getBackPositionOnLane(const MSLane *lane) const
Get the vehicle&#39;s position relative to the given lane.
Definition: MSVehicle.cpp:2198
SUMOReal getSpeed() const
Returns the vehicle&#39;s current speed.
Definition: MSVehicle.h:441
bool mayChange(int direction) const
whether changing to the lane in the given direction should be considered
SUMOReal getLengthWithGap() const
Get vehicle&#39;s length including the minimum gap [m].
ChangerIt myCandi
static const Position INVALID
Definition: Position.h:261