bool MSCalibrator::VehicleRemover::notifyEnter(SUMOTrafficObject& veh, Notification /* reason */, const MSLane* /* enteredLane */) {
if (myParent == nullptr) {
return false;
}
if (myParent->isActive()) {
myParent->updateMeanData();
const bool calibrateFlow = myParent->myCurrentStateInterval->q >= 0;
const int totalWishedNum = myParent->totalWished();
int adaptedNum = myParent->passed() + myParent->myClearedInJam;
MSVehicle* vehicle = dynamic_cast<MSVehicle*>(&veh);
if (calibrateFlow && adaptedNum > totalWishedNum) {
#ifdef MSCalibrator_DEBUG
std::cout << time2string(MSNet::getInstance()->getCurrentTimeStep()) << " " << myParent->getID()
<< " vaporizing " << vehicle->getID() << " to reduce flow\n";
#endif
if (myParent->scheduleRemoval(vehicle)) {
myParent->myRemoved++;
}
} else if (myParent->invalidJam(myLaneIndex)) {
#ifdef MSCalibrator_DEBUG
std::cout << time2string(MSNet::getInstance()->getCurrentTimeStep()) << " " << myParent->getID()
<< " vaporizing " << vehicle->getID() << " to clear jam\n";
#endif
if (!myParent->myHaveWarnedAboutClearingJam) {
WRITE_WARNING("Clearing jam at calibrator '" + myParent->myID + "' at time "
+ time2string(MSNet::getInstance()->getCurrentTimeStep()));
myParent->myHaveWarnedAboutClearingJam = true;
}
if (myParent->scheduleRemoval(vehicle)) {
myParent->myClearedInJam++;
}
}
}
return true;
}
bool
MSCalibrator::childCheckEmit(MSCalibratorChild *child) {
if (myToEmit.find(child)==myToEmit.end()) {
// should not happen - a child is calling and should have a vehicle added
throw 1;
}
if (child!=myActiveChild||myDestLane->getEdge().isVaporizing()) {
// check whether this is due to vaporization
if (myDestLane->getEdge().isVaporizing()) {
myToEmit[child].first->setWasVaporized(true);
}
// remove the vehicle previously inserted by the child
delete myToEmit[child].first;
// erase the child information
myToEmit.erase(myToEmit.find(child));
// inform child to process the next one (the current was not used)
return true;
}
// get the vehicle and the speed the child has read/generated
MSVehicle *veh = myToEmit[child].first;
SUMOReal speed = myToEmit[child].second;
// check whether the speed shall be patched
//TM
SUMOReal pos = myPos+1;
if (speed<0) {
speed = MIN2(myDestLane->getMaxSpeed(), veh->getMaxSpeed());
}
// try to emit
#ifdef HAVE_MESOSIM
if (MSGlobals::gUseMesoSim) {
if (myDestLane->getEdge().emit(*veh, myNet.getCurrentTimeStep())) {
veh->onDepart();
// insert vehicle into the dictionary
if (!myNet.getVehicleControl().addVehicle(veh->getID(), veh)) {
// !!!
throw 1;
}
// erase the child information
myToEmit.erase(myToEmit.find(child));
return true;
}
} else {
#endif
if (myDestLane->isEmissionSuccess(veh, speed, pos, false)) {
veh->onDepart();
// insert vehicle into the dictionary
if (!myNet.getVehicleControl().addVehicle(veh->getID(), veh)) {
// !!!
throw 1;
}
// erase the child information
myToEmit.erase(myToEmit.find(child));
return true;
}
#ifdef HAVE_MESOSIM
}
#endif
return false;
}
void
MSInductLoop::leaveDetectorByMove(MSVehicle& veh,
SUMOReal leaveTimestep) throw() {
int mainStripNum = 0;int flag = 0;
for( MSLane::StripContIter it = myLane->myStrips.begin(); it!=myLane->myStrips.end(); it ++){
if(veh.isMainStrip(**it)){
flag = 1;
break;
}
mainStripNum++;
}
VehicleMap::iterator it = myVehiclesOnDet[mainStripNum].find(&veh);
//uncommnet following for extra check
// assert(it != myVehiclesOnDet[mainStripNum].end());
if(it!= myVehiclesOnDet[mainStripNum].end()){
SUMOReal entryTimestep = it->second;
myVehiclesOnDet[mainStripNum].erase(it);
myCurrentVehicle[mainStripNum] = 0;
//uncomment following for extra check
//assert(entryTimestep < leaveTimestep);
myVehicleDataCont[mainStripNum].push_back(VehicleData(veh.getID(), veh.getVehicleType().getLength(), entryTimestep, leaveTimestep));
myLastOccupancy[mainStripNum] = 0;
veh.quitRemindedLeft(this);
}
else {
//exit(0);
}
}
std::vector<std::string>
MSE2Collector::getCurrentVehicleIDs() const {
std::vector<std::string> ret;
for (std::list<SUMOVehicle*>::const_iterator i = myKnownVehicles.begin(); i != myKnownVehicles.end(); ++i) {
MSVehicle* veh = static_cast<MSVehicle*>(*i);
ret.push_back(veh->getID());
}
std::sort(ret.begin(), ret.end());
return ret;
}
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;
}
void
MSDevice_Routing::buildVehicleDevices(MSVehicle &v, std::vector<MSDevice*> &into) throw() {
OptionsCont &oc = OptionsCont::getOptions();
if (oc.getFloat("device.routing.probability")==0&&!oc.isSet("device.routing.knownveh")) {
// no route computation is modelled
return;
}
// route computation is enabled
bool haveByNumber = false;
if (oc.getBool("device.routing.deterministic")) {
haveByNumber = ((myVehicleIndex%1000) < (int)(oc.getFloat("device.routing.probability")*1000.));
} else {
haveByNumber = RandHelper::rand()<=oc.getFloat("device.routing.probability");
}
bool haveByName = oc.isSet("device.routing.knownveh") && OptionsCont::getOptions().isInStringVector("device.routing.knownveh", v.getID());
if (haveByNumber||haveByName) {
// build the device
MSDevice_Routing* device = new MSDevice_Routing(v, "routing_" + v.getID(),
string2time(oc.getString("device.routing.period")),
string2time(oc.getString("device.routing.pre-period")));
into.push_back(device);
// initialise edge efforts if not done before
if (myEdgeEfforts.size()==0) {
const std::vector<MSEdge*> &edges = MSNet::getInstance()->getEdgeControl().getEdges();
for (std::vector<MSEdge*>::const_iterator i=edges.begin(); i!=edges.end(); ++i) {
myEdgeEfforts[*i] = (*i)->getCurrentTravelTime();
}
}
// make the weights be updated
if (myEdgeWeightSettingCommand==0) {
myEdgeWeightSettingCommand = new StaticCommand< MSDevice_Routing >(&MSDevice_Routing::adaptEdgeEfforts);
MSNet::getInstance()->getEndOfTimestepEvents().addEvent(
myEdgeWeightSettingCommand, 0, MSEventControl::ADAPT_AFTER_EXECUTION);
myAdaptationWeight = oc.getFloat("device.routing.adaptation-weight");
myAdaptationInterval = string2time(oc.getString("device.routing.adaptation-interval"));
}
myWithTaz = oc.getBool("device.routing.with-taz");
if (myWithTaz) {
if (MSEdge::dictionary(v.getParameter().fromTaz+"-source") == 0) {
WRITE_ERROR("Source district '" + v.getParameter().fromTaz + "' not known when rerouting '" + v.getID() + "'!");
return;
}
if (MSEdge::dictionary(v.getParameter().toTaz+"-sink") == 0) {
WRITE_ERROR("Destination district '" + v.getParameter().toTaz + "' not known when rerouting '" + v.getID() + "'!");
return;
}
}
}
myVehicleIndex++;
}
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;
}
int
MSLaneChanger::checkChange(
int laneOffset,
const MSLane* targetLane,
const std::pair<MSVehicle* const, SUMOReal>& leader,
const std::pair<MSVehicle* const, SUMOReal>& neighLead,
const std::pair<MSVehicle* const, SUMOReal>& neighFollow,
const std::vector<MSVehicle::LaneQ>& preb) const {
MSVehicle* vehicle = veh(myCandi);
// Debug (Leo)
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout
<< "\n" << SIMTIME << " checkChange() for vehicle '" << vehicle->getID() << "'"
<< std::endl;
}
#endif
int blocked = 0;
int blockedByLeader = (laneOffset == -1 ? LCA_BLOCKED_BY_RIGHT_LEADER : LCA_BLOCKED_BY_LEFT_LEADER);
int blockedByFollower = (laneOffset == -1 ? LCA_BLOCKED_BY_RIGHT_FOLLOWER : LCA_BLOCKED_BY_LEFT_FOLLOWER);
// overlap
if (neighFollow.first != 0 && neighFollow.second < 0) {
blocked |= (blockedByFollower | LCA_OVERLAPPING);
// Debug (Leo)
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " overlapping with follower..."
<< std::endl;
}
#endif
}
if (neighLead.first != 0 && neighLead.second < 0) {
blocked |= (blockedByLeader | LCA_OVERLAPPING);
// Debug (Leo)
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " overlapping with leader..."
<< std::endl;
}
#endif
}
// safe back gap
if ((blocked & blockedByFollower) == 0 && neighFollow.first != 0) {
// !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren
if (neighFollow.second < neighFollow.first->getCarFollowModel().getSecureGap(neighFollow.first->getSpeed(), vehicle->getSpeed(), vehicle->getCarFollowModel().getMaxDecel())) {
blocked |= blockedByFollower;
// Debug (Leo)
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " back gap unsafe: "
<< "gap = " << neighFollow.second
<< ", secureGap = "
<< neighFollow.first->getCarFollowModel().getSecureGap(neighFollow.first->getSpeed(),
vehicle->getSpeed(), vehicle->getCarFollowModel().getMaxDecel())
<< std::endl;
}
#endif
}
}
// safe front gap
if ((blocked & blockedByLeader) == 0 && neighLead.first != 0) {
// !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren
if (neighLead.second < vehicle->getCarFollowModel().getSecureGap(vehicle->getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())) {
blocked |= blockedByLeader;
// Debug (Leo)
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " front gap unsafe: "
<< "gap = " << neighLead.second
<< ", secureGap = "
<< vehicle->getCarFollowModel().getSecureGap(vehicle->getSpeed(),
neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())
<< std::endl;
}
#endif
}
}
MSAbstractLaneChangeModel::MSLCMessager msg(leader.first, neighLead.first, neighFollow.first);
int state = blocked | vehicle->getLaneChangeModel().wantsChange(
laneOffset, msg, blocked, leader, neighLead, neighFollow, *targetLane, preb, &(myCandi->lastBlocked), &(myCandi->firstBlocked));
if (blocked == 0 && (state & LCA_WANTS_LANECHANGE) != 0 && neighLead.first != 0) {
// do are more carefull (but expensive) check to ensure that a
// safety-critical leader is not being overloocked
const SUMOReal seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
const SUMOReal speed = vehicle->getSpeed();
const SUMOReal dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
if (seen < dist) {
std::pair<MSVehicle* const, SUMOReal> neighLead2 = targetLane->getCriticalLeader(dist, seen, speed, *vehicle);
if (neighLead2.first != 0 && neighLead2.first != neighLead.first
&& (neighLead2.second < vehicle->getCarFollowModel().getSecureGap(
vehicle->getSpeed(), neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel()))) {
state |= blockedByLeader;
}
}
}
if (blocked == 0 && (state & LCA_WANTS_LANECHANGE)) {
// ensure that merging is safe for any upcoming zipper links after changing
if (vehicle->unsafeLinkAhead(targetLane)) {
state |= blockedByLeader;
}
}
if ((state & LCA_BLOCKED) == 0 && (state & LCA_WANTS_LANECHANGE) != 0 && MSGlobals::gLaneChangeDuration > DELTA_T) {
// ensure that a continuous lane change manoeuvre can be completed
// before the next turning movement
SUMOReal seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
const SUMOReal decel = vehicle->getCarFollowModel().getMaxDecel() * STEPS2TIME(MSGlobals::gLaneChangeDuration);
const SUMOReal avgSpeed = 0.5 * (
MAX2((SUMOReal)0, vehicle->getSpeed() - ACCEL2SPEED(vehicle->getCarFollowModel().getMaxDecel())) +
MAX2((SUMOReal)0, vehicle->getSpeed() - decel));
const SUMOReal space2change = avgSpeed * STEPS2TIME(MSGlobals::gLaneChangeDuration);
// for finding turns it doesn't matter whether we look along the current lane or the target lane
const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation();
int view = 1;
MSLane* nextLane = vehicle->getLane();
MSLinkCont::const_iterator link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
while (!nextLane->isLinkEnd(link) && seen <= space2change) {
if ((*link)->getDirection() == LINKDIR_LEFT || (*link)->getDirection() == LINKDIR_RIGHT
// the lanes after an internal junction are on different
// edges and do not allow lane-changing
|| (nextLane->getEdge().isInternal() && (*link)->getViaLaneOrLane()->getEdge().isInternal())
) {
state |= LCA_INSUFFICIENT_SPACE;
break;
}
#ifdef HAVE_INTERNAL_LANES
if ((*link)->getViaLane() == 0) {
view++;
}
#else
view++;
#endif
nextLane = (*link)->getViaLaneOrLane();
seen += nextLane->getLength();
// get the next link used
link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
}
if (nextLane->isLinkEnd(link) && seen < space2change) {
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout << SIMTIME << " checkChange insufficientSpace: seen=" << seen << " space2change=" << space2change << "\n";
}
#endif
state |= LCA_INSUFFICIENT_SPACE;
}
if ((state & LCA_BLOCKED) == 0) {
// check for dangerous leaders in case the target lane changes laterally between
// now and the lane-changing midpoint
const SUMOReal speed = vehicle->getSpeed();
seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
nextLane = vehicle->getLane();
view = 1;
const SUMOReal dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
MSLinkCont::const_iterator link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
while (!nextLane->isLinkEnd(link) && seen <= space2change && seen <= dist) {
nextLane = (*link)->getViaLaneOrLane();
MSLane* targetLane = nextLane->getParallelLane(laneOffset);
if (targetLane == 0) {
state |= LCA_INSUFFICIENT_SPACE;
break;
} else {
std::pair<MSVehicle* const, SUMOReal> neighLead2 = targetLane->getLeader(vehicle, -seen, std::vector<MSLane*>());
if (neighLead2.first != 0 && neighLead2.first != neighLead.first
&& (neighLead2.second < vehicle->getCarFollowModel().getSecureGap(
vehicle->getSpeed(), neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel()))) {
state |= blockedByLeader;
break;
}
}
#ifdef HAVE_INTERNAL_LANES
if ((*link)->getViaLane() == 0) {
view++;
}
#else
view++;
#endif
seen += nextLane->getLength();
// get the next link used
link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
}
}
}
#ifndef NO_TRACI
#ifdef DEBUG_CHECK_CHANGE
const int oldstate = state;
#endif
// let TraCI influence the wish to change lanes and the security to take
state = vehicle->influenceChangeDecision(state);
#endif
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " veh=" << vehicle->getID()
<< " oldState=" << toString((LaneChangeAction)oldstate)
<< " newState=" << toString((LaneChangeAction)state)
<< ((blocked & LCA_BLOCKED) ? " (blocked)" : "")
<< ((blocked & LCA_OVERLAPPING) ? " (overlap)" : "")
<< "\n";
}
#endif
return state;
}
std::pair<MSVehicle* const, SUMOReal>
MSLaneChanger::getRealFollower(const ChangerIt& target) const {
assert(veh(myCandi) != 0);
#ifdef DEBUG_SURROUNDING_VEHICLES
MSVehicle* vehicle = veh(myCandi);
if (DEBUG_COND) {
std::cout << SIMTIME << " veh '" << vehicle->getID() << "' looks for follower on lc-target lane '" << target->lane->getID() << "'." << std::endl;
}
#endif
MSVehicle* candi = veh(myCandi);
const SUMOReal candiPos = candi->getPositionOnLane();
MSVehicle* neighFollow = veh(target);
#ifdef DEBUG_SURROUNDING_VEHICLES
if (DEBUG_COND) {
if (neighFollow != 0) {
std::cout << "veh(target) returns '" << neighFollow->getID() << "' at position " << neighFollow->getPositionOnLane() << std::endl;
} else {
std::cout << "veh(target) returns none." << std::endl;
}
}
#endif
#ifdef DEBUG_SURROUNDING_VEHICLES
if (DEBUG_COND) {
if (getCloserFollower(candiPos, neighFollow, target->hoppedVeh) != neighFollow) {
std::cout << "Hopped vehicle '" << target->hoppedVeh->getID() << "' at position " << target->hoppedVeh->getPositionOnLane() << " is closer." << std::endl;
}
}
#endif
// check whether the hopped vehicle became the follower
neighFollow = getCloserFollower(candiPos, neighFollow, target->hoppedVeh);
#ifdef DEBUG_SURROUNDING_VEHICLES
if (DEBUG_COND) {
MSVehicle* partialBehind = getCloserFollower(candiPos, neighFollow, target->lane->getPartialBehind(candi));
if (partialBehind != 0 && partialBehind != neighFollow) {
std::cout << "'Partial behind'-vehicle '" << target->lane->getPartialBehind(candi)->getID() << "' at position " << target->hoppedVeh->getPositionOnLane() << " is closer." << std::endl;
}
}
#endif
// or a follower which is partially lapping into the target lane
neighFollow = getCloserFollower(candiPos, neighFollow, target->lane->getPartialBehind(candi));
if (neighFollow == 0) {
std::pair<MSVehicle* const, SUMOReal> consecutiveFollower = target->lane->getFollowerOnConsecutive(
candi->getPositionOnLane() - candi->getVehicleType().getLength(),
candi->getSpeed(), candi->getCarFollowModel().getMaxDecel());
#ifdef DEBUG_SURROUNDING_VEHICLES
if (DEBUG_COND) {
if (consecutiveFollower.first == 0) {
std::cout << "no follower found." << std::endl;
} else {
std::cout << "found follower '" << consecutiveFollower.first->getID() << "' on consecutive lanes." << std::endl;
}
}
#endif
return consecutiveFollower;
} else {
#ifdef DEBUG_SURROUNDING_VEHICLES
if (DEBUG_COND) {
std::cout << "found follower '" << neighFollow->getID() << "'." << std::endl;
}
#endif
MSVehicle* candi = veh(myCandi);
return std::pair<MSVehicle* const, SUMOReal>(neighFollow,
candi->getPositionOnLane() - candi->getVehicleType().getLength() - neighFollow->getPositionOnLane() - neighFollow->getVehicleType().getMinGap());
}
}
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());
}
}
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;
}
}
SUMOTime
MSCalibrator::execute(SUMOTime currentTime) {
// get current simulation values (valid for the last simulation second)
// XXX could we miss vehicle movements if this is called less often than every DELTA_T (default) ?
updateMeanData();
const bool hadRemovals = removePending();
// check whether an adaptation value exists
if (isCurrentStateActive(currentTime)) {
myAmActive = true;
// all happens in isCurrentStateActive()
} else {
myAmActive = false;
reset();
if (!mySpeedIsDefault) {
// reset speed to default
if (myLane == nullptr) {
myEdge->setMaxSpeed(myDefaultSpeed);
} else {
myLane->setMaxSpeed(myDefaultSpeed);
}
mySpeedIsDefault = true;
}
if (myCurrentStateInterval == myIntervals.end()) {
// keep calibrator alive for gui but do not call again
return TIME2STEPS(86400);
}
return myFrequency;
}
// we are active
if (!myDidSpeedAdaption && myCurrentStateInterval->v >= 0) {
if (myLane == nullptr) {
myEdge->setMaxSpeed(myCurrentStateInterval->v);
} else {
myLane->setMaxSpeed(myCurrentStateInterval->v);
}
mySpeedIsDefault = false;
myDidSpeedAdaption = true;
}
const bool calibrateFlow = myCurrentStateInterval->q >= 0;
const int totalWishedNum = totalWished();
int adaptedNum = passed() + myClearedInJam;
#ifdef MSCalibrator_DEBUG
std::cout << time2string(currentTime) << " " << myID
<< " q=" << myCurrentStateInterval->q
<< " totalWished=" << totalWishedNum
<< " adapted=" << adaptedNum
<< " jam=" << invalidJam(myLane == 0 ? -1 : myLane->getIndex())
<< " entered=" << myEdgeMeanData.nVehEntered
<< " departed=" << myEdgeMeanData.nVehDeparted
<< " arrived=" << myEdgeMeanData.nVehArrived
<< " left=" << myEdgeMeanData.nVehLeft
<< " waitSecs=" << myEdgeMeanData.waitSeconds
<< " vaporized=" << myEdgeMeanData.nVehVaporized
<< "\n";
#endif
if (calibrateFlow && adaptedNum < totalWishedNum && !hadRemovals) {
// we need to insert some vehicles
const double hourFraction = STEPS2TIME(currentTime - myCurrentStateInterval->begin + DELTA_T) / (double) 3600.;
const int wishedNum = (int)std::floor(myCurrentStateInterval->q * hourFraction + 0.5); // round to closest int
// only the difference between inflow and aspiredFlow should be added, thus
// we should not count vehicles vaporized from a jam here
// if we have enough time left we can add missing vehicles later
const int relaxedInsertion = (int)std::floor(STEPS2TIME(myCurrentStateInterval->end - currentTime) / 3);
const int insertionSlack = MAX2(0, adaptedNum + relaxedInsertion - totalWishedNum);
// increase number of vehicles
#ifdef MSCalibrator_DEBUG
std::cout
<< " wished:" << wishedNum
<< " slack:" << insertionSlack
<< " before:" << adaptedNum
<< "\n";
#endif
while (wishedNum > adaptedNum + insertionSlack) {
SUMOVehicleParameter* pars = myCurrentStateInterval->vehicleParameter;
const MSRoute* route = myProbe != nullptr ? myProbe->getRoute() : nullptr;
if (route == nullptr) {
route = MSRoute::dictionary(pars->routeid);
}
if (route == nullptr) {
WRITE_WARNING("No valid routes in calibrator '" + myID + "'.");
break;
}
if (!route->contains(myEdge)) {
WRITE_WARNING("Route '" + route->getID() + "' in calibrator '" + myID + "' does not contain edge '" + myEdge->getID() + "'.");
break;
}
const int routeIndex = (int)std::distance(route->begin(),
std::find(route->begin(), route->end(), myEdge));
MSVehicleType* vtype = MSNet::getInstance()->getVehicleControl().getVType(pars->vtypeid);
assert(route != 0 && vtype != 0);
// build the vehicle
SUMOVehicleParameter* newPars = new SUMOVehicleParameter(*pars);
newPars->id = myID + "." + toString((int)STEPS2TIME(myCurrentStateInterval->begin)) + "." + toString(myInserted);
newPars->depart = currentTime;
newPars->routeid = route->getID();
MSVehicle* vehicle;
try {
vehicle = dynamic_cast<MSVehicle*>(MSNet::getInstance()->getVehicleControl().buildVehicle(
newPars, route, vtype, true, false));
} catch (const ProcessError& e) {
if (!MSGlobals::gCheckRoutes) {
WRITE_WARNING(e.what());
vehicle = nullptr;
break;
} else {
throw e;
}
}
#ifdef MSCalibrator_DEBUG
std::cout << " resetting route pos: " << routeIndex << "\n";
#endif
vehicle->resetRoutePosition(routeIndex);
if (myEdge->insertVehicle(*vehicle, currentTime)) {
if (!MSNet::getInstance()->getVehicleControl().addVehicle(vehicle->getID(), vehicle)) {
throw ProcessError("Emission of vehicle '" + vehicle->getID() + "' in calibrator '" + getID() + "'failed!");
}
myInserted++;
adaptedNum++;
#ifdef MSCalibrator_DEBUG
std::cout << "I ";
#endif
} else {
// could not insert vehicle
#ifdef MSCalibrator_DEBUG
std::cout << "F ";
#endif
MSNet::getInstance()->getVehicleControl().deleteVehicle(vehicle, true);
break;
}
}
}
if (myCurrentStateInterval->end <= currentTime + myFrequency) {
writeXMLOutput();
}
return myFrequency;
}
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::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;
}
}