41 #define DEBUG_COND (vehicle->getLaneChangeModel().debugVehicle())
57 if (
myChanger.front().lane->isInternal()) {
60 if (ce != ce2 && ce->lane->getIncomingLanes().front().lane == ce2->lane->getIncomingLanes().front().lane) {
62 ce->siblings.push_back(ce2->lane->getIndex() - ce->lane->getIndex());
77 ce->ahead = ce->lane->getPartialBeyond();
92 myCandi->ahead.addLeader(lead,
false, 0);
98 (
myChanger.begin() + shadowLane->
getIndex())->ahead.addLeader(lead,
false, latOffset);
114 #ifdef DEBUG_ACTIONSTEPS
116 std::cout <<
"\n" <<
SIMTIME <<
" CHANGE veh=" << vehicle->
getID() <<
" lane=" << vehicle->
getLane()->
getID() <<
"\n";
119 assert(vehicle->
getLane() == (*myCandi).lane);
126 #ifdef DEBUG_ACTIONSTEPS
128 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' skips regular change checks." << std::endl;
138 #ifdef DEBUG_ACTIONSTEPS
150 #ifdef DEBUG_ACTIONSTEPS
152 std::cout <<
"\n" <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' plans lanechange maneuver." << std::endl;
158 for (
int i = 0; i < (int)
myChanger.size(); ++i) {
167 int sublaneIndex = 0;
170 for (
int offset : ce->siblings) {
175 sublaneIndex += ce->ahead.numSublanes();
179 #ifdef DEBUG_ACTIONSTEPS
192 || traciRequestOpposite
197 if (leaders.
hasVehicles() || isOpposite || stopOpposite || traciRequestOpposite) {
200 if ((leader.first !=
nullptr || isOpposite || stopOpposite || traciRequestOpposite)
220 #ifdef DEBUG_DECISION
228 #ifdef DEBUG_MANEUVER
230 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' performing sublane change..." << std::endl;
254 #ifdef DEBUG_MANEUVER
256 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' aborts LC-continuation."
265 #ifdef DEBUG_MANEUVER
290 result.
dir = laneOffset;
292 (
myCandi + laneOffset)->lastBlocked = vehicle;
293 if ((
myCandi + laneOffset)->firstBlocked ==
nullptr) {
294 (
myCandi + laneOffset)->firstBlocked = vehicle;
308 if (remLatDist == 0) {
313 #ifdef DEBUG_MANEUVER
315 std::cout <<
SIMTIME <<
" vehicle '" << vehicle->
getID() <<
"' continueChangeSublane()"
316 <<
" remLatDist=" << remLatDist <<
" nextLatDist=" << nextLatDist
331 MSLane* source = from->lane;
336 std::swap(distToRightLaneBorder, distToLeftLaneBorder);
340 if (latDist > 0 && latDist > distToLeftLaneBorder) {
342 }
else if (latDist < 0 && -latDist > distToRightLaneBorder) {
347 #ifdef DEBUG_MANEUVER
349 std::cout <<
SIMTIME <<
" vehicle '" << vehicle->
getID() <<
"' latDist=" << latDist <<
" maneuverDist=" << maneuverDist
350 <<
" distRight=" << distToRightLaneBorder <<
" distLeft=" << distToLeftLaneBorder
351 <<
" dir=" << direction <<
" cDir=" << changerDirection <<
" mayChange=" <<
mayChange(changerDirection) <<
"\n";
355 to = from + changerDirection;
356 }
else if (changerDirection == 1 && source->
getOpposite() !=
nullptr) {
381 #ifdef DEBUG_MANEUVER
383 std::cout <<
SIMTIME <<
" vehicle '" << vehicle->
getID() <<
"' with maneuverDist=" << maneuverDist
385 <<
" increments lateral position by latDist=" << latDist << std::endl;
388 #ifdef DEBUG_SURROUNDING
390 std::cout <<
SIMTIME <<
" vehicle '" << vehicle->
getID() <<
"'\n to->ahead=" << to->ahead.toString()
391 <<
"'\n to->aheadNext=" << to->aheadNext.toString()
396 const bool completedManeuver = fabs(maneuverDist - latDist) < NUMERICAL_EPS;
402 #ifdef DEBUG_MANEUVER
404 std::cout <<
SIMTIME <<
" vehicle '" << vehicle->
getID()
405 <<
"' completedPriorManeuver=" << completedPriorManeuver
406 <<
" completedManeuver=" << completedManeuver
410 <<
" maneuverDist=" << maneuverDist
411 <<
" latDist=" << latDist
415 if (!completedManeuver && !completedPriorManeuver && priorReason != 0 &&
417 || priorReason != reason)) {
420 #ifdef DEBUG_MANEUVER
436 if (shadowLane !=
nullptr && shadowLane != oldShadowLane
440 (
myChanger.begin() + shadowLane->
getIndex())->ahead.addLeader(vehicle,
false, latOffset);
442 if (completedManeuver) {
448 if (!changedToNewLane && targetLane !=
nullptr
455 const double latOffset = vehicle->
getLatOffset(targetLane) + actionStepDist;
456 target->ahead.addLeader(vehicle,
false, latOffset);
470 laneAngle = vehicle->
getFurtherLanes().front()->getShape().rotationAtOffset(-NUMERICAL_EPS);
474 double changeAngle = 0;
476 if (fabs(latDist) > NUMERICAL_EPS) {
487 #ifdef DEBUG_MANEUVER
489 std::cout <<
SIMTIME <<
" startChangeSublane"
490 <<
" oldLane=" << from->lane->getID()
491 <<
" newLane=" << to->lane->getID()
492 <<
" shadowLane=" << (shadowLane !=
nullptr ? shadowLane->
getID() :
"NULL")
493 <<
" targetLane=" << (targetLane !=
nullptr ? targetLane->
getID() :
"NULL")
495 <<
" latDist=" << latDist
498 <<
" laneA=" <<
RAD2DEG(laneAngle)
499 <<
" changeA=" <<
RAD2DEG(changeAngle)
501 <<
" newA=" <<
RAD2DEG(laneAngle + changeAngle)
503 <<
" changedToNewLane=" << changedToNewLane
507 vehicle->
setAngle(laneAngle + changeAngle, completedManeuver);
512 #ifdef DEBUG_MANEUVER
519 from->lane->requireCollisionCheck();
520 to->lane->requireCollisionCheck();
521 return changedToNewLane;
527 const bool opposite = (&from->lane->getEdge() != &to->lane->getEdge());
528 const bool changedToNewLane = (to->lane != from->lane
530 && (
mayChange(direction * oppositeSign) || opposite));
531 if (changedToNewLane) {
532 vehicle->
myState.
myPosLat -= direction * 0.5 * (from->lane->getWidth() + to->lane->getWidth()) * oppositeSign;
534 to->lane->myTmpVehicles.insert(to->lane->myTmpVehicles.begin(), vehicle);
550 to->ahead.addLeader(vehicle,
false, 0);
553 from->ahead.addLeader(vehicle,
false, 0);
555 return changedToNewLane;
602 #ifdef DEBUG_SURROUNDING
604 std::cout <<
SIMTIME <<
" getLeaders lane=" << target->lane->getID() <<
" ego=" << vehicle->
getID() <<
" ahead=" << target->ahead.toString() <<
"\n";
608 for (
int i = 0; i < target->ahead.numSublanes(); ++i) {
610 if (
veh !=
nullptr) {
612 #ifdef DEBUG_SURROUNDING
629 const std::vector<MSVehicle::LaneQ>& preb,
631 double& maneuverDist)
const {
635 const MSLane& neighLane = *(target->lane);
645 #ifdef DEBUG_SURROUNDING
647 <<
" checkChangeSublane: veh=" << vehicle->
getID()
648 <<
" laneOffset=" << laneOffset
649 <<
"\n leaders=" << leaders.
toString()
650 <<
"\n neighLeaders=" << neighLeaders.
toString()
651 <<
"\n followers=" << followers.
toString()
652 <<
"\n neighFollowers=" << neighFollowers.
toString()
658 laneOffset, alternatives,
659 leaders, followers, blockers,
660 neighLeaders, neighFollowers, neighBlockers,
662 &(
myCandi->lastBlocked), &(
myCandi->firstBlocked), latDist, maneuverDist, blocked);
663 int state = blocked | wish;
674 const int oldstate = state;
682 if (laneOffset != 0) {
694 const std::pair<MSVehicle* const, double>& leader,
695 const std::pair<MSVehicle* const, double>& neighLead,
696 const std::pair<MSVehicle* const, double>& neighFollow,
697 const std::vector<MSVehicle::LaneQ>& preb) {
704 const MSLane& neighLane = *targetLane;
722 targetLane->
addLeaders(vehicle, posOnTarget, neighLeaders);
723 int sublaneIndex = 0;
724 for (
int i = 0; i < targetLane->
getIndex(); i++) {
732 targetLane->
addLeaders(vehicle, backPosOnTarget, neighFollowers,
true);
739 #ifdef DEBUG_CHANGE_OPPOSITE
741 <<
" checkChangeOppositeSublane: veh=" << vehicle->
getID()
743 <<
" laneOffset=" << laneOffset
744 <<
"\n leaders=" << leaders.
toString()
745 <<
"\n neighLeaders=" << neighLeaders.
toString()
746 <<
"\n followers=" << followers.
toString()
747 <<
"\n neighFollowers=" << neighFollowers.
toString()
756 double maneuverDist = 0;
758 laneOffset, alternatives,
759 leaders, followers, blockers,
760 neighLeaders, neighFollowers, neighBlockers,
762 &(
myCandi->lastBlocked), &(
myCandi->firstBlocked), latDist, maneuverDist, blocked);
763 int state = blocked | wish;
765 const int oldstate = state;
767 #ifdef DEBUG_CHANGE_OPPOSITE
773 if (laneOffset != 0) {
779 #ifdef DEBUG_CHANGE_OPPOSITE
781 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' performing sublane change latDist=" << latDist <<
" maneuverDist=" << maneuverDist <<
"\n";
792 std::pair<MSVehicle*, double>
795 std::pair<MSVehicle*, double> leader(
nullptr, std::numeric_limits<double>::max());
798 if (cand.first !=
nullptr) {
799 const double rightSide = cand.first->getRightSideOnLane();
800 if (cand.second < leader.second
801 && rightSide < egoWidth
802 && vehicle->
getLane()->
getWidth() - rightSide - cand.first->getVehicleType().getWidth() < egoWidth) {
803 leader.first =
const_cast<MSVehicle*
>(cand.first);
804 leader.second = cand.second;
std::pair< const MSVehicle *, double > CLeaderDist
@ SVC_EMERGENCY
public emergency vehicles
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.
#define UNUSED_PARAMETER(x)
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
virtual double computeSpeedLat(double latDist, double &maneuverDist, bool urgent) const
decides the next lateral speed depending on the remaining lane change distance to be covered and upda...
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)
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
bool alreadyChanged() const
reset the flag whether a vehicle already moved to false
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.
SUMOVehicleClass getVClass() const
Returns the vehicle's access class.
MSLaneChanger * myLaneChanger
This member will do the lane-change.
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
static bool gLefthand
Whether lefthand-drive is being simulated.
Performs lane changing of vehicles.
const bool myChangeToOpposite
whether this edge allows changing to the opposite direction edge
static bool hasOppositeStop(MSVehicle *vehicle)
whether vehicle has an opposite-direction stop within relevant range
void checkTraCICommands(MSVehicle *vehicle)
Take into account traci LC-commands.
virtual void initChanger()
Initialize the changer before looping over all vehicles.
Changer & getChanger()
return changer (only to be used by MSLaneChangerSublane from another instance)
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
bool changeOpposite(MSVehicle *vehicle, std::pair< MSVehicle *, double > leader)
static std::vector< MSVehicle::LaneQ > getBestLanesOpposite(MSVehicle *vehicle, const MSLane *stopLane, double oppositeLength)
add LaneQ for opposite lanes
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
static std::pair< MSVehicle *, double > findClosestLeader(const MSLeaderDistanceInfo &leaders, const MSVehicle *vehicle)
find the closest leader that prevents ego vehicle from passing on the current lane
bool myCheckedChangeOpposite
whether checkChangeOpposite was called for the current vehicle
virtual void initChanger()
Initialize the changer before looping over all vehicles.
bool checkChangeOpposite(MSVehicle *vehicle, int laneOffset, MSLane *targetLane, const std::pair< MSVehicle *const, double > &leader, const std::pair< MSVehicle *const, double > &neighLead, const std::pair< MSVehicle *const, double > &neighFollow, const std::vector< MSVehicle::LaneQ > &preb)
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.
void addLeaders(const MSVehicle *vehicle, double vehPos, MSLeaderDistanceInfo &result, bool oppositeDirection=false)
get leaders for ego on the given lane
double getRightSideOnEdge() const
int getIndex() const
Returns the lane's index.
double getOppositePos(double pos) const
return the corresponding position on the opposite lane
MSLeaderDistanceInfo getFollowersOnConsecutive(const MSVehicle *ego, double backOffset, bool allSublanes, double searchDist=-1, bool ignoreMinorLinks=false) const
return the sublane followers with the largest missing rear gap among all predecessor lanes (within di...
MSEdge & getEdge() const
Returns the lane's edge.
const PositionVector & getShape() const
Returns this lane's shape.
MSLane * getOpposite() const
return the neighboring opposite direction lane for lane changing or nullptr
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
void fixOppositeGaps(bool isFollower)
subtract vehicle length from all gaps if the leader vehicle is driving in the opposite direction
virtual int addLeader(const MSVehicle *veh, double gap, double latOffset=0, int sublane=-1)
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.
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.
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 getMinGapLat() const
Get the minimum lateral gap that vehicles of this type maintain.
double getWidth() const
Get the width which vehicles of this class shall have when being drawn.
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.