Improved algorithm for traffic lights

examples/slow_charge_tl.cpp

@@ -226,6 +226,8 @@ int main(int argc, char** argv) {
   // dynamics.setForcePriorities(false);
   dynamics.setSpeedFluctuationSTD(0.1);
   dynamics.setMinSpeedRateo(0.95);
+  if (OPTIMIZE)
+    dynamics.setDataUpdatePeriod(30);  // Store data every 30 time steps
   dynamics.updatePaths();
 
   const auto TM = dynamics.turnMapping();
@@ -308,7 +310,7 @@ int main(int argc, char** argv) {
     }
     dynamics.evolve(false);
     if (OPTIMIZE && (dynamics.time() % 420 == 0)) {
-      dynamics.optimizeTrafficLights(std::floor(420. / 60), 0.15);
+      dynamics.optimizeTrafficLights(std::floor(420. / 60), 0.15, 3. / 10);
     }
     if (dynamics.time() % 2400 == 0 && nAgents > 0) {
       // auto meanDelta = std::accumulate(deltas.begin(), deltas.end(), 0) /

src/dsm/headers/Dynamics.hpp

@@ -84,8 +84,10 @@
     std::vector<unsigned int> m_travelTimes;
     std::unordered_map<Id, Id> m_agentNextStreetId;
     bool m_forcePriorities;
+    std::optional<Delay> m_dataUpdatePeriod;
     std::unordered_map<Id, std::array<unsigned long long, 4>> m_turnCounts;
     std::unordered_map<Id, std::array<long, 4>> m_turnMapping;
+    std::unordered_map<Id, unsigned long long> m_streetTails;
 
     /// @brief Get the next street id
     /// @param agentId The id of the agent
@@ -147,6 +149,12 @@
     /// @param forcePriorities The flag
     /// @details If true, if an agent cannot move to the next street, the whole node is skipped
     void setForcePriorities(bool forcePriorities) { m_forcePriorities = forcePriorities; }
+    /// @brief Set the data update period.
+    /// @param dataUpdatePeriod Delay, The period
+    /// @details Some data, i.e. the street queue lengths, are stored only after a fixed amount of time which is represented by this variable.
+    void setDataUpdatePeriod(Delay dataUpdatePeriod) {
+      m_dataUpdatePeriod = dataUpdatePeriod;
+    }
 
     /// @brief Update the paths of the itineraries based on the actual travel times
     virtual void updatePaths();
@@ -161,11 +169,14 @@
     /// @param reinsert_agents If true, the agents are reinserted in the simulation after they reach their destination
     virtual void evolve(bool reinsert_agents = false);
     /// @brief Optimize the traffic lights by changing the green and red times
-    /// @param percentage double, The percentage of the TOTAL cycle time to add or subtract to the green time
+    /// @param nCycles Delay, The number of cycles (times agents are being added) between two calls of this function
     /// @param threshold double, The percentage of the mean capacity of the streets used as threshold for the delta between the two tails.
+    /// @param densityTolerance double, The algorithm will consider all streets with density up to densityTolerance*meanDensity
     /// @details The function cycles over the traffic lights and, if the difference between the two tails is greater than
     ///   the threshold multiplied by the mean capacity of the streets, it changes the green and red times of the traffic light, keeping the total cycle time constant.
-    void optimizeTrafficLights(double percentage, double threshold = 0.);
+    void optimizeTrafficLights(Delay nCycles,
+                               double threshold = 0.,
+                               double densityTolerance = 0.);
 
     /// @brief Get the graph
     /// @return const Graph<Id, Size>&, The graph
@@ -325,6 +336,7 @@
         m_maxFlowPercentage{1.},
         m_forcePriorities{false} {
     for (const auto& [streetId, street] : m_graph.streetSet()) {
+      m_streetTails.emplace(streetId, 0);
       m_turnCounts.emplace(streetId, std::array<unsigned long long, 4>{0, 0, 0, 0});
       // fill turn mapping as [streetId, [left street Id, straight street Id, right street Id, U self street Id]]
       m_turnMapping.emplace(streetId, std::array<long, 4>{-1, -1, -1, -1});
@@ -414,6 +426,12 @@
       if (m_agents[agentId]->delay() > 0) {
         continue;
       }
+      if (m_dataUpdatePeriod.has_value()) {
+        if (m_time % m_dataUpdatePeriod.value() == 0) {
+          //m_streetTails[streetId] += street->queue().size();
+          m_streetTails[streetId] += street->waitingAgents().size();
+        }
+      }
       m_agents[agentId]->setSpeed(0.);
       const auto& destinationNode{this->m_graph.nodeSet()[street->nodePair().second]};
       if (destinationNode->isFull()) {
@@ -722,8 +740,22 @@
   template <typename Id, typename Size, typename Delay>
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&
              is_numeric_v<Delay>)
-  void Dynamics<Id, Size, Delay>::optimizeTrafficLights(double percentage,
-                                                        double threshold) {
+  void Dynamics<Id, Size, Delay>::optimizeTrafficLights(Delay nCycles,
+                                                        double threshold,
+                                                        double densityTolerance) {
+    if (threshold < 0 || threshold > 1) {
+      throw std::invalid_argument(
+          buildLog(std::format("The threshold parameter is a percentage and must be "
+                               "bounded between 0-1. Inserted value: {}",
+                               threshold)));
+    }
+    if (densityTolerance < 0 || densityTolerance > 1) {
+      throw std::invalid_argument(buildLog(
+          std::format("The densityTolerance parameter is a percentage and must be "
+                      "bounded between 0-1. Inserted value: {}",
+                      densityTolerance)));
+    }
+    const auto meanDensityGlob = streetMeanDensity().mean;  // Measurement<double>
     for (const auto& [nodeId, node] : m_graph.nodeSet()) {
       if (!node->isTrafficLight()) {
         continue;
@@ -734,21 +766,20 @@
       }
       auto [greenTime, redTime] = tl.delay().value();
       const auto cycleTime = greenTime + redTime;
-      // const Delay delta = cycleTime * percentage;
       const auto& streetPriorities = tl.streetPriorities();
       Size greenSum{0}, greenQueue{0};
       Size redSum{0}, redQueue{0};
       for (const auto& [streetId, _] : m_graph.adjMatrix().getCol(nodeId, true)) {
         if (streetPriorities.contains(streetId)) {
-          greenSum += m_graph.streetSet()[streetId]->nAgents();
+          greenSum += m_streetTails[streetId];
           greenQueue += m_graph.streetSet()[streetId]->queue().size();
         } else {
-          redSum += m_graph.streetSet()[streetId]->nAgents();
+          redSum += m_streetTails[streetId];
           redQueue += m_graph.streetSet()[streetId]->queue().size();
         }
       }
       const Delay delta =
-          std::floor(std::abs(static_cast<int>(greenQueue - redQueue)) / percentage);
+          std::floor(std::fabs(static_cast<int>(greenQueue - redQueue)) / nCycles);
       if (delta == 0) {
         continue;
       }
@@ -757,19 +788,85 @@
         tl.setDelay(std::floor(cycleTime / 2));
         continue;
       }
-      if ((greenSum > redSum) && !(greenTime > redTime) && (greenQueue > redQueue)) {
-        if (redTime > delta) {
-          greenTime += delta;
-          redTime -= delta;
-          tl.setDelay(std::make_pair(greenTime, redTime));
+      // If the difference is not less than the threshold
+      //    - Check that the incoming streets have a density less than the mean one (eventually + tolerance): I want to avoid being into the cluster, better to be out or on the border
+      //    - If the previous check fails, do nothing
+      double meanDensity_streets{0.};
+      {
+        // Store the ids of outgoing streets
+        const auto& row{m_graph.adjMatrix().getRow(nodeId, true)};
+        for (const auto& [streetId, _] : row) {
+          meanDensity_streets += m_graph.streetSet()[streetId]->density();
+        }
+        // Take the mean density of the outgoing streets
+        const auto nStreets = row.size();
+        if (nStreets > 1) {
+          meanDensity_streets /= nStreets;
+        }
+      }
+      //std::cout << '\t' << " -> Mean network density: " << std::setprecision(7) << meanDensityGlob << '\n';
+      //std::cout << '\t' << " -> Mean density of 4 outgoing streets: " << std::setprecision(7) << meanDensity_streets << '\n';
+      const auto ratio = meanDensityGlob / meanDensity_streets;
+      // densityTolerance represents the max border we want to consider
+      const auto dyn_thresh = std::tanh(ratio) * densityTolerance;
+      //std::cout << '\t' << " -> Parametro ratio: " << std::setprecision(7) << ratio << '\n';
+      //std::cout << '\t' << " -> Parametro dyn_thresh: " << std::setprecision(7) << dyn_thresh << '\n';
+      if (meanDensityGlob * (1. + dyn_thresh) > meanDensity_streets) {
+        //std::cout << '\t' << " -> I'm on the cluster's border" << '\n';
+        if (meanDensityGlob > meanDensity_streets) {
+          //std::cout << '\t' << " -> LESS than max density" << '\n';
+          if (!(redTime > greenTime) && (redSum > greenSum) && (greenTime > delta)) {
+            greenTime -= delta;
+            redTime += delta;
+            tl.setDelay(std::make_pair(greenTime, redTime));
+          } else if (!(greenTime > redTime) && (greenSum > redSum) && (redTime > delta)) {
+            greenTime += delta;
+            redTime -= delta;
+            tl.setDelay(std::make_pair(greenTime, redTime));
+          } else {
+            //std::cout << '\t' << " -> NOT entered into previous ifs" << '\n';
+            tl.setDelay(std::make_pair(greenTime, redTime));
+          }
+          //std::cout << '\t' << " -> greenTime: " << static_cast<unsigned int>(greenTime) << '\n';
+          //std::cout << '\t' << " -> redTime: " << static_cast<unsigned int>(redTime) << '\n';
+          //std::cout << '\t' << " -> modTime: " << tl.modTime() << '\n';
+        } else {
+          //std::cout << '\t' << " -> GREATER than max density" << '\n';
+          if (!(redTime > greenTime) && (redSum > greenSum) &&
+              (greenTime > ratio * delta)) {
+            greenTime -= dyn_thresh * delta;  //
+            redTime += delta;
+            tl.setDelay(std::make_pair(greenTime, redTime));
+          } else if (!(greenTime > redTime) && (greenSum > redSum) &&
+                     (redTime > ratio * delta)) {
+            greenTime += delta;
+            redTime -= dyn_thresh * delta;  //
+            tl.setDelay(std::make_pair(greenTime, redTime));
+          } else if (!(redTime > greenTime) && (redSum < greenSum) && (redTime > delta)) {
+            greenTime += dyn_thresh * delta;  //
+            redTime -= delta;
+            tl.setDelay(std::make_pair(greenTime, redTime));
+          } else if (!(redTime < greenTime) && (redSum > greenSum) &&
+                     (greenTime > delta)) {
+            greenTime -= delta;
+            redTime += dyn_thresh * delta;  //
+            tl.setDelay(std::make_pair(greenTime, redTime));
+          } else {
+            //std::cout << '\t' << " -> NON sono entrato negli if precedenti" << '\n';
+            tl.setDelay(std::make_pair(greenTime, redTime));
+          }
+          //std::cout << '\t' << " -> greenTime: " << static_cast<unsigned int>(greenTime) << '\n';
+          //std::cout << '\t' << " -> redTime: " << static_cast<unsigned int>(redTime) << '\n';
+          //std::cout << '\t' << " -> modTime: " << tl.modTime() << '\n';
         }
-      } else if (!(redTime > greenTime) && (greenTime > delta) &&
-                 (redQueue > greenQueue)) {
-        greenTime -= delta;
-        redTime += delta;
-        tl.setDelay(std::make_pair(greenTime, redTime));
+      } else {
+        //std::cout << '\t' << " -> I'm INTO the cluster" << '\n';
+        //std::cout << '\t' << " -> modTime: " << tl.modTime() << '\n';
       }
     }
+    for (auto& [id, element] : m_streetTails) {
+      element = 0;
+    }
   }
 
   template <typename Id, typename Size, typename Delay>

test/Test_dynamics.cpp

@@ -426,6 +426,70 @@
       }
     }
   }
+  SUBCASE("Traffic Lights optimization algorithm") {
+    GIVEN("A dynamics object with a traffic light intersection") {
+      TrafficLight tl{1};
+      tl.setDelay(4);
+      tl.setPhase(3);
+      double length{90.}, max_speed{15.};
+      Street s_01{1, 10, length, max_speed, std::make_pair(0, 1)};
+      Street s_10{5, 10, length, max_speed, std::make_pair(1, 0)};
+      Street s_12{7, 10, length, max_speed, std::make_pair(1, 2)};
+      Street s_21{11, 10, length, max_speed, std::make_pair(2, 1)};
+      Street s_13{8, 10, length, max_speed, std::make_pair(1, 3)};
+      Street s_31{16, 10, length, max_speed, std::make_pair(3, 1)};
+      Street s_14{9, 10, length, max_speed, std::make_pair(1, 4)};
+      Street s_41{21, 10, length, max_speed, std::make_pair(4, 1)};
+      tl.addStreetPriority(1);
+      tl.addStreetPriority(11);
+      Graph graph2;
+      graph2.addNode(std::make_unique<TrafficLight>(tl));
+      graph2.addStreets(s_01, s_10, s_12, s_21, s_13, s_31, s_14, s_41);
+      graph2.buildAdj();
+      Dynamics dynamics{graph2};
+      Itinerary it_0{0, 0}, it_1{1, 2}, it_2{2, 3}, it_3{3, 4};
+      dynamics.addItinerary(it_0);
+      dynamics.addItinerary(it_1);
+      dynamics.addItinerary(it_2);
+      dynamics.addItinerary(it_3);
+      dynamics.updatePaths();
+      dynamics.addAgents(0, 7, 2);
+      dynamics.addAgents(1, 7, 0);
+      dynamics.setDataUpdatePeriod(1);
+      WHEN("We evolve the dynamics and optimize traffic lights") {
+        for (int i = 0; i < 8; ++i) {
+          dynamics.evolve(false);
+        }
+        dynamics.optimizeTrafficLights(2, 0.1, 0.);
+        THEN("Green and red time are different") {
+          const auto timing =
+              dynamic_cast<TrafficLight&>(*dynamics.graph().nodeSet().at(1))
+                  .delay()
+                  .value();
+          CHECK(timing.first > timing.second);
+        }
+      }
+      WHEN(
+          "We evolve the dynamics and optimize traffic lights with outgoing streets "
+          "full") {
+        dynamics.addAgents(0, 5, 1);
+        dynamics.addAgents(1, 5, 1);
+        dynamics.addAgents(2, 5, 1);
+        dynamics.addAgents(3, 5, 1);
+        for (int i = 0; i < 15; ++i) {
+          dynamics.evolve(false);
+        }
+        dynamics.optimizeTrafficLights(2, 0.1, 0.);
+        THEN("Green and red time are equal") {
+          const auto timing =
+              dynamic_cast<TrafficLight&>(*dynamics.graph().nodeSet().at(1))
+                  .delay()
+                  .value();
+          CHECK_EQ(timing.first, timing.second);
+        }
+      }
+    }
+  }
   SUBCASE("Roundabout") {
     GIVEN(
         "A dynamics object with four streets, one agent for each street, two itineraries "