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-2017 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 #include <utils/geom/GeomHelper.h>
43 
44 
45 // ===========================================================================
46 // member method definitions
47 // ===========================================================================
48 MSLaneChangerSublane::MSLaneChangerSublane(const std::vector<MSLane*>* lanes, bool allowChanging) :
49  MSLaneChanger(lanes, allowChanging) {
50 }
51 
52 
54 
55 
56 void
58  MSLaneChanger::updateChanger(vehHasChanged);
59  if (!vehHasChanged) {
60  MSVehicle* lead = myCandi->lead;
61  //std::cout << SIMTIME << " updateChanger lane=" << myCandi->lane->getID() << " lead=" << Named::getIDSecure(lead) << "\n";
62  myCandi->ahead.addLeader(lead, false, 0);
63  MSLane* shadowLane = lead->getLaneChangeModel().getShadowLane();
64  if (shadowLane != 0) {
65  const double latOffset = lead->getLane()->getRightSideOnEdge() - shadowLane->getRightSideOnEdge();
66  //std::cout << SIMTIME << " updateChanger shadowLane=" << shadowLane->getID() << " lead=" << Named::getIDSecure(lead) << "\n";
67  (myChanger.begin() + shadowLane->getIndex())->ahead.addLeader(lead, false, latOffset);
68  }
69  }
70  //std::cout << SIMTIME << " updateChanger: lane=" << myCandi->lane->getID() << " lead=" << Named::getIDSecure(myCandi->lead) << " ahead=" << myCandi->ahead.toString() << " vehHasChanged=" << vehHasChanged << "\n";
71  //for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
72  // std::cout << " lane=" << ce->lane->getID() << " vehicles=" << toString(ce->lane->myVehicles) << "\n";
73  //}
74 }
75 
76 
77 bool
79  // variant of change() for the sublane case
81  MSVehicle* vehicle = veh(myCandi);
82 #ifdef DEBUG_VEHICLE_GUI_SELECTION
83  if (gDebugSelectedVehicle == vehicle->getID()) {
84  int bla = 0;
85  }
86 #endif
87  assert(vehicle->getLane() == (*myCandi).lane);
88  assert(!vehicle->getLaneChangeModel().isChangingLanes());
89 #ifndef NO_TRACI
90  if (vehicle->isRemoteControlled()) {
91  return false; // !!! temporary; just because it broke, here
92  }
93 #endif
94  vehicle->updateBestLanes(); // needed?
95  for (int i = 0; i < (int) myChanger.size(); ++i) {
96  vehicle->adaptBestLanesOccupation(i, myChanger[i].dens);
97  }
98 
99  // update expected speeds
100  int sublaneIndex = 0;
101  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
102  vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(ce->ahead, sublaneIndex, ce->lane->getIndex());
103  sublaneIndex += ce->ahead.numSublanes();
104  }
105 
106  StateAndDist right = checkChangeHelper(vehicle, -1);
107  StateAndDist left = checkChangeHelper(vehicle, 1);
108  StateAndDist current = checkChangeHelper(vehicle, 0);
109 
110  StateAndDist decision = vehicle->getLaneChangeModel().decideDirection(current,
111  vehicle->getLaneChangeModel().decideDirection(right, left));
112  if ((decision.state & LCA_WANTS_LANECHANGE) != 0 && (decision.state & LCA_BLOCKED) == 0) {
113  // change if the vehicle wants to and is allowed to change
114  if (vehicle->getLaneChangeModel().debugVehicle()) {
115  std::cout << SIMTIME << " decision=" << toString((LaneChangeAction)decision.state) << " latDist=" << decision.latDist << "\n";
116  }
117  vehicle->getLaneChangeModel().setOwnState(decision.state);
118  return startChangeSublane(vehicle, myCandi, decision.latDist);
119  }
120 
121  if ((right.state & (LCA_URGENT)) != 0 && (left.state & (LCA_URGENT)) != 0) {
122  // ... wants to go to the left AND to the right
123  // just let them go to the right lane...
124  left.state = 0;
125  }
126  vehicle->getLaneChangeModel().setOwnState(right.state | left.state | current.state);
127 
128  registerUnchanged(vehicle);
129  return false;
130 }
131 
132 
135  StateAndDist result = StateAndDist(0, 0, 0);
136  if (mayChange(laneOffset)) {
137  const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes();
138  result.state = checkChangeSublane(laneOffset, preb, result.latDist);
139  result.dir = laneOffset;
140  if ((result.state & LCA_WANTS_LANECHANGE) != 0 && (result.state & LCA_URGENT) != 0 && (result.state & LCA_BLOCKED) != 0) {
141  (myCandi + laneOffset)->lastBlocked = vehicle;
142  if ((myCandi + laneOffset)->firstBlocked == 0) {
143  (myCandi + laneOffset)->firstBlocked = vehicle;
144  }
145  }
146  }
147  return result;
148 }
149 
150 
151 bool
153  // 1) update vehicles lateral position according to latDist and target lane
154  vehicle->myState.myPosLat += latDist;
156 
157  // 2) distinguish several cases
158  // a) vehicle moves completely within the same lane
159  // b) vehicle intersects another lane
160  // - vehicle must be moved to the lane where it's midpoint is (either old or new)
161  // - shadow vehicle must be created/moved to the other lane if the vehicle intersects it
162  // 3) updated dens of all lanes that hold the vehicle or its shadow
163  const int direction = vehicle->getLateralPositionOnLane() < 0 ? -1 : 1;
164  ChangerIt to = from;
165  if (mayChange(direction)) {
166  to = from + direction;
167  } else {
169  }
170  const bool changedToNewLane = to != from && fabs(vehicle->getLateralPositionOnLane()) > 0.5 * vehicle->getLane()->getWidth() && mayChange(direction);
171  if (changedToNewLane) {
172  vehicle->myState.myPosLat -= direction * 0.5 * (from->lane->getWidth() + to->lane->getWidth());
173  to->lane->myTmpVehicles.insert(to->lane->myTmpVehicles.begin(), vehicle);
174  to->dens += vehicle->getVehicleType().getLengthWithGap();
175  vehicle->getLaneChangeModel().startLaneChangeManeuver(from->lane, to->lane, direction);
176  to->ahead.addLeader(vehicle, false, 0);
177  } else {
178  registerUnchanged(vehicle);
179  from->ahead.addLeader(vehicle, false, 0);
180  }
181 
182  MSLane* oldShadowLane = vehicle->getLaneChangeModel().getShadowLane();
184  MSLane* shadowLane = vehicle->getLaneChangeModel().getShadowLane();
185  if (shadowLane != 0 && shadowLane != oldShadowLane) {
186  assert(to != from);
187  const double latOffset = vehicle->getLane()->getRightSideOnEdge() - shadowLane->getRightSideOnEdge();
188  (myChanger.begin() + shadowLane->getIndex())->ahead.addLeader(vehicle, false, latOffset);
189  }
190 
191 
192  // compute new angle of the vehicle from the x- and y-distances travelled within last time step
193  // (should happen last because primaryLaneChanged() also triggers angle computation)
194  // this part of the angle comes from the orientation of our current lane
195  double laneAngle = vehicle->getLane()->getShape().rotationAtOffset(vehicle->getLane()->interpolateLanePosToGeometryPos(vehicle->getPositionOnLane())) ;
196  // this part of the angle comes from the vehicle's lateral movement
197  double changeAngle = 0;
198  // avoid flicker
199  if (fabs(latDist) > NUMERICAL_EPS) {
200  // avoid extreme angles by using vehicle length as a proxy for turning radius
201  changeAngle = atan2(latDist, SPEED2DIST(MAX2(vehicle->getVehicleType().getLength(), vehicle->getSpeed())));
202  }
203  if (vehicle->getLaneChangeModel().debugVehicle()) std::cout << SIMTIME << " startChangeSublane shadowLane"
204  << " latDist=" << latDist
205  << " old=" << Named::getIDSecure(oldShadowLane)
206  << " new=" << Named::getIDSecure(vehicle->getLaneChangeModel().getShadowLane())
207  << " laneA=" << RAD2DEG(laneAngle)
208  << " changeA=" << RAD2DEG(changeAngle)
209  << " oldA=" << RAD2DEG(vehicle->getAngle())
210  << " newA=" << RAD2DEG(laneAngle + changeAngle)
211  << "\n";
212  vehicle->setAngle(laneAngle + changeAngle);
213 
214  return changedToNewLane;
215 }
216 
217 
219 MSLaneChangerSublane::getLeaders(const ChangerIt& target, const MSVehicle* ego) const {
220  //if (ego->getID() == "C" && SIMTIME == 17) {
221  // std::cout << "DEBUG\n";
222  //}
223  // get the leading vehicle on the lane to change to
224  if (gDebugFlag1) {
225  std::cout << SIMTIME << " getLeaders lane=" << target->lane->getID() << " ego=" << ego->getID() << " ahead=" << target->ahead.toString() << "\n";
226  }
227  MSLeaderDistanceInfo result(target->lane, 0, 0);
228  for (int i = 0; i < target->ahead.numSublanes(); ++i) {
229  const MSVehicle* veh = target->ahead[i];
230  if (veh != 0) {
231  assert(veh != 0);
232  const double gap = veh->getBackPositionOnLane() - ego->getPositionOnLane() - ego->getVehicleType().getMinGap();
233  if (gDebugFlag1) {
234  std::cout << " ahead lead=" << veh->getID() << " leadBack=" << veh->getBackPositionOnLane() << " gap=" << gap << "\n";
235  }
236  result.addLeader(veh, gap, 0, i);
237  }
238  }
239  // if there are vehicles on the target lane with the same position as ego,
240  // they may not have been added to 'ahead' yet
241  const MSLeaderInfo& aheadSamePos = target->lane->getLastVehicleInformation(0, 0, ego->getPositionOnLane(), false);
242  for (int i = 0; i < aheadSamePos.numSublanes(); ++i) {
243  const MSVehicle* veh = aheadSamePos[i];
244  if (veh != 0 && veh != ego) {
245  const double gap = veh->getBackPositionOnLane(target->lane) - ego->getPositionOnLane() - ego->getVehicleType().getMinGap();
246  if (gDebugFlag1) {
247  std::cout << " further lead=" << veh->getID() << " leadBack=" << veh->getBackPositionOnLane(target->lane) << " gap=" << gap << "\n";
248  }
249  result.addLeader(veh, gap, 0, i);
250  }
251  }
252 
253  if (result.numFreeSublanes() > 0) {
254  MSLane* targetLane = target->lane;
255 
256  double seen = ego->getLane()->getLength() - ego->getPositionOnLane();
257  double speed = ego->getSpeed();
258  double dist = ego->getCarFollowModel().brakeGap(speed) + ego->getVehicleType().getMinGap();
259  if (seen > dist) {
260  return result;
261  }
262  const std::vector<MSLane*>& bestLaneConts = veh(myCandi)->getBestLanesContinuation(targetLane);
263  target->lane->getLeadersOnConsecutive(dist, seen, speed, ego, bestLaneConts, result);
264  }
265  return result;
266 }
267 
268 
269 int
271  int laneOffset,
272  const std::vector<MSVehicle::LaneQ>& preb,
273  double& latDist) const {
274 
275  ChangerIt target = myCandi + laneOffset;
276  MSVehicle* vehicle = veh(myCandi);
277  const MSLane& neighLane = *(target->lane);
278  int blocked = 0;
279 
280  //gDebugFlag1 = vehicle->getLaneChangeModel().debugVehicle();
281 
282  MSLeaderDistanceInfo neighLeaders = getLeaders(target, vehicle);
283  MSLeaderDistanceInfo neighFollowers = target->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
284  MSLeaderDistanceInfo neighBlockers(&neighLane, vehicle, vehicle->getLane()->getRightSideOnEdge() - neighLane.getRightSideOnEdge());
285  MSLeaderDistanceInfo leaders = getLeaders(myCandi, vehicle);
286  MSLeaderDistanceInfo followers = myCandi->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
287  MSLeaderDistanceInfo blockers(vehicle->getLane(), vehicle, 0);
288 
289  if (gDebugFlag1) std::cout << SIMTIME
290  << " checkChangeSublane: veh=" << vehicle->getID()
291  << " laneOffset=" << laneOffset
292  << "\n leaders=" << leaders.toString()
293  << "\n neighLeaders=" << neighLeaders.toString()
294  << "\n";
295 
296 
297  const int wish = vehicle->getLaneChangeModel().wantsChangeSublane(
298  laneOffset,
299  leaders, followers, blockers,
300  neighLeaders, neighFollowers, neighBlockers,
301  neighLane, preb,
302  &(myCandi->lastBlocked), &(myCandi->firstBlocked), latDist, blocked);
303  int state = blocked | wish;
304 
305  // XXX
306  // do are more carefull (but expensive) check to ensure that a
307  // safety-critical leader is not being overloocked
308 
309  // XXX
310  // ensure that a continuous lane change manoeuvre can be completed
311  // before the next turning movement
312 
313  const int oldstate = state;
314 #ifndef NO_TRACI
315  // let TraCI influence the wish to change lanes and the security to take
316  state = vehicle->influenceChangeDecision(state);
317  //if (vehicle->getID() == "150_2_36000000") {
318  // std::cout << STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep()) << " veh=" << vehicle->getID() << " oldstate=" << oldstate << " newstate=" << state << "\n";
319  //}
320 #endif
321  vehicle->getLaneChangeModel().saveState(laneOffset, oldstate, state);
322  gDebugFlag1 = false;
323  return state;
324 }
325 
326 /****************************************************************************/
327 
bool gDebugFlag1
global utility flags for debugging
Definition: StdDefs.cpp:33
void setAngle(double angle)
Set a custom vehicle angle in rad.
Definition: MSVehicle.cpp:822
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].
StateAndDist checkChangeHelper(MSVehicle *vehicle, int laneOffset)
helper function that calls checkChangeSublane and sets blocker information
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver&#39;s reaction time, assuming that during...
Definition: MSCFModel.h:263
saves leader/follower vehicles and their distances relative to an ego vehicle
Definition: MSLeaderInfo.h:136
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:4037
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, double &latDist, int &blocked)
double getAngle() const
Returns the vehicle&#39;s direction in radians.
Definition: MSVehicle.h:594
#define SPEED2DIST(x)
Definition: SUMOTime.h:55
State myState
This Vehicles driving state (pos and speed)
Definition: MSVehicle.h:1506
virtual std::string toString() const
print a debugging representation
MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:488
double getPositionOnLane() const
Get the vehicle&#39;s position along the lane.
Definition: MSVehicle.h:375
double myPosLat
the stored lateral position
Definition: MSVehicle.h:146
MSVehicle * veh(ConstChangerIt ce) const
T MAX2(T a, T b)
Definition: StdDefs.h:70
double getLength() const
Returns the lane&#39;s length.
Definition: MSLane.h:484
const PositionVector & getShape() const
Returns this lane&#39;s shape.
Definition: MSLane.h:426
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
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:3315
#define RAD2DEG(x)
Definition: GeomHelper.h:46
double getWidth() const
Returns the lane&#39;s width.
Definition: MSLane.h:500
virtual void updateChanger(bool vehHasChanged)
ChangerIt findCandidate()
Find current candidate. If there is none, myChanger.end() is returned.
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:2921
#define SIMTIME
Definition: SUMOTime.h:70
std::string gDebugSelectedVehicle
Definition: StdDefs.cpp:37
void saveState(const int dir, const int stateWithoutTraCI, const int state)
Performs lane changing of vehicles.
Definition: MSLaneChanger.h:55
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:796
int getIndex() const
Returns the lane&#39;s index.
Definition: MSLane.h:507
MSLaneChangerSublane()
Default constructor.
virtual void updateChanger(bool vehHasChanged)
blocked in all directions
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:56
The action is urgent (to be defined by lc-model)
int checkChangeSublane(int laneOffset, const std::vector< MSVehicle::LaneQ > &preb, double &latDist) const
check whether sub-lane changing in the given direction is desirable and possible
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:2939
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
Definition: MSVehicle.cpp:3276
Position myCachedPosition
Definition: MSVehicle.h:1570
bool startChangeSublane(MSVehicle *vehicle, ChangerIt &from, double latDist)
change by the specified amount and return whether a new lane was entered
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:407
double getRightSideOnEdge() const
Definition: MSLane.h:924
bool startLaneChangeManeuver(MSLane *source, MSLane *target, int direction)
start the lane change maneuver and return whether it continues
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.
double interpolateLanePosToGeometryPos(double lanePos) const
Definition: MSLane.h:442
Changer::iterator ChangerIt
the iterator moving over the ChangeElems
double getLength() const
Get vehicle&#39;s length [m].
MSAbstractLaneChangeModel::StateAndDist StateAndDist
Changer myChanger
Container for ChangeElemements, one for every lane in the edge.
double getBackPositionOnLane(const MSLane *lane) const
Get the vehicle&#39;s position relative to the given lane.
Definition: MSVehicle.cpp:2403
void registerUnchanged(MSVehicle *vehicle)
#define NUMERICAL_EPS
Definition: config.h:151
int numSublanes() const
Definition: MSLeaderInfo.h:95
MSLane * getShadowLane() const
Returns the lane the vehicles shadow is on during continuous/sublane lane change. ...
int numFreeSublanes() const
Definition: MSLeaderInfo.h:99
const std::vector< LaneQ > & getBestLanes() const
Returns the description of best lanes to use in order to continue the route.
Definition: MSVehicle.cpp:2933
double getSpeed() const
Returns the vehicle&#39;s current speed.
Definition: MSVehicle.h:442
int influenceChangeDecision(int state)
allow TraCI to influence a lane change decision
Definition: MSVehicle.cpp:4017
const std::string & getID() const
Returns the name of the vehicle.
Representation of a lane in the micro simulation.
Definition: MSLane.h:79
bool mayChange(int direction) const
whether changing to the lane in the given direction should be considered
ChangerIt myCandi
static const Position INVALID
used to indicate that a position is valid
Definition: Position.h:278