bool
MSDevice_Example::notifyMove(SUMOVehicle& veh, SUMOReal /* oldPos */,
SUMOReal /* newPos */, SUMOReal newSpeed) {
std::cout << "device '" << getID() << "' notifyMove: newSpeed=" << newSpeed << "\n";
// check whether another device is present on the vehicle:
MSDevice_Tripinfo* otherDevice = static_cast<MSDevice_Tripinfo*>(veh.getDevice(typeid(MSDevice_Tripinfo)));
if (otherDevice != 0) {
std::cout << " veh '" << veh.getID() << " has device '" << otherDevice->getID() << "'\n";
}
return true; // keep the device
}
bool
MSRailSignal::hasLinkConflict(int index) const {
#ifdef DEBUG_SIGNALSTATE_PRIORITY
//if (DEBUG_COND) std::cout << SIMTIME << " railSignal=" << getID() << " index=" << index << " hasLinkConflict...\n";
#endif
MSLink* currentLink = myLinks[index][0];
double foeMaxSpeed = -1;
double foeMinDist = std::numeric_limits<double>::max();
SUMOTime foeMinETA = std::numeric_limits<SUMOTime>::max();
long long foeMinNumericalID = std::numeric_limits<long long>::max(); // tie braker
// check for vehicles that enter the (unconditional) conflict area and
// resolve conflict according to priority
std::vector<int> checkRouteConflict;
const auto& cLinks = myConflictLinks[index];
for (int clIndex = 0; clIndex < (int)cLinks.size(); ++clIndex) {
if (myRouteConflictLanes[index][clIndex].size() > 0) {
// record links where the conditional conflict area may be occupied
checkRouteConflict.push_back(clIndex);
}
const MSLink* link = cLinks[clIndex];
if (link->getApproaching().size() > 0) {
const MSTrafficLightLogic* foeTLL = link->getTLLogic();
assert(foeTLL != nullptr);
const MSRailSignal* foeRS = dynamic_cast<const MSRailSignal*>(foeTLL);
if (foeRS != nullptr) {
if (foeRS->conflictLaneOccupied(link->getTLIndex())) {
#ifdef DEBUG_SIGNALSTATE_PRIORITY
if (DEBUG_COND) std::cout << SIMTIME << " railSignal=" << getID() << " index=" << index
<< " foeLink " << link->getViaLaneOrLane()->getID() << " (ignored)\n";
#endif
continue;
}
} else if (link->getState() == LINKSTATE_TL_RED) {
// ignore foe vehicles waiting at a regular traffic light
continue;
}
}
for (auto apprIt : link->getApproaching()) {
MSLink::ApproachingVehicleInformation info = apprIt.second;
foeMaxSpeed = MAX2(apprIt.first->getSpeed(), foeMaxSpeed);
foeMinDist = MIN2(info.dist, foeMinDist);
if (info.willPass) {
foeMinETA = MIN2(info.arrivalTime, foeMinETA);
}
foeMinNumericalID = MIN2(foeMinNumericalID, apprIt.first->getNumericalID());
}
}
SUMOVehicle* closest = nullptr;
if (foeMaxSpeed >= 0 || checkRouteConflict.size() > 0) {
// check against vehicles approaching this link
double maxSpeed = -1;
double minDist = std::numeric_limits<double>::max();
SUMOTime minETA = std::numeric_limits<SUMOTime>::max();
long long minNumericalID = std::numeric_limits<long long>::max(); // tie braker
for (auto apprIt : currentLink->getApproaching()) {
MSLink::ApproachingVehicleInformation info = apprIt.second;
maxSpeed = MAX2(apprIt.first->getSpeed(), maxSpeed);
if (info.dist < minDist) {
minDist = info.dist;
closest = const_cast<SUMOVehicle*>(apprIt.first);
}
if (info.willPass) {
minETA = MIN2(info.arrivalTime, minETA);
}
minNumericalID = MIN2(minNumericalID, apprIt.first->getNumericalID());
}
#ifdef DEBUG_SIGNALSTATE_PRIORITY
if (DEBUG_COND) std::cout << SIMTIME << " railSignal=" << getID() << " index=" << index
<< " fms=" << foeMaxSpeed << " ms=" << maxSpeed
<< " fmd=" << foeMinDist << " md=" << minDist
<< " fmE=" << foeMinETA << " mE=" << minETA
<< " fmI=" << foeMinNumericalID << " mI=" << minNumericalID
<< "\n";
#endif
if (foeMinETA < minETA) {
return true;
} else if (foeMinETA == minETA) {
if (foeMaxSpeed > maxSpeed) {
return true;
} else if (foeMaxSpeed == maxSpeed) {
if (foeMinDist < minDist) {
return true;
} else if (foeMinDist == minDist) {
if (foeMinNumericalID < minNumericalID) {
return true;
}
}
}
}
#ifdef DEBUG_SIGNALSTATE_PRIORITY
//if (DEBUG_COND) std::cout << SIMTIME << " railSignal=" << getID() << " index=" << index << " closestVeh=" << Named::getIDSecure(closest) << " routeFoes " << toString(routeFoes) << "\n";
#endif
if (closest != nullptr) {
for (int clIndex : checkRouteConflict) {
const MSEdge& firstBidi = myRouteConflictLanes[index][clIndex].front()->getEdge();
const MSEdge* first = firstBidi.getBidiEdge();
assert(first != nullptr);
if (std::find(closest->getCurrentRouteEdge(), closest->getRoute().end(), first) != closest->getRoute().end()) {
// vehicle wants to drive passt this conflict link
//const ConstMSEdgeVector& route = closest->getRoute().getEdges();
for (const MSLane* cLane : myRouteConflictLanes[index][clIndex]) {
if (cLane->getVehicleNumberWithPartials() > 0) {
if (DEBUG_COND) std::cout << SIMTIME << " railSignal=" << getID() << " index=" << index
<< " closestVeh=" << closest->getID() << " route edge " << cLane->getEdge().getID() << " blocked\n";
// trigger rerouting
MSDevice_Routing* rDev = static_cast<MSDevice_Routing*>(closest->getDevice(typeid(MSDevice_Routing)));
if (rDev != nullptr) {
SUMOTime now = MSNet::getInstance()->getCurrentTimeStep();
if (myLastRerouteAttempt[index].first != closest
// reroute each vehicle only once if no
// periodic routing is allowed, otherwise
// with the specified period
|| (rDev->getPeriod() > 0 && myLastRerouteAttempt[index].second + rDev->getPeriod() <= now)) {
MSEdgeVector routeFoes;
routeFoes.push_back(const_cast<MSEdge*>(first));
SUMOAbstractRouter<MSEdge, SUMOVehicle>& router = MSRoutingEngine::getRouterTT(routeFoes);
myLastRerouteAttempt[index] = std::make_pair(closest, now);
try {
closest->reroute(now, "railSignal:" + getID(), router, false, false, true); // silent
} catch (ProcessError& error) {
#ifdef DEBUG_SIGNALSTATE_PRIORITY
if (DEBUG_COND) {
std::cout << " rerouting failed: " << error.what() << "\n";
}
#endif
}
}
}
return true;
}
}
}
}
}
}
return false;
}