From d3a2ea85cca7233c7f124522a6720d20dd1967a6 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Wed, 6 Mar 2024 21:04:14 +0100 Subject: [PATCH 1/3] add latency to status --- crazyflie/scripts/gui.py | 2 +- crazyflie/src/crazyflie_server.cpp | 4 ++++ crazyflie_interfaces/msg/Status.msg | 1 + 3 files changed, 6 insertions(+), 1 deletion(-) diff --git a/crazyflie/scripts/gui.py b/crazyflie/scripts/gui.py index 086d6d315..a0f221b71 100755 --- a/crazyflie/scripts/gui.py +++ b/crazyflie/scripts/gui.py @@ -225,7 +225,7 @@ def on_status(self, msg, name) -> None: battery_ok = False self.robotmodels[name].battery_text = battery_text - radio_text = f'{msg.rssi} dBm; Unicast: {msg.num_rx_unicast} / {msg.num_tx_unicast}; Broadcast: {msg.num_rx_broadcast} / {msg.num_tx_broadcast}' + radio_text = f'{msg.rssi} dBm; Unicast: {msg.num_rx_unicast} / {msg.num_tx_unicast}; Broadcast: {msg.num_rx_broadcast} / {msg.num_tx_broadcast}; Latency: {msg.latency_unicast} ms' self.robotmodels[name].radio_text = radio_text # save status here diff --git a/crazyflie/src/crazyflie_server.cpp b/crazyflie/src/crazyflie_server.cpp index de21f2544..98e964fa0 100644 --- a/crazyflie/src/crazyflie_server.cpp +++ b/crazyflie/src/crazyflie_server.cpp @@ -816,6 +816,8 @@ class CrazyflieROS msg.num_tx_broadcast = deltaTxBc; previous_stats_broadcast_ = statsBc; + msg.latency_unicast = last_latency_in_ms_; + publisher_status_->publish(msg); // warnings @@ -896,6 +898,7 @@ class CrazyflieROS RCLCPP_WARN(logger_, "[%s] Latency: %.1f ms", name_.c_str(), latency_in_us / 1000.0); } last_on_latency_ = std::chrono::steady_clock::now(); + last_latency_in_ms_ = (uint16_t)(latency_in_us / 1000.0); } private: @@ -949,6 +952,7 @@ class CrazyflieROS // link statistics rclcpp::TimerBase::SharedPtr link_statistics_timer_; std::chrono::time_point last_on_latency_; + uint16_t last_latency_in_ms_; float warning_freq_; float max_latency_; float min_ack_rate_; diff --git a/crazyflie_interfaces/msg/Status.msg b/crazyflie_interfaces/msg/Status.msg index d7c2f2352..61494cc13 100644 --- a/crazyflie_interfaces/msg/Status.msg +++ b/crazyflie_interfaces/msg/Status.msg @@ -29,3 +29,4 @@ uint32 num_rx_broadcast # number of received broadcast packets during the last uint32 num_tx_broadcast # number of broadcast packets sent during the last period uint32 num_rx_unicast # number of received unicast packets during the last period uint32 num_tx_unicast # number of unicast packets sent during the last period +uint16 latency_unicast # latency (in ms) for unicast packets From ec415695e3662e19ceb2f07c0242f0a792e8daf5 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Wed, 6 Mar 2024 21:04:43 +0100 Subject: [PATCH 2/3] launch.py: disable joystick teleop by default --- crazyflie/launch/launch.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/crazyflie/launch/launch.py b/crazyflie/launch/launch.py index bd958cd10..f20f49550 100644 --- a/crazyflie/launch/launch.py +++ b/crazyflie/launch/launch.py @@ -87,11 +87,12 @@ def generate_launch_description(): name='teleop', remappings=[ ('emergency', 'all/emergency'), - ('takeoff', 'cf6/takeoff'), - ('land', 'cf6/land'), - ('cmd_vel_legacy', 'cf6/cmd_vel_legacy'), - ('cmd_full_state', 'cf6/cmd_full_state'), - ('notify_setpoints_stop', 'cf6/notify_setpoints_stop'), + ('takeoff', 'all/takeoff'), + ('land', 'all/land'), + # uncomment to manually control (and update teleop.yaml) + # ('cmd_vel_legacy', 'cf6/cmd_vel_legacy'), + # ('cmd_full_state', 'cf6/cmd_full_state'), + # ('notify_setpoints_stop', 'cf6/notify_setpoints_stop'), ], parameters=[teleop_params] ), From 6a9d1c396e8fa92bee7d2ade4855a9e422bd6da5 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Wed, 6 Mar 2024 21:05:07 +0100 Subject: [PATCH 3/3] server [cpp] log message improvements --- crazyflie/src/crazyflie_server.cpp | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/crazyflie/src/crazyflie_server.cpp b/crazyflie/src/crazyflie_server.cpp index 98e964fa0..a31a5e5e1 100644 --- a/crazyflie/src/crazyflie_server.cpp +++ b/crazyflie/src/crazyflie_server.cpp @@ -821,23 +821,23 @@ class CrazyflieROS 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_rx_unicast > msg.num_tx_unicast * 1.05 /*allow some slack*/) { + RCLCPP_WARN(logger_, "[%s] Unexpected number of unicast packets. Sent: %d. Received: %d", name_.c_str(), 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); + RCLCPP_WARN(logger_, "[%s] Low unicast receive rate (%.2f < %.2f). Sent: %d. Received: %d", name_.c_str(), 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_rx_broadcast > msg.num_tx_broadcast * 1.05 /*allow some slack*/) { + RCLCPP_WARN(logger_, "[%s] Unexpected number of broadcast packets. Sent: %d. Received: %d", name_.c_str(), 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); + RCLCPP_WARN(logger_, "[%s] Low broadcast receive rate (%.2f < %.2f). Sent: %d. Received: %d", name_.c_str(), broadcast_receive_rate, min_broadcast_receive_rate_, msg.num_tx_broadcast, msg.num_rx_broadcast); } } } @@ -871,7 +871,7 @@ class CrazyflieROS if (stats.ack_count > 0) { float ack_rate = stats.sent_count / stats.ack_count; if (ack_rate < min_ack_rate_) { - RCLCPP_WARN(logger_, "Ack rate: %.1f %%", name_.c_str(), ack_rate * 100); + RCLCPP_WARN(logger_, "[%s] Ack rate: %.1f %%", name_.c_str(), ack_rate * 100); } } @@ -895,7 +895,7 @@ class CrazyflieROS void on_latency(uint64_t latency_in_us) { if (latency_in_us / 1000.0 > max_latency_) { - RCLCPP_WARN(logger_, "[%s] Latency: %.1f ms", name_.c_str(), latency_in_us / 1000.0); + RCLCPP_WARN(logger_, "[%s] High latency: %.1f ms", name_.c_str(), latency_in_us / 1000.0); } last_on_latency_ = std::chrono::steady_clock::now(); last_latency_in_ms_ = (uint16_t)(latency_in_us / 1000.0); @@ -1361,11 +1361,15 @@ class CrazyflieServer : public rclcpp::Node // a) check if the rate was within specified bounds if (mocap_data_received_timepoints_.size() >= 2) { double mean_rate = 0; + double min_rate = std::numeric_limits::max(); + double max_rate = 0; int num_rates_wrong = 0; for (size_t i = 0; i < mocap_data_received_timepoints_.size() - 1; ++i) { std::chrono::duration diff = mocap_data_received_timepoints_[i+1] - mocap_data_received_timepoints_[i]; double rate = 1.0 / diff.count(); mean_rate += rate; + min_rate = std::min(min_rate, rate); + max_rate = std::max(max_rate, rate); if (rate <= mocap_min_rate_ || rate >= mocap_max_rate_) { num_rates_wrong++; } @@ -1373,7 +1377,7 @@ class CrazyflieServer : public rclcpp::Node mean_rate /= (mocap_data_received_timepoints_.size() - 1); if (num_rates_wrong > 0) { - RCLCPP_WARN(logger_, "[all] Motion capture rate off (#: %d, Avg: %.1f)", num_rates_wrong, mean_rate); + RCLCPP_WARN(logger_, "[all] Motion capture rate off (#: %d, Avg: %.1f, Min: %.1f, Max: %.1f)", num_rates_wrong, mean_rate, min_rate, max_rate); } } else if (mocap_enabled_) { // b) warn if no data was received