42 #define DEBUG_COND (vehicle->getLaneChangeModel().debugVehicle()) 56 if (
myChanger.front().lane->isInternal()) {
59 if (ce != ce2 && ce->lane->getIncomingLanes().front().lane == ce2->lane->getIncomingLanes().front().lane) {
61 ce->siblings.push_back(ce2->lane->getIndex() - ce->lane->getIndex());
76 ce->ahead = ce->lane->getPartialBeyond();
91 myCandi->ahead.addLeader(lead,
false, 0);
93 if (shadowLane !=
nullptr) {
96 (
myChanger.begin() + shadowLane->
getIndex())->ahead.addLeader(lead,
false, latOffset);
112 #ifdef DEBUG_ACTIONSTEPS 114 std::cout <<
"\nCHANGE" << std::endl;
117 assert(vehicle->
getLane() == (*myCandi).lane);
124 #ifdef DEBUG_ACTIONSTEPS 126 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' skips regular change checks." << std::endl;
136 #ifdef DEBUG_ACTIONSTEPS 145 #ifdef DEBUG_ACTIONSTEPS 147 std::cout <<
"\n" <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' plans lanechange maneuver." << std::endl;
151 for (
int i = 0; i < (int)
myChanger.size(); ++i) {
160 int sublaneIndex = 0;
163 for (
int offset : ce->siblings) {
168 sublaneIndex += ce->ahead.numSublanes();
180 #ifdef DEBUG_DECISION 188 #ifdef DEBUG_MANEUVER 190 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' performing sublane change..." << std::endl;
209 #ifdef DEBUG_MANEUVER 211 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' aborts LC-continuation." 220 #ifdef DEBUG_MANEUVER 238 const std::vector<MSVehicle::LaneQ>& preb = vehicle->
getBestLanes();
240 result.
dir = laneOffset;
242 (
myCandi + laneOffset)->lastBlocked = vehicle;
243 if ((
myCandi + laneOffset)->firstBlocked ==
nullptr) {
244 (
myCandi + laneOffset)->firstBlocked = vehicle;
258 if (remLatDist == 0) {
263 #ifdef DEBUG_MANEUVER 265 std::cout <<
SIMTIME <<
" vehicle '" << vehicle->
getID() <<
"' continueChangeSublane()" 266 <<
" remLatDist=" << remLatDist <<
" nextLatDist=" << nextLatDist
286 const int direction = (latDist >= -distToRightLaneBorder && latDist <= distToLeftLaneBorder) ? 0 : (latDist < 0 ? -1 : 1);
289 to = from + direction;
312 #ifdef DEBUG_MANEUVER 314 std::cout <<
SIMTIME <<
" vehicle '" << vehicle->
getID() <<
"' with maneuverDist=" << maneuverDist
316 <<
" increments lateral position by latDist=" << latDist << std::endl;
319 #ifdef DEBUG_SURROUNDING 321 std::cout <<
SIMTIME <<
" vehicle '" << vehicle->
getID() <<
"'\n to->ahead=" << to->ahead.toString()
322 <<
"'\n to->aheadNext=" << to->aheadNext.toString()
327 const bool completedManeuver = fabs(maneuverDist - latDist) <
NUMERICAL_EPS;
333 #ifdef DEBUG_MANEUVER 335 std::cout <<
SIMTIME <<
" vehicle '" << vehicle->
getID()
336 <<
"' completedPriorManeuver=" << completedPriorManeuver
337 <<
" completedManeuver=" << completedManeuver
341 <<
" maneuverDist=" << maneuverDist
342 <<
" latDist=" << latDist
346 if (!completedManeuver && !completedPriorManeuver && priorReason != 0 &&
348 || priorReason != reason)) {
351 #ifdef DEBUG_MANEUVER 367 if (shadowLane !=
nullptr && shadowLane != oldShadowLane) {
368 assert(to != from || oldShadowLane == 0);
370 (
myChanger.begin() + shadowLane->
getIndex())->ahead.addLeader(vehicle,
false, latOffset);
372 if (completedManeuver) {
378 if (!changedToNewLane && targetLane !=
nullptr 383 const double latOffset = vehicle->
getLatOffset(targetLane) + actionStepDist;
384 target->ahead.addLeader(vehicle,
false, latOffset);
402 double changeAngle = 0;
411 #ifdef DEBUG_MANEUVER 413 std::cout <<
SIMTIME <<
" startChangeSublane()" 414 <<
" shadowLane=" << (shadowLane !=
nullptr ? shadowLane->
getID() :
"NULL")
415 <<
" targetLane=" << (targetLane !=
nullptr ? targetLane->
getID() :
"NULL")
417 <<
" latDist=" << latDist
420 <<
" laneA=" <<
RAD2DEG(laneAngle)
421 <<
" changeA=" <<
RAD2DEG(changeAngle)
423 <<
" newA=" <<
RAD2DEG(laneAngle + changeAngle)
427 vehicle->
setAngle(laneAngle + changeAngle, completedManeuver);
436 from->lane->requireCollisionCheck();
437 to->lane->requireCollisionCheck();
438 return changedToNewLane;
444 if (changedToNewLane) {
445 vehicle->
myState.
myPosLat -= direction * 0.5 * (from->lane->getWidth() + to->lane->getWidth());
446 to->lane->myTmpVehicles.insert(to->lane->myTmpVehicles.begin(), vehicle);
460 to->ahead.addLeader(vehicle,
false, 0);
463 from->ahead.addLeader(vehicle,
false, 0);
465 return changedToNewLane;
512 #ifdef DEBUG_SURROUNDING 514 std::cout <<
SIMTIME <<
" getLeaders lane=" << target->lane->getID() <<
" ego=" << vehicle->
getID() <<
" ahead=" << target->ahead.toString() <<
"\n";
518 for (
int i = 0; i < target->ahead.numSublanes(); ++i) {
520 if (veh !=
nullptr) {
522 #ifdef DEBUG_SURROUNDING 533 for (
int i = 0; i < aheadSamePos.
numSublanes(); ++i) {
535 if (veh !=
nullptr && veh != vehicle) {
537 #ifdef DEBUG_SURROUNDING 539 std::cout <<
" further lead=" << veh->
getID() <<
" leadBack=" << veh->
getBackPositionOnLane(target->lane) <<
" gap=" << gap <<
"\n";
547 MSLane* targetLane = target->lane;
553 #ifdef DEBUG_SURROUNDING 555 std::cout <<
" aborting forward search. dist=" << dist <<
" seen=" << seen <<
"\n";
561 #ifdef DEBUG_SURROUNDING 563 std::cout <<
" add consecutive before=" << result.
toString() <<
" dist=" << dist;
566 target->lane->getLeadersOnConsecutive(dist, seen, speed, vehicle, bestLaneConts, result);
567 #ifdef DEBUG_SURROUNDING 569 std::cout <<
" after=" << result.
toString() <<
"\n";
581 const std::vector<MSVehicle::LaneQ>& preb,
583 double& maneuverDist)
const {
587 const MSLane& neighLane = *(target->lane);
597 #ifdef DEBUG_SURROUNDING 599 <<
" checkChangeSublane: veh=" << vehicle->
getID()
600 <<
" laneOffset=" << laneOffset
601 <<
"\n leaders=" << leaders.toString()
602 <<
"\n neighLeaders=" << neighLeaders.
toString()
603 <<
"\n followers=" << followers.
toString()
604 <<
"\n neighFollowers=" << neighFollowers.
toString()
610 laneOffset, alternatives,
611 leaders, followers, blockers,
612 neighLeaders, neighFollowers, neighBlockers,
614 &(
myCandi->lastBlocked), &(
myCandi->firstBlocked), latDist, maneuverDist, blocked);
615 int state = blocked | wish;
626 const int oldstate = state;
634 if (laneOffset != 0) {
bool isChangingLanes() const
return true if the vehicle currently performs a lane change maneuver
double getLengthWithGap() const
Get vehicle's length including the minimum gap [m].
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver's reaction time tau (i...
double length2D() const
Returns the length.
saves leader/follower vehicles and their distances relative to an ego vehicle
Representation of a vehicle in the micro simulation.
bool isRemoteControlled() const
Returns the information whether the vehicle is fully controlled via TraCI.
double getAngle() const
Returns the vehicle's direction in radians.
State myState
This Vehicles driving state (pos and speed)
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 'to->lane' during a sublane LC-step ...
MSLane * getLane() const
Returns the lane the vehicle is on.
void setLeaderGaps(CLeaderDist, double secGap)
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 clearNeighbors()
Clear info on neighboring vehicle from previous step.
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's position along the lane.
double myPosLat
the stored lateral position
MSVehicle * veh(ConstChangerIt ce) const
void outputLCStarted(MSVehicle *vehicle, ChangerIt &from, ChangerIt &to, int direction, double maneuverDist)
optional output for start of lane-change maneuvre
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
StateAndDist checkChangeHelper(MSVehicle *vehicle, int laneOffset, LaneChangeAction alternatives)
helper function that calls checkChangeSublane and sets blocker information
double getPreviousManeuverDist() const
double getLength() const
Returns the lane's length.
const PositionVector & getShape() const
Returns this lane's shape.
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
bool isStoppedOnLane() const
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
const std::string & getID() const
Returns the id.
void outputLCEnded(MSVehicle *vehicle, ChangerIt &from, ChangerIt &to, int direction)
optional output for end of lane-change maneuvre
double getWidth() const
Returns the lane's width.
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()
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.
virtual int addLeader(const MSVehicle *veh, double gap, double latOffset=0, int sublane=-1)
double getLatOffset(const MSLane *lane) const
Get the offset that that must be added to interpret myState.myPosLat for the given lane...
void setSpeedLat(double speedLat)
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
int getIndex() const
Returns the lane's index.
MSLaneChangerSublane()
Default constructor.
double getActionStepLengthSecs() const
Returns the vehicle's action step length in secs, i.e. the interval between two action points...
virtual void updateChanger(bool vehHasChanged)
void setSublaneChange(double latDist)
Sets a new sublane-change request.
void setAngle(double angle, bool straightenFurther=false)
Set a custom vehicle angle in rad, optionally updates furtherLanePosLat.
blocked in all directions
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
The action is urgent (to be defined by lc-model)
double getCommittedSpeed() const
SUMOTime getActionStepLength() const
Returns the vehicle's action step length in millisecs, i.e. the interval between two action points...
void updateBestLanes(bool forceRebuild=false, const MSLane *startLane=0)
computes the best lanes to use in order to continue the route
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
Position myCachedPosition
const std::vector< MSLane * > & getFurtherLanes() const
virtual void initChanger()
Initialize the changer before looping over all vehicles.
MSLane * updateTargetLane()
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's lateral position on the lane.
double getRightSideOnEdge() const
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
double getMaxSpeedLat() const
Get vehicle's maximum lateral speed [m/s].
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.
MSLeaderDistanceInfo getLeaders(const ChangerIt &target, const MSVehicle *ego) const
get leaders for ego on the given lane
LaneChangeAction
The state of a vehicle's lane-change behavior.
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...
virtual void setOwnState(const int state)
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
bool isActive() const
Returns whether the current simulation step is an action point for the vehicle.
static bool haveLCOutput()
whether lanechange-output is active
double interpolateLanePosToGeometryPos(double lanePos) const
Changer::iterator ChangerIt
the iterator moving over the ChangeElems
bool startChangeSublane(MSVehicle *vehicle, ChangerIt &from, double latDist, double maneuverDist)
change by the specified amount and return whether a new lane was entered
double getLength() const
Get vehicle'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)
virtual double computeSpeedLat(double latDist, double &maneuverDist)
decides the next lateral speed depending on the remaining lane change distance to be covered and upda...
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's position relative to the given lane.
void registerUnchanged(MSVehicle *vehicle)
static bool outputLCStarted()
whether start of maneuvers shall be recorede
MSLane * getShadowLane() const
Returns the lane the vehicle'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
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.
double getSpeed() const
Returns the vehicle's current speed.
int influenceChangeDecision(int state)
allow TraCI to influence a lane change decision
const std::string & getID() const
Returns the name of the vehicle.
Representation of a lane in the micro simulation.
bool mayChange(int direction) const
whether changing to the lane in the given direction should be considered
void saveLCState(const int dir, const int stateWithoutTraCI, const int state)
double getSpeedLat() const
return the lateral speed of the current lane change maneuver
double getWidth() const
Returns the vehicle's width.
static const Position INVALID
used to indicate that a position is valid