From 14bdc226541dfab1a3ec1284a7c59ea24034863f Mon Sep 17 00:00:00 2001
From: Wolfgang Hoenig <hoenig@tu-berlin.de>
Date: Thu, 1 Feb 2024 16:03:45 +0100
Subject: [PATCH] server cpp: add new communication warnings

* unicast_receive_rate measures the end-to-end rate of received unicast packets over sent unicast packets
* broadcast_receive_rate measures the end-to-end rate of received broadcast packets over sent unicast packets

Warnings if this is larger than 1 or smaller than a user-defined threshold.
---
 crazyflie/config/crazyflies.yaml   |  2 ++
 crazyflie/config/server.yaml       |  2 ++
 crazyflie/src/crazyflie_server.cpp | 27 +++++++++++++++++++++++++++
 3 files changed, 31 insertions(+)

diff --git a/crazyflie/config/crazyflies.yaml b/crazyflie/config/crazyflies.yaml
index e136fcd8b..adefcbfd1 100644
--- a/crazyflie/config/crazyflies.yaml
+++ b/crazyflie/config/crazyflies.yaml
@@ -77,6 +77,8 @@ all:
      # remove to disable default topic
       pose:
         frequency: 10 # Hz
+      status:
+        frequency: 1 # Hz
     #custom_topics:
     #  topic_name1:
     #    frequency: 10 # Hz
diff --git a/crazyflie/config/server.yaml b/crazyflie/config/server.yaml
index 6ec8a3989..59a3ebcb5 100644
--- a/crazyflie/config/server.yaml
+++ b/crazyflie/config/server.yaml
@@ -7,6 +7,8 @@
       communication:
         max_unicast_latency: 10.0 # ms
         min_unicast_ack_rate: 0.9
+        min_unicast_receive_rate: 0.9 # requires status topic to be enabled
+        min_broadcast_receive_rate: 0.9 # requires status topic to be enabled
         publish_stats: false
     firmware_params:
       query_all_values_on_connect: False
diff --git a/crazyflie/src/crazyflie_server.cpp b/crazyflie/src/crazyflie_server.cpp
index 4a71e0245..1dfe1de98 100644
--- a/crazyflie/src/crazyflie_server.cpp
+++ b/crazyflie/src/crazyflie_server.cpp
@@ -181,6 +181,8 @@ class CrazyflieROS
     warning_freq_ = node->get_parameter("warnings.frequency").get_parameter_value().get<float>();
     max_latency_ = node->get_parameter("warnings.communication.max_unicast_latency").get_parameter_value().get<float>();
     min_ack_rate_ = node->get_parameter("warnings.communication.min_unicast_ack_rate").get_parameter_value().get<float>();
+    min_unicast_receive_rate_ = node->get_parameter("warnings.communication.min_unicast_receive_rate").get_parameter_value().get<float>();
+    min_broadcast_receive_rate_ = node->get_parameter("warnings.communication.min_broadcast_receive_rate").get_parameter_value().get<float>();
     publish_stats_ = node->get_parameter("warnings.communication.publish_stats").get_parameter_value().get<bool>();
     if (publish_stats_) {
       publisher_connection_stats_ = node->create_publisher<crazyflie_interfaces::msg::ConnectionStatisticsArray>(name + "/connection_statistics", 10);
@@ -801,6 +803,27 @@ class CrazyflieROS
       previous_stats_broadcast_ = statsBc;
 
       publisher_status_->publish(msg);
+
+      // warnings
+      if (msg.num_rx_unicast > msg.num_tx_unicast) {
+        RCLCPP_WARN(logger_, "Unexpected number of unicast packets. Sent: %d. Received: %d", msg.num_tx_unicast, msg.num_rx_unicast);
+      }
+      if (msg.num_tx_unicast > 0) {
+        float unicast_receive_rate = msg.num_rx_unicast / (float)msg.num_tx_unicast;
+        if (unicast_receive_rate < min_unicast_receive_rate_) {
+          RCLCPP_WARN(logger_, "Low unicast receive rate (%.2f < %.2f). Sent: %d. Received: %d", unicast_receive_rate, min_unicast_receive_rate_, msg.num_tx_unicast, msg.num_rx_unicast);
+        }
+      }
+
+      if (msg.num_rx_broadcast > msg.num_tx_broadcast) {
+        RCLCPP_WARN(logger_, "Unexpected number of broadcast packets. Sent: %d. Received: %d", msg.num_tx_broadcast, msg.num_rx_broadcast);
+      }
+      if (msg.num_tx_broadcast > 0) {
+        float broadcast_receive_rate = msg.num_rx_broadcast / (float)msg.num_tx_broadcast;
+        if (broadcast_receive_rate < min_broadcast_receive_rate_) {
+          RCLCPP_WARN(logger_, "Low broadcast receive rate (%.2f < %.2f). Sent: %d. Received: %d", broadcast_receive_rate, min_broadcast_receive_rate_, msg.num_tx_broadcast, msg.num_rx_broadcast);
+        }
+      }
     }
   }
 
@@ -912,6 +935,8 @@ class CrazyflieROS
   float warning_freq_;
   float max_latency_;
   float min_ack_rate_;
+  float min_unicast_receive_rate_;
+  float min_broadcast_receive_rate_;
   bool publish_stats_;
   rclcpp::Publisher<crazyflie_interfaces::msg::ConnectionStatisticsArray>::SharedPtr publisher_connection_stats_;
 };
@@ -980,6 +1005,8 @@ class CrazyflieServer : public rclcpp::Node
 
     this->declare_parameter("warnings.communication.max_unicast_latency", 10.0);
     this->declare_parameter("warnings.communication.min_unicast_ack_rate", 0.9);
+    this->declare_parameter("warnings.communication.min_unicast_receive_rate", 0.9);
+    this->declare_parameter("warnings.communication.min_broadcast_receive_rate", 0.9);
     this->declare_parameter("warnings.communication.publish_stats", false);
 
     publish_stats_ = this->get_parameter("warnings.communication.publish_stats").get_parameter_value().get<bool>();