Line data    Source code

       1             : #include "Dynamic/Policy/PathPolicy.hpp"
       2             : 
       3             : #include <cassert>
       4             : #include <memory>
       5             : #include <random>
       6             : #include <stdexcept>
       7             : 
       8             : #include "Alg/Graph.hpp"
       9             : #include "Dynamic/Env/Connection.hpp"
      10             : #include "Dynamic/Env/Env.hpp"
      11             : #include "Dynamic/Env/Lane.hpp"
      12             : #include "Dynamic/Env/Vehicle.hpp"
      13             : 
      14             : using namespace std;
      15             : using namespace Dynamic;
      16             : 
      17             : typedef Alg::Graph::Path Path;
      18             : 
      19             : const Env::Edge::ID PathPolicy::START = -1;
      20             : const Env::Edge::ID PathPolicy::END   = -2;
      21             : 
      22           0 : PathPolicy::Action::Action(
      23             :     Env::Connection &connection_,
      24             :     Env::Lane       &lane_
      25           0 : ):
      26           0 :     Env::Action{connection_, lane_} {}
      27             : 
      28           0 : void PathPolicy::Action::reward(Reward) {
      29             :     // Do nothing
      30           0 : }
      31             : 
      32           0 : PathPolicy::PathPolicy(
      33             :     Vehicle::ID             id_,
      34             :     const Alg::Graph::Path &path,
      35             :     shared_ptr<mt19937>     gen_
      36           0 : ):
      37             :     id(id_),
      38           0 :     gen(gen_) {
      39           0 :     assert(path.size() % 2 == 1);
      40             : 
      41           0 :     Alg::Graph::Path newPath;
      42           0 :     for(size_t i = 0; i < path.size(); ++i) {
      43             :         // Because edges with odd indices are connections, so not normal edges
      44           0 :         if(i % 2 == 0) newPath.push_back(path[i]);
      45             :     }
      46             : 
      47           0 :     nextEdgeMap[START] = newPath.front().id;
      48             : 
      49           0 :     for(
      50           0 :         Path::const_iterator it     = newPath.begin(),
      51           0 :                              nextIt = ++newPath.begin();
      52           0 :         it != newPath.end();
      53           0 :         ++it, ++nextIt
      54             :     ) {
      55           0 :         Env::Edge::ID u = (Env::Edge::ID)it->id;
      56           0 :         Env::Edge::ID v = (nextIt == newPath.end() ? END : (Env::Edge::ID)nextIt->id);
      57             : 
      58           0 :         nextEdgeMap[u] = v;
      59             :     }
      60           0 : }
      61             : 
      62           0 : Env::Lane &PathPolicy::pickInitialLane(
      63             :     Vehicle &,
      64             :     Env::Env &env
      65             : ) {
      66           0 :     Env::Edge::ID edgeID = nextEdgeMap.at(START);
      67           0 :     Env::Edge    &edge   = env.getEdge(edgeID);
      68             : 
      69           0 :     Env::Edge::ID nextEdgeID = nextEdgeMap.at(edge.id);
      70           0 :     if(nextEdgeID == END) {
      71           0 :         vector<Env::Lane> &lanes = edge.lanes;
      72             : 
      73           0 :         uniform_int_distribution<size_t> lanesDistribution(0, lanes.size() - 1);
      74             : 
      75           0 :         auto it = lanes.begin();
      76           0 :         advance(it, lanesDistribution(*gen));
      77             : 
      78           0 :         Env::Lane &lane = *it;
      79             : 
      80           0 :         return lane;
      81             :     } else {
      82           0 :         const Env::Edge &nextEdge = env.getEdge(nextEdgeID);
      83             : 
      84           0 :         list<reference_wrapper<Env::Connection>> connections =
      85           0 :             edge.getOutgoingConnections(nextEdge);
      86             : 
      87           0 :         if(connections.empty()) {
      88             :             // clang-format off
      89           0 :             throw out_of_range(
      90           0 :                 "PathPolicy::pickInitialLane: Edge " + to_string(edge.id) +
      91           0 :                 " does not have any outgoing connections to edge " + to_string(nextEdge.id) +
      92           0 :                 "; vehicle ID is " + to_string(id)
      93           0 :             );
      94             :             // clang-format on
      95             :         }
      96             : 
      97           0 :         uniform_int_distribution<size_t> connectionsDistribution(0, connections.size() - 1);
      98             : 
      99           0 :         auto it = connections.begin();
     100           0 :         advance(it, connectionsDistribution(*gen));
     101             : 
     102           0 :         const Env::Connection &connection = *it;
     103             : 
     104           0 :         Env::Lane &lane = connection.fromLane;
     105             : 
     106           0 :         return lane;
     107             :     }
     108             : }
     109             : 
     110           0 : shared_ptr<Env::Action> PathPolicy::pickConnection(Env::Env &env) {
     111           0 :     const Env::Vehicle &vehicle = env.getVehicle(id);
     112           0 :     const Env::Lane    &lane    = vehicle.position.lane;
     113           0 :     const Env::Edge    &edge    = lane.edge;
     114             : 
     115           0 :     Env::Edge::ID nextEdgeID;
     116           0 :     try {
     117           0 :         nextEdgeID = nextEdgeMap.at(edge.id);
     118           0 :     } catch(out_of_range &e) {
     119           0 :         throw out_of_range("PathPolicy::pickConnection: Edge " + to_string(edge.id) + " does not belong to the path of vehicle " + to_string(id));
     120             :     }
     121             : 
     122           0 :     if(nextEdgeID == END) {
     123           0 :         return make_shared<PathPolicy::Action>(Env::Connection::LEAVE, Env::Lane::INVALID);
     124             :     }
     125             : 
     126           0 :     Env::Edge &nextEdge = env.getEdge(nextEdgeID);
     127             : 
     128           0 :     list<reference_wrapper<Env::Connection>> connections =
     129           0 :         lane.getOutgoingConnections(nextEdge);
     130             : 
     131           0 :     if(connections.empty()) {
     132             :         // clang-format off
     133           0 :         throw out_of_range(
     134           0 :             "PathPolicy::pickConnection: Edge " + to_string(edge.id) +
     135           0 :             " does not have any outgoing connections to edge " + to_string(nextEdge.id) +
     136           0 :             "; vehicle ID is " + to_string(id)
     137           0 :         );
     138             :         // clang-format on
     139             :     }
     140             : 
     141           0 :     uniform_int_distribution<size_t> connectionsDistribution(0, connections.size() - 1);
     142             : 
     143           0 :     auto itConnection = connections.begin();
     144           0 :     advance(itConnection, connectionsDistribution(*gen));
     145             : 
     146           0 :     Env::Connection &connection = *itConnection;
     147             : 
     148           0 :     Env::Edge::ID nextNextEdgeID;
     149           0 :     try {
     150           0 :         nextNextEdgeID = nextEdgeMap.at(nextEdgeID);
     151           0 :     } catch(out_of_range &e) {
     152           0 :         throw out_of_range("PathPolicy::pickConnection: Edge " + to_string(nextEdgeID) + " does not belong to the path of vehicle " + to_string(id));
     153             :     }
     154             : 
     155           0 :     if(nextNextEdgeID == END) {
     156           0 :         vector<Env::Lane>               &lanes = nextEdge.lanes;
     157           0 :         uniform_int_distribution<size_t> lanesDistribution(0, lanes.size() - 1);
     158             : 
     159           0 :         auto itLane = lanes.begin();
     160           0 :         advance(itLane, lanesDistribution(*gen));
     161             : 
     162           0 :         Env::Lane &nextLane = *itLane;
     163             : 
     164           0 :         return make_shared<PathPolicy::Action>(connection, nextLane);
     165             :     } else {
     166           0 :         const Env::Edge &nextNextEdge = env.getEdge(nextNextEdgeID);
     167             : 
     168           0 :         list<reference_wrapper<Env::Connection>> nextConnections =
     169           0 :             nextEdge.getOutgoingConnections(nextNextEdge);
     170             : 
     171           0 :         if(nextConnections.empty()) {
     172             :             // clang-format off
     173           0 :             throw out_of_range(
     174           0 :                 "PathPolicy::pickConnection: Edge " + to_string(nextEdge.id) +
     175           0 :                 " does not have any outgoing connections to edge " + to_string(nextNextEdge.id) +
     176           0 :                 "; vehicle ID is " + to_string(id)
     177           0 :             );
     178             :             // clang-format on
     179             :         }
     180             : 
     181           0 :         uniform_int_distribution<size_t> nextConnectionsDistribution(0, nextConnections.size() - 1);
     182             : 
     183           0 :         auto itNextConnection = nextConnections.begin();
     184           0 :         advance(itNextConnection, nextConnectionsDistribution(*gen));
     185             : 
     186           0 :         const Env::Connection &nextConnection = *itNextConnection;
     187             : 
     188           0 :         return make_shared<PathPolicy::Action>(connection, nextConnection.fromLane);
     189             :     }
     190             : }
     191             : 
     192           0 : PathPolicy::ShortestPathFactory::ShortestPathFactory(const Env::Env &env):
     193           0 :     ShortestPathFactory(env, 0) {}
     194             : 
     195           0 : PathPolicy::ShortestPathFactory::ShortestPathFactory(
     196             :     const Env::Env            &env,
     197             :     random_device::result_type seed
     198           0 : ):
     199           0 :     gen(make_shared<mt19937>(seed)) {
     200           0 :     Alg::Graph G = env.toGraph();
     201             : 
     202           0 :     vector<Alg::Graph::Node> startNodes;
     203           0 :     for(const Env::TAZ &taz: env.getTAZs())
     204           0 :         for(const Env::Edge &source: taz.sources)
     205           0 :             startNodes.push_back(source.u);
     206             : 
     207           0 :     sp.solve(G, startNodes);
     208           0 : }
     209             : 
     210           0 : shared_ptr<Policy> PathPolicy::ShortestPathFactory::create(
     211             :     Vehicle::ID id_,
     212             :     Time,
     213             :     const Env::TAZ &fromTAZ,
     214             :     const Env::TAZ &toTAZ
     215             : ) {
     216           0 :     const size_t NUMBER_TRIES = 100;
     217           0 :     for(size_t i = 0; i < NUMBER_TRIES; ++i) {
     218           0 :         uniform_int_distribution<size_t> fromDistribution(0, fromTAZ.sources.size() - 1);
     219           0 :         uniform_int_distribution<size_t> toDistribution(0, toTAZ.sinks.size() - 1);
     220             : 
     221           0 :         auto itFrom = fromTAZ.sources.begin();
     222           0 :         advance(itFrom, fromDistribution(*gen));
     223             : 
     224           0 :         auto itTo = toTAZ.sinks.begin();
     225           0 :         advance(itTo, toDistribution(*gen));
     226             : 
     227           0 :         const Env::Edge &from = *itFrom;
     228           0 :         const Env::Edge &to   = *itTo;
     229             : 
     230           0 :         if(!sp.hasVisited(from.u, to.v)) {
     231           0 :             continue;
     232             :         }
     233             : 
     234           0 :         Alg::Graph::Path path = sp.getPath(from.u, to.v);
     235             : 
     236           0 :         return make_shared<PathPolicy>(id_, path, gen);
     237             :     }
     238             : 
     239             :     // clang-format off
     240           0 :     throw logic_error(
     241           0 :         "PathPolicy::ShortestPathFactory::create: "s +
     242           0 :         "Could not find a path between TAZs " + to_string(fromTAZ.id) + 
     243           0 :         " and " + to_string(toTAZ.id)
     244           0 :     );
     245             :     // clang-format on
     246             : }