Skip to content

Commit

Permalink
Merge pull request #447 from IMRCLab:feature_status
Browse files Browse the repository at this point in the history 
status and log message improvements
  • Loading branch information
@Khaledwahba1994
Khaledwahba1994 authored Mar 15, 2024
2 parents 6019132 + 6a9d1c3 commit 15529ae
Show file tree
Hide file tree
Showing 4 changed files with 25 additions and 15 deletions.
11 changes: 6 additions & 5 deletions crazyflie/launch/launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
),
Expand Down
2 changes: 1 addition & 1 deletion crazyflie/scripts/gui.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
26 changes: 17 additions & 9 deletions crazyflie/src/crazyflie_server.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -816,26 +816,28 @@ class CrazyflieROS
msg.num_tx_broadcast = deltaTxBc;
previous_stats_broadcast_ = statsBc;

msg.latency_unicast = last_latency_in_ms_;

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);
}
}
}
Expand Down Expand Up @@ -869,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);
}
}

Expand All @@ -893,9 +895,10 @@ 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);
}

private:
Expand Down Expand Up @@ -949,6 +952,7 @@ class CrazyflieROS
// link statistics
rclcpp::TimerBase::SharedPtr link_statistics_timer_;
std::chrono::time_point<std::chrono::steady_clock> last_on_latency_;
uint16_t last_latency_in_ms_;
float warning_freq_;
float max_latency_;
float min_ack_rate_;
Expand Down Expand Up @@ -1357,19 +1361,23 @@ 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<double>::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<double> 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++;
}
}
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
Expand Down
1 change: 1 addition & 0 deletions crazyflie_interfaces/msg/Status.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

0 comments on commit 15529ae

Please sign in to comment.