Skip to content

Commit

Permalink
crazyflie_server (cpp): add more connection statistics
Browse files Browse the repository at this point in the history
* Adds new types and topics to publish connection statistics for each connection
* Adds a new warning if ack rate (for unicast) is low
  • Loading branch information
whoenig committed Jan 15, 2024
1 parent ec7344c commit d71ee98
Show file tree
Hide file tree
Showing 6 changed files with 79 additions and 4 deletions.
2 changes: 2 additions & 0 deletions crazyflie/config/server.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion crazyflie/deps/crazyflie_tools
69 changes: 66 additions & 3 deletions crazyflie/src/crazyflie_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -151,6 +152,11 @@ class CrazyflieROS
// link statistics
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>();
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);
}

if (warning_freq_ >= 0.0) {
cf_.setLatencyCallback(std::bind(&CrazyflieROS::on_latency, this, std::placeholders::_1));
Expand Down Expand Up @@ -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();
}
Expand Down Expand Up @@ -758,7 +786,9 @@ class CrazyflieROS
std::chrono::time_point<std::chrono::steady_clock> last_on_latency_;
float warning_freq_;
float max_latency_;

float min_ack_rate_;
bool publish_stats_;
rclcpp::Publisher<crazyflie_interfaces::msg::ConnectionStatisticsArray>::SharedPtr publisher_connection_stats_;
};

class CrazyflieServer : public rclcpp::Node
Expand Down Expand Up @@ -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<bool>();
if (publish_stats_) {
publisher_connection_stats_ = this->create_publisher<crazyflie_interfaces::msg::ConnectionStatisticsArray>("all/connection_statistics", 10);
}

// load crazyflies from params
auto node_parameters_iface = this->get_node_parameters_interface();
Expand Down Expand Up @@ -1160,14 +1197,38 @@ 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
RCLCPP_WARN(logger_, "Motion capture did not receive data!");
}

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<class T>
Expand Down Expand Up @@ -1232,6 +1293,8 @@ class CrazyflieServer : public rclcpp::Node
float mocap_min_rate_;
float mocap_max_rate_;
std::vector<std::chrono::time_point<std::chrono::steady_clock>> mocap_data_received_timepoints_;
bool publish_stats_;
rclcpp::Publisher<crazyflie_interfaces::msg::ConnectionStatisticsArray>::SharedPtr publisher_connection_stats_;

// multithreading
rclcpp::CallbackGroup::SharedPtr callback_group_mocap_;
Expand Down
2 changes: 2 additions & 0 deletions crazyflie_interfaces/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
6 changes: 6 additions & 0 deletions crazyflie_interfaces/msg/ConnectionStatistics.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
string uri
uint64 sent_count
uint64 sent_ping_count
uint64 receive_count
uint64 enqueued_count
uint64 ack_count
2 changes: 2 additions & 0 deletions crazyflie_interfaces/msg/ConnectionStatisticsArray.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
std_msgs/Header header
ConnectionStatistics[] stats

0 comments on commit d71ee98

Please sign in to comment.