diff --git a/crazyflie/config/server.yaml b/crazyflie/config/server.yaml index e0d8d6af6..6ec8a3989 100644 --- a/crazyflie/config/server.yaml +++ b/crazyflie/config/server.yaml @@ -6,6 +6,8 @@ warning_if_rate_outside: [80.0, 120.0] communication: max_unicast_latency: 10.0 # ms + min_unicast_ack_rate: 0.9 + publish_stats: false firmware_params: query_all_values_on_connect: False # simulation related diff --git a/crazyflie/deps/crazyflie_tools b/crazyflie/deps/crazyflie_tools index fbd1f7306..8cf7ee6b0 160000 --- a/crazyflie/deps/crazyflie_tools +++ b/crazyflie/deps/crazyflie_tools @@ -1 +1 @@ -Subproject commit fbd1f730636f01632b3b99abd0fb2a0789a2d221 +Subproject commit 8cf7ee6b0e91b78e428e167914c7a9a267e11ac2 diff --git a/crazyflie/src/crazyflie_server.cpp b/crazyflie/src/crazyflie_server.cpp index a76fdfc80..c49ac63c4 100644 --- a/crazyflie/src/crazyflie_server.cpp +++ b/crazyflie/src/crazyflie_server.cpp @@ -21,6 +21,7 @@ #include "crazyflie_interfaces/msg/full_state.hpp" #include "crazyflie_interfaces/msg/position.hpp" #include "crazyflie_interfaces/msg/log_data_generic.hpp" +#include "crazyflie_interfaces/msg/connection_statistics_array.hpp" using std::placeholders::_1; using std::placeholders::_2; @@ -151,6 +152,11 @@ class CrazyflieROS // link statistics 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(); + 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); + } if (warning_freq_ >= 0.0) { cf_.setLatencyCallback(std::bind(&CrazyflieROS::on_latency, this, std::placeholders::_1)); @@ -706,12 +712,34 @@ class CrazyflieROS if (elapsed.count() > 1.0 / warning_freq_) { RCLCPP_WARN(logger_, "last latency update: %f s", elapsed.count()); } + + auto stats = cf_.connectionStatsDelta(); + float ack_rate = stats.sent_count / stats.ack_count; + if (ack_rate < min_ack_rate_) { + RCLCPP_WARN(logger_, "Ack rate: %.1f %%", ack_rate * 100); + } + + if (publish_stats_) { + crazyflie_interfaces::msg::ConnectionStatisticsArray msg; + msg.header.stamp = node_->get_clock()->now(); + msg.header.frame_id = "world"; + msg.stats.resize(1); + + msg.stats[0].uri = cf_.uri(); + msg.stats[0].sent_count = stats.sent_count; + msg.stats[0].sent_ping_count = stats.sent_ping_count; + msg.stats[0].receive_count = stats.receive_count; + msg.stats[0].enqueued_count = stats.enqueued_count; + msg.stats[0].ack_count = stats.ack_count; + + publisher_connection_stats_->publish(msg); + } } void on_latency(uint64_t latency_in_us) { if (latency_in_us / 1000.0 > max_latency_) { - RCLCPP_WARN(logger_, "Latency: %f ms", latency_in_us / 1000.0); + RCLCPP_WARN(logger_, "Latency: %.1f ms", latency_in_us / 1000.0); } last_on_latency_ = std::chrono::steady_clock::now(); } @@ -758,7 +786,9 @@ class CrazyflieROS std::chrono::time_point last_on_latency_; float warning_freq_; float max_latency_; - + float min_ack_rate_; + bool publish_stats_; + rclcpp::Publisher::SharedPtr publisher_connection_stats_; }; class CrazyflieServer : public rclcpp::Node @@ -822,6 +852,13 @@ class CrazyflieServer : public rclcpp::Node mocap_max_rate_ = rate_range[1]; 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.publish_stats", false); + + publish_stats_ = this->get_parameter("warnings.communication.publish_stats").get_parameter_value().get(); + if (publish_stats_) { + publisher_connection_stats_ = this->create_publisher("all/connection_statistics", 10); + } // load crazyflies from params auto node_parameters_iface = this->get_node_parameters_interface(); @@ -1160,7 +1197,7 @@ class CrazyflieServer : public rclcpp::Node mean_rate /= (mocap_data_received_timepoints_.size() - 1); if (num_rates_wrong > 0) { - RCLCPP_WARN(logger_, "Motion capture rate off (#: %d, Avg: %f)", num_rates_wrong, mean_rate); + RCLCPP_WARN(logger_, "Motion capture rate off (#: %d, Avg: %.1f)", num_rates_wrong, mean_rate); } } else if (mocap_enabled_) { // b) warn if no data was received @@ -1168,6 +1205,30 @@ class CrazyflieServer : public rclcpp::Node } mocap_data_received_timepoints_.clear(); + + if (publish_stats_) { + + crazyflie_interfaces::msg::ConnectionStatisticsArray msg; + msg.header.stamp = this->get_clock()->now(); + msg.header.frame_id = "world"; + msg.stats.resize(broadcaster_.size()); + + size_t i = 0; + for (auto &bc : broadcaster_) { + auto &cfbc = bc.second; + + auto stats = cfbc->connectionStatsDelta(); + + msg.stats[i].uri = cfbc->uri(); + msg.stats[i].sent_count = stats.sent_count; + msg.stats[i].sent_ping_count = stats.sent_ping_count; + msg.stats[i].receive_count = stats.receive_count; + msg.stats[i].enqueued_count = stats.enqueued_count; + msg.stats[i].ack_count = stats.ack_count; + ++i; + } + publisher_connection_stats_->publish(msg); + } } template @@ -1232,6 +1293,8 @@ class CrazyflieServer : public rclcpp::Node float mocap_min_rate_; float mocap_max_rate_; std::vector> mocap_data_received_timepoints_; + bool publish_stats_; + rclcpp::Publisher::SharedPtr publisher_connection_stats_; // multithreading rclcpp::CallbackGroup::SharedPtr callback_group_mocap_; diff --git a/crazyflie_interfaces/CMakeLists.txt b/crazyflie_interfaces/CMakeLists.txt index f5b8b6cc7..583e95fa1 100644 --- a/crazyflie_interfaces/CMakeLists.txt +++ b/crazyflie_interfaces/CMakeLists.txt @@ -18,6 +18,8 @@ find_package(std_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} + "msg/ConnectionStatistics.msg" + "msg/ConnectionStatisticsArray.msg" "msg/FullState.msg" "msg/LogDataGeneric.msg" "msg/Hover.msg" diff --git a/crazyflie_interfaces/msg/ConnectionStatistics.msg b/crazyflie_interfaces/msg/ConnectionStatistics.msg new file mode 100644 index 000000000..a28d08c1b --- /dev/null +++ b/crazyflie_interfaces/msg/ConnectionStatistics.msg @@ -0,0 +1,6 @@ +string uri +uint64 sent_count +uint64 sent_ping_count +uint64 receive_count +uint64 enqueued_count +uint64 ack_count diff --git a/crazyflie_interfaces/msg/ConnectionStatisticsArray.msg b/crazyflie_interfaces/msg/ConnectionStatisticsArray.msg new file mode 100644 index 000000000..8451db2ad --- /dev/null +++ b/crazyflie_interfaces/msg/ConnectionStatisticsArray.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +ConnectionStatistics[] stats