Line data    Source code

       1             : #include "Dynamic/Env/Env.hpp"
       2             : 
       3             : #include <spdlog/spdlog.h>
       4             : 
       5             : #include <chrono>
       6             : #include <cstdarg>
       7             : #include <functional>
       8             : #include <iomanip>
       9             : #include <iostream>
      10             : #include <memory>
      11             : #include <stdexcept>
      12             : 
      13             : #include "Alg/Graph.hpp"
      14             : #include "Dynamic/Demand/Demand.hpp"
      15             : #include "Dynamic/Dynamic.hpp"
      16             : #include "Dynamic/Env/Edge.hpp"
      17             : #include "Dynamic/Env/Event/Event.hpp"
      18             : #include "Dynamic/Env/Event/EventComposite.hpp"
      19             : #include "Dynamic/Env/Event/EventDump.hpp"
      20             : #include "Dynamic/Env/Event/EventLog.hpp"
      21             : #include "Dynamic/Env/Event/EventMoveVehicle.hpp"
      22             : #include "Dynamic/Env/Event/EventPopQueue.hpp"
      23             : #include "Dynamic/Env/Event/EventSpawnVehicle.hpp"
      24             : #include "Dynamic/Env/Event/EventUpdateTrafficLight.hpp"
      25             : #include "Dynamic/Env/Event/EventUpdateVehicle.hpp"
      26             : #include "Dynamic/Env/Lane.hpp"
      27             : #include "Dynamic/Env/TrafficLight.hpp"
      28             : #include "Dynamic/Env/Vehicle.hpp"
      29             : #include "Dynamic/Policy/RewardFunction/RewardFunction.hpp"
      30             : #include "Dynamic/SUMOAdapter.hpp"
      31             : #include "Log/ProgressLogger.hpp"
      32             : 
      33             : using namespace std;
      34             : using namespace Dynamic::Env;
      35             : 
      36             : typedef chrono::steady_clock clk;
      37             : 
      38             : // clang-format off
      39           0 : Env::Env(
      40             :     Dynamic::RewardFunction &rewardFunction_,
      41             :     Time startTime
      42           0 : ):
      43             :     rewardFunction(rewardFunction_),
      44           0 :     t(startTime)
      45           0 : {}
      46             : // clang-format on
      47             : 
      48           0 : Dynamic::Time Env::getTime() const {
      49           0 :     return t;
      50             : }
      51             : 
      52           0 : size_t Env::getNumberProcessedEvents() const { return numberProcessedEvents; }
      53             : 
      54           0 : Alg::Graph Env::toGraph() const {
      55           0 :     Alg::Graph G;
      56             : 
      57           0 :     for(const auto &[_, edge]: edges) {
      58           0 :         Time                     travelTime = edge.length / edge.maxSpeed;
      59           0 :         Alg::Graph::Edge::Weight w          = travelTime;
      60           0 :         G.addEdge(edge.id, edge.u, edge.v, w);
      61             : 
      62           0 :         unordered_map<Edge::ID, Connection::ID> toNodes;
      63           0 :         for(const Connection &connection: edge.getOutgoingConnections()) {
      64           0 :             const Edge &to = connection.toLane.edge;
      65             : 
      66           0 :             Alg::Graph::Edge::Weight wConn = connection.getMinExpectedStopTimeTL();
      67             : 
      68           0 :             G.addEdge(connection.id + 1000000, edge.v, to.u, wConn);
      69             :         }
      70             :     }
      71             : 
      72           0 :     return G;
      73             : }
      74             : 
      75           0 : Edge &Env::addEdge(Edge::ID id, Node u, Node v, Length length, Speed speed, Edge::Priority priority, size_t nLanes) {
      76           0 :     if(edges.count(id))
      77           0 :         throw runtime_error("Edge " + to_string(id) + "already exists");
      78           0 :     return edges[id] = Edge(id, u, v, length, speed, priority, nLanes);
      79             : }
      80             : 
      81           0 : void Env::initializeTrafficLights(Time begin) {
      82           0 :     for(auto &[_, trafficLight]: trafficLights) {
      83           0 :         const auto &p = trafficLight.getPhase(begin);
      84             : 
      85           0 :         const auto &[phase, tStart] = p;
      86             : 
      87           0 :         pushEvent(make_shared<EventUpdateTrafficLight>(
      88             :             tStart,
      89             :             trafficLight,
      90             :             phase
      91             :         ));
      92             :     }
      93           0 : }
      94             : 
      95           0 : void Env::addDemand(const Demand &demand) {
      96           0 :     vector<Dynamic::Vehicle> vehs = demand.getVehicles();
      97           0 :     for(const Dynamic::Vehicle &vehicle: vehs) {
      98           0 :         eventQueue.push(make_shared<EventSpawnVehicle>(
      99           0 :             vehicle.depart,
     100             :             vehicle
     101             :         ));
     102             :     }
     103           0 : }
     104             : 
     105           0 : size_t Env::getNumberVehicles() const {
     106           0 :     return vehicles.size();
     107             : }
     108             : 
     109           0 : size_t Env::getQueueSize() const {
     110           0 :     return eventQueue.size();
     111             : }
     112             : 
     113           0 : void Env::pushEvent(shared_ptr<Event> event) {
     114           0 :     eventQueue.push(event);
     115           0 : }
     116             : 
     117           0 : TrafficLight &Env::addTrafficLight(TrafficLight::ID id, Time offset) {
     118           0 :     auto [it, success] = trafficLights.emplace(id, TrafficLight(id, offset));
     119           0 :     if(!success) throw runtime_error("TrafficLight already exists");
     120             : 
     121           0 :     TrafficLight &trafficLight = it->second;
     122             : 
     123           0 :     return trafficLight;
     124             : }
     125             : 
     126           0 : TrafficLight &Env::getTrafficLight(const TrafficLight::ID &id) {
     127           0 :     try {
     128           0 :         return trafficLights.at(id);
     129           0 :     } catch(const out_of_range &e) {
     130           0 :         throw out_of_range("Env::getTrafficLight: TrafficLight " + to_string(id) + " not found");
     131             :     }
     132             : }
     133             : 
     134           0 : Connection &Env::addConnection(Connection::ID id, Lane &fromLane, Lane &toLane) {
     135           0 :     auto [it, success] = connections.emplace(id, Connection(id, fromLane, toLane));
     136           0 :     if(!success) throw runtime_error("Connection with ID " + to_string(id) + "already exists");
     137             : 
     138           0 :     Connection &connection = it->second;
     139             : 
     140             :     // clang-format off
     141           0 :     if(
     142           0 :         !fromLane.outgoingConnections[toLane.edge.id].emplace(toLane.index, connection).second ||
     143           0 :         !toLane.incomingConnections[fromLane.edge.id].emplace(fromLane.index, connection).second
     144             :     )
     145           0 :         throw runtime_error(
     146           0 :             "Connection between lanes " +
     147           0 :             fromLane.idAsString() + " and " +
     148           0 :             toLane.idAsString() + " already exists"
     149           0 :         );
     150             :     // clang-format on
     151             : 
     152           0 :     return connection;
     153             : }
     154             : 
     155           0 : const Edge &Env::getEdge(const Edge::ID &id) const {
     156           0 :     try {
     157           0 :         return edges.at(id);
     158           0 :     } catch(const out_of_range &e) {
     159           0 :         throw out_of_range("Env::getEdge: Edge " + to_string(id) + " not found");
     160             :     }
     161             : }
     162             : 
     163           0 : Edge &Env::getEdge(const Edge::ID &id) {
     164           0 :     try {
     165           0 :         return edges.at(id);
     166           0 :     } catch(const out_of_range &e) {
     167           0 :         throw out_of_range("Env::getEdge: Edge " + to_string(id) + " not found");
     168             :     }
     169             : }
     170             : 
     171           0 : list<reference_wrapper<Edge>> Env::getEdges() {
     172           0 :     list<reference_wrapper<Edge>> edgesList;
     173           0 :     for(auto &[_, edge]: edges) {
     174           0 :         edgesList.push_back(edge);
     175             :     }
     176           0 :     return edgesList;
     177             : }
     178             : 
     179           0 : Vehicle &Env::getVehicle(const Vehicle::ID &id) {
     180           0 :     try {
     181           0 :         return vehicles.at(id);
     182           0 :     } catch(const out_of_range &e) {
     183           0 :         throw out_of_range("Env::getVehicle: Vehicle " + to_string(id) + " not found");
     184             :     }
     185             : }
     186             : 
     187           0 : list<reference_wrapper<Vehicle>> Env::getVehicles() {
     188           0 :     list<reference_wrapper<Vehicle>> vehiclesList;
     189           0 :     for(auto &[_, vehicle]: vehicles) {
     190           0 :         vehiclesList.push_back(vehicle);
     191             :     }
     192           0 :     return vehiclesList;
     193             : }
     194             : 
     195           0 : list<reference_wrapper<const Vehicle>> Env::getVehicles() const {
     196           0 :     list<reference_wrapper<const Vehicle>> vehiclesList;
     197           0 :     for(auto &[_, vehicle]: vehicles) {
     198           0 :         vehiclesList.push_back(vehicle);
     199             :     }
     200           0 :     return vehiclesList;
     201             : }
     202             : 
     203           0 : Connection       &Env::getConnection(const Connection::ID &id) { return connections.at(id); }
     204           0 : const Connection &Env::getConnection(const Connection::ID &id) const { return connections.at(id); }
     205             : 
     206           0 : TAZ &Env::addTAZ(TAZ::ID id) {
     207           0 :     auto [it, success] = tazs.emplace(id, TAZ(id));
     208           0 :     if(!success) throw runtime_error("TAZ already exists");
     209             : 
     210           0 :     TAZ &taz = it->second;
     211             : 
     212           0 :     return taz;
     213             : }
     214             : 
     215           0 : TAZ &Env::getTAZ(TAZ::ID id) {
     216           0 :     try {
     217           0 :         return tazs.at(id);
     218           0 :     } catch(const out_of_range &e) {
     219           0 :         throw out_of_range("Env::getTAZ: TAZ " + to_string(id) + " not found");
     220             :     }
     221             : }
     222             : 
     223           0 : list<reference_wrapper<const TAZ>> Env::getTAZs() const {
     224           0 :     list<reference_wrapper<const TAZ>> tazsList;
     225           0 :     for(auto &[_, taz]: tazs) {
     226           0 :         tazsList.push_back(taz);
     227             :     }
     228           0 :     return tazsList;
     229             : }
     230             : 
     231           0 : Vehicle &Env::addVehicle(Dynamic::Vehicle dynamicVehicle, Time t_, const Position &position, Speed speed) {
     232           0 :     auto [it, success] = vehicles.emplace(dynamicVehicle.id, Vehicle(dynamicVehicle, t_, position, speed, Vehicle::State::MOVING));
     233           0 :     if(!success) throw runtime_error("Vehicle already exists");
     234             : 
     235           0 :     Vehicle &vehicle = it->second;
     236             : 
     237           0 :     vehicle.position.lane.moving.insert(vehicle.id);
     238             : 
     239           0 :     return vehicle;
     240             : }
     241             : 
     242           0 : void Env::runUntil(Time tEnd) {
     243           0 :     while(!eventQueue.empty()) {
     244           0 :         if(eventQueue.top()->t > tEnd) break;
     245             : 
     246           0 :         shared_ptr<Event> event = eventQueue.top();
     247           0 :         eventQueue.pop();
     248             : 
     249           0 :         if(event->t < t) {
     250           0 :             spdlog::warn("Event time {} is less than current time {}", event->t, t);
     251             :         }
     252             : 
     253           0 :         t = max(t, event->t);
     254             : 
     255           0 :         ++numberProcessedEvents;
     256             : 
     257           0 :         event->process(*this);
     258             : 
     259           0 :         event.reset();
     260             :     }
     261           0 : }
     262             : 
     263           0 : void Env::updateAllVehicles(Time t_) {
     264           0 :     runUntil(t_);
     265           0 :     for(auto &[_, vehicle]: vehicles) {
     266           0 :         if(vehicle.state == Vehicle::State::LEFT) continue;
     267           0 :         EventMoveVehicle event(t_, vehicle);
     268           0 :         event.process(*this);
     269             :     }
     270           0 :     runUntil(t_);
     271           0 : }
     272             : 
     273           0 : void Env::log(Log::ProgressLogger &logger, Time tStartSim, Time tEndSim, Time delta) {
     274           0 :     policyLogger = make_shared<Policy::Logger>();
     275           0 :     Env::log(logger, tStartSim, tEndSim, delta, *policyLogger);
     276           0 : }
     277             : 
     278           0 : void Env::log(Log::ProgressLogger &logger, Time tStartSim, Time tEndSim, Time delta, Policy::Logger &pLogger) {
     279           0 :     logger << fixed << setprecision(6);
     280             : 
     281           0 :     logger << Log::ProgressLogger::Elapsed(0)
     282           0 :            << Log::ProgressLogger::Progress(0)
     283           0 :            << Log::ProgressLogger::ETA(1)
     284           0 :            << Log::ProgressLogger::StartText()
     285           0 :            << "t"
     286           0 :            << "\t#vehTot"
     287           0 :            << "\t#veh"
     288           0 :            << "\t#dspawn"
     289           0 :            << "\ttravelTime"
     290           0 :            << "\ttravelTInterval"
     291           0 :            << "\t#procE"
     292           0 :            << "\t";
     293           0 :     pLogger.header(logger);
     294           0 :     logger << Log::ProgressLogger::EndMessage();
     295             : 
     296           0 :     clk::time_point now = clk::now();
     297           0 :     for(Time time = tStartSim; time <= tEndSim; time += delta) {
     298           0 :         pushEvent(make_shared<EventLog>(
     299             :             time,
     300             :             tStartSim,
     301             :             tEndSim,
     302             :             now,
     303             :             logger,
     304             :             pLogger
     305             :         ));
     306             :     }
     307           0 : }
     308             : 
     309           0 : void Env::dump(SUMO::NetState &netState, const SUMOAdapter &adapter, Time tStartSim, Time delta, size_t numberDumps, bool closeAfterAllDumps) {
     310           0 :     for(size_t i = 0; i < numberDumps; ++i) {
     311           0 :         pushEvent(make_shared<EventDump>(
     312           0 :             tStartSim + (Time)i * delta,
     313             :             netState,
     314             :             adapter,
     315           0 :             closeAfterAllDumps && (i == numberDumps - 1)
     316             :         ));
     317             :     }
     318           0 : }
     319             : 
     320           0 : void Env::setDiscardVehicles(bool discardVehicles_) {
     321           0 :     discardVehicles = discardVehicles_;
     322           0 : }
     323             : 
     324           0 : void Env::discardVehicle(const Vehicle &vehicle) {
     325           0 :     if(discardVehicles) {
     326           0 :         vehicles.erase(vehicle.id);
     327             :     }
     328           0 : }
     329             : 
     330           0 : void Env::setDespawnTime(Time despawnTime_) {
     331           0 :     despawnTime = despawnTime_;
     332           0 : }
     333             : 
     334           0 : Dynamic::Time Env::getDespawnTime() const {
     335           0 :     return despawnTime;
     336             : }
     337             : 
     338           0 : size_t &Env::numberOfDespawnedVehicles() {
     339           0 :     return numberDespawnedVehicles;
     340             : }