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, bool allowSwap) :
52  MSLaneChanger(lanes, allowChanging, allowSwap) {
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  return changedToNewLane;
198 }
199 
200 
202 MSLaneChangerSublane::getLeaders(const ChangerIt& target, const MSVehicle* ego) const {
203  //if (ego->getID() == "C" && SIMTIME == 17) {
204  // std::cout << "DEBUG\n";
205  //}
206  // get the leading vehicle on the lane to change to
207  if (gDebugFlag1) {
208  std::cout << SIMTIME << " getLeaders lane=" << target->lane->getID() << " ego=" << ego->getID() << " ahead=" << target->ahead.toString() << "\n";
209  }
210  MSLeaderDistanceInfo result(target->lane, 0, 0);
211  for (int i = 0; i < target->ahead.numSublanes(); ++i) {
212  const MSVehicle* veh = target->ahead[i];
213  if (veh != 0) {
214  assert(veh != 0);
215  const SUMOReal gap = veh->getBackPositionOnLane() - ego->getPositionOnLane() - ego->getVehicleType().getMinGap();
216  if (gDebugFlag1) {
217  std::cout << " ahead lead=" << veh->getID() << " leadBack=" << veh->getBackPositionOnLane() << " gap=" << gap << "\n";
218  }
219  result.addLeader(veh, gap, 0, i);
220  }
221  }
222  // if there are vehicles on the target lane with the same position as ego,
223  // they may not have been added to 'ahead' yet
224  const MSLeaderInfo& aheadSamePos = target->lane->getLastVehicleInformation(0, 0, ego->getPositionOnLane(), false);
225  for (int i = 0; i < aheadSamePos.numSublanes(); ++i) {
226  const MSVehicle* veh = aheadSamePos[i];
227  if (veh != 0 && veh != ego) {
228  const SUMOReal gap = veh->getBackPositionOnLane(target->lane) - ego->getPositionOnLane() - ego->getVehicleType().getMinGap();
229  if (gDebugFlag1) {
230  std::cout << " further lead=" << veh->getID() << " leadBack=" << veh->getBackPositionOnLane(target->lane) << " gap=" << gap << "\n";
231  }
232  result.addLeader(veh, gap, 0, i);
233  }
234  }
235 
236  if (result.numFreeSublanes() > 0) {
237  MSLane* targetLane = target->lane;
238 
239  SUMOReal seen = ego->getLane()->getLength() - ego->getPositionOnLane();
240  SUMOReal speed = ego->getSpeed();
241  SUMOReal dist = ego->getCarFollowModel().brakeGap(speed) + ego->getVehicleType().getMinGap();
242  if (seen > dist) {
243  return result;
244  }
245  const std::vector<MSLane*>& bestLaneConts = veh(myCandi)->getBestLanesContinuation(targetLane);
246  target->lane->getLeadersOnConsecutive(dist, seen, speed, ego, bestLaneConts, result);
247  }
248  return result;
249 }
250 
251 
252 int
254  int laneOffset,
255  const std::vector<MSVehicle::LaneQ>& preb,
256  SUMOReal& latDist) const {
257 
258  ChangerIt target = myCandi + laneOffset;
259  MSVehicle* vehicle = veh(myCandi);
260  const MSLane& neighLane = *(target->lane);
261  int blocked = 0;
262 
263  //gDebugFlag1 = vehicle->getLaneChangeModel().debugVehicle();
264 
265  MSLeaderDistanceInfo neighLeaders = getLeaders(target, vehicle);
266  MSLeaderDistanceInfo neighFollowers = target->lane->getFollowersOnConsecutive(vehicle, true);
267  MSLeaderDistanceInfo neighBlockers(&neighLane, vehicle, vehicle->getLane()->getRightSideOnEdge() - neighLane.getRightSideOnEdge());
268  MSLeaderDistanceInfo leaders = getLeaders(myCandi, vehicle);
269  MSLeaderDistanceInfo followers = myCandi->lane->getFollowersOnConsecutive(vehicle, true);
270  MSLeaderDistanceInfo blockers(vehicle->getLane(), vehicle, 0);
271 
272  if (gDebugFlag1) std::cout << SIMTIME
273  << " checkChangeSublane: veh=" << vehicle->getID()
274  << " laneOffset=" << laneOffset
275  << "\n leaders=" << leaders.toString()
276  << "\n neighLeaders=" << neighLeaders.toString()
277  << "\n";
278 
279 
280  const int wish = vehicle->getLaneChangeModel().wantsChangeSublane(
281  laneOffset,
282  leaders, followers, blockers,
283  neighLeaders, neighFollowers, neighBlockers,
284  neighLane, preb,
285  &(myCandi->lastBlocked), &(myCandi->firstBlocked), latDist, blocked);
286  int state = blocked | wish;
287 
288  // XXX
289  // do are more carefull (but expensive) check to ensure that a
290  // safety-critical leader is not being overloocked
291 
292  // XXX
293  // ensure that a continuous lane change manoeuvre can be completed
294  // before the next turning movement
295 
296 #ifndef NO_TRACI
297  // let TraCI influence the wish to change lanes and the security to take
298  //const int oldstate = state;
299  state = vehicle->influenceChangeDecision(state);
300  //if (vehicle->getID() == "150_2_36000000") {
301  // std::cout << STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep()) << " veh=" << vehicle->getID() << " oldstate=" << oldstate << " newstate=" << state << "\n";
302  //}
303 #endif
304  gDebugFlag1 = false;
305  return state;
306 }
307 
308 /****************************************************************************/
309 
bool gDebugFlag1
global utility flags for debugging
Definition: StdDefs.cpp:91
bool isRemoteControlled() const
Returns the information whether the vehicle is fully controlled via TraCI.
Definition: MSVehicle.cpp:3492
StateAndDist checkChangeHelper(MSVehicle *vehicle, int laneOffset)
helper function that calls checkChangeSublane and sets blocker information
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:81
bool startChangeSublane(MSVehicle *vehicle, ChangerIt &from, SUMOReal latDist)
change by the specified amount and return whether a new lane was entered
const MSCFModel & getCarFollowModel() const
Returns the vehicle&#39;s car following model definition.
Definition: MSVehicle.h:700
int numSublanes() const
Definition: MSLeaderInfo.h:86
State myState
This Vehicles driving state (pos and speed)
Definition: MSVehicle.h:1379
bool isChangingLanes() const
return true if the vehicle currently performs a lane change maneuver
SUMOReal getLengthWithGap() const
Get vehicle&#39;s length including the minimum gap [m].
virtual bool debugVehicle() const
whether the current vehicles shall be debugged
virtual std::string toString() const
print a debugging representation
SUMOReal getLength() const
Returns the lane&#39;s length.
Definition: MSLane.h:475
bool mayChange(int direction) const
whether changing to the lane in the given direction should be considered
SUMOReal getWidth() const
Returns the lane&#39;s width.
Definition: MSLane.h:491
MSVehicle * veh(ConstChangerIt ce) const
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
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
SUMOReal getPositionOnLane() const
Get the vehicle&#39;s position along the lane.
Definition: MSVehicle.h:350
virtual void updateChanger(bool vehHasChanged)
ChangerIt findCandidate()
Find current candidate. If there is none, myChanger.end() is returned.
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:2462
#define SIMTIME
Definition: SUMOTime.h:70
std::string gDebugSelectedVehicle
Definition: StdDefs.cpp:95
SUMOReal getLateralPositionOnLane() const
Get the vehicle&#39;s lateral position on the lane.
Definition: MSVehicle.h:375
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
int numFreeSublanes() const
Definition: MSLeaderInfo.h:90
SUMOReal brakeGap(const SUMOReal speed) const
Returns the distance the vehicle needs to halt including driver&#39;s reaction time.
Definition: MSCFModel.h:234
MSLaneChangerSublane()
Default constructor.
virtual void updateChanger(bool vehHasChanged)
The action is urgent (to be defined by lc-model)
SUMOReal getMinGap() const
Get the free space in front of vehicles of this class.
virtual void updateExpectedSublaneSpeeds(const MSLeaderInfo &ahead, int sublaneOffset, int laneIndex)
update expected speeds for each sublane of the current edge
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:2808
void updateBestLanes(bool forceRebuild=false, const MSLane *startLane=0)
computes the best lanes to use in order to continue the route
Definition: MSVehicle.cpp:2480
Position myCachedPosition
Definition: MSVehicle.h:1430
MSLeaderDistanceInfo getLeaders(const ChangerIt &target, const MSVehicle *ego) const
get leaders for ego on the given lane
virtual StateAndDist decideDirection(StateAndDist sd1, StateAndDist sd2) const
decide in which direction to move in case both directions are desirable
virtual void setOwnState(int state)
virtual int addLeader(const MSVehicle *veh, SUMOReal gap, SUMOReal latOffset=0, int sublane=-1)
MSLane * getShadowLane() const
Returns the lane the vehicles shadow is on during continuous/sublane lane change. ...
std::string toString(const T &t, std::streamsize accuracy=OUTPUT_ACCURACY)
Definition: ToString.h:55
int getIndex() const
Returns the lane&#39;s index.
Definition: MSLane.h:498
bool gDebugFlag4
Definition: StdDefs.cpp:94
const std::vector< LaneQ > & getBestLanes() const
Returns the description of best lanes to use in order to continue the route.
Definition: MSVehicle.cpp:2474
~MSLaneChangerSublane()
Destructor.
bool startLaneChangeManeuver(MSLane *source, MSLane *target, int direction)
start the lane change maneuver and return whether it continues
LaneChangeAction
The state of a vehicle&#39;s lane-change behavior.
const MSVehicleType & getVehicleType() const
Returns the vehicle&#39;s type definition.
Definition: MSBaseVehicle.h:97
Changer::iterator ChangerIt
the iterator moving over the ChangeElems
SUMOReal getSpeed() const
Returns the vehicle&#39;s current speed.
Definition: MSVehicle.h:410
SUMOReal getBackPositionOnLane(const MSLane *lane) const
Get the vehicle&#39;s position relative to the given lane.
Definition: MSVehicle.cpp:1962
MSAbstractLaneChangeModel::StateAndDist StateAndDist
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:139
#define SUMOReal
Definition: config.h:213
void adaptBestLanesOccupation(int laneIndex, SUMOReal density)
update occupation from MSLaneChanger
Definition: MSVehicle.cpp:2846
MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:447
int influenceChangeDecision(int state)
allow TraCI to influence a lane change decision
Definition: MSVehicle.cpp:3472
Representation of a lane in the micro simulation.
Definition: MSLane.h:79
ChangerIt myCandi
static const Position INVALID
Definition: Position.h:261
SUMOReal getRightSideOnEdge() const
Definition: MSLane.h:886
const std::string & getID() const
Returns the name of the vehicle.