MSDevice_SSM.h

Go to the documentation of this file.

/****************************************************************************/
// 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.
/****************************************************************************/
// SUMO, Simulation of Urban MObility; see http://sumo.dlr.de/
// Copyright (C) 2013-2017 DLR (http://www.dlr.de/) and contributors
/****************************************************************************/
//
//   This file is part of SUMO.
//   SUMO is free software: you can redistribute it and/or modify
//   it under the terms of the GNU General Public License as published by
//   the Free Software Foundation, either version 3 of the License, or
//   (at your option) any later version.
//
/****************************************************************************/
#ifndef MSDevice_SSM_h
#define MSDevice_SSM_h


// ===========================================================================
// included modules
// ===========================================================================
#ifdef _MSC_VER
#include <windows_config.h>
#else
#include <config.h>
#endif

#include <queue>
#include "MSDevice.h"
#include <utils/common/SUMOTime.h>
#include <utils/iodevices/OutputDevice_File.h>
#include <utils/geom/Position.h>


// ===========================================================================
// class declarations
// ===========================================================================
class SUMOVehicle;


// ===========================================================================
// class definitions
// ===========================================================================
class MSCrossSection;

class MSDevice_SSM : public MSDevice {

private:
    static std::set<MSDevice*>* instances;

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, 
        // TODO: use ENTERED types for indicating an ongoing conflict.
        // 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, 
        // Collision (currently unused, might be differentiated further)
        ENCOUNTER_TYPE_COLLISION = 111 
    };

private:
    class Encounter {
        // TODO: Shouldn't the time lines for the conflict point locations be stored? YES!
    private:
        struct Trajectory {
            // positions
            PositionVector x;
            // momentary speeds
            PositionVector v;
        };

    public:
        Encounter(const MSVehicle* _ego, const MSVehicle* const _foe, double _begin, double extraTime);
        ~Encounter();

        void add(double time, EncounterType type, Position egoX, Position egoV, Position foeX, Position foeV,
                 Position conflictPoint, double egoDistToConflict, double foeDistToConflict, double ttc, double drac, std::pair<double, double> pet);

        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) {
                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<std::pair<std::pair<const MSLane*, double>, double> > egoPETCrossSections;
        std::vector<std::pair<std::pair<const MSLane*, double>, double> > foePETCrossSections;

        std::pair<std::pair<double, double>, Position> minTTC;
        std::pair<std::pair<double, double>, Position> maxDRAC;
        std::pair<std::pair<double, double>, Position> PET;

        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;
        bool egoLeftConflict;
        bool foeLeftConflict;
        double ttc;
        double drac;
        std::pair<double, double> pet;
        std::pair<const MSLane*, double> egoConflictEntryCrossSection;
        std::pair<const MSLane*, double> foeConflictEntryCrossSection;
    };


    struct 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


    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<MSDevice*>& into);


    static const std::set<MSDevice*>& getInstances();

    void updateAndWriteOutput();

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 MSEdge* edge, double pos, double range, double egoDistToConflictLane, const MSLane* const egoConflictLane, FoeInfoMap& foeCollector, std::set<const MSJunction*> seenJunctions);

    static void getVehiclesOnJunction(const MSJunction*, double egoDistToConflictLane, const MSLane* const egoConflictLane, FoeInfoMap& foeCollector);



    bool notifyMove(SUMOVehicle& veh, double oldPos,
                    double newPos, double newSpeed);


    bool notifyEnter(SUMOVehicle& veh, MSMoveReminder::Notification reason, const MSLane* enteredLane = 0);


    bool notifyLeave(SUMOVehicle& veh, double lastPos,
                     MSMoveReminder::Notification reason, const MSLane* enteredLane = 0);


    const std::string deviceName() const {
        return "ssm";
    }

    void generateOutput() 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);

    void processEncounters(FoeInfoMap& foes, bool forceClose = false);


    void storeEncountersExceedingMaxLength();



    void createEncounters(FoeInfoMap& foes);


    void resetEncounters();

    void flushConflicts(bool all = false);

    void 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 determineTTCandDRAC(EncounterApproachInfo& eInfo) const;


    double computeTTC(double gap, double followerSpeed, double leaderSpeed) const;


    static double computeDRAC(double gap, double followerSpeed, double leaderSpeed);

    static double computeDRAC(double d1, double d2, double v1, double v2, double t1, double t2);

    static std::string makeStringWithNAs(std::vector<double> v, double NA, std::string sep = " ");

    static std::string getOutputFilename(const SUMOVehicle& v, std::string deviceID);
    static double getDetectionRange(const SUMOVehicle& v);
    static double getExtraTime(const SUMOVehicle& v);
    static bool useGeoCoords(const SUMOVehicle& v, std::string deviceID);
    static bool requestsTrajectories(const SUMOVehicle& v);
    static bool getMeasuresAndThresholds(const SUMOVehicle& v, std::string deviceID,
                                         std::map<std::string, double>& thresholds);

private:
    std::map<std::string, double> myThresholds;
    bool mySaveTrajectories;
    double myRange;
    double myExtraTime;
    bool myUseGeoCoords;
    bool myComputeTTC, myComputeDRAC, myComputePET;
    MSVehicle* myHolderMS;


    EncounterVector myActiveEncounters;
    double myOldestActiveEncounterBegin;
    EncounterQueue myPastConflicts;

    OutputDevice* myOutputFile;

private:
    MSDevice_SSM(const MSDevice_SSM&);

    MSDevice_SSM& operator=(const MSDevice_SSM&);


};

#endif

/****************************************************************************/