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);
397 laneAngle = vehicle->
getFurtherLanes().front()->getShape().rotationAtOffset(-NUMERICAL_EPS);
401 double changeAngle = 0;
403 if (fabs(latDist) > NUMERICAL_EPS) {
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;
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) {
LaneChangeAction
The state of a vehicle's lane-change behavior.
@ LCA_BLOCKED
blocked in all directions
@ LCA_URGENT
The action is urgent (to be defined by lc-model)
@ LCA_STAY
Needs to stay on the current lane.
@ LCA_SUBLANE
used by the sublane model
@ LCA_LEFT
Wants go to the left.
@ LCA_CHANGE_REASONS
reasons of lane change
@ LCA_TRACI
The action is due to a TraCI request.
@ LCA_WANTS_LANECHANGE
lane can change
@ LCA_RIGHT
Wants go to the right.
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
void saveLCState(const int dir, const int stateWithoutTraCI, const int state)
void setFollowerGaps(CLeaderDist follower, double secGap)
MSLane * updateTargetLane()
double getPreviousManeuverDist() const
virtual void setOwnState(const int state)
double getCommittedSpeed() const
static bool outputLCEnded()
whether start of maneuvers shall be recorede
double getManeuverDist() const
Returns the remaining unblocked distance for the current maneuver. (only used by sublane model)
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)
MSLane * getShadowLane() const
Returns the lane the vehicle's shadow is on during continuous/sublane lane change.
bool startLaneChangeManeuver(MSLane *source, MSLane *target, int direction)
start the lane change maneuver and return whether it continues
static bool haveLCOutput()
whether lanechange-output is active
virtual void updateExpectedSublaneSpeeds(const MSLeaderDistanceInfo &ahead, int sublaneOffset, int laneIndex)
update expected speeds for each sublane of the current edge
void setLeaderGaps(CLeaderDist, double secGap)
virtual double computeSpeedLat(double latDist, double &maneuverDist) const
decides the next lateral speed depending on the remaining lane change distance to be covered and upda...
void setOrigLeaderGaps(CLeaderDist, double secGap)
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)
void setManeuverDist(const double dist)
Updates the remaining distance for the current maneuver while it is continued within non-action steps...
void setSpeedLat(double speedLat)
set the lateral speed and update lateral acceleraton
virtual void updateSafeLatDist(const double travelledLatDist)
Updates the value of safe lateral distances (in SL2015) during maneuver continuation in non-action st...
double getSpeedLat() const
return the lateral speed of the current lane change maneuver
virtual StateAndDist decideDirection(StateAndDist sd1, StateAndDist sd2) const
decide in which direction to move in case both directions are desirable
static bool outputLCStarted()
whether start of maneuvers shall be recorede
virtual bool debugVehicle() const
whether the current vehicles shall be debugged
bool isChangingLanes() const
return true if the vehicle currently performs a lane change maneuver
void clearNeighbors()
Clear info on neighboring vehicle from previous 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 getWidth() const
Returns the vehicle's width.
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver's reaction time tau (i....
static bool gLefthand
Whether lefthand-drive is being simulated.
Performs lane changing of vehicles.
void checkTraCICommands(MSVehicle *vehicle)
Take into account traci LC-commands.
virtual void initChanger()
Initialize the changer before looping over all vehicles.
MSVehicle * veh(ConstChangerIt ce) const
Changer myChanger
Container for ChangeElemements, one for every lane in the edge.
ChangerIt findCandidate()
Find current candidate. If there is none, myChanger.end() is returned.
bool mayChange(int direction) const
whether changing to the lane in the given direction should be considered
void registerUnchanged(MSVehicle *vehicle)
Changer::iterator ChangerIt
the iterator moving over the ChangeElems
virtual void updateChanger(bool vehHasChanged)
bool startChangeSublane(MSVehicle *vehicle, ChangerIt &from, double latDist, double maneuverDist)
change by the specified amount and return whether a new lane was entered
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
void outputLCStarted(MSVehicle *vehicle, ChangerIt &from, ChangerIt &to, int direction, double maneuverDist)
optional output for start of lane-change maneuvre
~MSLaneChangerSublane()
Destructor.
MSLeaderDistanceInfo getLeaders(const ChangerIt &target, const MSVehicle *ego) const
get leaders for ego on the given lane
StateAndDist checkChangeHelper(MSVehicle *vehicle, int laneOffset, LaneChangeAction alternatives)
helper function that calls checkChangeSublane and sets blocker information
virtual void initChanger()
Initialize the changer before looping over all vehicles.
virtual void updateChanger(bool vehHasChanged)
void abortLCManeuver(MSVehicle *vehicle)
immediately stop lane-changing and register vehicle as unchanged
MSLaneChangerSublane()
Default constructor.
void outputLCEnded(MSVehicle *vehicle, ChangerIt &from, ChangerIt &to, int direction)
optional output for end of lane-change maneuvre
MSAbstractLaneChangeModel::StateAndDist StateAndDist
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
bool continueChangeSublane(MSVehicle *vehicle, ChangerIt &from)
Continue a sublane-lane change maneuver and return whether the midpoint was passed in this step.
Representation of a lane in the micro simulation.
double getLength() const
Returns the lane's length.
double getRightSideOnEdge() const
int getIndex() const
Returns the lane's index.
const PositionVector & getShape() const
Returns this lane's shape.
double interpolateLanePosToGeometryPos(double lanePos) const
double getWidth() const
Returns the lane's width.
saves leader/follower vehicles and their distances relative to an ego vehicle
virtual std::string toString() const
print a debugging representation
virtual int addLeader(const MSVehicle *veh, double gap, double latOffset=0, int sublane=-1)
int numFreeSublanes() const
void setSublaneChange(double latDist)
Sets a new sublane-change request.
double myPosLat
the stored lateral position
Representation of a vehicle in the micro simulation.
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 * > & getFurtherLanes() const
bool isStoppedOnLane() const
bool isActive() const
Returns whether the current simulation step is an action point for the vehicle.
MSAbstractLaneChangeModel & getLaneChangeModel()
double getActionStepLengthSecs() const
Returns the vehicle's action step length in secs, i.e. the interval between two action points.
int influenceChangeDecision(int state)
allow TraCI to influence a lane change decision
bool isRemoteControlled() const
Returns the information whether the vehicle is fully controlled via TraCI.
void adaptBestLanesOccupation(int laneIndex, double density)
update occupation from MSLaneChanger
void setAngle(double angle, bool straightenFurther=false)
Set a custom vehicle angle in rad, optionally updates furtherLanePosLat.
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
double getBackPositionOnLane(const MSLane *lane) const
Get the vehicle's position relative to the given lane.
double getLatOffset(const MSLane *lane) const
Get the offset that that must be added to interpret myState.myPosLat for the given lane.
SUMOTime getActionStepLength() const
Returns the vehicle's action step length in millisecs, i.e. the interval between two action points.
Influencer & getInfluencer()
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
double getSpeed() const
Returns the vehicle's current speed.
Position myCachedPosition
const std::vector< LaneQ > & getBestLanes() const
Returns the description of best lanes to use in order to continue the route.
double getPositionOnLane() const
Get the vehicle's position along the lane.
const MSLane * getLane() const
Returns the lane the vehicle is on.
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
double getAngle() const
Returns the vehicle's direction in radians.
State myState
This Vehicles driving state (pos and speed)
double getLengthWithGap() const
Get vehicle's length including the minimum gap [m].
double getMinGap() const
Get the free space in front of vehicles of this class.
double getMaxSpeedLat() const
Get vehicle's maximum lateral speed [m/s].
double getLength() const
Get vehicle's length [m].
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
const std::string & getID() const
Returns the id.
static const Position INVALID
used to indicate that a position is valid
double length2D() const
Returns the length.
double rotationAtOffset(double pos) const
Returns the rotation at the given length.