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()); } }
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)); } }
std::pair<MSVehicle* const, SUMOReal> MSLaneChanger::getRealLeader(const ChangerIt& target) const { // get the leading vehicle on the lane to change to MSVehicle* neighLead = target->lead; // check whether the hopped vehicle got the leader if (target->hoppedVeh != 0) { SUMOReal hoppedPos = target->hoppedVeh->getPositionOnLane(); if (hoppedPos > veh(myCandi)->getPositionOnLane() && (neighLead == 0 || neighLead->getPositionOnLane() > hoppedPos)) { neighLead = target->hoppedVeh; } } if (neighLead == 0) { MSLane* targetLane = target->lane; MSVehicle* predP = targetLane->getPartialOccupator(); if (predP != 0) { return std::pair<MSVehicle*, SUMOReal>(predP, targetLane->getPartialOccupatorEnd() - veh(myCandi)->getPositionOnLane() - veh(myCandi)->getVehicleType().getMinGap()); } const std::vector<MSLane*>& bestLaneConts = veh(myCandi)->getBestLanesContinuation(myCandi->lane); SUMOReal seen = myCandi->lane->getLength() - veh(myCandi)->getPositionOnLane(); SUMOReal speed = veh(myCandi)->getSpeed(); SUMOReal dist = veh(myCandi)->getCarFollowModel().brakeGap(speed) + veh(myCandi)->getVehicleType().getMinGap(); if (seen > dist) { return std::pair<MSVehicle* const, SUMOReal>(static_cast<MSVehicle*>(0), -1); } return target->lane->getLeaderOnConsecutive(dist, seen, speed, *veh(myCandi), bestLaneConts); } else { MSVehicle* candi = veh(myCandi); return std::pair<MSVehicle* const, SUMOReal>(neighLead, neighLead->getPositionOnLane() - neighLead->getVehicleType().getLength() - candi->getPositionOnLane() - candi->getVehicleType().getMinGap()); } }
void MSLaneChanger::updateChanger(bool vehHasChanged) { assert(veh(myCandi) != 0); // "Push" the vehicles to the back, i.e. follower becomes vehicle, // vehicle becomes leader, and leader becomes predecessor of vehicle, // if it exists. if (!vehHasChanged || MSGlobals::gLaneChangeDuration > DELTA_T) { //std::cout << SIMTIME << " updateChanger: lane=" << myCandi->lane->getID() << " has new lead=" << veh(myCandi)->getID() << "\n"; myCandi->lead = veh(myCandi); } MSLane::VehCont& vehicles = myCandi->lane->myVehicles; vehicles.pop_back(); //std::cout << SIMTIME << " updateChanger lane=" << myCandi->lane->getID() << " vehicles=" << toString(myCandi->lane->myVehicles) << "\n"; }
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()); } }
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; }
bool MSLaneChangerSublane::change() { // variant of change() for the sublane case myCandi = findCandidate(); MSVehicle* vehicle = veh(myCandi); #ifdef DEBUG_VEHICLE_GUI_SELECTION if (gDebugSelectedVehicle == vehicle->getID()) { int bla = 0; } #endif assert(vehicle->getLane() == (*myCandi).lane); assert(!vehicle->getLaneChangeModel().isChangingLanes()); #ifndef NO_TRACI if (vehicle->isRemoteControlled()) { return false; // !!! temporary; just because it broke, here } #endif vehicle->updateBestLanes(); // needed? for (int i = 0; i < (int) myChanger.size(); ++i) { vehicle->adaptBestLanesOccupation(i, myChanger[i].dens); } // update expected speeds int sublaneIndex = 0; for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) { vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(ce->ahead, sublaneIndex, ce->lane->getIndex()); sublaneIndex += ce->ahead.numSublanes(); } StateAndDist right = checkChangeHelper(vehicle, -1); StateAndDist left = checkChangeHelper(vehicle, 1); StateAndDist current = checkChangeHelper(vehicle, 0); StateAndDist decision = vehicle->getLaneChangeModel().decideDirection(current, vehicle->getLaneChangeModel().decideDirection(right, left)); if ((decision.state & LCA_WANTS_LANECHANGE) != 0 && (decision.state & LCA_BLOCKED) == 0) { // change if the vehicle wants to and is allowed to change if (vehicle->getLaneChangeModel().debugVehicle()) { std::cout << SIMTIME << " decision=" << toString((LaneChangeAction)decision.state) << " latDist=" << decision.latDist << "\n"; } vehicle->getLaneChangeModel().setOwnState(decision.state); return startChangeSublane(vehicle, myCandi, decision.latDist); } if ((right.state & (LCA_URGENT)) != 0 && (left.state & (LCA_URGENT)) != 0) { // ... wants to go to the left AND to the right // just let them go to the right lane... left.state = 0; } vehicle->getLaneChangeModel().setOwnState(right.state | left.state | current.state); registerUnchanged(vehicle); return false; }
MSLaneChanger::ChangerIt MSLaneChanger::findCandidate() { // Find the vehicle in myChanger with the largest position. If there // is no vehicle in myChanger (shouldn't happen) , return myChanger.end(). ChangerIt max = myChanger.end(); for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) { if (veh(ce) == 0) { continue; } if (max == myChanger.end()) { //std::cout << SIMTIME << " new max vehicle=" << veh(ce)->getID() << " pos=" << veh(ce)->getPositionOnLane() << " lane=" << ce->lane->getID() << " isFrontOnLane=" << veh(ce)->isFrontOnLane(ce->lane) << "\n"; max = ce; continue; } assert(veh(ce) != 0); assert(veh(max) != 0); if (veh(max)->getPositionOnLane() < veh(ce)->getPositionOnLane()) { //std::cout << SIMTIME << " new max vehicle=" << veh(ce)->getID() << " pos=" << veh(ce)->getPositionOnLane() << " lane=" << ce->lane->getID() << " isFrontOnLane=" << veh(ce)->isFrontOnLane(ce->lane) << " oldMaxPos=" << veh(max)->getPositionOnLane() << "\n"; max = ce; } } assert(max != myChanger.end()); assert(veh(max) != 0); return max; }
int MSLaneChanger::change2right(const std::pair<MSVehicle* const, SUMOReal>& leader, const std::pair<MSVehicle* const, SUMOReal>& rLead, const std::pair<MSVehicle* const, SUMOReal>& rFollow, const std::vector<MSVehicle::LaneQ>& preb) const { ChangerIt target = myCandi - 1; int blocked = overlapWithHopped(target) ? target->hoppedVeh->getPositionOnLane() < veh(myCandi)->getPositionOnLane() ? LCA_BLOCKED_BY_RIGHT_FOLLOWER : LCA_BLOCKED_BY_RIGHT_LEADER : 0; // overlap if (rFollow.first != 0 && rFollow.second < 0) { blocked |= (LCA_BLOCKED_BY_RIGHT_FOLLOWER); } if (rLead.first != 0 && rLead.second < 0) { blocked |= (LCA_BLOCKED_BY_RIGHT_LEADER); } // safe back gap if (rFollow.first != 0) { // !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren if (rFollow.second < rFollow.first->getCarFollowModel().getSecureGap(rFollow.first->getSpeed(), veh(myCandi)->getSpeed(), veh(myCandi)->getCarFollowModel().getMaxDecel())) { blocked |= LCA_BLOCKED_BY_RIGHT_FOLLOWER; } } // safe front gap if (rLead.first != 0) { // !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren if (rLead.second < veh(myCandi)->getCarFollowModel().getSecureGap(veh(myCandi)->getSpeed(), rLead.first->getSpeed(), rLead.first->getCarFollowModel().getMaxDecel())) { blocked |= LCA_BLOCKED_BY_RIGHT_LEADER; } } MSAbstractLaneChangeModel::MSLCMessager msg(leader.first, rLead.first, rFollow.first); return blocked | veh(myCandi)->getLaneChangeModel().wantsChangeToRight( msg, blocked, leader, rLead, rFollow, *(myCandi - 1)->lane, preb, &(myCandi->lastBlocked)); }
MSLeaderDistanceInfo MSLaneChangerSublane::getLeaders(const ChangerIt& target, const MSVehicle* ego) const { //if (ego->getID() == "C" && SIMTIME == 17) { // std::cout << "DEBUG\n"; //} // get the leading vehicle on the lane to change to if (gDebugFlag1) { std::cout << SIMTIME << " getLeaders lane=" << target->lane->getID() << " ego=" << ego->getID() << " ahead=" << target->ahead.toString() << "\n"; } MSLeaderDistanceInfo result(target->lane, 0, 0); for (int i = 0; i < target->ahead.numSublanes(); ++i) { const MSVehicle* veh = target->ahead[i]; if (veh != 0) { assert(veh != 0); const SUMOReal gap = veh->getBackPositionOnLane() - ego->getPositionOnLane() - ego->getVehicleType().getMinGap(); if (gDebugFlag1) { std::cout << " ahead lead=" << veh->getID() << " leadBack=" << veh->getBackPositionOnLane() << " gap=" << gap << "\n"; } result.addLeader(veh, gap, 0, i); } } // if there are vehicles on the target lane with the same position as ego, // they may not have been added to 'ahead' yet const MSLeaderInfo& aheadSamePos = target->lane->getLastVehicleInformation(0, 0, ego->getPositionOnLane(), false); for (int i = 0; i < aheadSamePos.numSublanes(); ++i) { const MSVehicle* veh = aheadSamePos[i]; if (veh != 0 && veh != ego) { const SUMOReal gap = veh->getBackPositionOnLane(target->lane) - ego->getPositionOnLane() - ego->getVehicleType().getMinGap(); if (gDebugFlag1) { std::cout << " further lead=" << veh->getID() << " leadBack=" << veh->getBackPositionOnLane(target->lane) << " gap=" << gap << "\n"; } result.addLeader(veh, gap, 0, i); } } if (result.numFreeSublanes() > 0) { MSLane* targetLane = target->lane; SUMOReal seen = ego->getLane()->getLength() - ego->getPositionOnLane(); SUMOReal speed = ego->getSpeed(); SUMOReal dist = ego->getCarFollowModel().brakeGap(speed) + ego->getVehicleType().getMinGap(); if (seen > dist) { return result; } const std::vector<MSLane*>& bestLaneConts = veh(myCandi)->getBestLanesContinuation(targetLane); target->lane->getLeadersOnConsecutive(dist, seen, speed, ego, bestLaneConts, result); } return result; }
void MSLaneChanger::updateChanger(bool vehHasChanged) { assert(myCandi->veh != myCandi->lane->myVehicles.rend()); // "Push" the vehicles to the back, i.e. follower becomes vehicle, // vehicle becomes leader, and leader becomes predecessor of vehicle, // if it exists. if (!vehHasChanged) { myCandi->lead = veh(myCandi); } myCandi->veh = myCandi->veh + 1; if (veh(myCandi) == 0) { assert(myCandi->follow == 0); // leader already 0. return; } if (myCandi->veh + 1 == myCandi->lane->myVehicles.rend()) { myCandi->follow = 0; } else { myCandi->follow = *(myCandi->veh + 1); } return; }
std::pair<MSVehicle* const, SUMOReal> MSLaneChanger::getRealLeader(const ChangerIt& target) const { // get the leading vehicle on the lane to change to MSVehicle* neighLead = target->lead; //if (veh(myCandi)->getID() == "disabled") std::cout << SIMTIME // << " target=" << target->lane->getID() // << " neighLead=" << Named::getIDSecure(neighLead) // << " hopped=" << Named::getIDSecure(target->hoppedVeh) // << " (416)\n"; // check whether the hopped vehicle became the leader if (target->hoppedVeh != 0) { SUMOReal hoppedPos = target->hoppedVeh->getPositionOnLane(); if (hoppedPos > veh(myCandi)->getPositionOnLane() && (neighLead == 0 || neighLead->getPositionOnLane() > hoppedPos)) { neighLead = target->hoppedVeh; //if (veh(myCandi)->getID() == "flow.21") std::cout << SIMTIME << " neighLead=" << Named::getIDSecure(neighLead) << " (422)\n"; } } if (neighLead == 0) { MSLane* targetLane = target->lane; if (targetLane->myPartialVehicles.size() > 0) { assert(targetLane->myPartialVehicles.size() == 1); MSVehicle* leader = targetLane->myPartialVehicles.front(); return std::pair<MSVehicle*, SUMOReal>(leader, leader->getBackPositionOnLane(targetLane) - veh(myCandi)->getPositionOnLane() - veh(myCandi)->getVehicleType().getMinGap()); } SUMOReal seen = myCandi->lane->getLength() - veh(myCandi)->getPositionOnLane(); SUMOReal speed = veh(myCandi)->getSpeed(); SUMOReal dist = veh(myCandi)->getCarFollowModel().brakeGap(speed) + veh(myCandi)->getVehicleType().getMinGap(); if (seen > dist) { return std::pair<MSVehicle* const, SUMOReal>(static_cast<MSVehicle*>(0), -1); } const std::vector<MSLane*>& bestLaneConts = veh(myCandi)->getBestLanesContinuation(targetLane); //if (veh(myCandi)->getID() == "flow.21") std::cout << SIMTIME << " calling getLeaderOnConsecutive (443)\n"; return target->lane->getLeaderOnConsecutive(dist, seen, speed, *veh(myCandi), bestLaneConts); } else { MSVehicle* candi = veh(myCandi); return std::pair<MSVehicle* const, SUMOReal>(neighLead, neighLead->getBackPositionOnLane(target->lane) - candi->getPositionOnLane() - candi->getVehicleType().getMinGap()); } }
MSLaneChanger::ChangerIt MSLaneChanger::findCandidate() { // Find the vehicle in myChanger with the smallest position. If there // is no vehicle in myChanger (shouldn't happen) , return // myChanger.end(). ChangerIt max = myChanger.end(); for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) { if (veh(ce) == 0) { continue; } if (max == myChanger.end()) { max = ce; continue; } assert(veh(ce) != 0); assert(veh(max) != 0); if (veh(max)->getPositionOnLane() < veh(ce)->getPositionOnLane()) { max = ce; } } assert(max != myChanger.end()); assert(veh(max) != 0); return max; }
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; }
void TrafficSimulation::import_vehicles(std::string vehicles_file) { // load input file std::ifstream infile; infile.exceptions(std::ifstream::badbit); try { infile.open(vehicles_file); // read and match lines std::string line; for (uint_fast32_t line_num = 1; std::getline(infile, line); ++line_num) { // split by fields std::vector<std::string> fields = nonstd::split(line, ':'); // 4 fields are required (time, name, from, to) if (fields.size() < 4) { std::cerr << "E: Line " << line_num << " of input file \"" << vehicles_file << "\" is not valid. See documentation " "for correct syntax. Skipping..." << std::endl; continue; } #ifndef NDEBUG std::cout << "Fields:" << std::endl; for (auto &f : fields) { std::cout << f << std::endl; } std::cout << "Matched vehicle on line " << line_num << std::endl << "name: " << fields[1] << std::endl << "from: " << fields[2] << std::endl << "to: " << fields[3] << std::endl << "starttime: " << fields[0] << std::endl; #endif // create vehicle; parse starttime, name, from and to try { Vehicle veh(*this, fields[2], fields[3], stoull(fields[0]), fields[1]); // parse optional properties for (int i = 4; i < fields.size(); ++i) { // split property field by = std::vector<std::string> property = nonstd::split(fields[i], '='); // assign property switch (nonstd::hash(property[0].c_str())) { case nonstd::hash("traffic_increase_caused"): veh.traffic_increase_caused = stof(property[1]); break; case nonstd::hash("max_speed"): veh.max_speed = stoul(property[1]); break; } } // add vehicle to simulation vehicles.push_back(veh); std::cout << "I: Vehicle " << veh.name << " loaded for simulation" << std::endl; } // vehicle construction errors catch (const std::runtime_error &e) { std::cerr << "E: " << vehicles_file << ":" << line_num << " Vehicle on line " << line_num << "could'n be loaded for simulation. " << e.what() << std::endl; } } } // input file errors catch (const std::ifstream::failure &e) { std::cerr << "E: Couldn't read file '" << vehicles_file << " " << e.what() << std::endl; } }
void CFAPI::initVehicleNetwork(std::vector<Vehicle>& vehList, int maxVehicles, int connPat, int patCnt) { bool flag1 = false; bool flag2 = false; if(connPat == ON_FOLLOW_OFF) { flag1 = true; flag2 = false; } else { flag1 = false; flag2 = true; } int vehId = 0; while(vehId < maxVehicles) { // // Enable/Disable safety device for(int i = 0; i < patCnt; i++, vehId++) { if(vehId >= maxVehicles) { break; } Vehicle veh(vehId, (double)((maxVehicles-1) - vehId) * m_initVehGap, m_initVel, m_initAcclr, m_initHdway, flag1, -1.0); vehList.push_back(veh); } // // Enable/Disable safety device for(int i = 0; i < (maxVehicles - patCnt); i++, vehId++) { if(vehId >= maxVehicles) { break; } Vehicle veh(vehId, (double)((maxVehicles-1) - vehId) * m_initVehGap, m_initVel, m_initAcclr, m_initHdway, flag2, -1.0); vehList.push_back(veh); } } // Configure the safety device start time int reactTimeCnt = 0; int hopCnt = 0; for(int i = 0; i <= maxVehicles-1; i++) { if (vehList.at(i).hasSafetyDev()) { // Set start time vehList.at(i).setSafetyDevStartTime(m_leaderCtrlStart + hopCnt * m_nwDelayDelta + reactTimeCnt * m_reactTime); } // Update counters for followers. Do not update for last vehicle. if (i < maxVehicles-1) { if(vehList.at(i).hasSafetyDev() && vehList.at(i+1).hasSafetyDev()) { hopCnt++; } else if ((vehList.at(i).hasSafetyDev() && !vehList.at(i+1).hasSafetyDev()) || (!vehList.at(i).hasSafetyDev() && vehList.at(i+1).hasSafetyDev()) || (!vehList.at(i).hasSafetyDev() && !vehList.at(i+1).hasSafetyDev())) { reactTimeCnt++; } } std::cout << "Veh: " << vehList.at(i).getId() << " trigger: " << vehList.at(i).getSafetyDevStartTime() << std::endl; } }
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; }
bool MSLaneChanger::mayChange(int direction) const { if (direction == 0) { return true; } if (!myAllowsChanging) { return false; } if (direction == -1) { return (myCandi != myChanger.begin() && (myCandi - 1)->lane->allowsVehicleClass(veh(myCandi)->getVehicleType().getVehicleClass())); } else if (direction == 1) { return (myCandi + 1) != myChanger.end() && (myCandi + 1)->lane->allowsVehicleClass(veh(myCandi)->getVehicleType().getVehicleClass()); } else { return false; } }
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 (gDebugSelectedVehicle == vehicle->getID()) { int bla = 0; } #endif if (vehicle->getLaneChangeModel().isChangingLanes()) { return continueChange(vehicle, myCandi); } if (!myAllowsChanging || vehicle->getLaneChangeModel().alreadyChanged()) { registerUnchanged(vehicle); return false; } std::pair<MSVehicle* const, SUMOReal> leader = getRealLeader(myCandi); if (myChanger.size() == 1 || vehicle->getLaneChangeModel().isOpposite()) { if (changeOpposite(leader)) { return true; } registerUnchanged(vehicle); return false; } #ifndef NO_TRACI if (vehicle->isRemoteControlled()) { return false; // !!! temporary; just because it broke, here } #endif vehicle->updateBestLanes(); // needed? for (int i = 0; i < (int) myChanger.size(); ++i) { vehicle->adaptBestLanesOccupation(i, myChanger[i].dens); } const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes(); // check whether the vehicle wants and is able to change to right lane int state1 = 0; if (mayChange(-1)) { state1 = checkChangeWithinEdge(-1, leader, preb); bool changingAllowed1 = (state1 & LCA_BLOCKED) == 0; // change if the vehicle wants to and is allowed to change if ((state1 & LCA_RIGHT) != 0 && changingAllowed1) { vehicle->getLaneChangeModel().setOwnState(state1); startChange(vehicle, myCandi, -1); return true; } if ((state1 & LCA_RIGHT) != 0 && (state1 & LCA_URGENT) != 0) { (myCandi - 1)->lastBlocked = vehicle; if ((myCandi - 1)->firstBlocked == 0) { (myCandi - 1)->firstBlocked = vehicle; } } } // check whether the vehicle wants and is able to change to left lane int state2 = 0; if (mayChange(1)) { state2 = checkChangeWithinEdge(1, leader, preb); bool changingAllowed2 = (state2 & LCA_BLOCKED) == 0; // change if the vehicle wants to and is allowed to change if ((state2 & LCA_LEFT) != 0 && changingAllowed2) { vehicle->getLaneChangeModel().setOwnState(state2); startChange(vehicle, myCandi, 1); return true; } if ((state2 & LCA_LEFT) != 0 && (state2 & LCA_URGENT) != 0) { (myCandi + 1)->lastBlocked = vehicle; if ((myCandi + 1)->firstBlocked == 0) { (myCandi + 1)->firstBlocked = vehicle; } } } 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(state2 | state1); // only emergency vehicles should change to the opposite side on a // multi-lane road if (vehicle->getVehicleType().getVehicleClass() == SVC_EMERGENCY && changeOpposite(leader)) { return true; } else { registerUnchanged(vehicle); return false; } }
void MSLaneChanger::registerUnchanged(MSVehicle* vehicle) { myCandi->lane->myTmpVehicles.insert(myCandi->lane->myTmpVehicles.begin(), veh(myCandi)); myCandi->dens += vehicle->getVehicleType().getLengthWithGap(); vehicle->getLaneChangeModel().unchanged(); }
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; }
std::pair<MSVehicle* const, SUMOReal> MSLaneChanger::getRealLeader(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 leader on lc-target lane '" << target->lane->getID() << "'." << std::endl; } #endif // get the leading vehicle on the lane to change to MSVehicle* neighLead = target->lead; #ifdef DEBUG_SURROUNDING_VEHICLES if (DEBUG_COND) { if (neighLead != 0) { std::cout << "Considering '" << neighLead->getID() << "' at position " << neighLead->getPositionOnLane() << std::endl; } } #endif //if (veh(myCandi)->getID() == "disabled") std::cout << SIMTIME // << " target=" << target->lane->getID() // << " neighLead=" << Named::getIDSecure(neighLead) // << " hopped=" << Named::getIDSecure(target->hoppedVeh) // << " (416)\n"; // check whether the hopped vehicle became the leader if (target->hoppedVeh != 0) { SUMOReal hoppedPos = target->hoppedVeh->getPositionOnLane(); #ifdef DEBUG_SURROUNDING_VEHICLES if (DEBUG_COND) { std::cout << "Considering hopped vehicle '" << target->hoppedVeh->getID() << "' at position " << hoppedPos << std::endl; } #endif if (hoppedPos > veh(myCandi)->getPositionOnLane() && (neighLead == 0 || neighLead->getPositionOnLane() > hoppedPos)) { neighLead = target->hoppedVeh; //if (veh(myCandi)->getID() == "flow.21") std::cout << SIMTIME << " neighLead=" << Named::getIDSecure(neighLead) << " (422)\n"; } } if (neighLead == 0) { #ifdef DEBUG_SURROUNDING_VEHICLES if (DEBUG_COND) { std::cout << "Looking for leader on consecutive lanes." << std::endl; } #endif // There's no leader on the target lane. Look for leaders on consecutive lanes. MSLane* targetLane = target->lane; if (targetLane->myPartialVehicles.size() > 0) { assert(targetLane->myPartialVehicles.size() > 0); std::vector<MSVehicle*>::const_iterator i = targetLane->myPartialVehicles.begin(); MSVehicle* leader = *i; SUMOReal leaderPos = leader->getBackPositionOnLane(targetLane); while (++i != targetLane->myPartialVehicles.end()) { if ((*i)->getBackPositionOnLane(targetLane) < leader->getBackPositionOnLane(targetLane)) { leader = *i; leaderPos = leader->getBackPositionOnLane(targetLane); } } return std::pair<MSVehicle*, SUMOReal>(leader, leaderPos - veh(myCandi)->getPositionOnLane() - veh(myCandi)->getVehicleType().getMinGap()); } SUMOReal seen = myCandi->lane->getLength() - veh(myCandi)->getPositionOnLane(); SUMOReal speed = veh(myCandi)->getSpeed(); SUMOReal dist = veh(myCandi)->getCarFollowModel().brakeGap(speed) + veh(myCandi)->getVehicleType().getMinGap(); if (seen > dist) { return std::pair<MSVehicle* const, SUMOReal>(static_cast<MSVehicle*>(0), -1); } const std::vector<MSLane*>& bestLaneConts = veh(myCandi)->getBestLanesContinuation(targetLane); return target->lane->getLeaderOnConsecutive(dist, seen, speed, *veh(myCandi), bestLaneConts); } else { MSVehicle* candi = veh(myCandi); return std::pair<MSVehicle* const, SUMOReal>(neighLead, neighLead->getBackPositionOnLane(target->lane) - candi->getPositionOnLane() - candi->getVehicleType().getMinGap()); } }
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()); } }
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::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 (gDebugSelectedVehicle == vehicle->getID()) { int bla = 0; } #endif if (vehicle->getLaneChangeModel().isChangingLanes()) { return continueChange(vehicle, myCandi); } if (!myAllowsChanging || vehicle->getLaneChangeModel().alreadyChanged()) { registerUnchanged(vehicle); return false; } std::pair<MSVehicle* const, SUMOReal> leader = getRealLeader(myCandi); if (myChanger.size() == 1) { if (changeOpposite(leader)) { return true; } registerUnchanged(vehicle); return false; } #ifndef NO_TRACI if (vehicle->isRemoteControlled()) { return false; // !!! temporary; just because it broke, here } #endif vehicle->updateBestLanes(); // needed? for (int i = 0; i < (int) myChanger.size(); ++i) { vehicle->adaptBestLanesOccupation(i, myChanger[i].dens); } const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes(); // check whether the vehicle wants and is able to change to right lane int state1 = 0; if (mayChange(-1)) { state1 = checkChangeWithinEdge(-1, leader, preb); bool changingAllowed1 = (state1 & LCA_BLOCKED) == 0; // change if the vehicle wants to and is allowed to change if ((state1 & LCA_RIGHT) != 0 && changingAllowed1) { vehicle->getLaneChangeModel().setOwnState(state1); startChange(vehicle, myCandi, -1); return true; } if ((state1 & LCA_RIGHT) != 0 && (state1 & LCA_URGENT) != 0) { (myCandi - 1)->lastBlocked = vehicle; if ((myCandi - 1)->firstBlocked == 0) { (myCandi - 1)->firstBlocked = vehicle; } } } // check whether the vehicle wants and is able to change to left lane int state2 = 0; if (mayChange(1)) { state2 = checkChangeWithinEdge(1, leader, preb); bool changingAllowed2 = (state2 & LCA_BLOCKED) == 0; // change if the vehicle wants to and is allowed to change if ((state2 & LCA_LEFT) != 0 && changingAllowed2) { vehicle->getLaneChangeModel().setOwnState(state2); startChange(vehicle, myCandi, 1); return true; } if ((state2 & LCA_LEFT) != 0 && (state2 & LCA_URGENT) != 0) { (myCandi + 1)->lastBlocked = vehicle; if ((myCandi + 1)->firstBlocked == 0) { (myCandi + 1)->firstBlocked = vehicle; } } } 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(state2 | state1); // check whether the vehicles should be swapped if (myAllowsSwap && ((state1 & (LCA_URGENT)) != 0 || (state2 & (LCA_URGENT)) != 0)) { // get the direction ... ChangerIt target; int direction = 0; if ((state1 & (LCA_URGENT)) != 0) { // ... wants to go right target = myCandi - 1; direction = -1; } if ((state2 & (LCA_URGENT)) != 0) { // ... wants to go left target = myCandi + 1; direction = 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()) { // 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()) { assert(*i == prohibitor); target->lane->myTmpVehicles.erase(i); startChange(vehicle, myCandi, direction); startChange(prohibitor, target, -direction); std::swap(vehicle->myState, prohibitor->myState); myCandi->lead = prohibitor; target->lead = vehicle; return true; } } } } if (!changeOpposite(leader)) { registerUnchanged(vehicle); return false; } else { return true; } }