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
}
Beispiel #2
0
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;
}