std::pair<MSVehicle* const, SUMOReal>
MSLaneChanger::getRealThisLeader(const ChangerIt& target) const {
// get the leading vehicle on the lane to change to
MSVehicle* leader = target->lead;
if (leader == 0) {
MSLane* targetLane = target->lane;
MSVehicle* predP = targetLane->getPartialOccupator();
if (predP != 0) {
return std::pair<MSVehicle*, SUMOReal>(predP, targetLane->getPartialOccupatorEnd() - veh(myCandi)->getPositionOnLane());
}
const std::vector<MSLane*>& bestLaneConts = veh(myCandi)->getBestLanesContinuation();
MSLinkCont::const_iterator link = targetLane->succLinkSec(*veh(myCandi), 1, *targetLane, bestLaneConts);
if (targetLane->isLinkEnd(link)) {
return std::pair<MSVehicle*, SUMOReal>(static_cast<MSVehicle*>(0), -1);
}
MSLane* nextLane = (*link)->getLane();
if (nextLane == 0) {
return std::pair<MSVehicle*, SUMOReal>(static_cast<MSVehicle*>(0), -1);
}
leader = nextLane->getLastVehicle();
if (leader == 0) {
return std::pair<MSVehicle*, SUMOReal>(static_cast<MSVehicle*>(0), -1);
}
SUMOReal gap =
leader->getPositionOnLane() - leader->getVehicleType().getLength()
+
(myCandi->lane->getLength() - veh(myCandi)->getPositionOnLane() - veh(myCandi)->getVehicleType().getMinGap()); // !!! recheck
return std::pair<MSVehicle* const, SUMOReal>(leader, MAX2((SUMOReal) 0, gap));
} else {
MSVehicle* candi = veh(myCandi);
SUMOReal gap = leader->getPositionOnLane() - leader->getVehicleType().getLength() - candi->getPositionOnLane() - candi->getVehicleType().getMinGap();
return std::pair<MSVehicle* const, SUMOReal>(leader, MAX2((SUMOReal) 0, gap));
}
}
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;
}
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;
}
