Add latency over time computation

irobot_benchmark/src/irobot_benchmark.cpp

@@ -161,6 +161,7 @@ int main(int argc, char ** argv)
   // Create ROS 2 system manager
   auto ros2_system = create_ros2_system(options, events_output_path);
   ros2_system->add_nodes(nodes_vec);
+  ros2_system->set_latency_callback(ru_logger);
 
   // now the system is complete and we can make it spin for the requested duration
   bool wait_for_discovery = true;

performance_metrics/include/performance_metrics/resource_usage_logger.hpp

@@ -16,6 +16,7 @@
 #include <atomic>
 #include <chrono>
 #include <fstream>
+#include <functional>
 #include <iostream>
 #include <string>
 #include <thread>
@@ -35,6 +36,7 @@ class ResourceUsageLogger
     uint64_t mem_mmap_KB = 0;
     uint64_t mem_max_rss_KB = 0;
     uint64_t mem_virtual_KB = 0;
+    uint64_t latency_us = 0;
   };
 
   ResourceUsageLogger() = delete;
@@ -45,6 +47,8 @@ class ResourceUsageLogger
 
   void start(std::chrono::milliseconds period = std::chrono::milliseconds(1000));
 
+  void set_get_latency_callback(std::function<uint64_t()> get_latency_fn);
+
   void stop();
 
   void print_resource_usage();
@@ -75,6 +79,7 @@ class ResourceUsageLogger
   std::chrono::time_point<std::chrono::steady_clock> m_t1_real_start;
   pid_t m_pid;
   int m_pagesize;
+  std::function<uint64_t()> m_get_average_latency;
 
   // the following values are used for comparing different plots using the python scripts
   bool m_has_system_info {false};

performance_metrics/include/performance_metrics/stat_logger.hpp

@@ -39,6 +39,9 @@ void log_trackers_latency_total_stats(
   const std::vector<Tracker> & trackers,
   const bool csv_out = false);
 
+uint64_t get_trackers_avg_latency(
+  const std::vector<const Tracker *> & trackers);
+
 template<typename T>
 void stream_out(
   const bool csv_out,

performance_metrics/include/performance_metrics/tracker.hpp

@@ -76,6 +76,14 @@ class Tracker
 
   Stat<uint64_t> stat() const {return m_stat;}
 
+  uint64_t delta_received() const {return m_delta_received_messages;}
+
+  void reset_delta_received() const {m_delta_received_messages = 0;}
+
+  Stat<uint64_t> delta_stat() const {return m_delta_stat;}
+
+  void reset_delta_stat() const {m_delta_stat = Stat<uint64_t>();}
+
   double throughput() const;
 
   void set_frequency(float f) {m_frequency = f;}
@@ -99,6 +107,8 @@ class Tracker
   std::string m_topic_srv_name;
   Options m_tracking_options;
 
+  mutable Stat<uint64_t> m_delta_stat;
+  mutable uint64_t m_delta_received_messages = 0;
   uint64_t m_last_latency = 0;
   uint64_t m_lost_messages = 0;
   uint64_t m_received_messages = 0;

performance_metrics/src/resource_usage_logger.cpp

@@ -75,6 +75,12 @@ void ResourceUsageLogger::start(std::chrono::milliseconds period)
     });
 }
 
+void ResourceUsageLogger::set_get_latency_callback(
+  std::function<uint64_t()> get_average_latency)
+{
+  m_get_average_latency = get_average_latency;
+}
+
 void ResourceUsageLogger::stop()
 {
   bool is_logging = m_is_logging.exchange(false);
@@ -155,6 +161,11 @@ void ResourceUsageLogger::_get()
   } else {
     m_resources.mem_virtual_KB = -1;
   }
+
+  // Call the latency callback to get the average latency in microseconds
+  if (m_get_average_latency) {
+    m_resources.latency_us = m_get_average_latency();
+  }
 }
 
 template<typename T>
@@ -176,6 +187,7 @@ void ResourceUsageLogger::_print_header(std::ostream & stream)
 {
   _stream_out(stream, "time_ms");
   _stream_out(stream, "cpu_perc", m_narrow_space);
+  _stream_out(stream, "latency_us", m_wide_space);
   _stream_out(stream, "arena_KB");
   _stream_out(stream, "in_use_KB");
   _stream_out(stream, "mmap_KB");
@@ -197,6 +209,7 @@ void ResourceUsageLogger::_print(std::ostream & stream)
 
   _stream_out(stream, std::round(m_resources.elasped_ms), m_wide_space, prec);
   _stream_out(stream, m_resources.cpu_usage, m_narrow_space);
+  _stream_out(stream, m_resources.latency_us, m_wide_space);
   _stream_out(stream, m_resources.mem_arena_KB);
   _stream_out(stream, m_resources.mem_in_use_KB);
   _stream_out(stream, m_resources.mem_mmap_KB);

performance_metrics/src/stat_logger.cpp

@@ -166,4 +166,24 @@ void log_trackers_latency_total_stats(
     average_latency, stream, csv_out);
 }
 
+uint64_t get_trackers_avg_latency(const std::vector<const Tracker *> & trackers)
+{
+  uint64_t total_received = 0;
+  double total_latency = 0;
+
+  for (const auto & tracker : trackers) {
+    total_received += tracker->delta_received();
+    total_latency += tracker->delta_received() * tracker->delta_stat().mean();
+    // Reset values after read
+    tracker->reset_delta_received();
+    tracker->reset_delta_stat();
+  }
+
+  if (total_received) {
+    return std::round(total_latency / total_received);
+  } else {
+    return 0;
+  }
+}
+
 }  // namespace performance_metrics

performance_metrics/src/tracker.cpp

@@ -116,6 +116,7 @@ void Tracker::scan(
   this->add_sample(now, lat_us, header.size, header.frequency);
 
   m_received_messages++;
+  m_delta_received_messages++;
 }
 
 void Tracker::add_sample(
@@ -133,6 +134,7 @@ void Tracker::add_sample(
 
   m_last_msg_time = now;
   m_stat.add_sample(latency_sample);
+  m_delta_stat.add_sample(latency_sample);
 }
 
 uint32_t Tracker::get_and_update_tracking_number()

performance_test/include/performance_test/performance_node_base.hpp

@@ -124,6 +124,7 @@ class PerformanceNodeBase
 
   void add_timer(std::chrono::microseconds period, std::function<void()> callback);
 
+  std::vector<const performance_metrics::Tracker*> sub_trackers_ptr();
   std::vector<performance_metrics::Tracker> sub_trackers();
   std::vector<performance_metrics::Tracker> service_trackers();
   std::vector<performance_metrics::Tracker> client_trackers();

performance_test/include/performance_test/system.hpp

@@ -18,6 +18,7 @@
 #include <vector>
 
 #include "performance_metrics/events_logger.hpp"
+#include "performance_metrics/resource_usage_logger.hpp"
 #include "performance_test/performance_node_base.hpp"
 #include "performance_test/executors.hpp"
 
@@ -69,6 +70,9 @@ class System
     const std::vector<std::string> & topology_json_list,
     const std::string & results_folder_path) const;
 
+  void set_latency_callback(
+    performance_metrics::ResourceUsageLogger & ru_logger);
+
 private:
   void wait_discovery();
 
@@ -86,6 +90,7 @@ class System
 
   std::chrono::seconds m_experiment_duration;
 
+  std::vector<const performance_metrics::Tracker *> m_all_subscription_trackers;
   std::vector<std::shared_ptr<performance_test::PerformanceNodeBase>> m_nodes;
   std::map<int, NamedExecutor> m_executors_map;
   std::vector<std::unique_ptr<std::thread>> m_threads;

performance_test/src/performance_node_base.cpp

@@ -92,6 +92,17 @@ PerformanceNodeBase::sub_trackers()
   return trackers;
 }
 
+std::vector<const performance_metrics::Tracker*>
+PerformanceNodeBase::sub_trackers_ptr()
+{
+  std::vector<const performance_metrics::Tracker*> trackers;
+  for (const auto& sub : m_subs) {
+    trackers.push_back(&(sub.second.second));
+  }
+
+  return trackers;
+}
+
 std::vector<performance_metrics::Tracker> PerformanceNodeBase::client_trackers()
 {
   std::vector<performance_metrics::Tracker> trackers;

performance_test/src/system.cpp

@@ -17,6 +17,7 @@
 #include <utility>
 #include <vector>
 
+#include "performance_metrics/resource_usage_logger.hpp"
 #include "performance_metrics/events_logger.hpp"
 #include "performance_metrics/stat_logger.hpp"
 #include "performance_test/system.hpp"
@@ -323,6 +324,20 @@ void System::log_latency_total_stats(
   performance_metrics::log_trackers_latency_total_stats(stream, all_trackers, m_csv_out);
 }
 
+void System::set_latency_callback(
+  performance_metrics::ResourceUsageLogger & ru_logger)
+{
+  // Get all subscription trackers for later use
+  for (const auto& node : m_nodes) {
+    auto sub_trackers = node->sub_trackers_ptr();
+    m_all_subscription_trackers.insert(
+      m_all_subscription_trackers.end(), sub_trackers.begin(), sub_trackers.end());
+  }
+
+  ru_logger.set_get_latency_callback(
+    [this]() { return performance_metrics::get_trackers_avg_latency(m_all_subscription_trackers); });
+}
+
 void System::print_aggregate_stats(
   const std::vector<std::string> & topology_json_list,
   const std::string & results_folder_path) const