Add utility to kill stagnant agents (#341)

* Implement `killStagnantAgents`

* Agent control in add function

* Bump version

Author: Grufoony

src/dsf/bindings.cpp

@@ -414,6 +414,11 @@ PYBIND11_MODULE(dsf_cpp, m) {
            &dsf::mobility::FirstOrderDynamics::setWeightFunction,
            pybind11::arg("weightFunction"),
            pybind11::arg("weightThreshold") = std::nullopt)
+      .def(
+          "killStagnantAgents",
+          &dsf::mobility::FirstOrderDynamics::killStagnantAgents,
+          pybind11::arg("timeToleranceFactor") = 3.,
+          dsf::g_docstrings.at("dsf::mobility::RoadDynamics::killStagnantAgents").c_str())
       .def(
           "setDestinationNodes",
           [](dsf::mobility::FirstOrderDynamics& self,

src/dsf/dsf.hpp

@@ -6,7 +6,7 @@
 
 static constexpr uint8_t DSF_VERSION_MAJOR = 4;
 static constexpr uint8_t DSF_VERSION_MINOR = 4;
-static constexpr uint8_t DSF_VERSION_PATCH = 1;
+static constexpr uint8_t DSF_VERSION_PATCH = 2;
 
 static auto const DSF_VERSION =
     std::format("{}.{}.{}", DSF_VERSION_MAJOR, DSF_VERSION_MINOR, DSF_VERSION_PATCH);

src/dsf/mobility/RoadDynamics.hpp

@@ -65,6 +65,7 @@ namespace dsf::mobility {
     double m_maxTravelDistance;
     std::time_t m_maxTravelTime;
     double m_weightTreshold;
+    std::optional<double> m_timeToleranceFactor;
     std::optional<delay_t> m_dataUpdatePeriod;
     bool m_bCacheEnabled;
     bool m_forcePriorities;
@@ -125,9 +126,16 @@ namespace dsf::mobility {
     /// @details The passage probability is the probability of passing through a node
     ///   It is useful in the case of random agents
     void setPassageProbability(double passageProbability);
-
+    /// @brief Set the time tolerance factor for killing stagnant agents.
+    ///   An agent will be considered stagnant if it has not moved for timeToleranceFactor * std::ceil(street_length / street_maxSpeed) time units.
+    /// @param timeToleranceFactor The time tolerance factor
+    /// @throw std::invalid_argument If the time tolerance factor is not positive
+    void killStagnantAgents(double timeToleranceFactor = 3.);
+    /// @brief Set the weight function
+    /// @param pathWeight The dsf::PathWeight function to use for the pathfinding
+    /// @param weightThreshold The weight threshold for updating the paths (default is std::nullopt)
     void setWeightFunction(PathWeight const pathWeight,
-                           std::optional<double> weigthThreshold = std::nullopt);
+                           std::optional<double> weightThreshold = std::nullopt);
     /// @brief Set the force priorities flag
     /// @param forcePriorities The flag
     /// @details If true, if an agent cannot move to the next street, the whole node is skipped
@@ -376,6 +384,7 @@ namespace dsf::mobility {
         m_passageProbability{std::nullopt},
         m_maxTravelDistance{std::numeric_limits<double>::max()},
         m_maxTravelTime{std::numeric_limits<std::time_t>::max()},
+        m_timeToleranceFactor{std::nullopt},
         m_bCacheEnabled{useCache},
         m_forcePriorities{false} {
     this->setWeightFunction(weightFunction, weightTreshold);
@@ -652,37 +661,31 @@ namespace dsf::mobility {
                         pAgentTemp->freeTime());
           continue;
         }
-        bool overtimed{false};
-        {
+
+        if (m_timeToleranceFactor.has_value()) {
           auto const timeDiff{this->time_step() - pAgentTemp->freeTime()};
-          // A minute of delay has never hurt anyone, right?
-          auto const timeTolerance{
-              std::max(static_cast<std::time_t>(60),
-                       static_cast<std::time_t>(
-                           3 * std::ceil(pStreet->length() / pStreet->maxSpeed())))};
+          auto const timeTolerance{m_timeToleranceFactor.value() *
+                                   std::ceil(pStreet->length() / pStreet->maxSpeed())};
           if (timeDiff > timeTolerance) {
-            overtimed = true;
             spdlog::warn(
-                "Time {} - {} currently on {} ({} turn - Traffic Light? {}), "
-                "has been still for more than {} seconds ({} seconds)",
+                "Time-step {} - {} currently on {} ({} turn - Traffic Light? {}), "
+                "has been still for more than {} seconds ({} seconds). Killing it.",
                 this->time_step(),
                 *pAgentTemp,
                 *pStreet,
                 directionToString.at(pStreet->laneMapping().at(queueIndex)),
                 this->graph().node(pStreet->target())->isTrafficLight(),
                 timeTolerance,
                 timeDiff);
+            // Kill the agent
+            auto pAgent{pStreet->dequeue(queueIndex)};
+            continue;
           }
         }
         pAgentTemp->setSpeed(0.);
         const auto& destinationNode{this->graph().node(pStreet->target())};
         if (destinationNode->isFull()) {
-          if (overtimed) {
-            spdlog::warn("Skipping due to full destination node {}", *destinationNode);
-          } else {
-            spdlog::debug("Skipping due to space at destination node {}",
-                          *destinationNode);
-          }
+          spdlog::debug("Skipping due to full destination node {}", *destinationNode);
           continue;
         }
         if (destinationNode->isTrafficLight()) {
@@ -792,17 +795,9 @@ namespace dsf::mobility {
           }
           if (!bCanPass) {
             spdlog::debug(
-                "Skipping agent emission from street {} -> {} due to right of way.",
+                "Skipping agent emission from street {} -> {} due to right of way",
                 pStreet->source(),
                 pStreet->target());
-            if (overtimed) {
-              spdlog::warn(
-                  "Skipping agent emission from street {} -> {} due to right of way "
-                  "and overtimed agent {}",
-                  pStreet->source(),
-                  pStreet->target(),
-                  pAgentTemp->id());
-            }
             continue;
           }
         }
@@ -817,14 +812,6 @@ namespace dsf::mobility {
                 "probability",
                 pStreet->source(),
                 pStreet->target());
-            if (overtimed) {
-              spdlog::warn(
-                  "Skipping agent emission from street {} -> {} due to passage "
-                  "probability and overtimed agent {}",
-                  pStreet->source(),
-                  pStreet->target(),
-                  pAgentTemp->id());
-            }
             continue;
           }
         }
@@ -865,21 +852,12 @@ namespace dsf::mobility {
         }
         auto const& nextStreet{this->graph().edge(pAgentTemp->nextStreetId().value())};
         if (nextStreet->isFull()) {
-          if (overtimed) {
-            spdlog::warn(
-                "Skipping agent emission from street {} -> {} due to full "
-                "next street: {}",
-                pStreet->source(),
-                pStreet->target(),
-                *nextStreet);
-          } else {
-            spdlog::debug(
-                "Skipping agent emission from street {} -> {} due to full "
-                "next street: {}",
-                pStreet->source(),
-                pStreet->target(),
-                *nextStreet);
-          }
+          spdlog::debug(
+              "Skipping agent emission from street {} -> {} due to full "
+              "next street: {}",
+              pStreet->source(),
+              pStreet->target(),
+              *nextStreet);
           continue;
         }
         auto pAgent{pStreet->dequeue(queueIndex)};
@@ -1042,6 +1020,15 @@ namespace dsf::mobility {
     }
     m_passageProbability = passageProbability;
   }
+  template <typename delay_t>
+    requires(is_numeric_v<delay_t>)
+  void RoadDynamics<delay_t>::killStagnantAgents(double timeToleranceFactor) {
+    if (timeToleranceFactor <= 0.) {
+      throw std::invalid_argument(std::format(
+          "The time tolerance factor ({}) must be positive", timeToleranceFactor));
+    }
+    m_timeToleranceFactor = timeToleranceFactor;
+  }
   template <typename delay_t>
     requires(is_numeric_v<delay_t>)
   void RoadDynamics<delay_t>::setWeightFunction(PathWeight const pathWeight,
@@ -1200,6 +1187,15 @@ namespace dsf::mobility {
     requires(is_numeric_v<delay_t>)
   void RoadDynamics<delay_t>::addAgentsUniformly(Size nAgents,
                                                  std::optional<Id> optItineraryId) {
+    if (m_timeToleranceFactor.has_value() && !m_agents.empty()) {
+      auto const nStagnantAgents{m_agents.size()};
+      spdlog::warn(
+          "Removing {} stagnant agents that were not inserted since the previous call to "
+          "addAgentsUniformly().",
+          nStagnantAgents);
+      m_agents.clear();
+      m_nAgents -= nStagnantAgents;
+    }
     if (optItineraryId.has_value() && !this->itineraries().contains(*optItineraryId)) {
       throw std::invalid_argument(
           std::format("No itineraries available. Cannot add agents with itinerary id {}",
@@ -1256,6 +1252,15 @@ namespace dsf::mobility {
   void RoadDynamics<delay_t>::addAgentsRandomly(Size nAgents,
                                                 const TContainer& src_weights,
                                                 const TContainer& dst_weights) {
+    if (m_timeToleranceFactor.has_value() && !m_agents.empty()) {
+      auto const nStagnantAgents{m_agents.size()};
+      spdlog::warn(
+          "Removing {} stagnant agents that were not inserted since the previous call to "
+          "addAgentsRandomly().",
+          nStagnantAgents);
+      m_agents.clear();
+      m_nAgents -= nStagnantAgents;
+    }
     auto const& nSources{src_weights.size()};
     auto const& nDestinations{dst_weights.size()};
     spdlog::debug("Init addAgentsRandomly for {} agents from {} nodes to {} nodes.",