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();
78 ce->outsideBounds.clear();
94 myCandi->outsideBounds.push_back(lead);
96 myCandi->ahead.addLeader(lead,
false, 0);
103 (
myChanger.begin() + shadowLane->
getIndex())->ahead.addLeader(lead,
false, latOffset);
119#ifdef DEBUG_ACTIONSTEPS
121 std::cout <<
"\n" <<
SIMTIME <<
" CHANGE veh=" << vehicle->
getID() <<
" lane=" << vehicle->
getLane()->
getID() <<
"\n";
124 assert(vehicle->
getLane() == (*myCandi).lane);
129 myCandi->lastStopped = vehicle;
134#ifdef DEBUG_ACTIONSTEPS
136 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' skips regular change checks." << std::endl;
146#ifdef DEBUG_ACTIONSTEPS
158#ifdef DEBUG_ACTIONSTEPS
160 std::cout <<
"\n" <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' plans lanechange maneuver." << std::endl;
166 for (
int i = 0; i < (int)
myChanger.size(); ++i) {
175 int sublaneIndex = 0;
178 for (
int offset : ce->siblings) {
183 sublaneIndex += ce->ahead.numSublanes();
187#ifdef DEBUG_ACTIONSTEPS
200 || traciRequestOpposite
205 if (leaders.
hasVehicles() || isOpposite || stopOpposite || traciRequestOpposite) {
208 if ((leader.first !=
nullptr || isOpposite || stopOpposite || traciRequestOpposite)
238 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' performing sublane change..." << std::endl;
264 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' aborts LC-continuation."
298 result.
dir = laneOffset;
300 (
myCandi + laneOffset)->lastBlocked = vehicle;
301 if ((
myCandi + laneOffset)->firstBlocked ==
nullptr) {
302 (
myCandi + laneOffset)->firstBlocked = vehicle;
316 if (remLatDist == 0) {
323 std::cout <<
SIMTIME <<
" vehicle '" << vehicle->
getID() <<
"' continueChangeSublane()"
324 <<
" remLatDist=" << remLatDist <<
" nextLatDist=" << nextLatDist
339 MSLane* source = from->lane;
344 std::swap(distToRightLaneBorder, distToLeftLaneBorder);
348 if (latDist > 0 && latDist > distToLeftLaneBorder) {
350 }
else if (latDist < 0 && -latDist > distToRightLaneBorder) {
357 std::cout <<
SIMTIME <<
" vehicle '" << vehicle->
getID() <<
"' latDist=" << latDist <<
" maneuverDist=" << maneuverDist
358 <<
" distRight=" << distToRightLaneBorder <<
" distLeft=" << distToLeftLaneBorder
359 <<
" dir=" << direction <<
" cDir=" << changerDirection <<
" mayChange=" <<
mayChange(changerDirection) <<
"\n";
363 to = from + changerDirection;
364 }
else if (changerDirection == 1 && source->
getOpposite() !=
nullptr) {
391 std::cout <<
SIMTIME <<
" vehicle '" << vehicle->
getID() <<
"' with maneuverDist=" << maneuverDist
393 <<
" increments lateral position by latDist=" << latDist << std::endl;
396#ifdef DEBUG_SURROUNDING
398 std::cout <<
SIMTIME <<
" vehicle '" << vehicle->
getID() <<
"'\n to->ahead=" << to->ahead.toString()
399 <<
"'\n to->aheadNext=" << to->aheadNext.toString()
404 const bool completedManeuver = fabs(maneuverDist - latDist) < NUMERICAL_EPS;
412 std::cout <<
SIMTIME <<
" vehicle '" << vehicle->
getID()
413 <<
"' completedPriorManeuver=" << completedPriorManeuver
414 <<
" completedManeuver=" << completedManeuver
418 <<
" maneuverDist=" << maneuverDist
419 <<
" latDist=" << latDist
423 if (!completedManeuver && !completedPriorManeuver && priorReason != 0 &&
425 || priorReason != reason)) {
444 if (shadowLane !=
nullptr && shadowLane != oldShadowLane
448 (
myChanger.begin() + shadowLane->
getIndex())->ahead.addLeader(vehicle,
false, latOffset);
450 if (completedManeuver) {
456 if (!changedToNewLane && targetLane !=
nullptr
463 const double latOffset = vehicle->
getLatOffset(targetLane) + actionStepDist;
464 target->ahead.addLeader(vehicle,
false, latOffset);
478 laneAngle = vehicle->
getFurtherLanes().front()->getShape().rotationAtOffset(-NUMERICAL_EPS);
482 double changeAngle = 0;
484 if (fabs(latDist) > NUMERICAL_EPS) {
497 std::cout <<
SIMTIME <<
" startChangeSublane"
498 <<
" oldLane=" << from->lane->getID()
499 <<
" newLane=" << to->lane->getID()
500 <<
" shadowLane=" << (shadowLane !=
nullptr ? shadowLane->
getID() :
"NULL")
501 <<
" targetLane=" << (targetLane !=
nullptr ? targetLane->
getID() :
"NULL")
503 <<
" latDist=" << latDist
506 <<
" laneA=" <<
RAD2DEG(laneAngle)
507 <<
" changeA=" <<
RAD2DEG(changeAngle)
509 <<
" newA=" <<
RAD2DEG(laneAngle + changeAngle)
511 <<
" changedToNewLane=" << changedToNewLane
515 vehicle->
setAngle(laneAngle + changeAngle, completedManeuver);
527 from->lane->requireCollisionCheck();
528 to->lane->requireCollisionCheck();
529 return changedToNewLane;
535 const bool opposite = (&from->lane->getEdge() != &to->lane->getEdge());
536 const bool changedToNewLane = (to->lane != from->lane
538 && (
mayChange(direction * oppositeSign) || opposite));
539 if (changedToNewLane) {
540 vehicle->
myState.
myPosLat -= direction * 0.5 * (from->lane->getWidth() + to->lane->getWidth()) * oppositeSign;
542 to->lane->myTmpVehicles.insert(to->lane->myTmpVehicles.begin(), vehicle);
558 to->ahead.addLeader(vehicle,
false, 0);
561 from->ahead.addLeader(vehicle,
false, 0);
563 return changedToNewLane;
610#ifdef DEBUG_SURROUNDING
612 std::cout <<
SIMTIME <<
" getLeaders lane=" << target->lane->getID() <<
" ego=" << vehicle->
getID() <<
" ahead=" << target->ahead.toString() <<
"\n";
616 if (target->lane == vehicle->
getLane()) {
624 for (
int i = 0; i < target->ahead.numSublanes(); ++i) {
626 if (
veh !=
nullptr) {
628#ifdef DEBUG_SURROUNDING
633 if (i + sublaneShift >= 0 && i + sublaneShift < result.
numSublanes()) {
638 if (sublaneShift != 0) {
639 for (
MSVehicle* cand : target->outsideBounds) {
644#ifdef DEBUG_SURROUNDING
646 std::cout <<
" outsideBounds=" <<
toString(target->outsideBounds) <<
" result=" << result.
toString() <<
"\n";
658 const std::vector<MSVehicle::LaneQ>& preb,
660 double& maneuverDist)
const {
664 const MSLane& neighLane = *(target->lane);
675 if (laneOffset == -1 && neighLeaders.
hasVehicles()) {
679#ifdef DEBUG_SURROUNDING
681 <<
" checkChangeSublane: veh=" << vehicle->
getID()
682 <<
" laneOffset=" << laneOffset
683 <<
"\n leaders=" << leaders.
toString()
684 <<
"\n neighLeaders=" << neighLeaders.
toString()
685 <<
"\n followers=" << followers.
toString()
686 <<
"\n neighFollowers=" << neighFollowers.
toString()
692 laneOffset, alternatives,
693 leaders, followers, blockers,
694 neighLeaders, neighFollowers, neighBlockers,
696 &(
myCandi->lastBlocked), &(
myCandi->firstBlocked), latDist, maneuverDist, blocked);
697 int state = blocked | wish;
708 const int oldstate = state;
716 if (laneOffset != 0) {
728 const std::pair<MSVehicle* const, double>& leader,
729 const std::pair<MSVehicle* const, double>& neighLead,
730 const std::pair<MSVehicle* const, double>& neighFollow,
731 const std::vector<MSVehicle::LaneQ>& preb) {
738 const MSLane& neighLane = *targetLane;
758 targetLane->
addLeaders(vehicle, posOnTarget, neighLeaders);
759 neighLeaders.
patchGaps(2 * POSITION_EPS);
760 int sublaneIndex = 0;
761 for (
int i = 0; i < targetLane->
getIndex(); i++) {
769 targetLane->
addLeaders(vehicle, backPosOnTarget, neighFollowers,
true);
776#ifdef DEBUG_CHANGE_OPPOSITE
778 <<
" checkChangeOppositeSublane: veh=" << vehicle->
getID()
780 <<
" laneOffset=" << laneOffset
781 <<
"\n leaders=" << leaders.
toString()
782 <<
"\n neighLeaders=" << neighLeaders.
toString()
783 <<
"\n followers=" << followers.
toString()
784 <<
"\n neighFollowers=" << neighFollowers.
toString()
793 double maneuverDist = 0;
795 laneOffset, alternatives,
796 leaders, followers, blockers,
797 neighLeaders, neighFollowers, neighBlockers,
799 &(
myCandi->lastBlocked), &(
myCandi->firstBlocked), latDist, maneuverDist, blocked);
800 int state = blocked | wish;
802 const int oldstate = state;
804#ifdef DEBUG_CHANGE_OPPOSITE
810 if (laneOffset != 0) {
816#ifdef DEBUG_CHANGE_OPPOSITE
818 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' performing sublane change latDist=" << latDist <<
" maneuverDist=" << maneuverDist <<
"\n";
829std::pair<MSVehicle*, double>
832 std::pair<MSVehicle*, double> leader(
nullptr, std::numeric_limits<double>::max());
835 if (cand.first !=
nullptr) {
836 const double rightSide = cand.first->getRightSideOnLane();
837 if (cand.second < leader.second
838 && rightSide < egoWidth
839 && vehicle->
getLane()->
getWidth() - rightSide - cand.first->getVehicleType().getWidth() < egoWidth) {
840 leader.first =
const_cast<MSVehicle*
>(cand.first);
841 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 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
void saveLCState(const int dir, int stateWithoutTraCI, const int state)
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.
SUMOVehicleClass getVClass() const
Returns the vehicle's access class.
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
MSLaneChanger * myLaneChanger
This member will do the lane-change.
static double gLateralResolution
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
bool changeOpposite(MSVehicle *vehicle, std::pair< MSVehicle *, double > leader, MSVehicle *lastStopped)
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.
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 & getChanger()
return changer (only to be used by MSLaneChangerSublane from another instance)
Changer::iterator ChangerIt
the iterator moving over the ChangeElems
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
const PositionVector & getShape() const
Returns this lane's shape.
double getRightSideOnEdge() const
int getIndex() const
Returns the lane's index.
double getOppositePos(double pos) const
return the corresponding position on the opposite lane
MSLane * getOpposite() const
return the neighboring opposite direction lane for lane changing or nullptr
double interpolateLanePosToGeometryPos(double lanePos) const
MSEdge & getEdge() const
Returns the lane's edge.
MSLeaderDistanceInfo getFollowersOnConsecutive(const MSVehicle *ego, double backOffset, bool allSublanes, double searchDist=-1, MinorLinkMode mLinkMode=FOLLOW_ALWAYS) const
return the sublane followers with the largest missing rear gap among all predecessor lanes (within di...
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
void patchGaps(double amount)
add given value to all gaps
virtual int addLeader(const MSVehicle *veh, double gap, double latOffset=0, int sublane=-1)
void moveSamePosTo(const MSVehicle *ego, MSLeaderDistanceInfo &other)
void setSublaneOffset(int offset)
set number of sublanes by which to shift positions
int getSublaneOffset() 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
bool isStoppedOnLane() const
bool isActive() const
Returns whether the current simulation step is an action point for the vehicle.
MSAbstractLaneChangeModel & getLaneChangeModel()
double getLeftSideOnLane() const
Get the lateral position of the vehicles left side on the lane:
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.
const MSLane * getLane() const
Returns the lane the vehicle is on.
Influencer & getInfluencer()
double getRightSideOnLane() const
Get the lateral position of the vehicles right side on the lane:
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< MSLane * > & getFurtherLanes() const
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.
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.
NLOHMANN_BASIC_JSON_TPL_DECLARATION void swap(nlohmann::NLOHMANN_BASIC_JSON_TPL &j1, nlohmann::NLOHMANN_BASIC_JSON_TPL &j2) noexcept(//NOLINT(readability-inconsistent-declaration-parameter-name) is_nothrow_move_constructible< nlohmann::NLOHMANN_BASIC_JSON_TPL >::value &&//NOLINT(misc-redundant-expression) is_nothrow_move_assignable< nlohmann::NLOHMANN_BASIC_JSON_TPL >::value)
exchanges the values of two JSON objects