int
MSLCM_LC2013::_wantsChange(
int laneOffset,
MSAbstractLaneChangeModel::MSLCMessager& msgPass,
int blocked,
const std::pair<MSVehicle*, SUMOReal>& leader,
const std::pair<MSVehicle*, SUMOReal>& neighLead,
const std::pair<MSVehicle*, SUMOReal>& neighFollow,
const MSLane& neighLane,
const std::vector<MSVehicle::LaneQ>& preb,
MSVehicle** lastBlocked,
MSVehicle** firstBlocked) {
assert(laneOffset == 1 || laneOffset == -1);
const SUMOTime currentTime = MSNet::getInstance()->getCurrentTimeStep();
// compute bestLaneOffset
MSVehicle::LaneQ curr, neigh, best;
int bestLaneOffset = 0;
SUMOReal currentDist = 0;
SUMOReal neighDist = 0;
int currIdx = 0;
MSLane* prebLane = myVehicle.getLane();
if (prebLane->getEdge().getPurpose() == MSEdge::EDGEFUNCTION_INTERNAL) {
// internal edges are not kept inside the bestLanes structure
prebLane = prebLane->getLinkCont()[0]->getLane();
}
for (int p = 0; p < (int) preb.size(); ++p) {
if (preb[p].lane == prebLane && p + laneOffset >= 0) {
assert(p + laneOffset < (int)preb.size());
curr = preb[p];
neigh = preb[p + laneOffset];
currentDist = curr.length;
neighDist = neigh.length;
bestLaneOffset = curr.bestLaneOffset;
if (bestLaneOffset == 0 && preb[p + laneOffset].bestLaneOffset == 0) {
bestLaneOffset = laneOffset;
}
best = preb[p + bestLaneOffset];
currIdx = p;
break;
}
}
// direction specific constants
const bool right = (laneOffset == -1);
const int lca = (right ? LCA_RIGHT : LCA_LEFT);
const int myLca = (right ? LCA_MRIGHT : LCA_MLEFT);
const int lcaCounter = (right ? LCA_LEFT : LCA_RIGHT);
const bool changeToBest = (right && bestLaneOffset < 0) || (!right && bestLaneOffset > 0);
// keep information about being a leader/follower
int ret = (myOwnState & 0xffff0000);
ret = slowDownForBlocked(lastBlocked, ret);
if (lastBlocked != firstBlocked) {
ret = slowDownForBlocked(firstBlocked, ret);
}
// we try to estimate the distance which is necessary to get on a lane
// we have to get on in order to keep our route
// we assume we need something that depends on our velocity
// and compare this with the free space on our wished lane
//
// if the free space is somehow less than the space we need, we should
// definitely try to get to the desired lane
//
// this rule forces our vehicle to change the lane if a lane changing is necessary soon
// lookAheadDistance:
// we do not want the lookahead distance to change all the time so we discrectize the speed a bit
if (myVehicle.getSpeed() > myLookAheadSpeed) {
myLookAheadSpeed = myVehicle.getSpeed();
} else {
myLookAheadSpeed = MAX2(LOOK_AHEAD_MIN_SPEED,
(LOOK_AHEAD_SPEED_MEMORY * myLookAheadSpeed + (1 - LOOK_AHEAD_SPEED_MEMORY) * myVehicle.getSpeed()));
}
SUMOReal laDist = myLookAheadSpeed * (right ? LOOK_FORWARD_RIGHT : LOOK_FORWARD_LEFT);
laDist += myVehicle.getVehicleType().getLengthWithGap() * (SUMOReal) 2.;
// free space that is available for changing
//const SUMOReal neighSpeed = (neighLead.first != 0 ? neighLead.first->getSpeed() :
// neighFollow.first != 0 ? neighFollow.first->getSpeed() :
// best.lane->getSpeedLimit());
// @note: while this lets vehicles change earlier into the correct direction
// it also makes the vehicles more "selfish" and prevents changes which are necessary to help others
int roundaboutEdgesAhead = 0;
for (std::vector<MSLane*>::iterator it = curr.bestContinuations.begin(); it != curr.bestContinuations.end(); ++it) {
if ((*it) != 0 && (*it)->getEdge().isRoundabout()) {
roundaboutEdgesAhead += 1;
} else if (roundaboutEdgesAhead > 0) {
// only check the next roundabout
break;
}
}
int roundaboutEdgesAheadNeigh = 0;
for (std::vector<MSLane*>::iterator it = neigh.bestContinuations.begin(); it != neigh.bestContinuations.end(); ++it) {
if ((*it) != 0 && (*it)->getEdge().isRoundabout()) {
roundaboutEdgesAheadNeigh += 1;
} else if (roundaboutEdgesAheadNeigh > 0) {
// only check the next roundabout
break;
}
}
if (roundaboutEdgesAhead > 1) {
currentDist += roundaboutEdgesAhead * ROUNDABOUT_DIST_BONUS;
neighDist += roundaboutEdgesAheadNeigh * ROUNDABOUT_DIST_BONUS;
}
const SUMOReal usableDist = (currentDist - myVehicle.getPositionOnLane() - best.occupation * JAM_FACTOR);
const SUMOReal maxJam = MAX2(preb[currIdx + laneOffset].occupation, preb[currIdx].occupation);
const SUMOReal neighLeftPlace = MAX2((SUMOReal) 0, neighDist - myVehicle.getPositionOnLane() - maxJam);
if (changeToBest && bestLaneOffset == curr.bestLaneOffset
&& currentDistDisallows(usableDist, bestLaneOffset, laDist)) {
/// @brief we urgently need to change lanes to follow our route
ret = ret | lca | LCA_STRATEGIC | LCA_URGENT;
} else {
if (!myAllowOvertakingRight && !right && !myVehicle.congested() && neighLead.first != 0) {
// check for slower leader on the left. we should not overtake but
// rather move left ourselves (unless congested)
MSVehicle* nv = neighLead.first;
if (nv->getSpeed() < myVehicle.getSpeed()) {
myVSafes.push_back(myCarFollowModel.followSpeed(
&myVehicle, myVehicle.getSpeed(), neighLead.second, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel()));
if (nv->getSpeed() + 5 / 3.6 < myVehicle.getSpeed()) {
mySpeedGainProbability += CHANGE_PROB_THRESHOLD_LEFT / 3;
}
}
}
if (!changeToBest && (currentDistDisallows(neighLeftPlace, abs(bestLaneOffset) + 2, laDist))) {
// the opposite lane-changing direction should be done than the one examined herein
// we'll check whether we assume we could change anyhow and get back in time...
//
// this rule prevents the vehicle from moving in opposite direction of the best lane
// unless the way till the end where the vehicle has to be on the best lane
// is long enough
ret = ret | LCA_STAY | LCA_STRATEGIC;
} else if (bestLaneOffset == 0 && (neighLeftPlace * 2. < laDist)) {
// the current lane is the best and a lane-changing would cause a situation
// of which we assume we will not be able to return to the lane we have to be on.
// this rule prevents the vehicle from leaving the current, best lane when it is
// close to this lane's end
ret = ret | LCA_STAY | LCA_STRATEGIC;
}
}
// check for overriding TraCI requests
ret = myVehicle.influenceChangeDecision(ret);
if ((ret & lcaCounter) != 0) {
// we are not interested in traci requests for the opposite direction here
ret &= ~(LCA_TRACI | lcaCounter | LCA_URGENT);
}
if ((ret & LCA_STAY) != 0) {
return ret;
}
if ((ret & LCA_URGENT) != 0) {
// prepare urgent lane change maneuver
// save the left space
myLeftSpace = currentDist - myVehicle.getPositionOnLane();
if (changeToBest && abs(bestLaneOffset) > 1) {
// there might be a vehicle which needs to counter-lane-change one lane further and we cannot see it yet
myLeadingBlockerLength = MAX2((SUMOReal)(right ? 20.0 : 40.0), myLeadingBlockerLength);
}
// letting vehicles merge in at the end of the lane in case of counter-lane change, step#1
// if there is a leader and he wants to change to the opposite direction
saveBlockerLength(neighLead.first, lcaCounter);
if (*firstBlocked != neighLead.first) {
saveBlockerLength(*firstBlocked, lcaCounter);
}
const SUMOReal remainingSeconds = ((ret & LCA_TRACI) == 0 ?
MAX2((SUMOReal)STEPS2TIME(TS), myLeftSpace / myLookAheadSpeed / abs(bestLaneOffset) / URGENCY) :
myVehicle.getInfluencer().changeRequestRemainingSeconds(currentTime));
const SUMOReal plannedSpeed = informLeader(msgPass, blocked, myLca, neighLead, remainingSeconds);
if (plannedSpeed >= 0) {
// maybe we need to deal with a blocking follower
informFollower(msgPass, blocked, myLca, neighFollow, remainingSeconds, plannedSpeed);
}
return ret;
}
if (roundaboutEdgesAhead > 1) {
// try to use the inner lanes of a roundabout to increase throughput
// unless we are approaching the exit
if (lca == LCA_LEFT) {
return ret | lca | LCA_COOPERATIVE;
} else {
return ret | LCA_STAY | LCA_COOPERATIVE;
}
}
// let's also regard the case where the vehicle is driving on a highway...
// in this case, we do not want to get to the dead-end of an on-ramp
if (right) {
if (bestLaneOffset == 0 && myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle) > 80. / 3.6 && myLookAheadSpeed > SUMO_const_haltingSpeed) {
return ret | LCA_STAY | LCA_STRATEGIC;
}
}
// --------
// -------- make place on current lane if blocking follower
//if (amBlockingFollowerPlusNB()) {
// std::cout << myVehicle.getID() << ", " << currentDistAllows(neighDist, bestLaneOffset, laDist)
// << " neighDist=" << neighDist
// << " currentDist=" << currentDist
// << "\n";
//}
if (amBlockingFollowerPlusNB()
&& (changeToBest || currentDistAllows(neighDist, abs(bestLaneOffset) + 1, laDist))) {
return ret | lca | LCA_COOPERATIVE | LCA_URGENT ;//| LCA_CHANGE_TO_HELP;
}
// --------
//// -------- security checks for krauss
//// (vsafe fails when gap<0)
//if ((blocked & LCA_BLOCKED) != 0) {
// return ret;
//}
//// --------
// -------- higher speed
//if ((congested(neighLead.first) && neighLead.second < 20) || predInteraction(leader.first)) { //!!!
// return ret;
//}
SUMOReal thisLaneVSafe = myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle);
SUMOReal neighLaneVSafe = neighLane.getVehicleMaxSpeed(&myVehicle);
if (neighLead.first == 0) {
neighLaneVSafe = MIN2(neighLaneVSafe, myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), neighDist, 0, 0));
} else {
// @todo: what if leader is below safe gap?!!!
neighLaneVSafe = MIN2(neighLaneVSafe, myCarFollowModel.followSpeed(
&myVehicle, myVehicle.getSpeed(), neighLead.second, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()));
}
if (leader.first == 0) {
thisLaneVSafe = MIN2(thisLaneVSafe, myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), currentDist, 0, 0));
} else {
// @todo: what if leader is below safe gap?!!!
thisLaneVSafe = MIN2(thisLaneVSafe, myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), leader.second, leader.first->getSpeed(), leader.first->getCarFollowModel().getMaxDecel()));
}
thisLaneVSafe = MIN3(thisLaneVSafe, myVehicle.getVehicleType().getMaxSpeed(), myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle));
neighLaneVSafe = MIN3(neighLaneVSafe, myVehicle.getVehicleType().getMaxSpeed(), neighLane.getVehicleMaxSpeed(&myVehicle));
const SUMOReal relativeGain = (neighLaneVSafe - thisLaneVSafe) / neighLaneVSafe;
if (right) {
// ONLY FOR CHANGING TO THE RIGHT
if (thisLaneVSafe - 5 / 3.6 > neighLaneVSafe) {
// ok, the current lane is faster than the right one...
if (mySpeedGainProbability < 0) {
mySpeedGainProbability /= 2.0;
//myKeepRightProbability /= 2.0;
}
} else {
// ok, the current lane is not faster than the right one
mySpeedGainProbability -= relativeGain;
// honor the obligation to keep right (Rechtsfahrgebot)
// XXX consider fast approaching followers on the current lane
//const SUMOReal vMax = myLookAheadSpeed;
const SUMOReal vMax = MIN2(myVehicle.getVehicleType().getMaxSpeed(), myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle));
const SUMOReal acceptanceTime = KEEP_RIGHT_ACCEPTANCE * vMax * MAX2((SUMOReal)1, myVehicle.getSpeed()) / myVehicle.getLane()->getSpeedLimit();
SUMOReal fullSpeedGap = MAX2((SUMOReal)0, neighDist - myVehicle.getCarFollowModel().brakeGap(vMax));
SUMOReal fullSpeedDrivingSeconds = MIN2(acceptanceTime, fullSpeedGap / vMax);
if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
fullSpeedGap = MAX2((SUMOReal)0, MIN2(fullSpeedGap,
neighLead.second - myVehicle.getCarFollowModel().getSecureGap(
vMax, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
fullSpeedDrivingSeconds = MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
}
const SUMOReal deltaProb = (CHANGE_PROB_THRESHOLD_RIGHT
* STEPS2TIME(DELTA_T)
* (fullSpeedDrivingSeconds / acceptanceTime) / KEEP_RIGHT_TIME);
myKeepRightProbability -= deltaProb;
if (gDebugFlag2) {
std::cout << STEPS2TIME(currentTime)
<< " veh=" << myVehicle.getID()
<< " vMax=" << vMax
<< " neighDist=" << neighDist
<< " brakeGap=" << myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed())
<< " leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
<< " secGap=" << (neighLead.first == 0 ? -1 : myVehicle.getCarFollowModel().getSecureGap(
myVehicle.getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
<< " acceptanceTime=" << acceptanceTime
<< " fullSpeedGap=" << fullSpeedGap
<< " fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
<< " dProb=" << deltaProb
<< "\n";
}
if (myKeepRightProbability < -CHANGE_PROB_THRESHOLD_RIGHT) {
return ret | lca | LCA_KEEPRIGHT;
}
}
if (mySpeedGainProbability < -CHANGE_PROB_THRESHOLD_RIGHT
&& neighDist / MAX2((SUMOReal) .1, myVehicle.getSpeed()) > 20.) { //./MAX2((SUMOReal) .1, myVehicle.getSpeed())) { // -.1
return ret | lca | LCA_SPEEDGAIN;
}
} else {
// ONLY FOR CHANGING TO THE LEFT
if (thisLaneVSafe > neighLaneVSafe) {
// this lane is better
if (mySpeedGainProbability > 0) {
mySpeedGainProbability /= 2.0;
}
} else {
// left lane is better
mySpeedGainProbability += relativeGain;
}
if (mySpeedGainProbability > CHANGE_PROB_THRESHOLD_LEFT && neighDist / MAX2((SUMOReal) .1, myVehicle.getSpeed()) > 20.) { // .1
return ret | lca | LCA_SPEEDGAIN;
}
}
// --------
if (changeToBest && bestLaneOffset == curr.bestLaneOffset
&& (right ? mySpeedGainProbability < 0 : mySpeedGainProbability > 0)) {
// change towards the correct lane, speedwise it does not hurt
return ret | lca | LCA_STRATEGIC;
}
return ret;
}
int
MSLCM_DK2004::wantsChangeToRight(MSAbstractLaneChangeModel::MSLCMessager& msgPass,
int blocked,
const std::pair<MSVehicle*, SUMOReal>& leader,
const std::pair<MSVehicle*, SUMOReal>& neighLead,
const std::pair<MSVehicle*, SUMOReal>& neighFollow,
const MSLane& neighLane,
const std::vector<MSVehicle::LaneQ>& preb,
MSVehicle** lastBlocked) {
#ifdef DEBUG_VEHICLE_GUI_SELECTION
if (gSelected.isSelected(GLO_VEHICLE, static_cast<const GUIVehicle*>(&myVehicle)->getGlID())) {
int bla = 0;
}
#endif
MSVehicle::LaneQ curr, best;
int bestLaneOffset = 0;
SUMOReal currentDist = 0;
SUMOReal neighDist = 0;
SUMOReal neighExtDist = 0;
SUMOReal currExtDist = 0;
int currIdx = 0;
for (int p = 0; p < (int) preb.size(); ++p) {
if (preb[p].lane == myVehicle.getLane()) {
curr = preb[p];
bestLaneOffset = curr.bestLaneOffset;
currentDist = curr.length;
currExtDist = curr.lane->getLength();
neighDist = preb[p - 1].length;
neighExtDist = preb[p - 1].lane->getLength();
best = preb[p + bestLaneOffset];
currIdx = p;
}
}
// keep information about being a leader/follower
int ret = (myOwnState & 0x00ffff00);
if (leader.first != 0
&&
(myOwnState & LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0
&&
(leader.first->getLaneChangeModel().getOwnState()&LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0) {
myOwnState &= (0xffffffff - LCA_AMBLOCKINGFOLLOWER_DONTBRAKE);
if (myVehicle.getSpeed() > 0.1) {
myOwnState |= LCA_AMBACKBLOCKER;
} else {
ret |= LCA_AMBACKBLOCKER;
myDontBrake = true;
}
}
// process information about the last blocked vehicle
// if this vehicle is blocking someone in front, we maybe decelerate to let him in
if ((*lastBlocked) != 0) {
SUMOReal gap = (*lastBlocked)->getPositionOnLane() - (*lastBlocked)->getVehicleType().getLength() - myVehicle.getPositionOnLane() - myVehicle.getVehicleType().getMinGap();
if (gap > 0.1) {
if (myVehicle.getSpeed() < ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxDecel())) {
if ((*lastBlocked)->getSpeed() < 0.1) {
ret |= LCA_AMBACKBLOCKER_STANDING;
} else {
ret |= LCA_AMBACKBLOCKER;
}
myVSafes.push_back(myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), (SUMOReal)(gap - 0.1), (*lastBlocked)->getSpeed(), (*lastBlocked)->getCarFollowModel().getMaxDecel()));
(*lastBlocked) = 0;
}
return ret;
}
}
// we try to estimate the distance which is necessary to get on a lane
// we have to get on in order to keep our route
// we assume we need something that depends on our velocity
// and compare this with the free space on our wished lane
//
// if the free space is somehow less than the space we need, we should
// definitely try to get to the desired lane
//
// this rule forces our vehicle to change the lane if a lane changing is necessary soon
SUMOReal rv = myVehicle.getSpeed() > LOOK_FORWARD_SPEED_DIVIDER
? myVehicle.getSpeed() * (SUMOReal) LOOK_FORWARD_FAR
: myVehicle.getSpeed() * (SUMOReal) LOOK_FORWARD_NEAR;
rv += myVehicle.getVehicleType().getLengthWithGap() * (SUMOReal) 2.;
SUMOReal tdist = currentDist - myVehicle.getPositionOnLane() - best.occupation * (SUMOReal) JAM_FACTOR2;
if (fabs(best.length - curr.length) > MIN2((SUMOReal) .1, best.lane->getLength()) && bestLaneOffset < 0 && currentDistDisallows(tdist/*currentDist*/, bestLaneOffset, rv)) {
informBlocker(msgPass, blocked, LCA_MRIGHT, neighLead, neighFollow);
if (neighLead.second > 0 && neighLead.second > leader.second) {
myVSafes.push_back(myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), neighLead.second, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()) - (SUMOReal) 0.5);
}
// letting vehicles merge in at the end of the lane in case of counter-lane change, step#1, right
// if there is a leader and he wants to change to left (we want to change to right)
if (neighLead.first != 0 && (neighLead.first->getLaneChangeModel().getOwnState()&LCA_LEFT) != 0) {
// save at least his length in myLeadingBlockerLength
myLeadingBlockerLength = MAX2(neighLead.first->getVehicleType().getLengthWithGap(), myLeadingBlockerLength);
// save the left space
myLeftSpace = currentDist - myVehicle.getPositionOnLane();
}
//
return ret | LCA_RIGHT | LCA_URGENT;
}
// the opposite lane-changing direction should be done than the one examined herein
// we'll check whether we assume we could change anyhow and get back in time...
//
// this rule prevents the vehicle from moving in opposite direction of the best lane
// unless the way till the end where the vehicle has to be on the best lane
// is long enough
SUMOReal maxJam = MAX2(preb[currIdx - 1].occupation, preb[currIdx].occupation);
SUMOReal neighLeftPlace = MAX2((SUMOReal) 0, neighDist - myVehicle.getPositionOnLane() - maxJam);
if (bestLaneOffset >= 0 && (currentDistDisallows(neighLeftPlace, bestLaneOffset + 2, rv))) {
// ...we will not change the lane if not
return ret;
}
// if the current lane is the best and a lane-changing would cause a situation
// of which we assume we will not be able to return to the lane we have to be on...
//
// this rule prevents the vehicle from leaving the current, best lane when it is
// close to this lane's end
if (currExtDist > neighExtDist && (neighLeftPlace * 2. < rv/*||currE[currIdx+1].length<currentDist*/)) {
return ret;
}
// let's also regard the case where the vehicle is driving on a highway...
// in this case, we do not want to get to the dead-end of an on-ramp
//
// THIS RULE APPLIES ONLY TO CHANGING TO THE RIGHT LANE
if (bestLaneOffset == 0 && preb[currIdx - 1].bestLaneOffset != 0 && myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle) > 80. / 3.6) {
return ret;
}
// --------
// -------- make place on current lane if blocking follower
if (amBlockingFollowerPlusNB()
&&
(currentDistAllows(neighDist, bestLaneOffset, rv) || neighDist >= currentDist)) {
return ret | LCA_RIGHT | LCA_URGENT;
}
// --------
// -------- security checks for krauss
// (vsafe fails when gap<0)
if ((blocked & LCA_BLOCKED) != 0) {
return ret;
}
// --------
// -------- higher speed
if ((congested(neighLead.first) && neighLead.second < 20) || predInteraction(leader.first)) { //!!!
return ret;
}
SUMOReal thisLaneVSafe = myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle);
SUMOReal neighLaneVSafe = neighLane.getVehicleMaxSpeed(&myVehicle);
if (neighLead.first == 0) {
neighLaneVSafe = MIN2(neighLaneVSafe, myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), neighDist, 0, 0));
} else {
// @todo: what if leader is below safe gap?!!!
neighLaneVSafe = MIN2(neighLaneVSafe, myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), neighLead.second, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()));
}
if (leader.first == 0) {
thisLaneVSafe = MIN2(thisLaneVSafe, myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), currentDist, 0, 0));
} else {
// @todo: what if leader is below safe gap?!!!
thisLaneVSafe = MIN2(thisLaneVSafe, myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), leader.second, leader.first->getSpeed(), leader.first->getCarFollowModel().getMaxDecel()));
}
thisLaneVSafe = MIN2(thisLaneVSafe, myVehicle.getVehicleType().getMaxSpeed());
neighLaneVSafe = MIN2(neighLaneVSafe, myVehicle.getVehicleType().getMaxSpeed());
if (thisLaneVSafe - neighLaneVSafe > 5. / 3.6) {
// ok, the current lane is faster than the right one...
if (myChangeProbability < 0) {
myChangeProbability /= 2.0;
}
} else {
// ok, the right lane is faster than the current
myChangeProbability -= (SUMOReal)((neighLaneVSafe - thisLaneVSafe) / (myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle)));
}
// let's recheck the "Rechtsfahrgebot"
SUMOReal vmax = MIN2(myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle), myVehicle.getVehicleType().getMaxSpeed());
vmax -= (SUMOReal)(5. / 2.6);
if (neighLaneVSafe >= vmax) {
#ifndef NO_TRACI
/* if there was a request by TraCI for changing to this lane
and holding it, this rule is ignored */
if (myChangeRequest != MSVehicle::REQUEST_HOLD) {
#endif
myChangeProbability -= (SUMOReal)((neighLaneVSafe - vmax) / (vmax));
#ifndef NO_TRACI
}
#endif
}
if (myChangeProbability < -2 && neighDist / MAX2((SUMOReal) .1, myVehicle.getSpeed()) > 20.) { //./MAX2((SUMOReal) .1, myVehicle.getSpeed())) { // -.1
return ret | LCA_RIGHT | LCA_SPEEDGAIN;
}
// --------
#ifndef NO_TRACI
// If there is a request by TraCI, try to change the lane
if (myChangeRequest == MSVehicle::REQUEST_RIGHT) {
return ret | LCA_RIGHT;
}
#endif
return ret;
}
bool
MSLaneChanger::changeOpposite(std::pair<MSVehicle*, SUMOReal> leader) {
if (!myChangeToOpposite) {
return false;
}
myCandi = findCandidate();
MSVehicle* vehicle = veh(myCandi);
MSLane* source = vehicle->getLane();
if (vehicle->isStopped()) {
// stopped vehicles obviously should not change lanes. Usually this is
// prevent by appropriate bestLane distances
return false;
}
const bool isOpposite = vehicle->getLaneChangeModel().isOpposite();
if (!isOpposite && leader.first == 0) {
// no reason to change unless there is a leader
// or we are changing back to the propper direction
// XXX also check whether the leader is so far away as to be irrelevant
return false;
}
MSLane* opposite = source->getOpposite();
if (opposite == 0) {
return false;
}
// changing into the opposite direction is always to the left (XXX except for left-hand networkds)
int direction = isOpposite ? -1 : 1;
std::pair<MSVehicle*, SUMOReal> neighLead((MSVehicle*)0, -1);
// preliminary sanity checks for overtaking space
SUMOReal timeToOvertake;
SUMOReal spaceToOvertake;
if (!isOpposite) {
assert(leader.first != 0);
// find a leader vehicle with sufficient space ahead for merging back
const SUMOReal overtakingSpeed = source->getVehicleMaxSpeed(vehicle); // just a guess
const SUMOReal mergeBrakeGap = vehicle->getCarFollowModel().brakeGap(overtakingSpeed);
std::pair<MSVehicle*, SUMOReal> columnLeader = leader;
SUMOReal egoGap = leader.second;
bool foundSpaceAhead = false;
SUMOReal seen = leader.second + leader.first->getVehicleType().getLengthWithGap();
std::vector<MSLane*> conts = vehicle->getBestLanesContinuation();
while (!foundSpaceAhead) {
const SUMOReal requiredSpaceAfterLeader = (columnLeader.first->getCarFollowModel().getSecureGap(
columnLeader.first->getSpeed(), overtakingSpeed, vehicle->getCarFollowModel().getMaxDecel())
+ vehicle->getVehicleType().getLengthWithGap());
// all leader vehicles on the current laneChanger edge are already moved into MSLane::myTmpVehicles
const bool checkTmpVehicles = (&columnLeader.first->getLane()->getEdge() == &source->getEdge());
std::pair<MSVehicle* const, SUMOReal> leadLead = columnLeader.first->getLane()->getLeader(
columnLeader.first, columnLeader.first->getPositionOnLane(), conts, requiredSpaceAfterLeader + mergeBrakeGap,
checkTmpVehicles);
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " leadLead=" << Named::getIDSecure(leadLead.first) << " gap=" << leadLead.second << "\n";
}
#endif
if (leadLead.first == 0) {
foundSpaceAhead = true;
} else {
const SUMOReal requiredSpace = (requiredSpaceAfterLeader
+ vehicle->getCarFollowModel().getSecureGap(overtakingSpeed, leadLead.first->getSpeed(), leadLead.first->getCarFollowModel().getMaxDecel()));
if (leadLead.second > requiredSpace) {
foundSpaceAhead = true;
} else {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " not enough space after columnLeader=" << columnLeader.first->getID() << " required=" << requiredSpace << "\n";
}
#endif
seen += MAX2((SUMOReal)0, leadLead.second) + leadLead.first->getVehicleType().getLengthWithGap();
if (seen > OPPOSITE_OVERTAKING_MAX_LOOKAHEAD) {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " cannot changeOpposite due to insufficient free space after columnLeader (seen=" << seen << " columnLeader=" << columnLeader.first->getID() << ")\n";
}
#endif
return false;
}
// see if merging after leadLead is possible
egoGap += columnLeader.first->getVehicleType().getLengthWithGap() + leadLead.second;
columnLeader = leadLead;
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " new columnLeader=" << columnLeader.first->getID() << "\n";
}
#endif
}
}
}
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " compute time/space to overtake for columnLeader=" << columnLeader.first->getID() << " gap=" << columnLeader.second << "\n";
}
#endif
computeOvertakingTime(vehicle, columnLeader.first, egoGap, timeToOvertake, spaceToOvertake);
// check for upcoming stops
if (vehicle->nextStopDist() < spaceToOvertake) {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " cannot changeOpposite due to upcoming stop (dist=" << vehicle->nextStopDist() << " spaceToOvertake=" << spaceToOvertake << ")\n";
}
#endif
return false;
}
neighLead = opposite->getOppositeLeader(vehicle, timeToOvertake * opposite->getSpeedLimit() * 2 + spaceToOvertake, true);
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " veh=" << vehicle->getID()
<< " changeOpposite opposite=" << opposite->getID()
<< " lead=" << Named::getIDSecure(leader.first)
<< " timeToOvertake=" << timeToOvertake
<< " spaceToOvertake=" << spaceToOvertake
<< "\n";
}
#endif
// check for dangerous oncoming leader
if (neighLead.first != 0) {
const MSVehicle* oncoming = neighLead.first;
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " oncoming=" << oncoming->getID()
<< " oncomingGap=" << neighLead.second
<< " leaderGap=" << leader.second
<< "\n";
}
#endif
if (neighLead.second - spaceToOvertake - timeToOvertake * oncoming->getSpeed() < 0) {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " cannot changeOpposite due to dangerous oncoming\n";
}
#endif
return false;
}
}
} else {
timeToOvertake = -1;
// look forward as far as possible
spaceToOvertake = std::numeric_limits<SUMOReal>::max();
leader = source->getOppositeLeader(vehicle, OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD, true);
// -1 will use getMaximumBrakeDist() as look-ahead distance
neighLead = opposite->getOppositeLeader(vehicle, -1, false);
}
// compute remaining space on the opposite side
// 1. the part that remains on the current lane
SUMOReal usableDist = isOpposite ? vehicle->getPositionOnLane() : source->getLength() - vehicle->getPositionOnLane();
if (usableDist < spaceToOvertake) {
// look forward along the next lanes
const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation();
assert(bestLaneConts.size() >= 1);
std::vector<MSLane*>::const_iterator it = bestLaneConts.begin() + 1;
while (usableDist < spaceToOvertake && it != bestLaneConts.end()) {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " usableDist=" << usableDist << " opposite=" << Named::getIDSecure((*it)->getOpposite()) << "\n";
}
#endif
if ((*it)->getOpposite() == 0) {
// opposite lane ends
break;
}
// do not overtake past a minor link or turn
if (*(it - 1) != 0) {
MSLink* link = MSLinkContHelper::getConnectingLink(**(it - 1), **it);
if (link == 0 || !link->havePriority() || link->getState() == LINKSTATE_ZIPPER || link->getDirection() != LINKDIR_STRAIGHT) {
break;
}
}
usableDist += (*it)->getLength();
++it;
}
}
if (!isOpposite && usableDist < spaceToOvertake) {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " cannot changeOpposite due to insufficient space (seen=" << usableDist << " spaceToOvertake=" << spaceToOvertake << ")\n";
}
#endif
return false;
}
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " usableDist=" << usableDist << " spaceToOvertake=" << spaceToOvertake << " timeToOvertake=" << timeToOvertake << "\n";
}
#endif
// compute wish to change
std::vector<MSVehicle::LaneQ> preb = vehicle->getBestLanes();
if (isOpposite) {
// compute the remaining distance that can be drive on the opposite side
// this value will put into LaneQ.length of the leftmost lane
// @note: length counts from the start of the current lane
// @note: see MSLCM_LC2013::_wantsChange @1092 (isOpposite()
MSVehicle::LaneQ& laneQ = preb[preb.size() - 1];
// position on the target lane
const SUMOReal forwardPos = source->getOppositePos(vehicle->getPositionOnLane());
// consider usableDist (due to minor links or end of opposite lanes)
laneQ.length = MIN2(laneQ.length, usableDist + forwardPos);
// consider upcoming stops
laneQ.length = MIN2(laneQ.length, vehicle->nextStopDist() + forwardPos);
// consider oncoming leaders
if (leader.first != 0) {
laneQ.length = MIN2(laneQ.length, leader.second / 2 + forwardPos);
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME << " found oncoming leader=" << leader.first->getID() << " gap=" << leader.second << "\n";
}
#endif
leader.first = 0; // ignore leader after this
}
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME << " veh=" << vehicle->getID() << " remaining dist=" << laneQ.length - forwardPos << " forwardPos=" << forwardPos << " laneQ.length=" << laneQ.length << "\n";
}
#endif
}
std::pair<MSVehicle* const, SUMOReal> neighFollow = opposite->getOppositeFollower(vehicle);
int state = checkChange(direction, opposite, leader, neighLead, neighFollow, preb);
bool changingAllowed = (state & LCA_BLOCKED) == 0;
// change if the vehicle wants to and is allowed to change
if ((state & LCA_WANTS_LANECHANGE) != 0 && changingAllowed
// do not change to the opposite direction for cooperative reasons
&& (isOpposite || (state & LCA_COOPERATIVE) == 0)) {
vehicle->getLaneChangeModel().startLaneChangeManeuver(source, opposite, direction);
/// XXX use a dedicated transformation function
vehicle->myState.myPos = source->getOppositePos(vehicle->myState.myPos);
/// XXX compute a better lateral position
opposite->forceVehicleInsertion(vehicle, vehicle->getPositionOnLane(), MSMoveReminder::NOTIFICATION_LANE_CHANGE, 0);
if (!isOpposite) {
vehicle->myState.myBackPos = source->getOppositePos(vehicle->myState.myBackPos);
}
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME << " changing to opposite veh=" << vehicle->getID() << " dir=" << direction << " opposite=" << Named::getIDSecure(opposite) << " state=" << state << "\n";
}
#endif
return true;
}
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME << " not changing to opposite veh=" << vehicle->getID() << " dir=" << direction
<< " opposite=" << Named::getIDSecure(opposite) << " state=" << toString((LaneChangeAction)state) << "\n";
}
#endif
return false;
}
bool
MSLaneChanger::changeOpposite(std::pair<MSVehicle*, SUMOReal> leader) {
if (!myChangeToOpposite) {
return false;
}
myCandi = findCandidate();
MSVehicle* vehicle = veh(myCandi);
MSLane* source = vehicle->getLane();
if (vehicle->isStopped()) {
// stopped vehicles obviously should not change lanes. Usually this is
// prevent by appropriate bestLane distances
return false;
}
const bool isOpposite = vehicle->getLaneChangeModel().isOpposite();
if (!isOpposite && leader.first == 0) {
// no reason to change unless there is a leader
// or we are changing back to the propper direction
// XXX also check whether the leader is so far away as to be irrelevant
return false;
}
if (!source->getEdge().canChangeToOpposite()) {
return false;
}
MSLane* opposite = source->getOpposite();
if (opposite == 0) {
return false;
}
// changing into the opposite direction is always to the left (XXX except for left-hand networkds)
int direction = vehicle->getLaneChangeModel().isOpposite() ? -1 : 1;
std::pair<MSVehicle*, SUMOReal> neighLead((MSVehicle*)0, -1);
// preliminary sanity checks for overtaking space
if (!isOpposite) {
assert(leader.first != 0);
// find a leader vehicle with sufficient space ahead for merging back
const SUMOReal overtakingSpeed = source->getVehicleMaxSpeed(vehicle); // just a guess
const SUMOReal mergeBrakeGap = vehicle->getCarFollowModel().brakeGap(overtakingSpeed);
const SUMOReal maxLookAhead = 150; // just a guess
std::pair<MSVehicle*, SUMOReal> columnLeader = leader;
SUMOReal egoGap = leader.second;
bool foundSpaceAhead = false;
SUMOReal seen = leader.second + leader.first->getVehicleType().getLengthWithGap();
std::vector<MSLane*> conts = vehicle->getBestLanesContinuation();
while (!foundSpaceAhead) {
const SUMOReal requiredSpaceAfterLeader = (columnLeader.first->getCarFollowModel().getSecureGap(
columnLeader.first->getSpeed(), overtakingSpeed, vehicle->getCarFollowModel().getMaxDecel())
+ vehicle->getVehicleType().getLengthWithGap());
std::pair<MSVehicle* const, SUMOReal> leadLead = columnLeader.first->getLane()->getLeader(
columnLeader.first, columnLeader.first->getPositionOnLane(), conts, requiredSpaceAfterLeader + mergeBrakeGap, true);
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " leadLead=" << Named::getIDSecure(leadLead.first) << " gap=" << leadLead.second << "\n";
}
#endif
if (leadLead.first == 0) {
foundSpaceAhead = true;
} else {
const SUMOReal requiredSpace = (requiredSpaceAfterLeader
+ vehicle->getCarFollowModel().getSecureGap(overtakingSpeed, leadLead.first->getSpeed(), leadLead.first->getCarFollowModel().getMaxDecel()));
if (leadLead.second > requiredSpace) {
foundSpaceAhead = true;
} else {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " not enough space after columnLeader=" << leadLead.first->getID() << " gap=" << leadLead.second << " required=" << requiredSpace << "\n";
}
#endif
seen += leadLead.second + leadLead.first->getVehicleType().getLengthWithGap();
if (seen > maxLookAhead) {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " cannot changeOpposite due to insufficient free space after columnLeader (seen=" << seen << " columnLeader=" << leadLead.first->getID() << ")\n";
}
#endif
return false;
}
// see if merging after leadLead is possible
egoGap += columnLeader.first->getVehicleType().getLengthWithGap() + leadLead.second;
columnLeader = leadLead;
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " new columnLeader=" << columnLeader.first->getID() << "\n";
}
#endif
}
}
}
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " compute time/space to overtake for columnLeader=" << columnLeader.first->getID() << " gap=" << columnLeader.second << "\n";
}
#endif
SUMOReal timeToOvertake;
SUMOReal spaceToOvertake;
computeOvertakingTime(vehicle, columnLeader.first, egoGap, timeToOvertake, spaceToOvertake);
// check for upcoming stops
if (vehicle->nextStopDist() < spaceToOvertake) {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " cannot changeOpposite due to upcoming stop (dist=" << vehicle->nextStopDist() << " spaceToOvertake=" << spaceToOvertake << ")\n";
}
#endif
return false;
}
neighLead = opposite->getOppositeLeader(vehicle, timeToOvertake * opposite->getSpeedLimit() * 2 + spaceToOvertake);
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " veh=" << vehicle->getID()
<< " changeOpposite opposite=" << opposite->getID()
<< " lead=" << Named::getIDSecure(leader.first)
<< " oncoming=" << Named::getIDSecure(neighLead.first)
<< " timeToOvertake=" << timeToOvertake
<< " spaceToOvertake=" << spaceToOvertake
<< "\n";
}
#endif
// check for dangerous oncoming leader
if (!vehicle->getLaneChangeModel().isOpposite() && neighLead.first != 0) {
const MSVehicle* oncoming = neighLead.first;
/// XXX what about overtaking multiple vehicles?
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " timeToOvertake=" << timeToOvertake
<< " spaceToOvertake=" << spaceToOvertake
<< " oncomingGap=" << neighLead.second
<< " leaderGap=" << leader.second
<< "\n";
}
#endif
if (neighLead.second - spaceToOvertake - timeToOvertake * oncoming->getSpeed() < 0) {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " cannot changeOpposite due to dangerous oncoming\n";
}
#endif
return false;
}
}
// check for sufficient space on the opposite side
seen = source->getLength() - vehicle->getPositionOnLane();
if (!vehicle->getLaneChangeModel().isOpposite() && seen < spaceToOvertake) {
const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation();
assert(bestLaneConts.size() >= 1);
std::vector<MSLane*>::const_iterator it = bestLaneConts.begin() + 1;
while (seen < spaceToOvertake && it != bestLaneConts.end()) {
if ((*it)->getOpposite() == 0) {
break;
}
// do not overtake past a minor link
if (*(it - 1) != 0) {
MSLink* link = MSLinkContHelper::getConnectingLink(**(it - 1), **it);
if (link == 0 || !link->havePriority() || link->getState() == LINKSTATE_ZIPPER) {
break;
}
}
seen += (*it)->getLength();
}
if (seen < spaceToOvertake) {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " cannot changeOpposite due to insufficient space (seen=" << seen << " spaceToOvertake=" << spaceToOvertake << ")\n";
}
#endif
return false;
}
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " seen=" << seen << " spaceToOvertake=" << spaceToOvertake << " timeToOvertake=" << timeToOvertake << "\n";
}
#endif
}
} else {
/// XXX compute sensible distance
leader = source->getOppositeLeader(vehicle, 200);
neighLead = opposite->getOppositeLeader(vehicle, -1);
}
// compute wish to change
std::vector<MSVehicle::LaneQ> preb = vehicle->getBestLanes();
if (isOpposite && leader.first != 0) {
MSVehicle::LaneQ& laneQ = preb[preb.size() - 1];
/// XXX compute sensible usable dist
laneQ.length -= MIN2(laneQ.length, opposite->getOppositePos(vehicle->getPositionOnLane()) + leader.second / 2);
leader.first = 0; // ignore leader
}
std::pair<MSVehicle* const, SUMOReal> neighFollow = opposite->getOppositeFollower(vehicle);
int state = checkChange(direction, opposite, leader, neighLead, neighFollow, preb);
bool changingAllowed = (state & LCA_BLOCKED) == 0;
// change if the vehicle wants to and is allowed to change
if ((state & LCA_WANTS_LANECHANGE) != 0 && changingAllowed) {
vehicle->getLaneChangeModel().startLaneChangeManeuver(source, opposite, direction);
/// XXX use a dedicated transformation function
vehicle->myState.myPos = source->getOppositePos(vehicle->myState.myPos);
vehicle->myState.myBackPos = source->getOppositePos(vehicle->myState.myBackPos);
/// XXX compute a bette lateral position
opposite->forceVehicleInsertion(vehicle, vehicle->getPositionOnLane(), MSMoveReminder::NOTIFICATION_LANE_CHANGE, 0);
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME << " changing to opposite veh=" << vehicle->getID() << " dir=" << direction << " opposite=" << Named::getIDSecure(opposite) << " state=" << state << "\n";
}
#endif
return true;
}
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME << " not changing to opposite veh=" << vehicle->getID() << " dir=" << direction << " opposite=" << Named::getIDSecure(opposite) << " state=" << state << "\n";
}
#endif
return false;
}
