add support for loaned messages and compute throughput stat

Author: alsora

irobot_interfaces_plugin/msg/StampedInt64.msg

@@ -1,2 +1,2 @@
 performance_test_msgs/PerformanceHeader header
-int64 data
+int64[1] data

performance_metrics/include/performance_metrics/tracker.hpp

@@ -46,62 +46,71 @@ class Tracker
     const std::string & node_name,
     const std::string & topic_srv_name,
     const Options & tracking_options)
-  : _node_name(node_name), _topic_srv_name(topic_srv_name), _tracking_options(tracking_options)
+  : m_node_name(node_name), m_topic_srv_name(topic_srv_name), m_tracking_options(tracking_options)
   {}
 
   void scan(
     const performance_test_msgs::msg::PerformanceHeader & header,
     const rclcpp::Time & now,
     std::shared_ptr<EventsLogger> elog = nullptr);
 
-  void add_sample(uint64_t latency_sample);
+  void add_sample(
+    const rclcpp::Time & now,
+    uint64_t latency_sample,
+    size_t size,
+    float frequency);
 
   uint32_t get_and_update_tracking_number();
 
-  uint64_t lost() const {return _lost_messages;}
+  uint64_t lost() const {return m_lost_messages;}
 
-  uint64_t late() const {return _late_messages;}
+  uint64_t late() const {return m_late_messages;}
 
-  uint64_t too_late() const {return _too_late_messages;}
+  uint64_t too_late() const {return m_too_late_messages;}
 
-  uint64_t received() const {return _received_messages;}
+  uint64_t received() const {return m_received_messages;}
 
-  size_t size() const {return _size;}
+  size_t size() const {return m_data_size;}
 
   float frequency() const {return m_frequency;}
 
-  Stat<uint64_t> stat() const {return _stat;}
+  Stat<uint64_t> stat() const {return m_stat;}
+
+  double throughput() const;
 
   void set_frequency(float f) {m_frequency = f;}
 
-  void set_size(size_t s) {_size = s;}
+  void set_size(size_t s) {m_data_size = s;}
 
-  uint64_t last() const {return _last_latency;}
+  uint64_t last() const {return m_last_latency;}
 
   std::string get_node_name() const
   {
-    return _node_name;
+    return m_node_name;
   }
 
   std::string get_entity_name() const
   {
-    return _topic_srv_name;
+    return m_topic_srv_name;
   }
 
 private:
-  std::string _node_name;
-  std::string _topic_srv_name;
-
-  uint64_t _last_latency = 0;
-  uint64_t _lost_messages = 0;
-  uint64_t _received_messages = 0;
-  uint64_t _late_messages = 0;
-  uint64_t _too_late_messages = 0;
-  size_t _size = 0;
+  std::string m_node_name;
+  std::string m_topic_srv_name;
+  Options m_tracking_options;
+
+  uint64_t m_last_latency = 0;
+  uint64_t m_lost_messages = 0;
+  uint64_t m_received_messages = 0;
+  uint64_t m_late_messages = 0;
+  uint64_t m_too_late_messages = 0;
+  size_t m_data_size = 0;
   float m_frequency = 0;
-  Stat<uint64_t> _stat;
-  uint32_t _tracking_number_count = 0;
-  Options _tracking_options;
+  Stat<uint64_t> m_stat;
+  uint32_t m_tracking_number_count = 0;
+
+  rclcpp::Time m_first_msg_time;
+  rclcpp::Time m_last_msg_time;
 };
 
 }  // namespace performance_metrics

performance_metrics/src/stat_logger.cpp

@@ -88,6 +88,8 @@ void log_latency_all_stats(
       stream << std::left << std::setw(narrow_space) << std::setfill(separator) << "min[us]";
       stream << std::left << std::setw(narrow_space) << std::setfill(separator) << "max[us]";
       stream << std::left << std::setw(narrow_space) << std::setfill(separator) << "freq[hz]";
+      stream << std::left << std::setw(wide_space) << std::setfill(separator) <<
+        "throughput[Mb/s]";
 
       stream << std::endl;
     };
@@ -119,6 +121,8 @@ void log_latency_all_stats(
         std::round(tracker.stat().max());
       stream << std::left << std::setw(narrow_space) << std::setfill(separator) <<
         tracker.frequency();
+      stream << std::left << std::setw(wide_space) << std::setfill(separator) <<
+        (tracker.throughput() / (1024 * 1024));
 
       stream << std::endl;
     };

performance_metrics/src/tracker.cpp

@@ -21,38 +21,32 @@ void Tracker::scan(
   const rclcpp::Time & now,
   std::shared_ptr<EventsLogger> elog)
 {
-  // If this is first message received store some info about it
-  if (stat().n() == 0) {
-    this->set_size(header.size);
-    this->set_frequency(header.frequency);
-  }
-
   // Compute latency
   rclcpp::Time stamp(header.stamp.sec, header.stamp.nanosec, RCL_ROS_TIME);
   auto lat = std::chrono::nanoseconds((now - stamp).nanoseconds());
   uint64_t lat_us = lat.count() / 1000;
   // store the last latency to be read from node
-  _last_latency = lat_us;
+  m_last_latency = lat_us;
 
   bool late = false;
   bool too_late = false;
 
-  if (_tracking_options.is_enabled) {
+  if (m_tracking_options.is_enabled) {
     // Check if we received the correct message. The assumption here is
     // that the messages arrive in chronological order
-    if (header.tracking_number == _tracking_number_count) {
-      _tracking_number_count++;
+    if (header.tracking_number == m_tracking_number_count) {
+      m_tracking_number_count++;
     } else {
       // We missed some mesages...
-      int64_t n_lost = header.tracking_number - _tracking_number_count;
-      _lost_messages += n_lost;
-      _tracking_number_count = header.tracking_number + 1;
+      int64_t n_lost = header.tracking_number - m_tracking_number_count;
+      m_lost_messages += n_lost;
+      m_tracking_number_count = header.tracking_number + 1;
 
       // Log the event
       if (elog != nullptr) {
         EventsLogger::Event ev;
         std::stringstream description;
-        ev.caller_name = _topic_srv_name + "->" + _node_name;
+        ev.caller_name = m_topic_srv_name + "->" + m_node_name;
         ev.code = EventsLogger::EventCode::lost_messages;
 
         if (n_lost == 1) {
@@ -67,13 +61,13 @@ void Tracker::scan(
     }
 
     // Check if the message latency qualifies the message as a lost or late message.
-    const int period_us = 1000000 / m_frequency;
+    const int period_us = 1000000 / header.frequency;
     const unsigned int latency_late_threshold_us = std::min(
-      _tracking_options.late_absolute_us,
-      _tracking_options.late_percentage * period_us / 100);
+      m_tracking_options.late_absolute_us,
+      m_tracking_options.late_percentage * period_us / 100);
     const unsigned int latency_too_late_threshold_us = std::min(
-      _tracking_options.too_late_absolute_us,
-      _tracking_options.too_late_percentage * period_us / 100);
+      m_tracking_options.too_late_absolute_us,
+      m_tracking_options.too_late_percentage * period_us / 100);
 
     too_late = lat_us > latency_too_late_threshold_us;
     late = lat_us > latency_late_threshold_us && !too_late;
@@ -86,13 +80,13 @@ void Tracker::scan(
           lat_us << "us > " << latency_late_threshold_us << "us";
 
         EventsLogger::Event ev;
-        ev.caller_name = _topic_srv_name + "->" + _node_name;
+        ev.caller_name = m_topic_srv_name + "->" + m_node_name;
         ev.code = EventsLogger::EventCode::late_message;
         ev.description = description.str();
 
         elog->write_event(ev);
       }
-      _late_messages++;
+      m_late_messages++;
     }
 
     if (too_late) {
@@ -103,35 +97,63 @@ void Tracker::scan(
           lat_us << "us > " << latency_too_late_threshold_us << "us";
 
         EventsLogger::Event ev;
-        ev.caller_name = _topic_srv_name + "->" + _node_name;
+        ev.caller_name = m_topic_srv_name + "->" + m_node_name;
         ev.code = EventsLogger::EventCode::too_late_message;
         ev.description = description.str();
 
         elog->write_event(ev);
       }
-      _too_late_messages++;
+      m_too_late_messages++;
     }
   }
 
   if (!too_late) {
     // Compute statistics with new sample. Don't add to this the msgs
     // that arrived too late.
-    this->add_sample(lat_us);
+    this->add_sample(now, lat_us, header.size, header.frequency);
   }
 
-  _received_messages++;
+  m_received_messages++;
 }
 
-void Tracker::add_sample(uint64_t latency_sample)
+void Tracker::add_sample(
+  const rclcpp::Time & now,
+  uint64_t latency_sample,
+  size_t size,
+  float frequency)
 {
-  _stat.add_sample(latency_sample);
+  // If this is first message received store some info about it
+  if (m_stat.n() == 0) {
+    m_data_size = size;
+    m_frequency = frequency;
+    m_first_msg_time = now;
+  }
+
+  m_last_msg_time = now;
+  m_stat.add_sample(latency_sample);
 }
 
 uint32_t Tracker::get_and_update_tracking_number()
 {
-  uint32_t old_number = _tracking_number_count;
-  _tracking_number_count++;
+  uint32_t old_number = m_tracking_number_count;
+  m_tracking_number_count++;
   return old_number;
 }
 
+double Tracker::throughput() const
+{
+  // We compute max because currently publishers update only n() and not m_received_messages,
+  // but for subscriptions n() will not include messages received too late.
+  uint64_t num_msg_received = std::max(m_stat.n(), m_received_messages);
+  if (num_msg_received < 2) {
+    return 0.0;
+  }
+
+  rclcpp::Duration msgs_received_interval = m_last_msg_time - m_first_msg_time;
+  double sample_rate_sec = (num_msg_received - 1) / msgs_received_interval.seconds();
+  double throughput = sample_rate_sec * m_data_size;
+
+  return throughput;
+}
+
 }  // namespace performance_metrics

performance_test/include/performance_test/communication.hpp

@@ -13,7 +13,8 @@
 enum msg_pass_by_t
 {
   PASS_BY_UNIQUE_PTR,
-  PASS_BY_SHARED_PTR
+  PASS_BY_SHARED_PTR,
+  PASS_BY_LOANED_MSG,
 };
 
 #endif  // PERFORMANCE_TEST__COMMUNICATION_HPP_

performance_test/include/performance_test/performance_node_base_impl.hpp

@@ -45,6 +45,7 @@ void PerformanceNodeBase::add_subscriber(
 {
   switch (msg_pass_by) {
     case PASS_BY_SHARED_PTR:
+    case PASS_BY_LOANED_MSG:
       add_subscriber_by_msg_variant<Msg, typename Msg::ConstSharedPtr>(
         topic_name,
         tracking_options,
@@ -205,15 +206,16 @@ void PerformanceNodeBase::publish_msg(
   float pub_frequency = 1000000.0 / period.count();
 
   auto tracking_number = tracker.get_and_update_tracking_number();
-  uint64_t pub_time_us = 0;
+  rclcpp::Time publish_time;
+  uint64_t pub_duration_us = 0;
   size_t msg_size = 0;
   switch (msg_pass_by) {
     case PASS_BY_SHARED_PTR:
       {
         // create a message and eventually resize it
         auto msg = std::make_shared<Msg>();
         msg_size = resize_msg(msg->data, size);
-        auto publish_time = m_node_interfaces.clock->get_clock()->now();
+        publish_time = m_node_interfaces.clock->get_clock()->now();
 
         msg->header = create_msg_header(
           publish_time,
@@ -224,7 +226,7 @@ void PerformanceNodeBase::publish_msg(
         pub->publish(*msg);
 
         auto end_time = m_node_interfaces.clock->get_clock()->now();
-        pub_time_us = (end_time - publish_time).nanoseconds() / 1000.0f;
+        pub_duration_us = (end_time - publish_time).nanoseconds() / 1000.0f;
 
         break;
       }
@@ -233,7 +235,7 @@ void PerformanceNodeBase::publish_msg(
         // create a message and eventually resize it
         auto msg = std::make_unique<Msg>();
         msg_size = resize_msg(msg->data, size);
-        auto publish_time = m_node_interfaces.clock->get_clock()->now();
+        publish_time = m_node_interfaces.clock->get_clock()->now();
 
         msg->header = create_msg_header(
           publish_time,
@@ -244,19 +246,43 @@ void PerformanceNodeBase::publish_msg(
         pub->publish(std::move(msg));
 
         auto end_time = m_node_interfaces.clock->get_clock()->now();
-        pub_time_us = (end_time - publish_time).nanoseconds() / 1000.0f;
+        pub_duration_us = (end_time - publish_time).nanoseconds() / 1000.0f;
+
+        break;
+      }
+    case PASS_BY_LOANED_MSG:
+      {
+        // create a message and eventually resize it
+        std::allocator<void> allocator;
+        rclcpp::LoanedMessage<Msg> loaned_msg(*pub, allocator);
+        auto & msg_ref = loaned_msg.get();
+        msg_size = resize_msg(msg_ref.data, size);
+        // Fill the loaned msg with 1s.
+        // This simulates a real application: users are expected to populate the loaned msg with new data every time.
+        std::fill(std::begin(msg_ref.data), std::end(msg_ref.data), 1);
+
+        publish_time = m_node_interfaces.clock->get_clock()->now();
+
+        msg_ref.header = create_msg_header(
+          publish_time,
+          pub_frequency,
+          tracking_number,
+          msg_size);
+
+        pub->publish(std::move(loaned_msg));
+
+        auto end_time = m_node_interfaces.clock->get_clock()->now();
+        pub_duration_us = (end_time - publish_time).nanoseconds() / 1000.0f;
 
         break;
       }
   }
 
-  tracker.set_frequency(pub_frequency);
-  tracker.set_size(msg_size);
-  tracker.add_sample(pub_time_us);
+  tracker.add_sample(publish_time, pub_duration_us, msg_size, pub_frequency);
 
   RCLCPP_DEBUG(
     this->get_node_logger(),
-    "Publishing to %s msg number %d took %lu us", name.c_str(), tracking_number, pub_time_us);
+    "Publishing to %s msg number %d took %lu us", name.c_str(), tracking_number, pub_duration_us);
 }
 
 template<typename DataT>

performance_test_msgs/msg/Sample.msg

@@ -1,2 +1,2 @@
 PerformanceHeader header
-byte data
+byte[1] data