Line data    Source code

       1             : #include "data/SUMO/NetState.hpp"
       2             : 
       3             : #include <spdlog/spdlog.h>
       4             : 
       5             : #include <algorithm>
       6             : #include <chrono>
       7             : #include <cstring>
       8             : #include <filesystem>
       9             : #include <iostream>
      10             : #include <mutex>
      11             : 
      12             : #include "rapidxml.hpp"
      13             : #include "utils/stringify.hpp"
      14             : #include "utils/xml.hpp"
      15             : 
      16             : using namespace std;
      17             : using namespace SUMO;
      18             : using namespace rapidxml;
      19             : using namespace utils::stringify;
      20             : 
      21             : namespace xml = utils::xml;
      22             : 
      23             : namespace fs = std::filesystem;
      24             : 
      25           0 : NetState::Timestep::Edge::Lane::Vehicle &NetState::Timestep::Edge::Lane::addVehicle(Vehicle::ID vehicleID, Length pos, Speed speed) {
      26           0 :     vehicles.emplace_back(Vehicle{vehicleID, pos, speed});
      27           0 :     return vehicles.back();
      28             : }
      29             : 
      30           0 : Index NetState::Timestep::Edge::Lane::index() const {
      31           0 :     size_t i = id.find_last_of("_");
      32           0 :     return stringify<size_t>::fromString(id.substr(i + 1));
      33             : }
      34             : 
      35           0 : NetState::Timestep::Edge::Lane &NetState::Timestep::Edge::addLane(Network::Edge::Lane::ID laneID) {
      36           0 :     lanes[laneID] = Lane{laneID};
      37           0 :     return lanes[laneID];
      38             : }
      39             : 
      40           0 : NetState::Timestep::Edge &NetState::Timestep::addEdge(Network::Edge::ID edgeID) {
      41           0 :     edges[edgeID] = Edge{edgeID};
      42           0 :     return edges[edgeID];
      43             : }
      44             : 
      45           0 : NetState::NetState(const string &filePath, ios_base::openmode openMode) {
      46             :     // is.exceptions(ios_base::failbit | ios_base::badbit);
      47           0 :     os.exceptions(ios_base::failbit | ios_base::badbit);
      48             : 
      49           0 :     fs::path p = fs::path(filePath).parent_path();
      50             : 
      51           0 :     if(openMode & ios_base::out) {
      52           0 :         if(!fs::is_directory(p)) {
      53           0 :             spdlog::info("Creating directory {}", p.string());
      54           0 :             if(!fs::create_directory(p)) {
      55           0 :                 throw ios_base::failure("Could not create directory " + p.string());
      56             :             }
      57             :         }
      58             :     }
      59             : 
      60           0 :     try {
      61           0 :         if(openMode & ios_base::in) is.open(filePath);
      62           0 :         if(openMode & ios_base::out) {
      63           0 :             os.open(filePath);
      64           0 :             os << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>"
      65           0 :                << "\n";
      66           0 :             os << "<netstate>"
      67           0 :                << "\n";
      68             :         }
      69           0 :     } catch(const ios_base::failure &ex) {
      70           0 :         throw ios_base::failure("Could not open file " + filePath);
      71             :     }
      72           0 : }
      73             : 
      74           0 : NetState::operator bool() const {
      75           0 :     return is && os;
      76             : }
      77             : 
      78           0 : void NetState::Timestep::toXML(xml_document<> &doc) const {
      79           0 :     xml_node<> &timestepEl = *doc.allocate_node(node_element, "timestep");
      80           0 :     doc.append_node(&timestepEl);
      81             : 
      82           0 :     xml::add_attribute(timestepEl, "time", time);
      83             : 
      84           0 :     for(const auto &[edgeID, edge]: edges) {
      85           0 :         xml_node<> &edgeEl = *doc.allocate_node(node_element, "edge");
      86           0 :         timestepEl.append_node(&edgeEl);
      87             : 
      88           0 :         xml::add_attribute(edgeEl, "id", edge.id);
      89             : 
      90           0 :         for(const auto &[laneID, lane]: edge.lanes) {
      91           0 :             xml_node<> &laneEl = *doc.allocate_node(node_element, "lane");
      92           0 :             edgeEl.append_node(&laneEl);
      93             : 
      94           0 :             xml::add_attribute(laneEl, "id", lane.id);
      95             : 
      96           0 :             for(const Timestep::Edge::Lane::Vehicle &vehicle: lane.vehicles) {
      97           0 :                 xml_node<> &vehicleEl = *doc.allocate_node(node_element, "vehicle");
      98           0 :                 laneEl.append_node(&vehicleEl);
      99             : 
     100           0 :                 xml::add_attribute(vehicleEl, "id", vehicle.id);
     101           0 :                 xml::add_attribute(vehicleEl, "pos", vehicle.pos);
     102           0 :                 xml::add_attribute(vehicleEl, "speed", vehicle.speed);
     103             :             }
     104             :         }
     105             :     }
     106           0 : }
     107             : 
     108           0 : NetState::Timestep NetState::Timestep::fromXML(xml_node<> &timestepEl) {
     109             :     // clang-format off
     110           0 :     Timestep ret{
     111           0 :         stringify<Time>::fromString(timestepEl.first_attribute("time")->value())
     112           0 :     };
     113             :     // clang-format on
     114             : 
     115           0 :     for(
     116           0 :         xml_node<> *edgeEl = timestepEl.first_node("edge");
     117           0 :         edgeEl;
     118           0 :         edgeEl = edgeEl->next_sibling("edge")
     119             :     ) {
     120           0 :         Edge &edge = ret.addEdge(
     121           0 :             edgeEl->first_attribute("id")->value()
     122           0 :         );
     123             : 
     124           0 :         for(
     125           0 :             xml_node<> *laneEl = edgeEl->first_node("lane");
     126           0 :             laneEl;
     127           0 :             laneEl = laneEl->next_sibling("lane")
     128             :         ) {
     129           0 :             xml_attribute<> *laneIDAttr = laneEl->first_attribute("id");
     130             : 
     131           0 :             if(laneIDAttr == nullptr) throw runtime_error("Lane ID attribute not found; t=" + to_string(ret.time) + ", edge is " + edge.id);
     132             : 
     133           0 :             Edge::Lane &lane = edge.addLane(
     134           0 :                 laneIDAttr->value()
     135           0 :             );
     136             : 
     137           0 :             for(
     138           0 :                 xml_node<> *vehicleEl = laneEl->first_node("vehicle");
     139           0 :                 vehicleEl;
     140           0 :                 vehicleEl = vehicleEl->next_sibling("vehicle")
     141             :             ) {
     142           0 :                 lane.addVehicle(
     143           0 :                     vehicleEl->first_attribute("id")->value(),
     144           0 :                     stringify<Length>::fromString(vehicleEl->first_attribute("pos")->value()),
     145           0 :                     stringify<Speed>::fromString(vehicleEl->first_attribute("speed")->value())
     146           0 :                 );
     147             :             }
     148             :         }
     149             :     }
     150             : 
     151           0 :     return ret;
     152             : }
     153             : 
     154           0 : NetState &NetState::operator<<(const NetState::Timestep &timestep) {
     155           0 :     lock_guard<mutex> lockAddQueue(*this);
     156             : 
     157           0 :     futuresQueue.push(async(launch::async, [timestep]() -> stringstream {
     158           0 :         stringstream   ss;
     159           0 :         xml_document<> doc;
     160           0 :         timestep.toXML(doc);
     161           0 :         ss << doc;
     162           0 :         return ss;
     163             :     }));
     164             : 
     165           0 :     if(futuresQueue.size() > maxQueueSize) {
     166           0 :         pool.push([this, timestep](int) -> void {
     167           0 :             lock_guard<mutex> lockPrint(*this);
     168           0 :             while(futuresQueue.size() > maxQueueSize) {
     169           0 :                 stringstream ss = futuresQueue.front().get();
     170           0 :                 os << ss.rdbuf();
     171           0 :                 futuresQueue.pop();
     172             :             }
     173           0 :         });
     174             :     }
     175             : 
     176           0 :     return *this;
     177             : }
     178             : 
     179           0 : NetState &NetState::operator>>(NetState::Timestep &timestep) {
     180           0 :     if(tsBuffer.empty()) {
     181           0 :         stringstream ss;
     182           0 :         ss << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n";
     183             : 
     184           0 :         string line;
     185             : 
     186           0 :         while(getline(is, line)) {
     187           0 :             if(line.find("<netstate>") != string::npos) continue;
     188           0 :             if(line.find("<?xml") != string::npos) continue;
     189           0 :             ss << line;
     190           0 :             if(line.find("</timestep>") != string::npos) break;
     191             :         }
     192             : 
     193           0 :         if(!is) {
     194           0 :             return *this;
     195             :         }
     196             : 
     197           0 :         const string       s = ss.str();
     198           0 :         unique_ptr<char[]> c = make_unique<char[]>(s.size() + 1);
     199           0 :         strcpy(c.get(), s.c_str());
     200             : 
     201           0 :         xml_document<> doc;
     202           0 :         doc.parse<0>(c.get());
     203             : 
     204           0 :         for(
     205           0 :             xml_node<> *timestepEl = doc.first_node("timestep");
     206           0 :             timestepEl;
     207           0 :             timestepEl = timestepEl->next_sibling("timestep")
     208             :         ) {
     209           0 :             Timestep ts = Timestep::fromXML(*timestepEl);
     210           0 :             tsBuffer.push(ts);
     211             :         }
     212             :     }
     213             : 
     214           0 :     assert(tsBuffer.size() > 0);
     215             : 
     216           0 :     timestep = tsBuffer.front();
     217             : 
     218           0 :     tsBuffer.pop();
     219             : 
     220           0 :     return *this;
     221             : }
     222             : 
     223           0 : void NetState::close() {
     224           0 :     if(is.is_open()) is.close();
     225           0 :     if(os.is_open()) {
     226           0 :         lock_guard<mutex> lock(*this);
     227           0 :         while(!futuresQueue.empty()) {
     228           0 :             stringstream ss = futuresQueue.front().get();
     229           0 :             os << ss.rdbuf();
     230           0 :             futuresQueue.pop();
     231             :         }
     232           0 :         os << "</netstate>"
     233           0 :            << "\n"
     234           0 :            << flush;
     235           0 :         os.close();
     236             :     }
     237           0 : }
     238             : 
     239           0 : NetState::~NetState() {
     240           0 :     close();
     241           0 : }