int
MSLaneChangerSublane::checkChangeSublane(
int laneOffset,
const std::vector<MSVehicle::LaneQ>& preb,
SUMOReal& latDist) const {
ChangerIt target = myCandi + laneOffset;
MSVehicle* vehicle = veh(myCandi);
const MSLane& neighLane = *(target->lane);
int blocked = 0;
//gDebugFlag1 = vehicle->getLaneChangeModel().debugVehicle();
MSLeaderDistanceInfo neighLeaders = getLeaders(target, vehicle);
MSLeaderDistanceInfo neighFollowers = target->lane->getFollowersOnConsecutive(vehicle, true);
MSLeaderDistanceInfo neighBlockers(&neighLane, vehicle, vehicle->getLane()->getRightSideOnEdge() - neighLane.getRightSideOnEdge());
MSLeaderDistanceInfo leaders = getLeaders(myCandi, vehicle);
MSLeaderDistanceInfo followers = myCandi->lane->getFollowersOnConsecutive(vehicle, true);
MSLeaderDistanceInfo blockers(vehicle->getLane(), vehicle, 0);
if (gDebugFlag1) std::cout << SIMTIME
<< " checkChangeSublane: veh=" << vehicle->getID()
<< " laneOffset=" << laneOffset
<< "\n leaders=" << leaders.toString()
<< "\n neighLeaders=" << neighLeaders.toString()
<< "\n";
const int wish = vehicle->getLaneChangeModel().wantsChangeSublane(
laneOffset,
leaders, followers, blockers,
neighLeaders, neighFollowers, neighBlockers,
neighLane, preb,
&(myCandi->lastBlocked), &(myCandi->firstBlocked), latDist, blocked);
int state = blocked | wish;
// XXX
// do are more carefull (but expensive) check to ensure that a
// safety-critical leader is not being overloocked
// XXX
// ensure that a continuous lane change manoeuvre can be completed
// before the next turning movement
#ifndef NO_TRACI
// let TraCI influence the wish to change lanes and the security to take
//const int oldstate = state;
state = vehicle->influenceChangeDecision(state);
//if (vehicle->getID() == "150_2_36000000") {
// std::cout << STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep()) << " veh=" << vehicle->getID() << " oldstate=" << oldstate << " newstate=" << state << "\n";
//}
#endif
gDebugFlag1 = false;
return state;
}
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;
}