MSDevice_SSM.h

Go to the documentation of this file.

/****************************************************************************/
// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
// Copyright (C) 2013-2024 German Aerospace Center (DLR) and others.
// This program and the accompanying materials are made available under the
// terms of the Eclipse Public License 2.0 which is available at
// https://www.eclipse.org/legal/epl-2.0/
// This Source Code may also be made available under the following Secondary
// Licenses when the conditions for such availability set forth in the Eclipse
// Public License 2.0 are satisfied: GNU General Public License, version 2
// or later which is available at
// https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
// SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
/****************************************************************************/
// An SSM-device logs encounters / conflicts of the carrying vehicle with other surrounding vehicles.
// XXX: Preliminary implementation. Use with care. Especially rerouting vehicles could be problematic.
/****************************************************************************/
#pragma once
#include <config.h>
 
#include <queue>
#include "MSVehicleDevice.h"
#include <utils/common/SUMOTime.h>
#include <utils/iodevices/OutputDevice_File.h>
#include <utils/geom/Position.h>
 
 
// ===========================================================================
// class declarations
// ===========================================================================
class SUMOVehicle;
class SUMOTrafficObject;
 
 
// ===========================================================================
// class definitions
// ===========================================================================
class MSCrossSection;
 
class MSDevice_SSM : public MSVehicleDevice {
 
private:
    static std::set<MSDevice_SSM*, ComparatorNumericalIdLess>* myInstances;
 
public:
    enum EncounterType {
        // Other vehicle is closer than range, but not on a lane conflicting with the ego's route ahead
        ENCOUNTER_TYPE_NOCONFLICT_AHEAD = 0,       
        // Ego and foe vehicles' edges form a part of a consecutive sequence of edges
        // This type may be specified further by ENCOUNTER_TYPE_FOLLOWING_LEADER or ENCOUNTER_TYPE_FOLLOWING_FOLLOWER
        ENCOUNTER_TYPE_FOLLOWING = 1,       
        // Ego vehicle is on an edge that has a sequence of successors connected to the other vehicle's edge
        ENCOUNTER_TYPE_FOLLOWING_FOLLOWER = 2,       
        // Other vehicle is on an edge that has a sequence of successors connected to the ego vehicle's current edge
        ENCOUNTER_TYPE_FOLLOWING_LEADER = 3,         
        // Other vehicle is on an edge that has a sequence of successors connected to the ego vehicle's current edge
        ENCOUNTER_TYPE_ON_ADJACENT_LANES = 4,         
        // Ego and foe share an upcoming edge of their routes while the merging point for the routes is still ahead
        // This type may be specified further by ENCOUNTER_TYPE_MERGING_LEADER or ENCOUNTER_TYPE_MERGING_FOLLOWER
        ENCOUNTER_TYPE_MERGING = 5,  
        // Other vehicle is on an edge that has a sequence of successors connected to an edge on the ego vehicle's route
        // and the estimated arrival vehicle at the merge point is earlier for the ego than for the foe
        ENCOUNTER_TYPE_MERGING_LEADER = 6,  
        // Other vehicle is on an edge that has a sequence of successors connected to an edge on the ego vehicle's route
        // and the estimated arrival vehicle at the merge point is earlier for the foe than for the ego
        ENCOUNTER_TYPE_MERGING_FOLLOWER = 7,
        // Vehicles' bestlanes lead to the same edge but to adjacent lanes
        ENCOUNTER_TYPE_MERGING_ADJACENT = 8,
        // Ego's and foe's routes have crossing edges
        // This type may be specified further by ENCOUNTER_TYPE_CROSSING_LEADER or ENCOUNTER_TYPE_CROSSING_FOLLOWER
        ENCOUNTER_TYPE_CROSSING = 9,  
        // Other vehicle is on an edge that has a sequence of successors leading to an internal edge that crosses the ego vehicle's edge at a junction
        // and the estimated arrival vehicle at the merge point is earlier for the ego than for the foe
        ENCOUNTER_TYPE_CROSSING_LEADER = 10, 
        // Other vehicle is on an edge that has a sequence of successors leading to an internal edge that crosses the ego vehicle's edge at a junction
        // and the estimated arrival vehicle at the merge point is earlier for the foe than for the ego
        ENCOUNTER_TYPE_CROSSING_FOLLOWER = 11, 
        // The encounter is a possible crossing conflict, and the ego vehicle has entered the conflict area
        ENCOUNTER_TYPE_EGO_ENTERED_CONFLICT_AREA = 12, 
        // The encounter is a possible crossing conflict, and the foe vehicle has entered the conflict area
        ENCOUNTER_TYPE_FOE_ENTERED_CONFLICT_AREA = 13, 
        // The encounter has been a possible crossing conflict, but the ego vehicle has left the conflict area
        ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA = 14, 
        // The encounter has been a possible crossing conflict, but the foe vehicle has left the conflict area
        ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA = 15, 
        // The encounter has been a possible crossing conflict, and both vehicles have entered the conflict area (one must have already left, otherwise this must be a collision)
        ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA = 16, 
        // The encounter has been a possible crossing conflict, but both vehicle have left the conflict area
        ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA = 17, 
        // FOLLOWING_PASSED and MERGING_PASSED are reserved to achieve that these encounter types may be tracked longer (see updatePassedEncounter)
        // The encounter has been a following situation, but is not active any more
        ENCOUNTER_TYPE_FOLLOWING_PASSED = 18, 
        // The encounter has been a merging situation, but is not active any more
        ENCOUNTER_TYPE_MERGING_PASSED = 19, 
        // Ego vehicle and foe are driving in opposite directions towards each other on the same lane (or sequence of consecutive lanes)
        ENCOUNTER_TYPE_ONCOMING = 20,
        // Collision (currently unused, might be differentiated further)
        ENCOUNTER_TYPE_COLLISION = 111 
    };
 
    static std::string encounterToString(EncounterType type) {
        switch (type) {
            case (ENCOUNTER_TYPE_NOCONFLICT_AHEAD):
                return ("NOCONFLICT_AHEAD");
            case (ENCOUNTER_TYPE_FOLLOWING):
                return ("FOLLOWING");
            case (ENCOUNTER_TYPE_FOLLOWING_FOLLOWER):
                return ("FOLLOWING_FOLLOWER");
            case (ENCOUNTER_TYPE_FOLLOWING_LEADER):
                return ("FOLLOWING_LEADER");
            case (ENCOUNTER_TYPE_ON_ADJACENT_LANES):
                return ("ON_ADJACENT_LANES");
            case (ENCOUNTER_TYPE_MERGING):
                return ("MERGING");
            case (ENCOUNTER_TYPE_MERGING_LEADER):
                return ("MERGING_LEADER");
            case (ENCOUNTER_TYPE_MERGING_FOLLOWER):
                return ("MERGING_FOLLOWER");
            case (ENCOUNTER_TYPE_MERGING_ADJACENT):
                return ("MERGING_ADJACENT");
            case (ENCOUNTER_TYPE_CROSSING):
                return ("CROSSING");
            case (ENCOUNTER_TYPE_CROSSING_LEADER):
                return ("CROSSING_LEADER");
            case (ENCOUNTER_TYPE_CROSSING_FOLLOWER):
                return ("CROSSING_FOLLOWER");
            case (ENCOUNTER_TYPE_EGO_ENTERED_CONFLICT_AREA):
                return ("EGO_ENTERED_CONFLICT_AREA");
            case (ENCOUNTER_TYPE_FOE_ENTERED_CONFLICT_AREA):
                return ("FOE_ENTERED_CONFLICT_AREA");
            case (ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA):
                return ("EGO_LEFT_CONFLICT_AREA");
            case (ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA):
                return ("FOE_LEFT_CONFLICT_AREA");
            case (ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA):
                return ("BOTH_ENTERED_CONFLICT_AREA");
            case (ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA):
                return ("BOTH_LEFT_CONFLICT_AREA");
            case (ENCOUNTER_TYPE_FOLLOWING_PASSED):
                return ("FOLLOWING_PASSED");
            case (ENCOUNTER_TYPE_MERGING_PASSED):
                return ("MERGING_PASSED");
            case (ENCOUNTER_TYPE_ONCOMING):
                return ("ONCOMING");
            case (ENCOUNTER_TYPE_COLLISION):
                return ("COLLISION");
        }
        return ("UNKNOWN");
    };
 
private:
    class Encounter {
    private:
        struct Trajectory {
            // positions
            PositionVector x;
            // lane IDs
            std::vector<std::string> lane;
            // lane positions
            std::vector<double> lanePos;
            // momentary speeds
            PositionVector v;
        };
        struct ConflictPointInfo {
            double time;
            Position pos;
            EncounterType type;
            double value;
            double speed;
 
            ConflictPointInfo(double time, Position x, EncounterType type, double ssmValue, double speed) :
                time(time), pos(x), type(type), value(ssmValue), speed(speed) {};
        };
 
    public:
        Encounter(const MSVehicle* _ego, const MSVehicle* const _foe, double _begin, double extraTime);
        ~Encounter();
 
        void add(double time, EncounterType type, Position egoX, std::string egoLane, double egoLanePos,
                 Position egoV, Position foeX, std::string foeLane, double foeLanePos, Position foeV,
                 Position conflictPoint, double egoDistToConflict, double foeDistToConflict, double ttc, double drac, std::pair<double, double> pet, double ppet, double mdrac);
 
        std::size_t size() const {
            return timeSpan.size();
        }
 
        void resetExtraTime(double value);
        void countDownExtraTime(double amount);
        double getRemainingExtraTime() const;
 
        struct compare {
            typedef bool value_type;
            bool operator()(Encounter* e1, Encounter* e2) {
                if (e1->begin == e2->begin) {
                    return e1->foeID > e2->foeID;
                } else {
                    return e1->begin > e2->begin;
                }
            };
        };
 
 
 
    public:
        const MSVehicle* ego;
        const MSVehicle* foe;
        const std::string egoID;
        const std::string foeID;
        double begin, end;
        EncounterType currentType;
 
        double remainingExtraTime;
 
        double egoConflictEntryTime, egoConflictExitTime;
        double foeConflictEntryTime, foeConflictExitTime;
 
        std::vector<double> timeSpan;
        std::vector<int> typeSpan;
        Trajectory egoTrajectory;
        Trajectory foeTrajectory;
        std::vector<double> egoDistsToConflict;
        std::vector<double> foeDistsToConflict;
 
        PositionVector conflictPointSpan;
 
        std::vector<double> TTCspan;
        std::vector<double> DRACspan;
        std::vector<double> MDRACspan;
        std::vector<double> PPETspan;
 
//        /// @brief Cross sections at which a PET shall be calculated for the corresponding vehicle
//        std::vector<std::pair<std::pair<const MSLane*, double>, double> > egoPETCrossSections;
//        std::vector<std::pair<std::pair<const MSLane*, double>, double> > foePETCrossSections;
 
        ConflictPointInfo minTTC;
        ConflictPointInfo maxDRAC;
        ConflictPointInfo maxMDRAC;
        ConflictPointInfo PET;
        ConflictPointInfo minPPET;
 
        bool closingRequested;
 
    private:
        Encounter(const Encounter&);
        Encounter& operator=(const Encounter&);
    };
 
 
    struct EncounterApproachInfo {
        EncounterApproachInfo(Encounter* e);
        Encounter* encounter;
        EncounterType type;
        Position conflictPoint;
        double egoConflictEntryDist;
        double foeConflictEntryDist;
        double egoConflictExitDist;
        double foeConflictExitDist;
        double egoEstimatedConflictEntryTime;
        double foeEstimatedConflictEntryTime;
        double egoEstimatedConflictExitTime;
        double foeEstimatedConflictExitTime;
        double egoConflictAreaLength;
        double foeConflictAreaLength;
        double ttc;
        double drac;
        double mdrac;
        std::pair<double, double> pet;  // (egoConflictEntryTime, PET);
        double ppet;
        std::pair<const MSLane*, double> egoConflictEntryCrossSection;
        std::pair<const MSLane*, double> foeConflictEntryCrossSection;
    };
 
 
    struct FoeInfo {
        virtual ~FoeInfo() {};
        const MSLane* egoConflictLane;
        double egoDistToConflictLane;
    };
    // TODO: consider introducing a class foeCollector, which holds the foe info content
    //       plus a vehicle container to be used in findSurrounding vehicles.
    //       findSurroundingVehicles() would then deliver a vector of such foeCollectors
    //       (one for each possible egoConflictLane) instead of a map vehicle->foeInfo
    //       This could be helpful to resolve the resolution for several different
    //          projected conflicts with the same foe.
 
 
    struct UpstreamScanStartInfo {
        UpstreamScanStartInfo(const MSEdge* edge, double pos, double range, double egoDistToConflictLane, const MSLane* egoConflictLane) :
            edge(edge), pos(pos), range(range), egoDistToConflictLane(egoDistToConflictLane), egoConflictLane(egoConflictLane) {};
        const MSEdge* edge;
        double pos;
        double range;
        double egoDistToConflictLane;
        const MSLane* egoConflictLane;
    };
 
    typedef std::priority_queue<Encounter*, std::vector<Encounter*>, Encounter::compare> EncounterQueue;
    typedef std::vector<Encounter*> EncounterVector;
    typedef std::map<const MSVehicle*, FoeInfo*> FoeInfoMap;
public:
 
    static void insertOptions(OptionsCont& oc);
 
 
    static void buildVehicleDevices(SUMOVehicle& v, std::vector<MSVehicleDevice*>& into);
 
 
    static const std::set<MSDevice_SSM*, ComparatorNumericalIdLess>& getInstances();
 
    static const std::set<const MSEdge*>& getEdgeFilter() {
        return myEdgeFilter;
    }
 
    void updateAndWriteOutput();
 
    std::string getParameter(const std::string& key) const;
 
    void setParameter(const std::string& key, const std::string& value);
 
private:
    void update();
    void writeOutConflict(Encounter* e);
 
    static void toGeo(Position& x);
    static void toGeo(PositionVector& x);
 
public:
    static void cleanup();
 
 
public:
    ~MSDevice_SSM();
 
 
    static void findSurroundingVehicles(const MSVehicle& veh, double range, FoeInfoMap& foeCollector);
 
    static void getUpstreamVehicles(const UpstreamScanStartInfo& scanStart, FoeInfoMap& foeCollector, std::set<const MSLane*>& seenLanes, const std::set<const MSJunction*>& routeJunctions);
 
    static void getVehiclesOnJunction(const MSJunction*, const MSLane* egoJunctionLane, double egoDistToConflictLane, const MSLane* const egoConflictLane, FoeInfoMap& foeCollector, std::set<const MSLane*>& seenLanes);
 
 
 
    bool notifyMove(SUMOTrafficObject& veh, double oldPos,
                    double newPos, double newSpeed);
 
 
    bool notifyEnter(SUMOTrafficObject& veh, MSMoveReminder::Notification reason, const MSLane* enteredLane = 0);
 
 
    bool notifyLeave(SUMOTrafficObject& veh, double lastPos,
                     MSMoveReminder::Notification reason, const MSLane* enteredLane = 0);
 
 
    const std::string deviceName() const {
        return "ssm";
    }
 
    void generateOutput(OutputDevice* tripinfoOut) const;
 
 
 
private:
    MSDevice_SSM(SUMOVehicle& holder, const std::string& id, std::string outputFilename, std::map<std::string, double> thresholds,
                 bool trajectories, double range, double extraTime, bool useGeoCoords, bool writePositions, bool writeLanesPositions,
                 std::vector<int> conflictOrder);
 
    void processEncounters(FoeInfoMap& foes, bool forceClose = false);
 
 
    void storeEncountersExceedingMaxLength();
 
 
 
    void createEncounters(FoeInfoMap& foes);
 
 
    void computeGlobalMeasures();
 
    void resetEncounters();
 
    void flushConflicts(bool all = false);
 
    void flushGlobalMeasures();
 
    bool updateEncounter(Encounter* e, FoeInfo* foeInfo);
 
    void updatePassedEncounter(Encounter* e, FoeInfo* foeInfo, EncounterApproachInfo& eInfo);
 
 
    EncounterType classifyEncounter(const FoeInfo* foeInfo, EncounterApproachInfo& eInfo) const;
 
 
    static void determineConflictPoint(EncounterApproachInfo& eInfo);
 
 
    static void estimateConflictTimes(EncounterApproachInfo& eInfo);
 
 
    static void checkConflictEntryAndExit(EncounterApproachInfo& eInfo);
 
 
    const MSLane* findFoeConflictLane(const MSVehicle* foe, const MSLane* egoConflictLane, double& distToConflictLane) const;
 
    void closeEncounter(Encounter* e);
 
    bool qualifiesAsConflict(Encounter* e);
 
    void computeSSMs(EncounterApproachInfo& e) const;
 
 
    void determinePET(EncounterApproachInfo& eInfo) const;
 
 
    void determineTTCandDRACandPPETandMDRAC(EncounterApproachInfo& eInfo) const;
 
 
    double computeTTC(double gap, double followerSpeed, double leaderSpeed) const;
 
    static double computeDRAC(double gap, double followerSpeed, double leaderSpeed);
 
    static double computeMDRAC(double gap, double followerSpeed, double leaderSpeed, double prt);
 
    static double computeDRAC(const EncounterApproachInfo& eInfo);
 
    static std::string makeStringWithNAs(const std::vector<double>& v, const double NA);
    static std::string makeStringWithNAs(const std::vector<double>& v, const std::vector<double>& NAs);
    std::string makeStringWithNAs(const PositionVector& v);
    std::string makeStringWithNAs(const Position& p);
    static std::string writeNA(double v, double NA = INVALID_DOUBLE);
 
    static std::string getOutputFilename(const SUMOVehicle& v, std::string deviceID);
    static double getDetectionRange(const SUMOVehicle& v);
    static double getMDRAC_PRT(const SUMOVehicle& v);
    static double getExtraTime(const SUMOVehicle& v);
    static bool useGeoCoords(const SUMOVehicle& v);
    static bool writePositions(const SUMOVehicle& v);
    static bool writeLanesPositions(const SUMOVehicle& v);
    static bool filterByConflictType(const SUMOVehicle& v, std::string deviceID, std::vector<int>& conflictTypes);
    static bool requestsTrajectories(const SUMOVehicle& v);
    static bool getMeasuresAndThresholds(const SUMOVehicle& v, std::string deviceID,
                                         std::map<std::string, double>& thresholds);
 
    static void initEdgeFilter();
 
 
private:
    std::map<std::string, double> myThresholds;
    bool mySaveTrajectories;
    double myRange;
    double myMDRACPRT;
    double myExtraTime;
    bool myUseGeoCoords;
    bool myWritePositions;
    bool myWriteLanesPositions;
    bool myFilterConflictTypes;
    std::vector<int> myDroppedConflictTypes;
 
    bool myComputeTTC, myComputeDRAC, myComputePET, myComputeBR, myComputeSGAP, myComputeTGAP, myComputePPET, myComputeMDRAC;
    MSVehicle* myHolderMS;
 
 
    EncounterVector myActiveEncounters;
    double myOldestActiveEncounterBegin;
    EncounterQueue myPastConflicts;
 
 
 
    std::vector<double> myGlobalMeasuresTimeSpan;
    PositionVector myGlobalMeasuresPositions;
    std::vector<std::string> myGlobalMeasuresLaneIDs;
    std::vector<double> myGlobalMeasuresLanesPositions;
    std::vector<double> myBRspan;
    std::vector<double> mySGAPspan;
    std::vector<double> myTGAPspan;
    std::pair<std::pair<double, Position>, double> myMaxBR;
    std::pair<std::pair<std::pair<double, Position>, double>, std::string>  myMinSGAP;
    std::pair<std::pair<std::pair<double, Position>, double>, std::string>  myMinTGAP;
 
    static std::set<const MSEdge*> myEdgeFilter;
    static bool myEdgeFilterInitialized;
    static bool myEdgeFilterActive;
 
    OutputDevice* myOutputFile;
 
    static std::set<std::string> myCreatedOutputFiles;
 
 
    static int myIssuedParameterWarnFlags;
    enum SSMParameterWarning {
        SSM_WARN_MEASURES = 1,
        SSM_WARN_THRESHOLDS = 1 << 1,
        SSM_WARN_TRAJECTORIES = 1 << 2,
        SSM_WARN_RANGE = 1 << 3,
        SSM_WARN_MDRAC_PRT = 1 << 3,
        SSM_WARN_EXTRATIME = 1 << 4,
        SSM_WARN_FILE = 1 << 5,
        SSM_WARN_GEO = 1 << 6,
        SSM_WARN_POS = 1 << 7,
        SSM_WARN_LANEPOS = 1 << 8,
        SSM_WARN_CONFLICTFILTER = 1 << 9
    };
 
    static const std::set<int> FOE_ENCOUNTERTYPES;
    static const std::set<int> EGO_ENCOUNTERTYPES;
 
private:
    MSDevice_SSM(const MSDevice_SSM&);
 
    MSDevice_SSM& operator=(const MSDevice_SSM&);
 
 
};