void
MSLCM_DK2004::informBlocker(MSAbstractLaneChangeModel::MSLCMessager& msgPass,
int& blocked,
int dir,
const std::pair<MSVehicle*, SUMOReal>& neighLead,
const std::pair<MSVehicle*, SUMOReal>& neighFollow) {
if ((blocked & LCA_BLOCKED_BY_FOLLOWER) != 0) {
assert(neighFollow.first != 0);
MSVehicle* nv = neighFollow.first;
SUMOReal decelGap =
neighFollow.second
+ SPEED2DIST(myVehicle.getSpeed()) * (SUMOReal) 2.0
- MAX2(nv->getSpeed() - (SUMOReal) ACCEL2DIST(nv->getCarFollowModel().getMaxDecel()) * (SUMOReal) 2.0, (SUMOReal) 0);
if (neighFollow.second > 0 && decelGap > 0 && decelGap >= nv->getCarFollowModel().getSecureGap(nv->getSpeed(), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel())) {
SUMOReal vsafe = myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), neighFollow.second, neighFollow.first->getSpeed(), neighFollow.first->getCarFollowModel().getMaxDecel());
msgPass.informNeighFollower(new Info(vsafe, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
} else {
SUMOReal vsafe = neighFollow.second <= 0 ? 0 : myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), neighFollow.second, neighFollow.first->getSpeed(), neighFollow.first->getCarFollowModel().getMaxDecel());
msgPass.informNeighFollower(new Info(vsafe, dir | LCA_AMBLOCKINGFOLLOWER_DONTBRAKE), &myVehicle);
}
}
if ((blocked & LCA_BLOCKED_BY_LEADER) != 0) {
if (neighLead.first != 0 && neighLead.second > 0) {
msgPass.informNeighLeader(new Info(0, dir | LCA_AMBLOCKINGLEADER), &myVehicle);
}
}
}
std::pair<MSVehicle* const, SUMOReal>
MSLaneChanger::getRealFollower(const ChangerIt& target) const {
MSVehicle* neighFollow = veh(target);
// check whether the hopped vehicle got the follower
if (target->hoppedVeh != 0) {
SUMOReal hoppedPos = target->hoppedVeh->getPositionOnLane();
if (hoppedPos <= veh(myCandi)->getPositionOnLane() && (neighFollow == 0 || neighFollow->getPositionOnLane() > hoppedPos)) {
neighFollow = target->hoppedVeh;
}
}
if (neighFollow == 0) {
SUMOReal speed = target->lane->getSpeedLimit();
// in order to look back, we'd need the minimum braking ability of vehicles in the net...
// we'll assume it to be 4m/s^2
// !!!revisit
SUMOReal dist = speed * speed / (2.*4.) + SPEED2DIST(speed);
dist = MIN2(dist, (SUMOReal) 500.);
MSVehicle* candi = veh(myCandi);
SUMOReal seen = candi->getPositionOnLane() - candi->getVehicleType().getLength();
return target->lane->getFollowerOnConsecutive(dist, seen, candi->getSpeed(), candi->getPositionOnLane() - candi->getVehicleType().getLength(), 4.5);//!!! recheck
} else {
MSVehicle* candi = veh(myCandi);
return std::pair<MSVehicle* const, SUMOReal>(neighFollow, candi->getPositionOnLane() - candi->getVehicleType().getLength() - neighFollow->getPositionOnLane() - neighFollow->getVehicleType().getMinGap());
}
}
//TODO: refactor out? only called by setCritical for getting the leader (last veh of lane)
MSVehicle*
MSLane::pop(SUMOTime) {
assert(! myVehicles.empty());
MSVehicle* first = myVehicles.back();
first->leaveLaneAtMove(SPEED2DIST(first->getSpeed())/* - first->pos()*/);
myVehicles.pop_back();
myVehicleLengthSum -= first->getVehicleType().getLength();
return first;
}
void
MSVehicleTransfer::loadState(const SUMOSAXAttributes& attrs, const SUMOTime offset, MSVehicleControl& vc) {
MSVehicle* veh = dynamic_cast<MSVehicle*>(vc.getVehicle(attrs.getString(SUMO_ATTR_ID)));
assert(veh != 0);
SUMOTime proceedTime = (SUMOTime)attrs.getLong(SUMO_ATTR_DEPART);
MSLane* parkingLane = attrs.hasAttribute(SUMO_ATTR_PARKING) ? MSLane::dictionary(attrs.getString(SUMO_ATTR_PARKING)): 0;
myVehicles.push_back(VehicleInformation(veh, proceedTime + offset, parkingLane != 0));
if (parkingLane != 0) {
myParkingVehicles[parkingLane].insert(veh);
veh->setTentativeLaneAndPosition(parkingLane, veh->getPositionOnLane());
veh->processNextStop(veh->getSpeed());
}
MSNet::getInstance()->getInsertionControl().alreadyDeparted(veh);
}
std::pair<MSVehicle* const, SUMOReal>
MSLaneChanger::getRealFollower(const ChangerIt& target) const {
MSVehicle* candi = veh(myCandi);
const SUMOReal candiPos = candi->getPositionOnLane();
MSVehicle* neighFollow = veh(target);
// check whether the hopped vehicle became the follower
neighFollow = getCloserFollower(candiPos, neighFollow, target->hoppedVeh);
neighFollow = getCloserFollower(candiPos, neighFollow, target->lane->getPartialBehind(candi));
if (neighFollow == 0) {
return target->lane->getFollowerOnConsecutive(
candi->getPositionOnLane() - candi->getVehicleType().getLength(),
candi->getSpeed(), candi->getCarFollowModel().getMaxDecel());
} else {
MSVehicle* candi = veh(myCandi);
return std::pair<MSVehicle* const, SUMOReal>(neighFollow,
candi->getPositionOnLane() - candi->getVehicleType().getLength() - neighFollow->getPositionOnLane() - neighFollow->getVehicleType().getMinGap());
}
}
bool
MSLink::maybeOccupied(MSLane* lane) {
MSVehicle* veh = lane->getLastVehicle();
SUMOReal distLeft = 0;
if (veh == 0) {
veh = lane->getPartialOccupator();
distLeft = lane->getLength() - lane->getPartialOccupatorEnd();
} else {
distLeft = lane->getLength() - veh->getPositionOnLane() + veh->getVehicleType().getLength();
}
if (veh == 0) {
return false;
} else {
assert(distLeft > 0);
// can we be sure that the vehicle leaves this lane in the next step?
bool result = distLeft > (veh->getSpeed() - veh->getCarFollowModel().getMaxDecel());
return result;
}
}
std::pair<MSVehicle * const, SUMOReal>
MSLane::getFollowerOnConsecutive(SUMOReal dist, SUMOReal seen, SUMOReal leaderSpeed, SUMOReal backOffset) const {
// ok, a vehicle has not noticed the lane about itself;
// iterate as long as necessary to search for an approaching one
std::set<MSLane*> visited;
std::vector<std::pair<MSVehicle *, SUMOReal> > possible;
std::vector<MSLane::IncomingLaneInfo> newFound;
std::vector<MSLane::IncomingLaneInfo> toExamine = myIncomingLanes;
while (toExamine.size()!=0) {
for (std::vector<MSLane::IncomingLaneInfo>::iterator i=toExamine.begin(); i!=toExamine.end(); ++i) {
/*
if ((*i).viaLink->getState()==MSLink::LINKSTATE_TL_RED) {
continue;
}
*/
MSLane *next = (*i).lane;
if (next->getFirstVehicle()!=0) {
MSVehicle * v = (MSVehicle*) next->getFirstVehicle();
SUMOReal agap = (*i).length - v->getPositionOnLane() + backOffset;
if (!v->getCarFollowModel().hasSafeGap(v->getCarFollowModel().maxNextSpeed(v->getSpeed()), agap, leaderSpeed, v->getLane().getMaxSpeed())) {
possible.push_back(std::make_pair(v, (*i).length-v->getPositionOnLane()+seen));
}
} else {
if ((*i).length+seen<dist) {
const std::vector<MSLane::IncomingLaneInfo> &followers = next->getIncomingLanes();
for (std::vector<MSLane::IncomingLaneInfo>::const_iterator j=followers.begin(); j!=followers.end(); ++j) {
if (visited.find((*j).lane)==visited.end()) {
visited.insert((*j).lane);
MSLane::IncomingLaneInfo ili;
ili.lane = (*j).lane;
ili.length = (*j).length + (*i).length;
ili.viaLink = (*j).viaLink;
newFound.push_back(ili);
}
}
}
}
}
toExamine.clear();
swap(newFound, toExamine);
}
if (possible.size()==0) {
return std::pair<MSVehicle * const, SUMOReal>(0, -1);
}
sort(possible.begin(), possible.end(), by_second_sorter());
return *(possible.begin());
}
bool
MSLane::isEmissionSuccess(MSVehicle* aVehicle,
SUMOReal speed, SUMOReal pos,
bool patchSpeed, size_t startStripId) throw(ProcessError) {
// and the speed is not too high (vehicle should decelerate)
// try to get a leader on consecutive lanes
// we have to do this even if we have found a leader on our lane because it may
// be driving into another direction
//std::cerr<<"EMISSION speed:"<<speed<<std::endl;
std::cerr<<"EMISSION vehicle:"<<aVehicle->getID()<<std::endl;
size_t endStripId = startStripId + aVehicle->getWidth() - 1;
assert(startStripId >=0 && endStripId < myStrips.size());
aVehicle->getBestLanes(true, this);
const MSCFModel &cfModel = aVehicle->getCarFollowModel();
const std::vector<MSLane*> &bestLaneConts = aVehicle->getBestLanesContinuation(this);
std::vector<MSLane*>::const_iterator ri = bestLaneConts.begin();
SUMOReal seen = getLength() - pos;
SUMOReal dist = cfModel.brakeGap(speed);
const MSRoute &r = aVehicle->getRoute();
MSRouteIterator ce = r.begin();
MSLane *currentLane = this;
MSLane *nextLane = this;
while (seen<dist&&ri!=bestLaneConts.end()&&nextLane!=0/*&&ce!=r.end()*/) {
// get the next link used...
MSLinkCont::const_iterator link = currentLane->succLinkSec(*aVehicle, 1, *currentLane, bestLaneConts);
// ...and the next used lane (including internal)
if (!currentLane->isLinkEnd(link) && (*link)->havePriority() && (*link)->getState()!=MSLink::LINKSTATE_TL_RED) { // red may have priority?
#ifdef HAVE_INTERNAL_LANES
bool nextInternal = false;
nextLane = (*link)->getViaLane();
if (nextLane==0) {
nextLane = (*link)->getLane();
} else {
nextInternal = true;
}
#else
nextLane = (*link)->getLane();
#endif
} else {
nextLane = 0;
}
// check how next lane effects the journey
if (nextLane!=0) {
SUMOReal gap = 0;
//TODO: fix get & set partial occupator to strip level
MSVehicle * leader = 0;//currentLane->getPartialOccupator();
if (leader!=0) {
gap = getPartialOccupatorEnd();
} else {
// check leader on next lane
leader = nextLane->getLastVehicle(aVehicle->getStrips());
if (leader!=0) {
gap = seen+leader->getPositionOnLane()-leader->getVehicleType().getLength();
}
}
if (leader!=0) {
SUMOReal nspeed = gap>=0 ? cfModel.ffeV(aVehicle, speed, gap, leader->getSpeed()) : 0;
if (nspeed<speed) {
if (patchSpeed) {
speed = MIN2(nspeed, speed);
dist = cfModel.brakeGap(speed);
} else {
// we may not drive with the given velocity - we crash into the leader
return false;
}
}
}
// check next lane's maximum velocity
SUMOReal nspeed = nextLane->getMaxSpeed();
if (nspeed<speed) {
// patch speed if needed
if (patchSpeed) {
speed = MIN2(cfModel.ffeV(aVehicle, speed, seen, nspeed), speed);
dist = cfModel.brakeGap(speed);
} else {
// we may not drive with the given velocity - we would be too fast on the next lane
return false;
}
}
// check traffic on next junctions
const SUMOTime arrivalTime = MSNet::getInstance()->getCurrentTimeStep() + TIME2STEPS(seen / speed);
#ifdef HAVE_INTERNAL_LANES
const SUMOTime leaveTime = (*link)->getViaLane()==0 ? arrivalTime + TIME2STEPS((*link)->getLength() * speed) : arrivalTime + TIME2STEPS((*link)->getViaLane()->getLength() * speed);
#else
const SUMOTime leaveTime = arrivalTime + TIME2STEPS((*link)->getLength() * speed);
#endif
if ((*link)->hasApproachingFoe(arrivalTime, leaveTime)) {
SUMOReal nspeed = cfModel.ffeV(aVehicle, speed, seen, 0);
if (nspeed<speed) {
if (patchSpeed) {
speed = MIN2(nspeed, speed);
dist = cfModel.brakeGap(speed);
} else {
// we may not drive with the given velocity - we crash into the leader
return false;
}
}
} else {
// we can only drive to the end of the current lane...
SUMOReal nspeed = cfModel.ffeV(aVehicle, speed, seen, 0);
if (nspeed<speed) {
if (patchSpeed) {
speed = MIN2(nspeed, speed);
dist = cfModel.brakeGap(speed);
} else {
// we may not drive with the given velocity - we crash into the leader
return false;
}
}
}
seen += nextLane->getLength();
++ce;
++ri;
currentLane = nextLane;
}
}
if (seen<dist) {
SUMOReal nspeed = cfModel.ffeV(aVehicle, speed, seen, 0);
if (nspeed<speed) {
if (patchSpeed) {
speed = MIN2(nspeed, speed);
dist = cfModel.brakeGap(speed);
} else {
// we may not drive with the given velocity - we crash into the leader
MsgHandler::getErrorInstance()->inform("Vehicle '" + aVehicle->getID() + "' will not be able to emit using given velocity!");
// !!! we probably should do something else...
return false;
}
}
}
// get the pointer to the vehicle next in front of the given position
MSVehicle *pred;
std::vector<MSVehicle *> predCont;
std::vector<MSVehicle *>::iterator predIt, it;
for (unsigned int i=startStripId; i<=endStripId; ++i) {
predCont.push_back(myStrips.at(i)->getPredAtPos(pos));
}
predIt = predCont.begin();
SUMOReal currMin = -1;
if (*predIt != 0) {
currMin = (*predIt)->getPositionOnLane();
} else {
// signals no leader in front
predIt = predCont.end();
}
for (it = predCont.begin(); it != predCont.end(); ++it) {
if (*it == 0) continue;
if ((*it)->getPositionOnLane() < currMin) {
predIt = it;
currMin = (*it)->getPositionOnLane();
}
}
if (predIt != predCont.end()) {
// ok, there is one (a leader)
MSVehicle* leader = *predIt;
SUMOReal frontGapNeeded = aVehicle->getCarFollowModel().getSecureGap(speed, leader->getCarFollowModel().getSpeedAfterMaxDecel(leader->getSpeed()));
SUMOReal gap = MSVehicle::gap(leader->getPositionOnLane(), leader->getVehicleType().getLength(), pos);
if (gap<frontGapNeeded) {
// too close to the leader on this lane
return false;
}
}
// FIXME: implement look back
// check back vehicle
if (false/*predIt!=myVehicles.begin()*/) {
// there is direct follower on this lane
MSVehicle *follower = *(predIt-1);
SUMOReal backGapNeeded = follower->getCarFollowModel().getSecureGap(follower->getSpeed(), aVehicle->getCarFollowModel().getSpeedAfterMaxDecel(speed));
SUMOReal gap = MSVehicle::gap(pos, aVehicle->getVehicleType().getLength(), follower->getPositionOnLane());
if (gap<backGapNeeded) {
// too close to the follower on this lane
return false;
}
} else if (false) {
// check approaching vehicle (consecutive follower)
SUMOReal lspeed = getMaxSpeed();
// in order to look back, we'd need the minimum braking ability of vehicles in the net...
// we'll assume it to be 4m/s^2
// !!!revisit
SUMOReal dist = lspeed * lspeed * SUMOReal(1./2.*4.) + SPEED2DIST(lspeed);
std::pair<const MSVehicle * const, SUMOReal> approaching = getFollowerOnConsecutive(dist, 0, speed, pos - aVehicle->getVehicleType().getLength());
if (approaching.first!=0) {
const MSVehicle *const follower = approaching.first;
SUMOReal backGapNeeded = follower->getCarFollowModel().getSecureGap(follower->getSpeed(), aVehicle->getCarFollowModel().getSpeedAfterMaxDecel(speed));
SUMOReal gap = approaching.second - pos - aVehicle->getVehicleType().getLength();
if (gap<backGapNeeded) {
// too close to the consecutive follower
return false;
}
}
// check for in-lapping vehicle
MSVehicle* leader = getPartialOccupator();
if (leader!=0) {
SUMOReal frontGapNeeded = aVehicle->getCarFollowModel().getSecureGap(speed, leader->getCarFollowModel().getSpeedAfterMaxDecel(leader->getSpeed()));
SUMOReal gap = getPartialOccupatorEnd() - pos;
if (gap<=frontGapNeeded) {
// too close to the leader on this lane
return false;
}
}
}
// may got negative while adaptation
if (speed<0) {
return false;
}
// enter
//XXX: later change to enterStripAtEmit()?
//if (speed < 0.0001) speed += 10.0;
StripCont strips;
strips.resize(aVehicle->getWidth());
StripCont::iterator start = myStrips.begin() + startStripId;
std::copy(start, start + aVehicle->getWidth(), strips.begin());
aVehicle->enterLaneAtEmit(this, pos, speed, strips);
bool wasInactive = getVehicleNumber()==0;
if (true/*predIt==myVehicles.end()*/) {
// vehicle will be the first on the lane
//std::cerr<<"startStripId:"<<startStripId<<", NumStrips:"<<strips.size()<<", VehWidth:"<<aVehicle->getWidth()<<std::endl;
for (size_t i=startStripId; i<startStripId+strips.size(); ++i) {
this->getStrip(i)->pushIntoStrip(aVehicle);
this->getStrip(i)->setVehLenSum(this->getStrip(i)->getVehLenSum() +
aVehicle->getVehicleType().getLength());
}
aVehicle->printDebugMsg("Emitting");
printDebugMsg();
} else {
//this->getStrip(0).insert(0, aVehicle);
}
//myVehicleLengthSum += aVehicle->getVehicleType().getLength();
if (wasInactive) {
MSNet::getInstance()->getEdgeControl().gotActive(this);
}
return true;
}
// ------ Vehicle emission ------
bool
MSLane::freeEmit(MSVehicle& veh, SUMOReal mspeed) throw() {
size_t stripId = getEmptyStartStripID(veh.getWidth());
bool adaptableSpeed = true;
if (getVehicleNumber()==0) {
if (isEmissionSuccess(&veh, mspeed, 0, adaptableSpeed,stripId)) {
return true;
}
} else {
// check whether the vehicle can be put behind the last one if there is such
MSVehicle *leader = getLastVehicle(veh.getStrips());
if (leader != 0) {
SUMOReal leaderPos = leader->getPositionOnLane() - leader->getVehicleType().getLength();
SUMOReal speed = mspeed;
if (adaptableSpeed) {
speed = leader->getSpeed();
}
SUMOReal frontGapNeeded = veh.getCarFollowModel().getSecureGap(speed, leader->getCarFollowModel().getSpeedAfterMaxDecel(leader->getSpeed()));
if (leaderPos-frontGapNeeded>=0) {
SUMOReal tspeed = MIN2(veh.getCarFollowModel().ffeV(&veh, mspeed, frontGapNeeded, leader->getSpeed()), mspeed);
// check whether we can emit in behind the last vehicle on the lane
if (isEmissionSuccess(&veh, tspeed, 0, adaptableSpeed, stripId)) {
return true;
}
else std::cerr << "not successful emission 1" ;
}
} else {
if (isEmissionSuccess(&veh, mspeed, 0, adaptableSpeed, stripId)) {
return true;
}
else std::cerr << "not successful emission 2" ;
}
}
/* ashu
StripCont strips =getMyStrips();
for (StripContConstIter it=strips.begin(); it != strips.end(); ++it)
{if ((*it)->freeEmitCheck(veh, mspeed))
return true;}
//TODO: Uncomment and fix
// go through the lane, look for free positions (starting after the last vehicle)
MSLane::VehCont::iterator predIt = myVehicles.begin();
while (predIt!=myVehicles.end()) {
// get leader (may be zero) and follower
const MSVehicle *leader = predIt!=myVehicles.end()-1 ? *(predIt+1) : getPartialOccupator();
const MSVehicle *follower = *predIt;
// patch speed if allowed
SUMOReal speed = mspeed;
if (adaptableSpeed&&leader!=0) {
speed = MIN2(leader->getSpeed(), mspeed);
}
// compute the space needed to not collide with leader
SUMOReal frontMax = getLength();
if (leader!=0) {
SUMOReal leaderRearPos = leader->getPositionOnLane() - leader->getVehicleType().getLength();
if (leader == getPartialOccupator()) {
leaderRearPos = getPartialOccupatorEnd();
}
frontMax = leaderRearPos - veh.getCarFollowModel().getSecureGap(speed, leader->getCarFollowModel().getSpeedAfterMaxDecel(leader->getSpeed()));
}
// compute the space needed to not let the follower collide
const SUMOReal followPos = follower->getPositionOnLane();
const SUMOReal backGapNeeded = follower->getCarFollowModel().getSecureGap(follower->getSpeed(), veh.getCarFollowModel().getSpeedAfterMaxDecel(speed));
const SUMOReal backMin = followPos + backGapNeeded + veh.getVehicleType().getLength();
// check whether there is enough room (given some extra space for rounding errors)
if (frontMax>0 && backMin+POSITION_EPS<frontMax) {
// try emit vehicle (should be always ok)
if (isEmissionSuccess(&veh, speed, backMin+POSITION_EPS, adaptableSpeed)) {
std::cerr << "FIX working" ;
return true;
}
}
++predIt;
}
*/
//TODO: Recheck01 ___AB oct 2011
StripCont strips =getMyStrips();
for (StripContConstIter it=strips.begin(); it != strips.end(); ++it)
{
// go through the lane, look for free positions (starting after the last vehicle)
MSLane::VehCont::iterator predIt = (*it)->myVehicles.begin();
while (predIt!=(*it)->myVehicles.end()) {
// get leader (may be zero) and follower
const MSVehicle *leader = predIt!=(*it)->myVehicles.end()-1 ? *(predIt+1) : (*it)->getPartialOccupator();
const MSVehicle *follower = *predIt;
// patch speed if allowed
SUMOReal speed = mspeed;
if (adaptableSpeed&&leader!=0) {
speed = MIN2(leader->getSpeed(), mspeed);
}
// compute the space needed to not collide with leader
SUMOReal frontMax = getLength();
if (leader!=0) {
SUMOReal leaderRearPos = leader->getPositionOnLane() - leader->getVehicleType().getLength();
if (leader == (*it)->getPartialOccupator()) {
leaderRearPos = (*it)->getPartialOccupatorEnd();
}
frontMax = leaderRearPos - veh.getCarFollowModel().getSecureGap(speed, leader->getCarFollowModel().getSpeedAfterMaxDecel(leader->getSpeed()));
}
// compute the space needed to not let the follower collide
const SUMOReal followPos = follower->getPositionOnLane();
const SUMOReal backGapNeeded = follower->getCarFollowModel().getSecureGap(follower->getSpeed(), veh.getCarFollowModel().getSpeedAfterMaxDecel(speed));
const SUMOReal backMin = followPos + backGapNeeded + veh.getVehicleType().getLength();
// check whether there is enough room (given some extra space for rounding errors)
if (frontMax>0 && backMin+POSITION_EPS<frontMax) {
// try emit vehicle (should be always ok)
if (isEmissionSuccess(&veh, speed, backMin+POSITION_EPS, adaptableSpeed, stripId)) {
std::cerr << "FIX working" ;
return true;
}
}
++predIt;
}
}//for
// first check at lane's begin
std::cerr << "not successful emission last" ;
return false;
}
void
MSE2Collector::detectorUpdate(const SUMOTime /* step */) {
JamInfo* currentJam = 0;
std::map<SUMOVehicle*, SUMOTime> haltingVehicles;
std::map<SUMOVehicle*, SUMOTime> intervalHaltingVehicles;
std::vector<JamInfo*> jams;
SUMOReal lengthSum = 0;
myCurrentMeanSpeed = 0;
myCurrentMeanLength = 0;
myCurrentStartedHalts = 0;
myCurrentHaltingsNumber = 0;
// go through the (sorted) list of vehicles positioned on the detector
// sum up values and prepare the list of jams
myKnownVehicles.sort(by_vehicle_position_sorter(getLane()));
for (std::list<SUMOVehicle*>::const_iterator i = myKnownVehicles.begin(); i != myKnownVehicles.end(); ++i) {
MSVehicle* veh = static_cast<MSVehicle*>(*i);
SUMOReal length = veh->getVehicleType().getLength();
if (veh->getLane() == getLane()) {
if (veh->getPositionOnLane() - veh->getVehicleType().getLength() < myStartPos) {
// vehicle entered detector partially
length -= (veh->getVehicleType().getLength() - (veh->getPositionOnLane() - myStartPos));
}
if (veh->getPositionOnLane() > myEndPos && veh->getPositionOnLane() - veh->getVehicleType().getLength() <= myEndPos) {
// vehicle left detector partially
length -= (veh->getPositionOnLane() - myEndPos);
}
} else {
// ok, the vehicle is only partially still on the detector, has already moved to the
// next lane; still, we do not know how far away it is
assert(veh == myLane->getPartialOccupator());
length = myEndPos - myLane->getPartialOccupatorEnd();
}
assert(length >= 0);
mySpeedSum += veh->getSpeed();
myCurrentMeanSpeed += veh->getSpeed();
lengthSum += length;
myCurrentMeanLength += length;
// jam-checking begins
bool isInJam = false;
// first, check whether the vehicle is slow enough to be states as halting
if (veh->getSpeed() < myJamHaltingSpeedThreshold) {
myCurrentHaltingsNumber++;
// we have to track the time it was halting;
// so let's look up whether it was halting before and compute the overall halting time
bool wasHalting = myHaltingVehicleDurations.find(veh) != myHaltingVehicleDurations.end();
if (wasHalting) {
haltingVehicles[veh] = myHaltingVehicleDurations[veh] + DELTA_T;
intervalHaltingVehicles[veh] = myIntervalHaltingVehicleDurations[veh] + DELTA_T;
} else {
haltingVehicles[veh] = DELTA_T;
intervalHaltingVehicles[veh] = DELTA_T;
myCurrentStartedHalts++;
myStartedHalts++;
}
// we now check whether the halting time is large enough
if (haltingVehicles[veh] > myJamHaltingTimeThreshold) {
// yep --> the vehicle is a part of a jam
isInJam = true;
}
} else {
// is not standing anymore; keep duration information
std::map<SUMOVehicle*, SUMOTime>::iterator v = myHaltingVehicleDurations.find(veh);
if (v != myHaltingVehicleDurations.end()) {
myPastStandingDurations.push_back((*v).second);
myHaltingVehicleDurations.erase(v);
}
v = myIntervalHaltingVehicleDurations.find(veh);
if (v != myIntervalHaltingVehicleDurations.end()) {
myPastIntervalStandingDurations.push_back((*v).second);
myIntervalHaltingVehicleDurations.erase(v);
}
}
// jam-building
if (isInJam) {
// the vehicle is in a jam;
// it may be a new one or already an existing one
if (currentJam == 0) {
// the vehicle is the first vehicle in a jam
currentJam = new JamInfo;
currentJam->firstStandingVehicle = i;
} else {
// ok, we have a jam already. But - maybe it is too far away
// ... honestly, I can hardly find a reason for doing this,
// but jams were defined this way in an earlier version...
if (veh->getPositionOnLane() - (*currentJam->lastStandingVehicle)->getPositionOnLane() > myJamDistanceThreshold) {
// yep, yep, yep - it's a new one...
// close the frist, build a new
jams.push_back(currentJam);
currentJam = new JamInfo;
currentJam->firstStandingVehicle = i;
}
}
currentJam->lastStandingVehicle = i;
} else {
// the vehicle is not part of a jam...
// maybe we have to close an already computed jam
if (currentJam != 0) {
jams.push_back(currentJam);
currentJam = 0;
}
}
}
if (currentJam != 0) {
jams.push_back(currentJam);
currentJam = 0;
}
myCurrentMaxJamLengthInMeters = 0;
myCurrentMaxJamLengthInVehicles = 0;
myCurrentJamLengthInMeters = 0;
myCurrentJamLengthInVehicles = 0;
// process jam information
for (std::vector<JamInfo*>::iterator i = jams.begin(); i != jams.end(); ++i) {
// compute current jam's values
SUMOReal jamLengthInMeters =
(*(*i)->firstStandingVehicle)->getPositionOnLane()
- (*(*i)->lastStandingVehicle)->getPositionOnLane()
+ (*(*i)->lastStandingVehicle)->getVehicleType().getLengthWithGap();
const MSVehicle* const occ = myLane->getPartialOccupator();
if (occ && occ == *(*i)->firstStandingVehicle && occ != *(*i)->lastStandingVehicle) {
jamLengthInMeters = myLane->getPartialOccupatorEnd() + occ->getVehicleType().getLengthWithGap()
- (*(*i)->lastStandingVehicle)->getPositionOnLane()
+ (*(*i)->lastStandingVehicle)->getVehicleType().getLengthWithGap();
}
unsigned jamLengthInVehicles = (unsigned) distance((*i)->firstStandingVehicle, (*i)->lastStandingVehicle) + 1;
// apply them to the statistics
myCurrentMaxJamLengthInMeters = MAX2(myCurrentMaxJamLengthInMeters, jamLengthInMeters);
myCurrentMaxJamLengthInVehicles = MAX2(myCurrentMaxJamLengthInVehicles, jamLengthInVehicles);
myJamLengthInMetersSum += jamLengthInMeters;
myJamLengthInVehiclesSum += jamLengthInVehicles;
myCurrentJamLengthInMeters += jamLengthInMeters;
myCurrentJamLengthInVehicles += jamLengthInVehicles;
}
myCurrentJamNo = (unsigned) jams.size();
unsigned noVehicles = (unsigned) myKnownVehicles.size();
myVehicleSamples += noVehicles;
myTimeSamples += 1;
// compute occupancy values
SUMOReal currentOccupancy = lengthSum / (myEndPos - myStartPos) * (SUMOReal) 100.;
myCurrentOccupancy = currentOccupancy;
myOccupancySum += currentOccupancy;
myMaxOccupancy = MAX2(myMaxOccupancy, currentOccupancy);
// compute jam values
myMeanMaxJamInVehicles += myCurrentMaxJamLengthInVehicles;
myMeanMaxJamInMeters += myCurrentMaxJamLengthInMeters;
myMaxJamInVehicles = MAX2(myMaxJamInVehicles, myCurrentMaxJamLengthInVehicles);
myMaxJamInMeters = MAX2(myMaxJamInMeters, myCurrentMaxJamLengthInMeters);
// save information about halting vehicles
myHaltingVehicleDurations = haltingVehicles;
myIntervalHaltingVehicleDurations = intervalHaltingVehicles;
// compute information about vehicle numbers
myMeanVehicleNumber += (unsigned) myKnownVehicles.size();
myMaxVehicleNumber = MAX2((unsigned) myKnownVehicles.size(), myMaxVehicleNumber);
// norm current values
myCurrentMeanSpeed = noVehicles != 0 ? myCurrentMeanSpeed / (SUMOReal) noVehicles : -1;
myCurrentMeanLength = noVehicles != 0 ? myCurrentMeanLength / (SUMOReal) noVehicles : -1;
// clean up
for (std::vector<JamInfo*>::iterator i = jams.begin(); i != jams.end(); ++i) {
delete *i;
}
jams.clear();
}
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;
}
void
MSLCM_LC2013::informFollower(MSAbstractLaneChangeModel::MSLCMessager& msgPass,
int blocked,
int dir,
const std::pair<MSVehicle*, SUMOReal>& neighFollow,
SUMOReal remainingSeconds,
SUMOReal plannedSpeed) {
if ((blocked & LCA_BLOCKED_BY_FOLLOWER) != 0) {
assert(neighFollow.first != 0);
MSVehicle* nv = neighFollow.first;
// are we fast enough to cut in without any help?
if (plannedSpeed - nv->getSpeed() >= HELP_OVERTAKE) {
const SUMOReal neededGap = nv->getCarFollowModel().getSecureGap(nv->getSpeed(), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel());
if ((neededGap - neighFollow.second) / remainingSeconds < (plannedSpeed - nv->getSpeed())) {
// follower might even accelerate but not to much
msgPass.informNeighFollower(new Info(plannedSpeed - HELP_OVERTAKE, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
return;
}
}
// decide whether we will request help to cut in before the follower or allow to be overtaken
// PARAMETERS
// assume other vehicle will assume the equivalent of 1 second of
// maximum deceleration to help us (will probably be spread over
// multiple seconds)
// -----------
const SUMOReal helpDecel = nv->getCarFollowModel().getMaxDecel() * HELP_DECEL_FACTOR ;
// change in the gap between ego and blocker over 1 second (not STEP!)
const SUMOReal neighNewSpeed = MAX2((SUMOReal)0, nv->getSpeed() - ACCEL2SPEED(helpDecel));
const SUMOReal neighNewSpeed1s = MAX2((SUMOReal)0, nv->getSpeed() - helpDecel);
const SUMOReal dv = plannedSpeed - neighNewSpeed1s;
// new gap between follower and self in case the follower does brake for 1s
const SUMOReal decelGap = neighFollow.second + dv;
const SUMOReal secureGap = nv->getCarFollowModel().getSecureGap(neighNewSpeed1s, plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel());
if (decelGap > 0 && decelGap >= secureGap) {
// if the blocking neighbor brakes it could actually help
// how hard does it actually need to be?
const SUMOReal vsafe = MAX2(neighNewSpeed, nv->getCarFollowModel().followSpeed(
nv, nv->getSpeed(), neighFollow.second, plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel()));
msgPass.informNeighFollower(new Info(vsafe, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
} else if (dv > 0 && dv * remainingSeconds > (secureGap - decelGap + POSITION_EPS)) {
// decelerating once is sufficient to open up a large enough gap in time
msgPass.informNeighFollower(new Info(neighNewSpeed, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
} else {
SUMOReal vhelp = MAX2(nv->getSpeed(), myVehicle.getSpeed() + HELP_OVERTAKE);
//if (dir == LCA_MRIGHT && myVehicle.getWaitingSeconds() > LCA_RIGHT_IMPATIENCE &&
// nv->getSpeed() > myVehicle.getSpeed()) {
if (nv->getSpeed() > myVehicle.getSpeed() &&
((dir == LCA_MRIGHT && myVehicle.getWaitingSeconds() > LCA_RIGHT_IMPATIENCE)
|| (dir == LCA_MLEFT && plannedSpeed > CUT_IN_LEFT_SPEED_THRESHOLD) // VARIANT_22 (slowDownLeft)
)) {
// let the follower slow down to increase the likelyhood that later vehicles will be slow enough to help
// follower should still be fast enough to open a gap
vhelp = MAX2(neighNewSpeed, myVehicle.getSpeed() + HELP_OVERTAKE);
if ((nv->getSpeed() - myVehicle.getSpeed()) / helpDecel < remainingSeconds) {
msgPass.informNeighFollower(new Info(vhelp, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
return;
}
}
msgPass.informNeighFollower(new Info(vhelp, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
// this follower is supposed to overtake us. slow down smoothly to allow this
const SUMOReal overtakeDist = (neighFollow.second // follower reaches ego back
+ myVehicle.getVehicleType().getLengthWithGap() // follower reaches ego front
+ nv->getVehicleType().getLength() // follower back at ego front
+ myVehicle.getCarFollowModel().getSecureGap( // follower has safe dist to ego
plannedSpeed, vhelp, nv->getCarFollowModel().getMaxDecel()));
// speed difference to create a sufficiently large gap
const SUMOReal needDV = overtakeDist / remainingSeconds;
// make sure the deceleration is not to strong
myVSafes.push_back(MAX2(vhelp - needDV, myVehicle.getSpeed() - ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxDecel())));
}
}
}
SUMOReal
MSLCM_LC2013::informLeader(MSAbstractLaneChangeModel::MSLCMessager& msgPass,
int blocked,
int dir,
const std::pair<MSVehicle*, SUMOReal>& neighLead,
SUMOReal remainingSeconds) {
SUMOReal plannedSpeed = MIN2(myVehicle.getSpeed(),
myVehicle.getCarFollowModel().stopSpeed(&myVehicle, myVehicle.getSpeed(), myLeftSpace - myLeadingBlockerLength));
for (std::vector<SUMOReal>::const_iterator i = myVSafes.begin(); i != myVSafes.end(); ++i) {
SUMOReal v = (*i);
if (v >= myVehicle.getSpeed() - ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxDecel())) {
plannedSpeed = MIN2(plannedSpeed, v);
}
}
if ((blocked & LCA_BLOCKED_BY_LEADER) != 0) {
assert(neighLead.first != 0);
MSVehicle* nv = neighLead.first;
// decide whether we want to overtake the leader or follow it
const SUMOReal dv = plannedSpeed - nv->getSpeed();
const SUMOReal overtakeDist = (neighLead.second // drive to back of follower
+ nv->getVehicleType().getLengthWithGap() // drive to front of follower
+ myVehicle.getVehicleType().getLength() // ego back reaches follower front
+ nv->getCarFollowModel().getSecureGap( // save gap to follower
nv->getSpeed(), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel()));
if (dv < 0
// overtaking on the right on an uncongested highway is forbidden (noOvertakeLCLeft)
|| (dir == LCA_MLEFT && !myVehicle.congested())
// not enough space to overtake?
|| myLeftSpace < overtakeDist
// not enough time to overtake?
|| dv * remainingSeconds < overtakeDist) {
// cannot overtake
msgPass.informNeighLeader(new Info(-1, dir | LCA_AMBLOCKINGLEADER), &myVehicle);
// slow down smoothly to follow leader
const SUMOReal targetSpeed = myCarFollowModel.followSpeed(
&myVehicle, myVehicle.getSpeed(), neighLead.second, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel());
if (targetSpeed < myVehicle.getSpeed()) {
// slow down smoothly to follow leader
const SUMOReal decel = ACCEL2SPEED(MIN2(myVehicle.getCarFollowModel().getMaxDecel(),
MAX2(MIN_FALLBEHIND, (myVehicle.getSpeed() - targetSpeed) / remainingSeconds)));
const SUMOReal nextSpeed = MIN2(plannedSpeed, myVehicle.getSpeed() - decel);
myVSafes.push_back(nextSpeed);
return nextSpeed;
} else {
// leader is fast enough anyway
myVSafes.push_back(targetSpeed);
return plannedSpeed;
}
} else {
// overtaking, leader should not accelerate
msgPass.informNeighLeader(new Info(nv->getSpeed(), dir | LCA_AMBLOCKINGLEADER), &myVehicle);
return -1;
}
} else if (neighLead.first != 0) { // (remainUnblocked)
// we are not blocked now. make sure we stay far enough from the leader
MSVehicle* nv = neighLead.first;
const SUMOReal nextNVSpeed = nv->getSpeed() - HELP_OVERTAKE; // conservative
const SUMOReal dv = SPEED2DIST(myVehicle.getSpeed() - nextNVSpeed);
const SUMOReal targetSpeed = myCarFollowModel.followSpeed(
&myVehicle, myVehicle.getSpeed(), neighLead.second - dv, nextNVSpeed, nv->getCarFollowModel().getMaxDecel());
myVSafes.push_back(targetSpeed);
return MIN2(targetSpeed, plannedSpeed);
} else {
// not overtaking
return plannedSpeed;
}
}
bool
MSLaneChanger::change() {
// Find change-candidate. If it is on an allowed lane, try to change
// to the right (there is a rule in Germany that you have to change
// to the right, unless you are overtaking). If change to the right
// isn't possible, check if there is a possibility to overtake (on the
// left.
// If candidate isn't on an allowed lane, changing to an allowed has
// priority.
myCandi = findCandidate();
MSVehicle* vehicle = veh(myCandi);
#ifdef DEBUG_VEHICLE_GUI_SELECTION
if (gSelected.isSelected(GLO_VEHICLE, static_cast<const GUIVehicle*>(vehicle)->getGlID())) {
int bla = 0;
}
#endif
const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes();
assert(preb.size() == myChanger.size());
for (int i = 0; i < (int) myChanger.size(); ++i) {
((std::vector<MSVehicle::LaneQ>&) preb)[i].occupation = myChanger[i].dens + preb[i].nextOccupation;
}
vehicle->getLaneChangeModel().prepareStep();
std::pair<MSVehicle* const, SUMOReal> leader = getRealThisLeader(myCandi);
// check whether the vehicle wants and is able to change to right lane
int state1 = 0;
if (myCandi != myChanger.begin() && (myCandi - 1)->lane->allowsVehicleClass(veh(myCandi)->getVehicleType().getVehicleClass())) {
std::pair<MSVehicle* const, SUMOReal> rLead = getRealLeader(myCandi - 1);
std::pair<MSVehicle* const, SUMOReal> rFollow = getRealFollower(myCandi - 1);
state1 = change2right(leader, rLead, rFollow, preb);
if ((state1 & LCA_URGENT) != 0 || (state1 & LCA_SPEEDGAIN) != 0) {
state1 |= LCA_RIGHT;
}
bool changingAllowed1 = (state1 & LCA_BLOCKED) == 0;
// change if the vehicle wants to and is allowed to change
if ((state1 & LCA_RIGHT) != 0 && changingAllowed1) {
#ifndef NO_TRACI
// inform lane change model about this change
vehicle->getLaneChangeModel().fulfillChangeRequest(MSVehicle::REQUEST_RIGHT);
#endif
(myCandi - 1)->hoppedVeh = vehicle;
(myCandi - 1)->lane->myTmpVehicles.push_front(vehicle);
vehicle->leaveLane(MSMoveReminder::NOTIFICATION_LANE_CHANGE);
myCandi->lane->leftByLaneChange(vehicle);
vehicle->enterLaneAtLaneChange((myCandi - 1)->lane);
(myCandi - 1)->lane->enteredByLaneChange(vehicle);
vehicle->myLastLaneChangeOffset = 0;
vehicle->getLaneChangeModel().changed();
(myCandi - 1)->dens += (myCandi - 1)->hoppedVeh->getVehicleType().getLengthWithGap();
return true;
}
if ((state1 & LCA_RIGHT) != 0 && (state1 & LCA_URGENT) != 0) {
(myCandi - 1)->lastBlocked = vehicle;
}
}
// check whether the vehicle wants and is able to change to left lane
int state2 = 0;
if ((myCandi + 1) != myChanger.end() && (myCandi + 1)->lane->allowsVehicleClass(veh(myCandi)->getVehicleType().getVehicleClass())) {
std::pair<MSVehicle* const, SUMOReal> lLead = getRealLeader(myCandi + 1);
std::pair<MSVehicle* const, SUMOReal> lFollow = getRealFollower(myCandi + 1);
state2 = change2left(leader, lLead, lFollow, preb);
if ((state2 & LCA_URGENT) != 0 || (state2 & LCA_SPEEDGAIN) != 0) {
state2 |= LCA_LEFT;
}
bool changingAllowed2 = (state2 & LCA_BLOCKED) == 0;
//vehicle->getLaneChangeModel().setOwnState(state2|state1);
// change if the vehicle wants to and is allowed to change
if ((state2 & LCA_LEFT) != 0 && changingAllowed2) {
#ifndef NO_TRACI
// inform lane change model about this change
vehicle->getLaneChangeModel().fulfillChangeRequest(MSVehicle::REQUEST_LEFT);
#endif
(myCandi + 1)->hoppedVeh = veh(myCandi);
(myCandi + 1)->lane->myTmpVehicles.push_front(veh(myCandi));
vehicle->leaveLane(MSMoveReminder::NOTIFICATION_LANE_CHANGE);
myCandi->lane->leftByLaneChange(vehicle);
vehicle->enterLaneAtLaneChange((myCandi + 1)->lane);
(myCandi + 1)->lane->enteredByLaneChange(vehicle);
vehicle->myLastLaneChangeOffset = 0;
vehicle->getLaneChangeModel().changed();
(myCandi + 1)->dens += (myCandi + 1)->hoppedVeh->getVehicleType().getLengthWithGap();
return true;
}
if ((state2 & LCA_LEFT) != 0 && (state2 & LCA_URGENT) != 0) {
(myCandi + 1)->lastBlocked = vehicle;
}
}
vehicle->getLaneChangeModel().setOwnState(state2 | state1);
if ((state1 & (LCA_URGENT)) != 0 && (state2 & (LCA_URGENT)) != 0) {
// ... wants to go to the left AND to the right
// just let them go to the right lane...
state2 = 0;
vehicle->getLaneChangeModel().setOwnState(state1);
}
// check whether the vehicles should be swapped
if (myAllowsSwap && ((state1 & (LCA_URGENT)) != 0 || (state2 & (LCA_URGENT)) != 0)) {
// get the direction ...
ChangerIt target;
if ((state1 & (LCA_URGENT)) != 0) {
// ... wants to go right
target = myCandi - 1;
}
if ((state2 & (LCA_URGENT)) != 0) {
// ... wants to go left
target = myCandi + 1;
}
MSVehicle* prohibitor = target->lead;
if (target->hoppedVeh != 0) {
SUMOReal hoppedPos = target->hoppedVeh->getPositionOnLane();
if (prohibitor == 0 || (hoppedPos > vehicle->getPositionOnLane() && prohibitor->getPositionOnLane() > hoppedPos)) {
prohibitor = 0;// !!! vehicles should not jump over more than one lanetarget->hoppedVeh;
}
}
if (prohibitor != 0
&&
((prohibitor->getLaneChangeModel().getOwnState() & (LCA_URGENT/*|LCA_SPEEDGAIN*/)) != 0
&&
(prohibitor->getLaneChangeModel().getOwnState() & (LCA_LEFT | LCA_RIGHT))
!=
(vehicle->getLaneChangeModel().getOwnState() & (LCA_LEFT | LCA_RIGHT))
)
) {
// check for position and speed
if (prohibitor->getVehicleType().getLengthWithGap() - vehicle->getVehicleType().getLengthWithGap() == 0) {
// ok, may be swapped
// remove vehicle to swap with
MSLane::VehCont::iterator i = find(target->lane->myTmpVehicles.begin(), target->lane->myTmpVehicles.end(), prohibitor);
if (i != target->lane->myTmpVehicles.end()) {
MSVehicle* bla = *i;
assert(bla == prohibitor);
target->lane->myTmpVehicles.erase(i);
// set this vehicle
target->hoppedVeh = vehicle;
target->lane->myTmpVehicles.push_front(vehicle);
myCandi->hoppedVeh = prohibitor;
myCandi->lane->myTmpVehicles.push_front(prohibitor);
// leave lane and detectors
vehicle->leaveLane(MSMoveReminder::NOTIFICATION_LANE_CHANGE);
prohibitor->leaveLane(MSMoveReminder::NOTIFICATION_LANE_CHANGE);
// patch position and speed
SUMOReal p1 = vehicle->getPositionOnLane();
vehicle->myState.myPos = prohibitor->myState.myPos;
prohibitor->myState.myPos = p1;
p1 = vehicle->getSpeed();
vehicle->myState.mySpeed = prohibitor->myState.mySpeed;
prohibitor->myState.mySpeed = p1;
// enter lane and detectors
vehicle->enterLaneAtLaneChange(target->lane);
prohibitor->enterLaneAtLaneChange(myCandi->lane);
// mark lane change
vehicle->getLaneChangeModel().changed();
vehicle->myLastLaneChangeOffset = 0;
prohibitor->getLaneChangeModel().changed();
prohibitor->myLastLaneChangeOffset = 0;
(myCandi)->dens += prohibitor->getVehicleType().getLengthWithGap();
(target)->dens += vehicle->getVehicleType().getLengthWithGap();
return true;
}
}
}
}
// Candidate didn't change lane.
myCandi->lane->myTmpVehicles.push_front(veh(myCandi));
vehicle->myLastLaneChangeOffset += DELTA_T;
(myCandi)->dens += vehicle->getVehicleType().getLengthWithGap();
return false;
}
int
MSLaneChanger::checkChange(
int laneOffset,
const MSLane* targetLane,
const std::pair<MSVehicle* const, SUMOReal>& leader,
const std::pair<MSVehicle* const, SUMOReal>& neighLead,
const std::pair<MSVehicle* const, SUMOReal>& neighFollow,
const std::vector<MSVehicle::LaneQ>& preb) const {
MSVehicle* vehicle = veh(myCandi);
// Debug (Leo)
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout
<< "\n" << SIMTIME << " checkChange() for vehicle '" << vehicle->getID() << "'"
<< std::endl;
}
#endif
int blocked = 0;
int blockedByLeader = (laneOffset == -1 ? LCA_BLOCKED_BY_RIGHT_LEADER : LCA_BLOCKED_BY_LEFT_LEADER);
int blockedByFollower = (laneOffset == -1 ? LCA_BLOCKED_BY_RIGHT_FOLLOWER : LCA_BLOCKED_BY_LEFT_FOLLOWER);
// overlap
if (neighFollow.first != 0 && neighFollow.second < 0) {
blocked |= (blockedByFollower | LCA_OVERLAPPING);
// Debug (Leo)
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " overlapping with follower..."
<< std::endl;
}
#endif
}
if (neighLead.first != 0 && neighLead.second < 0) {
blocked |= (blockedByLeader | LCA_OVERLAPPING);
// Debug (Leo)
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " overlapping with leader..."
<< std::endl;
}
#endif
}
// safe back gap
if ((blocked & blockedByFollower) == 0 && neighFollow.first != 0) {
// !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren
if (neighFollow.second < neighFollow.first->getCarFollowModel().getSecureGap(neighFollow.first->getSpeed(), vehicle->getSpeed(), vehicle->getCarFollowModel().getMaxDecel())) {
blocked |= blockedByFollower;
// Debug (Leo)
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " back gap unsafe: "
<< "gap = " << neighFollow.second
<< ", secureGap = "
<< neighFollow.first->getCarFollowModel().getSecureGap(neighFollow.first->getSpeed(),
vehicle->getSpeed(), vehicle->getCarFollowModel().getMaxDecel())
<< std::endl;
}
#endif
}
}
// safe front gap
if ((blocked & blockedByLeader) == 0 && neighLead.first != 0) {
// !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren
if (neighLead.second < vehicle->getCarFollowModel().getSecureGap(vehicle->getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())) {
blocked |= blockedByLeader;
// Debug (Leo)
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " front gap unsafe: "
<< "gap = " << neighLead.second
<< ", secureGap = "
<< vehicle->getCarFollowModel().getSecureGap(vehicle->getSpeed(),
neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())
<< std::endl;
}
#endif
}
}
MSAbstractLaneChangeModel::MSLCMessager msg(leader.first, neighLead.first, neighFollow.first);
int state = blocked | vehicle->getLaneChangeModel().wantsChange(
laneOffset, msg, blocked, leader, neighLead, neighFollow, *targetLane, preb, &(myCandi->lastBlocked), &(myCandi->firstBlocked));
if (blocked == 0 && (state & LCA_WANTS_LANECHANGE) != 0 && neighLead.first != 0) {
// do are more carefull (but expensive) check to ensure that a
// safety-critical leader is not being overloocked
const SUMOReal seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
const SUMOReal speed = vehicle->getSpeed();
const SUMOReal dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
if (seen < dist) {
std::pair<MSVehicle* const, SUMOReal> neighLead2 = targetLane->getCriticalLeader(dist, seen, speed, *vehicle);
if (neighLead2.first != 0 && neighLead2.first != neighLead.first
&& (neighLead2.second < vehicle->getCarFollowModel().getSecureGap(
vehicle->getSpeed(), neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel()))) {
state |= blockedByLeader;
}
}
}
if (blocked == 0 && (state & LCA_WANTS_LANECHANGE)) {
// ensure that merging is safe for any upcoming zipper links after changing
if (vehicle->unsafeLinkAhead(targetLane)) {
state |= blockedByLeader;
}
}
if ((state & LCA_BLOCKED) == 0 && (state & LCA_WANTS_LANECHANGE) != 0 && MSGlobals::gLaneChangeDuration > DELTA_T) {
// ensure that a continuous lane change manoeuvre can be completed
// before the next turning movement
SUMOReal seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
const SUMOReal decel = vehicle->getCarFollowModel().getMaxDecel() * STEPS2TIME(MSGlobals::gLaneChangeDuration);
const SUMOReal avgSpeed = 0.5 * (
MAX2((SUMOReal)0, vehicle->getSpeed() - ACCEL2SPEED(vehicle->getCarFollowModel().getMaxDecel())) +
MAX2((SUMOReal)0, vehicle->getSpeed() - decel));
const SUMOReal space2change = avgSpeed * STEPS2TIME(MSGlobals::gLaneChangeDuration);
// for finding turns it doesn't matter whether we look along the current lane or the target lane
const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation();
int view = 1;
MSLane* nextLane = vehicle->getLane();
MSLinkCont::const_iterator link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
while (!nextLane->isLinkEnd(link) && seen <= space2change) {
if ((*link)->getDirection() == LINKDIR_LEFT || (*link)->getDirection() == LINKDIR_RIGHT
// the lanes after an internal junction are on different
// edges and do not allow lane-changing
|| (nextLane->getEdge().isInternal() && (*link)->getViaLaneOrLane()->getEdge().isInternal())
) {
state |= LCA_INSUFFICIENT_SPACE;
break;
}
#ifdef HAVE_INTERNAL_LANES
if ((*link)->getViaLane() == 0) {
view++;
}
#else
view++;
#endif
nextLane = (*link)->getViaLaneOrLane();
seen += nextLane->getLength();
// get the next link used
link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
}
if (nextLane->isLinkEnd(link) && seen < space2change) {
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout << SIMTIME << " checkChange insufficientSpace: seen=" << seen << " space2change=" << space2change << "\n";
}
#endif
state |= LCA_INSUFFICIENT_SPACE;
}
if ((state & LCA_BLOCKED) == 0) {
// check for dangerous leaders in case the target lane changes laterally between
// now and the lane-changing midpoint
const SUMOReal speed = vehicle->getSpeed();
seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
nextLane = vehicle->getLane();
view = 1;
const SUMOReal dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
MSLinkCont::const_iterator link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
while (!nextLane->isLinkEnd(link) && seen <= space2change && seen <= dist) {
nextLane = (*link)->getViaLaneOrLane();
MSLane* targetLane = nextLane->getParallelLane(laneOffset);
if (targetLane == 0) {
state |= LCA_INSUFFICIENT_SPACE;
break;
} else {
std::pair<MSVehicle* const, SUMOReal> neighLead2 = targetLane->getLeader(vehicle, -seen, std::vector<MSLane*>());
if (neighLead2.first != 0 && neighLead2.first != neighLead.first
&& (neighLead2.second < vehicle->getCarFollowModel().getSecureGap(
vehicle->getSpeed(), neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel()))) {
state |= blockedByLeader;
break;
}
}
#ifdef HAVE_INTERNAL_LANES
if ((*link)->getViaLane() == 0) {
view++;
}
#else
view++;
#endif
seen += nextLane->getLength();
// get the next link used
link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
}
}
}
#ifndef NO_TRACI
#ifdef DEBUG_CHECK_CHANGE
const int oldstate = state;
#endif
// let TraCI influence the wish to change lanes and the security to take
state = vehicle->influenceChangeDecision(state);
#endif
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " veh=" << vehicle->getID()
<< " oldState=" << toString((LaneChangeAction)oldstate)
<< " newState=" << toString((LaneChangeAction)state)
<< ((blocked & LCA_BLOCKED) ? " (blocked)" : "")
<< ((blocked & LCA_OVERLAPPING) ? " (overlap)" : "")
<< "\n";
}
#endif
return state;
}
std::pair<MSVehicle* const, SUMOReal>
MSLaneChanger::getRealFollower(const ChangerIt& target) const {
assert(veh(myCandi) != 0);
#ifdef DEBUG_SURROUNDING_VEHICLES
MSVehicle* vehicle = veh(myCandi);
if (DEBUG_COND) {
std::cout << SIMTIME << " veh '" << vehicle->getID() << "' looks for follower on lc-target lane '" << target->lane->getID() << "'." << std::endl;
}
#endif
MSVehicle* candi = veh(myCandi);
const SUMOReal candiPos = candi->getPositionOnLane();
MSVehicle* neighFollow = veh(target);
#ifdef DEBUG_SURROUNDING_VEHICLES
if (DEBUG_COND) {
if (neighFollow != 0) {
std::cout << "veh(target) returns '" << neighFollow->getID() << "' at position " << neighFollow->getPositionOnLane() << std::endl;
} else {
std::cout << "veh(target) returns none." << std::endl;
}
}
#endif
#ifdef DEBUG_SURROUNDING_VEHICLES
if (DEBUG_COND) {
if (getCloserFollower(candiPos, neighFollow, target->hoppedVeh) != neighFollow) {
std::cout << "Hopped vehicle '" << target->hoppedVeh->getID() << "' at position " << target->hoppedVeh->getPositionOnLane() << " is closer." << std::endl;
}
}
#endif
// check whether the hopped vehicle became the follower
neighFollow = getCloserFollower(candiPos, neighFollow, target->hoppedVeh);
#ifdef DEBUG_SURROUNDING_VEHICLES
if (DEBUG_COND) {
MSVehicle* partialBehind = getCloserFollower(candiPos, neighFollow, target->lane->getPartialBehind(candi));
if (partialBehind != 0 && partialBehind != neighFollow) {
std::cout << "'Partial behind'-vehicle '" << target->lane->getPartialBehind(candi)->getID() << "' at position " << target->hoppedVeh->getPositionOnLane() << " is closer." << std::endl;
}
}
#endif
// or a follower which is partially lapping into the target lane
neighFollow = getCloserFollower(candiPos, neighFollow, target->lane->getPartialBehind(candi));
if (neighFollow == 0) {
std::pair<MSVehicle* const, SUMOReal> consecutiveFollower = target->lane->getFollowerOnConsecutive(
candi->getPositionOnLane() - candi->getVehicleType().getLength(),
candi->getSpeed(), candi->getCarFollowModel().getMaxDecel());
#ifdef DEBUG_SURROUNDING_VEHICLES
if (DEBUG_COND) {
if (consecutiveFollower.first == 0) {
std::cout << "no follower found." << std::endl;
} else {
std::cout << "found follower '" << consecutiveFollower.first->getID() << "' on consecutive lanes." << std::endl;
}
}
#endif
return consecutiveFollower;
} else {
#ifdef DEBUG_SURROUNDING_VEHICLES
if (DEBUG_COND) {
std::cout << "found follower '" << neighFollow->getID() << "'." << std::endl;
}
#endif
MSVehicle* candi = veh(myCandi);
return std::pair<MSVehicle* const, SUMOReal>(neighFollow,
candi->getPositionOnLane() - candi->getVehicleType().getLength() - neighFollow->getPositionOnLane() - neighFollow->getVehicleType().getMinGap());
}
}