server cpp: add new communication warnings

crazyflie/config/crazyflies.yaml

@@ -75,8 +75,8 @@ all:
  enabled: true
  default_topics:
  # remove to disable default topic
- # pose:
- #  frequency: 10 # Hz
+ pose:
+ frequency: 10 # Hz
  status:
  frequency: 1 # Hz
  #custom_topics:

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

crazyflie/src/crazyflie_server.cpp

@@ -188,6 +188,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);
@@ -815,6 +817,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);
+ }
+ }
  }
  }
 
@@ -926,6 +949,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_;
 };
@@ -994,6 +1019,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>();