Auditing memory utilization

rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp

@@ -88,6 +88,7 @@ class MirrorManager::Implementation
   rclcpp::TimerBase::SharedPtr update_timer;
   rclcpp::TimerBase::SharedPtr redo_query_registration_timer;
   rclcpp::TimerBase::SharedPtr reconnect_services_timer;
+  rclcpp::TimerBase::SharedPtr memory_utilization_timer;
   RegisterQueryClient register_query_client;
 
   std::shared_ptr<rmf_traffic::schedule::Mirror> mirror;
@@ -122,6 +123,19 @@ class MirrorManager::Implementation
       {
         handle_startup_event(*msg);
       });
+
+    memory_utilization_timer = node->create_wall_timer(
+      std::chrono::minutes(5), [this, logger = node->get_logger()]()
+      {
+        if (this->mirror)
+        {
+          RCLCPP_INFO(
+            logger,
+            "Memory footprint: stored waypoints %zu - timeline entries %zu",
+            this->mirror->waypoints_in_storage(),
+            this->mirror->entries_in_timeline());
+        }
+      });
   }
 
   bool reconnect_schedule(

rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Negotiation.cpp

@@ -34,6 +34,8 @@
 
 #include <rclcpp/logging.hpp>
 
+#include <algorithm>
+
 namespace rmf_traffic_ros2 {
 namespace schedule {
 
@@ -215,6 +217,8 @@ class Negotiation::Implementation
   std::shared_ptr<Worker> worker;
   rmf_traffic::Duration timeout = std::chrono::seconds(15);
 
+  rclcpp::TimerBase::SharedPtr memory_utilization_timer;
+
   using Repeat = rmf_traffic_msgs::msg::NegotiationRepeat;
   using RepeatSub = rclcpp::Subscription<Repeat>;
   using RepeatPub = rclcpp::Publisher<Repeat>;
@@ -319,6 +323,48 @@ class Negotiation::Implementation
     // TODO(MXG): Make the QoS configurable
     const auto qos = rclcpp::ServicesQoS().reliable().keep_last(1000);
 
+    memory_utilization_timer = node.create_wall_timer(
+      rmf_traffic::time::from_seconds(600.0),
+      [this]()
+      {
+        const std::size_t num_negotiations = this->negotiations.size();
+        std::vector<std::size_t> counts;
+        std::size_t i = 0;
+        for (const auto& [_, negotiation] : this->negotiations)
+        {
+          ++i;
+          if (i > 100)
+          {
+            // Don't keep counting too long or this will block other
+            // responsibilities of the program.
+            break;
+          }
+
+          const std::size_t count = negotiation.room.negotiation.count_tables();
+          counts.push_back(count);
+        }
+
+        std::sort(counts.begin(), counts.end());
+        std::stringstream ss;
+        i = 0;
+        for (auto r_it = counts.rbegin(); r_it != counts.rend(); ++r_it)
+        {
+          if (i > 10)
+          {
+            // Don't make the message too large, just give the top 10 counts
+            break;
+          }
+
+          ss << " [" << *r_it << "]";
+        }
+
+        RCLCPP_INFO(
+          this->node.get_logger(),
+          "%zu active negotiations, largest sizes:%s",
+          num_negotiations,
+          ss.str().c_str());
+      });
+
     repeat_sub = node.create_subscription<Repeat>(
       NegotiationRepeatTopicName, qos,
       [&](const Repeat::UniquePtr msg)

rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Node.cpp

@@ -626,6 +626,16 @@ void ScheduleNode::setup_cull_timer()
 {
   cull_timer = create_wall_timer(
     std::chrono::minutes(1), [this]() { cull(); });
+
+  memory_utilization_timer = create_wall_timer(
+    std::chrono::minutes(1), [this]()
+    {
+      RCLCPP_INFO(
+        this->get_logger(),
+        "Memory footprint: stored waypoints %zu - timeline entries %zu",
+        this->database->waypoints_in_storage(),
+        this->database->entries_in_timeline());
+    });
 }
 
 //==============================================================================

rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/internal_Node.hpp

@@ -544,6 +544,8 @@ class ScheduleNode : public rclcpp::Node
   // description versions separately from itinerary versions.
   std::size_t last_known_participants_version = 0;
   std::size_t current_participants_version = 1;
+
+  rclcpp::TimerBase::SharedPtr memory_utilization_timer;
 };
 
 } // namespace schedule