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);
111 #ifdef DEBUG_ACTIONSTEPS 113 std::cout <<
"\nCHANGE" << std::endl;
116 assert(vehicle->
getLane() == (*myCandi).lane);
127 #ifdef DEBUG_ACTIONSTEPS 129 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' skips regular change checks." << std::endl;
139 #ifdef DEBUG_ACTIONSTEPS 148 #ifdef DEBUG_ACTIONSTEPS 150 std::cout <<
"\n" <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' at plans sublane maneuver." 155 for (
int i = 0; i < (int)
myChanger.size(); ++i) {
164 int sublaneIndex = 0;
167 for (
int offset : ce->siblings) {
172 sublaneIndex += ce->ahead.numSublanes();
184 #ifdef DEBUG_DECISION 193 #ifdef DEBUG_MANEUVER 195 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' performing sublane change..." << std::endl;
214 #ifdef DEBUG_MANEUVER 216 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' aborts LC-continuation." 230 const std::vector<MSVehicle::LaneQ>& preb = vehicle->
getBestLanes();
232 result.
dir = laneOffset;
234 (
myCandi + laneOffset)->lastBlocked = vehicle;
235 if ((
myCandi + laneOffset)->firstBlocked ==
nullptr) {
236 (
myCandi + laneOffset)->firstBlocked = vehicle;
250 if (remLatDist == 0) {
255 #ifdef DEBUG_MANEUVER 257 std::cout <<
SIMTIME <<
" vehicle '" << vehicle->
getID() <<
"' continueChangeSublane()" 258 <<
" remLatDist=" << remLatDist <<
" nextLatDist=" << nextLatDist
274 const int direction = (latDist >= -distToRightLaneBorder && latDist <= distToLeftLaneBorder) ? 0 : (latDist < 0 ? -1 : 1);
277 to = from + direction;
300 #ifdef DEBUG_MANEUVER 304 <<
" increments lateral position by latDist=" << latDist << std::endl;
307 #ifdef DEBUG_SURROUNDING 309 std::cout <<
SIMTIME <<
" vehicle '" << vehicle->
getID() <<
"'\n to->ahead=" << to->ahead.toString()
310 <<
"'\n to->aheadNext=" << to->aheadNext.toString()
324 if (shadowLane !=
nullptr && shadowLane != oldShadowLane) {
325 assert(to != from || oldShadowLane == 0);
327 (
myChanger.begin() + shadowLane->
getIndex())->ahead.addLeader(vehicle,
false, latOffset);
329 if (completedManeuver) {
348 double changeAngle = 0;
354 #ifdef DEBUG_MANEUVER 357 std::cout <<
SIMTIME <<
" startChangeSublane()" 358 <<
" shadowLane=" << (shadowLane !=
nullptr ? shadowLane->
getID() :
"NULL")
359 <<
" targetLane=" << (targetLane !=
nullptr ? targetLane->
getID() :
"NULL")
361 <<
" latDist=" << latDist
364 <<
" laneA=" <<
RAD2DEG(laneAngle)
365 <<
" changeA=" <<
RAD2DEG(changeAngle)
367 <<
" newA=" <<
RAD2DEG(laneAngle + changeAngle)
371 vehicle->
setAngle(laneAngle + changeAngle, completedManeuver);
380 from->lane->requireCollisionCheck();
381 to->lane->requireCollisionCheck();
382 return changedToNewLane;
388 if (changedToNewLane) {
389 vehicle->
myState.
myPosLat -= direction * 0.5 * (from->lane->getWidth() + to->lane->getWidth());
390 to->lane->myTmpVehicles.insert(to->lane->myTmpVehicles.begin(), vehicle);
404 to->ahead.addLeader(vehicle,
false, 0);
407 from->ahead.addLeader(vehicle,
false, 0);
409 return changedToNewLane;
455 #ifdef DEBUG_SURROUNDING 457 std::cout <<
SIMTIME <<
" getLeaders lane=" << target->lane->getID() <<
" ego=" << vehicle->
getID() <<
" ahead=" << target->ahead.toString() <<
"\n";
461 for (
int i = 0; i < target->ahead.numSublanes(); ++i) {
463 if (veh !=
nullptr) {
465 #ifdef DEBUG_SURROUNDING 476 for (
int i = 0; i < aheadSamePos.
numSublanes(); ++i) {
478 if (veh !=
nullptr && veh != vehicle) {
480 #ifdef DEBUG_SURROUNDING 482 std::cout <<
" further lead=" << veh->
getID() <<
" leadBack=" << veh->
getBackPositionOnLane(target->lane) <<
" gap=" << gap <<
"\n";
490 MSLane* targetLane = target->lane;
496 #ifdef DEBUG_SURROUNDING 498 std::cout <<
" aborting forward search. dist=" << dist <<
" seen=" << seen <<
"\n";
504 #ifdef DEBUG_SURROUNDING 506 std::cout <<
" add consecutive before=" << result.
toString() <<
" dist=" << dist;
509 target->lane->getLeadersOnConsecutive(dist, seen, speed, vehicle, bestLaneConts, result);
510 #ifdef DEBUG_SURROUNDING 512 std::cout <<
" after=" << result.
toString() <<
"\n";
524 const std::vector<MSVehicle::LaneQ>& preb,
526 double& maneuverDist)
const {
530 const MSLane& neighLane = *(target->lane);
540 #ifdef DEBUG_SURROUNDING 542 <<
" checkChangeSublane: veh=" << vehicle->
getID()
543 <<
" laneOffset=" << laneOffset
544 <<
"\n leaders=" << leaders.toString()
545 <<
"\n neighLeaders=" << neighLeaders.
toString()
546 <<
"\n followers=" << followers.
toString()
547 <<
"\n neighFollowers=" << neighFollowers.
toString()
553 laneOffset, alternatives,
554 leaders, followers, blockers,
555 neighLeaders, neighFollowers, neighBlockers,
557 &(
myCandi->lastBlocked), &(
myCandi->firstBlocked), latDist, maneuverDist, blocked);
558 int state = blocked | wish;
569 const int oldstate = state;
void outputLCStarted(MSVehicle *vehicle, ChangerIt &from, ChangerIt &to, int direction)
optional output for start of lane-change maneuvre
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)
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
StateAndDist checkChangeHelper(MSVehicle *vehicle, int laneOffset, LaneChangeAction alternatives)
helper function that calls checkChangeSublane and sets blocker information
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)
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.
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
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
bool startChangeSublane(MSVehicle *vehicle, ChangerIt &from, double latDist)
change by the specified amount and return whether a new lane was entered
virtual void initChanger()
Initialize the changer before looping over all vehicles.
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
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.
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
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)
MSLane * getTargetLane() const
Returns the lane the vehicle has committed to enter during a sublane lane change. ...
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
void laneChangeOutput(const std::string &tag, MSLane *source, MSLane *target, int direction)
called once the vehicle ends a lane change manoeuvre (non-instant)
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