 |
Eclipse SUMO - Simulation of Urban MObility
|
Go to the documentation of this file.
41 #define DEBUG_COND (vehicle->getLaneChangeModel().debugVehicle())
55 if (
myChanger.front().lane->isInternal()) {
58 if (ce != ce2 && ce->lane->getIncomingLanes().front().lane == ce2->lane->getIncomingLanes().front().lane) {
60 ce->siblings.push_back(ce2->lane->getIndex() - ce->lane->getIndex());
75 ce->ahead = ce->lane->getPartialBeyond();
90 myCandi->ahead.addLeader(lead,
false, 0);
92 if (shadowLane !=
nullptr) {
95 (
myChanger.begin() + shadowLane->
getIndex())->ahead.addLeader(lead,
false, latOffset);
111 #ifdef DEBUG_ACTIONSTEPS
113 std::cout <<
"\nCHANGE" << std::endl;
116 assert(vehicle->
getLane() == (*myCandi).lane);
123 #ifdef DEBUG_ACTIONSTEPS
125 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' skips regular change checks." << std::endl;
135 #ifdef DEBUG_ACTIONSTEPS
144 #ifdef DEBUG_ACTIONSTEPS
146 std::cout <<
"\n" <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' plans lanechange maneuver." << std::endl;
150 for (
int i = 0; i < (int)
myChanger.size(); ++i) {
159 int sublaneIndex = 0;
162 for (
int offset : ce->siblings) {
167 sublaneIndex += ce->ahead.numSublanes();
179 #ifdef DEBUG_DECISION
187 #ifdef DEBUG_MANEUVER
189 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' performing sublane change..." << std::endl;
208 #ifdef DEBUG_MANEUVER
210 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' aborts LC-continuation."
219 #ifdef DEBUG_MANEUVER
237 const std::vector<MSVehicle::LaneQ>& preb = vehicle->
getBestLanes();
239 result.
dir = laneOffset;
241 (
myCandi + laneOffset)->lastBlocked = vehicle;
242 if ((
myCandi + laneOffset)->firstBlocked ==
nullptr) {
243 (
myCandi + laneOffset)->firstBlocked = vehicle;
257 if (remLatDist == 0) {
262 #ifdef DEBUG_MANEUVER
264 std::cout <<
SIMTIME <<
" vehicle '" << vehicle->
getID() <<
"' continueChangeSublane()"
265 <<
" remLatDist=" << remLatDist <<
" nextLatDist=" << nextLatDist
285 const int direction = (latDist >= -distToRightLaneBorder && latDist <= distToLeftLaneBorder) ? 0 : (latDist < 0 ? -1 : 1);
288 to = from + direction;
311 #ifdef DEBUG_MANEUVER
313 std::cout <<
SIMTIME <<
" vehicle '" << vehicle->
getID() <<
"' with maneuverDist=" << maneuverDist
315 <<
" increments lateral position by latDist=" << latDist << std::endl;
318 #ifdef DEBUG_SURROUNDING
320 std::cout <<
SIMTIME <<
" vehicle '" << vehicle->
getID() <<
"'\n to->ahead=" << to->ahead.toString()
321 <<
"'\n to->aheadNext=" << to->aheadNext.toString()
326 const bool completedManeuver = fabs(maneuverDist - latDist) <
NUMERICAL_EPS;
332 #ifdef DEBUG_MANEUVER
334 std::cout <<
SIMTIME <<
" vehicle '" << vehicle->
getID()
335 <<
"' completedPriorManeuver=" << completedPriorManeuver
336 <<
" completedManeuver=" << completedManeuver
340 <<
" maneuverDist=" << maneuverDist
341 <<
" latDist=" << latDist
345 if (!completedManeuver && !completedPriorManeuver && priorReason != 0 &&
347 || priorReason != reason)) {
350 #ifdef DEBUG_MANEUVER
366 if (shadowLane !=
nullptr && shadowLane != oldShadowLane) {
367 assert(to != from || oldShadowLane == 0);
369 (
myChanger.begin() + shadowLane->
getIndex())->ahead.addLeader(vehicle,
false, latOffset);
371 if (completedManeuver) {
377 if (!changedToNewLane && targetLane !=
nullptr
382 const double latOffset = vehicle->
getLatOffset(targetLane) + actionStepDist;
383 target->ahead.addLeader(vehicle,
false, latOffset);
401 double changeAngle = 0;
410 #ifdef DEBUG_MANEUVER
412 std::cout <<
SIMTIME <<
" startChangeSublane()"
413 <<
" shadowLane=" << (shadowLane !=
nullptr ? shadowLane->
getID() :
"NULL")
414 <<
" targetLane=" << (targetLane !=
nullptr ? targetLane->
getID() :
"NULL")
416 <<
" latDist=" << latDist
419 <<
" laneA=" <<
RAD2DEG(laneAngle)
420 <<
" changeA=" <<
RAD2DEG(changeAngle)
422 <<
" newA=" <<
RAD2DEG(laneAngle + changeAngle)
426 vehicle->
setAngle(laneAngle + changeAngle, completedManeuver);
435 from->lane->requireCollisionCheck();
436 to->lane->requireCollisionCheck();
437 return changedToNewLane;
443 if (changedToNewLane) {
444 vehicle->
myState.
myPosLat -= direction * 0.5 * (from->lane->getWidth() + to->lane->getWidth());
445 to->lane->myTmpVehicles.insert(to->lane->myTmpVehicles.begin(), vehicle);
459 to->ahead.addLeader(vehicle,
false, 0);
462 from->ahead.addLeader(vehicle,
false, 0);
464 return changedToNewLane;
511 #ifdef DEBUG_SURROUNDING
513 std::cout <<
SIMTIME <<
" getLeaders lane=" << target->lane->getID() <<
" ego=" << vehicle->
getID() <<
" ahead=" << target->ahead.toString() <<
"\n";
517 for (
int i = 0; i < target->ahead.numSublanes(); ++i) {
519 if (
veh !=
nullptr) {
521 #ifdef DEBUG_SURROUNDING
532 for (
int i = 0; i < aheadSamePos.
numSublanes(); ++i) {
534 if (
veh !=
nullptr &&
veh != vehicle) {
536 #ifdef DEBUG_SURROUNDING
546 MSLane* targetLane = target->lane;
552 #ifdef DEBUG_SURROUNDING
554 std::cout <<
" aborting forward search. dist=" << dist <<
" seen=" << seen <<
"\n";
560 #ifdef DEBUG_SURROUNDING
562 std::cout <<
" add consecutive before=" << result.
toString() <<
" dist=" << dist;
565 target->lane->getLeadersOnConsecutive(dist, seen, speed, vehicle, bestLaneConts, result);
566 #ifdef DEBUG_SURROUNDING
568 std::cout <<
" after=" << result.
toString() <<
"\n";
580 const std::vector<MSVehicle::LaneQ>& preb,
582 double& maneuverDist)
const {
586 const MSLane& neighLane = *(target->lane);
596 #ifdef DEBUG_SURROUNDING
598 <<
" checkChangeSublane: veh=" << vehicle->
getID()
599 <<
" laneOffset=" << laneOffset
600 <<
"\n leaders=" << leaders.toString()
601 <<
"\n neighLeaders=" << neighLeaders.
toString()
602 <<
"\n followers=" << followers.
toString()
603 <<
"\n neighFollowers=" << neighFollowers.
toString()
609 laneOffset, alternatives,
610 leaders, followers, blockers,
611 neighLeaders, neighFollowers, neighBlockers,
613 &(
myCandi->lastBlocked), &(
myCandi->firstBlocked), latDist, maneuverDist, blocked);
614 int state = blocked | wish;
625 const int oldstate = state;
633 if (laneOffset != 0) {
void updateBestLanes(bool forceRebuild=false, const MSLane *startLane=0)
computes the best lanes to use in order to continue the route
MSLeaderDistanceInfo getLeaders(const ChangerIt &target, const MSVehicle *ego) const
get leaders for ego on the given lane
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver's reaction time tau (i....
MSVehicle * veh(ConstChangerIt ce) const
const std::vector< LaneQ > & getBestLanes() const
Returns the description of best lanes to use in order to continue the route.
Representation of a lane in the micro simulation.
bool isActive() const
Returns whether the current simulation step is an action point for the vehicle.
virtual void updateExpectedSublaneSpeeds(const MSLeaderDistanceInfo &ahead, int sublaneOffset, int laneIndex)
update expected speeds for each sublane of the current edge
virtual void initChanger()
Initialize the changer before looping over all vehicles.
Performs lane changing of vehicles.
saves leader/follower vehicles and their distances relative to an ego vehicle
bool isRemoteControlled() const
Returns the information whether the vehicle is fully controlled via TraCI.
bool mayChange(int direction) const
whether changing to the lane in the given direction should be considered
int influenceChangeDecision(int state)
allow TraCI to influence a lane change decision
static bool outputLCStarted()
whether start of maneuvers shall be recorede
void setOrigLeaderGaps(CLeaderDist, double secGap)
static const Position INVALID
used to indicate that a position is valid
double getLatOffset(const MSLane *lane) const
Get the offset that that must be added to interpret myState.myPosLat for the given lane.
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
void setManeuverDist(const double dist)
Updates the remaining distance for the current maneuver while it is continued within non-action steps...
double getMaxSpeedLat() const
Get vehicle's maximum lateral speed [m/s].
double getRightSideOnEdge() const
void setSpeedLat(double speedLat)
SUMOTime getActionStepLength() const
Returns the vehicle's action step length in millisecs, i.e. the interval between two action points.
void outputLCEnded(MSVehicle *vehicle, ChangerIt &from, ChangerIt &to, int direction)
optional output for end of lane-change maneuvre
virtual void setOwnState(const int state)
double getSpeedLat() const
return the lateral speed of the current lane change maneuver
int numFreeSublanes() const
The action is urgent (to be defined by lc-model)
void setLeaderGaps(CLeaderDist, double secGap)
bool isStoppedOnLane() const
static bool outputLCEnded()
whether start of maneuvers shall be recorede
bool startChangeSublane(MSVehicle *vehicle, ChangerIt &from, double latDist, double maneuverDist)
change by the specified amount and return whether a new lane was entered
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
MSLane * updateTargetLane()
MSAbstractLaneChangeModel::StateAndDist StateAndDist
bool isChangingLanes() const
return true if the vehicle currently performs a lane change maneuver
~MSLaneChangerSublane()
Destructor.
bool startLaneChangeManeuver(MSLane *source, MSLane *target, int direction)
start the lane change maneuver and return whether it continues
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
bool continueChangeSublane(MSVehicle *vehicle, ChangerIt &from)
Continue a sublane-lane change maneuver and return whether the midpoint was passed in this step.
void saveNeighbors(const int dir, const MSLeaderDistanceInfo &followers, const MSLeaderDistanceInfo &leaders)
Saves the lane change relevant vehicles, which are currently on neighboring lanes in the given direct...
double getPositionOnLane() const
Get the vehicle's position along the lane.
double getActionStepLengthSecs() const
Returns the vehicle's action step length in secs, i.e. the interval between two action points.
LaneChangeAction
The state of a vehicle's lane-change behavior.
MSLane * getShadowLane() const
Returns the lane the vehicle's shadow is on during continuous/sublane lane change.
double getLengthWithGap() const
Get vehicle's length including the minimum gap [m].
void checkTraCICommands(MSVehicle *vehicle)
Take into account traci LC-commands.
ChangerIt findCandidate()
Find current candidate. If there is none, myChanger.end() is returned.
double interpolateLanePosToGeometryPos(double lanePos) const
double getLength() const
Returns the lane's length.
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)
double myPosLat
the stored lateral position
void setSublaneChange(double latDist)
Sets a new sublane-change request.
MSAbstractLaneChangeModel & getLaneChangeModel()
double getAngle() const
Returns the vehicle's direction in radians.
double getMinGap() const
Get the free space in front of vehicles of this class.
used by the sublane model
double getPreviousManeuverDist() const
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
virtual void updateChanger(bool vehHasChanged)
double length2D() const
Returns the length.
double getManeuverDist() const
Returns the remaining unblocked distance for the current maneuver. (only used by sublane model)
Position myCachedPosition
void setAngle(double angle, bool straightenFurther=false)
Set a custom vehicle angle in rad, optionally updates furtherLanePosLat.
bool checkChangeToNewLane(MSVehicle *vehicle, const int direction, ChangerIt from, ChangerIt to)
check whether the given vehicle has entered the new lane 'to->lane' during a sublane LC-step
MSLane * getLane() const
Returns the lane the vehicle is on.
virtual void updateChanger(bool vehHasChanged)
void clearNeighbors()
Clear info on neighboring vehicle from previous step.
Influencer & getInfluencer()
Returns the velocity/lane influencer.
void setFollowerGaps(CLeaderDist follower, double secGap)
void adaptBestLanesOccupation(int laneIndex, double density)
update occupation from MSLaneChanger
State myState
This Vehicles driving state (pos and speed)
virtual bool debugVehicle() const
whether the current vehicles shall be debugged
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
const PositionVector & getShape() const
Returns this lane's shape.
Changer myChanger
Container for ChangeElemements, one for every lane in the edge.
void abortLCManeuver(MSVehicle *vehicle)
immediately stop lane-changing and register vehicle as unchanged
const std::vector< MSLane * > & getFurtherLanes() const
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Needs to stay on the current lane.
void registerUnchanged(MSVehicle *vehicle)
void outputLCStarted(MSVehicle *vehicle, ChangerIt &from, ChangerIt &to, int direction, double maneuverDist)
optional output for start of lane-change maneuvre
double rotationAtOffset(double pos) const
Returns the rotation at the given length.
double getLength() const
Get vehicle's length [m].
static bool haveLCOutput()
whether lanechange-output is active
virtual void updateSafeLatDist(const double travelledLatDist)
Updates the value of safe lateral distances (in SL2015) during maneuver continuation in non-action st...
Changer::iterator ChangerIt
the iterator moving over the ChangeElems
virtual int addLeader(const MSVehicle *veh, double gap, double latOffset=0, int sublane=-1)
MSLaneChangerSublane()
Default constructor.
void laneChangeOutput(const std::string &tag, MSLane *source, MSLane *target, int direction, double maneuverDist=0)
called once the vehicle ends a lane change manoeuvre (non-instant)
void saveLCState(const int dir, const int stateWithoutTraCI, const int state)
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
const std::string & getID() const
Returns the name of the vehicle.
double getBackPositionOnLane(const MSLane *lane) const
Get the vehicle's position relative to the given lane.
virtual std::string toString() const
print a debugging representation
double getWidth() const
Returns the lane's width.
double getCommittedSpeed() const
double getWidth() const
Returns the vehicle's width.
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
double getSpeed() const
Returns the vehicle's current speed.
virtual void initChanger()
Initialize the changer before looping over all vehicles.
virtual double computeSpeedLat(double latDist, double &maneuverDist)
decides the next lateral speed depending on the remaining lane change distance to be covered and upda...
virtual StateAndDist decideDirection(StateAndDist sd1, StateAndDist sd2) const
decide in which direction to move in case both directions are desirable
const std::string & getID() const
Returns the id.
StateAndDist checkChangeHelper(MSVehicle *vehicle, int laneOffset, LaneChangeAction alternatives)
helper function that calls checkChangeSublane and sets blocker information
int getIndex() const
Returns the lane's index.
The action is due to a TraCI request.
blocked in all directions
Representation of a vehicle in the micro simulation.