diff --git a/crazyflie/config/crazyflies.yaml b/crazyflie/config/crazyflies.yaml index f102bd78a..5a49a0e0e 100644 --- a/crazyflie/config/crazyflies.yaml +++ b/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: 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 b4e7e65ea..6f65df3d5 100644 --- a/crazyflie/src/crazyflie_server.cpp +++ b/crazyflie/src/crazyflie_server.cpp @@ -188,6 +188,8 @@ class CrazyflieROS warning_freq_ = node->get_parameter("warnings.frequency").get_parameter_value().get(); max_latency_ = node->get_parameter("warnings.communication.max_unicast_latency").get_parameter_value().get(); min_ack_rate_ = node->get_parameter("warnings.communication.min_unicast_ack_rate").get_parameter_value().get(); + min_unicast_receive_rate_ = node->get_parameter("warnings.communication.min_unicast_receive_rate").get_parameter_value().get(); + min_broadcast_receive_rate_ = node->get_parameter("warnings.communication.min_broadcast_receive_rate").get_parameter_value().get(); publish_stats_ = node->get_parameter("warnings.communication.publish_stats").get_parameter_value().get(); if (publish_stats_) { publisher_connection_stats_ = node->create_publisher(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::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();