From b22d9881b300858b981adfbdfd1ecedf36c43c84 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Tue, 7 Nov 2023 11:26:27 +0100 Subject: [PATCH] server (cpp): continous unicast latency monitoring --- crazyflie/CMakeLists.txt | 1 + crazyflie/config/server.yaml | 2 + crazyflie/deps/crazyflie_tools | 2 +- crazyflie/src/crazyflie_server.cpp | 63 ++++++++++++++++++++++++------ 4 files changed, 56 insertions(+), 12 deletions(-) diff --git a/crazyflie/CMakeLists.txt b/crazyflie/CMakeLists.txt index ca4dc3764..8553b314d 100644 --- a/crazyflie/CMakeLists.txt +++ b/crazyflie/CMakeLists.txt @@ -85,6 +85,7 @@ ament_target_dependencies(crazyflie_server # Install C++ executables install(TARGETS # crazyflie_tools + comCheck scan listParams listLogVariables diff --git a/crazyflie/config/server.yaml b/crazyflie/config/server.yaml index d1ca14a8e..e0d8d6af6 100644 --- a/crazyflie/config/server.yaml +++ b/crazyflie/config/server.yaml @@ -4,6 +4,8 @@ frequency: 1.0 # report/run checks once per second motion_capture: warning_if_rate_outside: [80.0, 120.0] + communication: + max_unicast_latency: 10.0 # ms firmware_params: query_all_values_on_connect: False # simulation related diff --git a/crazyflie/deps/crazyflie_tools b/crazyflie/deps/crazyflie_tools index f26165753..65a8fcdf4 160000 --- a/crazyflie/deps/crazyflie_tools +++ b/crazyflie/deps/crazyflie_tools @@ -1 +1 @@ -Subproject commit f26165753fb8cb527ca4f78f81b2db0a7c4c251f +Subproject commit 65a8fcdf48762994dce277db506793cdcc9c7135 diff --git a/crazyflie/src/crazyflie_server.cpp b/crazyflie/src/crazyflie_server.cpp index d0910f3b5..8c7d9c3c8 100644 --- a/crazyflie/src/crazyflie_server.cpp +++ b/crazyflie/src/crazyflie_server.cpp @@ -119,6 +119,7 @@ class CrazyflieROS , name_(name) , node_(node) , tf_broadcaster_(node) + , last_on_latency_(std::chrono::steady_clock::now()) { auto sub_opt_cf_cmd = rclcpp::SubscriptionOptions(); sub_opt_cf_cmd.callback_group = callback_group_cf_cmd; @@ -143,6 +144,18 @@ class CrazyflieROS // spinning timer spin_timer_ = node->create_wall_timer(std::chrono::milliseconds(100), std::bind(&CrazyflieROS::spin_once, this), callback_group_cf_srv); + // 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(); + + if (warning_freq_ >= 0.0) { + cf_.setLatencyCallback(std::bind(&CrazyflieROS::on_latency, this, std::placeholders::_1)); + link_statistics_timer_ = + node->create_wall_timer( + std::chrono::milliseconds((int)(1000.0/warning_freq_)), + std::bind(&CrazyflieROS::on_link_statistics_timer, this), callback_group_cf_srv); + + } auto start = std::chrono::system_clock::now(); @@ -679,6 +692,25 @@ class CrazyflieROS (*pub)->publish(msg); } + void on_link_statistics_timer() + { + cf_.triggerLatencyMeasurement(); + + auto now = std::chrono::steady_clock::now(); + std::chrono::duration elapsed = now - last_on_latency_; + if (elapsed.count() > 1.0 / warning_freq_) { + RCLCPP_WARN(logger_, "last latency update: %f s", elapsed.count()); + } + } + + 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); + } + last_on_latency_ = std::chrono::steady_clock::now(); + } + private: rclcpp::Logger logger_; CrazyflieLogger cf_logger_; @@ -715,7 +747,13 @@ class CrazyflieROS // multithreading rclcpp::CallbackGroup::SharedPtr callback_group_cf_; rclcpp::TimerBase::SharedPtr spin_timer_; - + + // link statistics + rclcpp::TimerBase::SharedPtr link_statistics_timer_; + std::chrono::time_point last_on_latency_; + float warning_freq_; + float max_latency_; + }; class CrazyflieServer : public rclcpp::Node @@ -767,6 +805,19 @@ class CrazyflieServer : public rclcpp::Node broadcasts_delay_between_repeats_ms_ = this->get_parameter("all.broadcasts.delay_between_repeats_ms").get_parameter_value().get(); mocap_enabled_ = false; + // Warnings + this->declare_parameter("warnings.frequency", 1.0); + float freq = this->get_parameter("warnings.frequency").get_parameter_value().get(); + if (freq >= 0.0) { + watchdog_timer_ = this->create_wall_timer(std::chrono::milliseconds((int)(1000.0/freq)), std::bind(&CrazyflieServer::on_watchdog_timer, this), callback_group_all_srv_); + } + this->declare_parameter("warnings.motion_capture.warning_if_rate_outside", std::vector({80.0, 120.0})); + auto rate_range = this->get_parameter("warnings.motion_capture.warning_if_rate_outside").get_parameter_value().get>(); + mocap_min_rate_ = rate_range[0]; + mocap_max_rate_ = rate_range[1]; + + this->declare_parameter("warnings.communication.max_unicast_latency", 10.0); + // load crazyflies from params auto node_parameters_iface = this->get_node_parameters_interface(); const std::map ¶meter_overrides = @@ -836,16 +887,6 @@ class CrazyflieServer : public rclcpp::Node param_subscriber_ = std::make_shared(this); cb_handle_ = param_subscriber_->add_parameter_event_callback(std::bind(&CrazyflieServer::on_parameter_event, this, _1)); - // Warnings - this->declare_parameter("warnings.frequency", 1.0); - float freq = this->get_parameter("warnings.frequency").get_parameter_value().get(); - if (freq >= 0.0) { - watchdog_timer_ = this->create_wall_timer(std::chrono::milliseconds((int)(1000.0/freq)), std::bind(&CrazyflieServer::on_watchdog_timer, this), callback_group_all_srv_); - } - this->declare_parameter("warnings.motion_capture.warning_if_rate_outside", std::vector({80.0, 120.0})); - auto rate_range = this->get_parameter("warnings.motion_capture.warning_if_rate_outside").get_parameter_value().get>(); - mocap_min_rate_ = rate_range[0]; - mocap_max_rate_ = rate_range[1]; }