Line data    Source code

       1             : #include "Dynamic/Env/Vehicle.hpp"
       2             : 
       3             : #include <spdlog/spdlog.h>
       4             : 
       5             : #include <limits>
       6             : #include <stdexcept>
       7             : 
       8             : #include "Dynamic/Env/Connection.hpp"
       9             : #include "Dynamic/Env/Env.hpp"
      10             : #include "Dynamic/Env/Event/EventUpdateVehicle.hpp"
      11             : #include "Dynamic/Env/Lane.hpp"
      12             : #include "Dynamic/Policy/Policy.hpp"
      13             : #include "Dynamic/Policy/RewardFunction/RewardFunctionGreedy.hpp"
      14             : 
      15             : using namespace std;
      16             : using namespace Dynamic::Env;
      17             : 
      18             : const Dynamic::Length Vehicle::LENGTH = 6.52;
      19             : 
      20           0 : Vehicle::Path::iterator Vehicle::Path::begin() {
      21           0 :     return v.begin();
      22             : }
      23             : 
      24           0 : Vehicle::Path::iterator Vehicle::Path::end() {
      25           0 :     return v.end();
      26             : }
      27             : 
      28           0 : Vehicle::Path::const_iterator Vehicle::Path::begin() const {
      29           0 :     return v.begin();
      30             : }
      31             : 
      32           0 : Vehicle::Path::const_iterator Vehicle::Path::end() const {
      33           0 :     return v.end();
      34             : }
      35             : 
      36           0 : const pair<Dynamic::Time, reference_wrapper<const Lane>> &Vehicle::Path::front() const {
      37           0 :     return v.front();
      38             : }
      39             : 
      40           0 : const pair<Dynamic::Time, reference_wrapper<const Lane>> &Vehicle::Path::back() const {
      41           0 :     return v.back();
      42             : }
      43             : 
      44           0 : void Vehicle::Path::emplace_connection(Time t, const Connection &connection) {
      45           0 :     if(!empty()) {
      46           0 :         assert(back().first <= t);
      47           0 :         assert(back().second.get() == connection.fromLane);
      48             :     }
      49             : 
      50           0 :     v.emplace_back(t, connection.toLane);
      51           0 : }
      52             : 
      53           0 : void Vehicle::Path::emplace_lane(Time t, const Lane &lane) {
      54           0 :     if(!empty()) {
      55           0 :         assert(back().first <= t);
      56           0 :         assert(back().second.get().edge == lane.edge);
      57             :     }
      58             : 
      59           0 :     v.emplace_back(t, lane);
      60           0 :     ++ms[lane];
      61           0 : }
      62             : 
      63           0 : size_t Vehicle::Path::count(const Lane &lane) const {
      64           0 :     auto it = ms.find(lane);
      65           0 :     return (it != ms.end() ? it->second : 0);
      66             : }
      67             : 
      68           0 : size_t Vehicle::Path::size() const {
      69           0 :     return v.size();
      70             : }
      71             : 
      72           0 : bool Vehicle::Path::empty() const {
      73           0 :     return v.empty();
      74             : }
      75             : 
      76           0 : Vehicle::Vehicle(
      77             :     const Dynamic::Vehicle &vehicle,
      78             :     Time                    t,
      79             :     Position                position_,
      80             :     Speed                   speed_,
      81             :     State                   state_
      82           0 : ):
      83             :     Dynamic::Vehicle(vehicle),
      84             :     lastUpdateTime(t),
      85             :     lastMoveTime(t),
      86             :     position(position_),
      87             :     speed(speed_),
      88             :     state(state_),
      89           0 :     enteredLane(t) {
      90           0 :     path.emplace_lane(t, position.lane);
      91           0 : }
      92             : 
      93           0 : shared_ptr<Action> Vehicle::pickConnection(Env &env) const {
      94           0 :     return policy->pickConnection(env);
      95             : }
      96             : 
      97           0 : void Vehicle::moveToAnotherEdge(Env &env, shared_ptr<Action> action) {
      98           0 :     assert(position.lane == action->connection.fromLane);
      99             : 
     100           0 :     assert(!action->connection.isRed());
     101           0 :     assert(!action->connection.toLane.isFull());
     102             : 
     103             :     // Reward
     104           0 :     if(prevAction) {
     105           0 :         Action::Reward r = env.rewardFunction(env, *this);
     106             : 
     107           0 :         prevAction->reward(r);
     108             :     }
     109             : 
     110           0 :     action->connection.lastUsed = env.getTime();
     111             : 
     112           0 :     path.emplace_connection(env.getTime(), action->connection);
     113             : 
     114             :     // clang-format off
     115           0 :     position = {
     116           0 :         action->lane,
     117             :         0
     118           0 :     };
     119             :     // clang-format on
     120           0 :     position.lane.moving.insert(id);
     121           0 :     path.emplace_lane(env.getTime(), position.lane);
     122           0 :     speed          = position.lane.calculateSpeed();
     123           0 :     state          = Vehicle::State::MOVING;
     124           0 :     lastUpdateTime = env.getTime();
     125           0 :     lastMoveTime   = env.getTime();
     126           0 :     enteredLane    = env.getTime();
     127             : 
     128           0 :     prevAction = action;
     129             : 
     130           0 :     size_t N = position.lane.moving.size();
     131           0 :     if(N >= 500 && N % 10 == 0) {
     132           0 :         spdlog::warn(
     133             :             "[t={}] lane {} has {} moving vehicles",
     134           0 :             env.getTime(),
     135           0 :             position.lane.idAsString(),
     136             :             N
     137             :         );
     138             :     }
     139             : 
     140           0 :     Time Dt      = (position.lane.queuePosition() - position.offset) / speed;
     141           0 :     Dt           = max(Dt, 0.0);
     142           0 :     Time tFuture = env.getTime() + Dt;
     143           0 :     env.pushEvent(make_shared<EventUpdateVehicle>(tFuture, *this));
     144           0 : }
     145             : 
     146           0 : bool Dynamic::Env::Vehicle::operator==(const Dynamic::Env::Vehicle &other) const {
     147           0 :     return Dynamic::Vehicle::operator==(other);
     148             : }
     149             : 
     150           0 : bool Dynamic::Env::Vehicle::operator!=(const Dynamic::Env::Vehicle &other) const {
     151           0 :     return !(*this == other);
     152             : }