Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

status and log message improvements #447

Merged
merged 3 commits into from
Mar 15, 2024
Merged

status and log message improvements #447

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
d3a2ea8
add latency to status
whoenig Mar 6, 2024
ec41569
launch.py: disable joystick teleop by default
whoenig Mar 6, 2024
6a9d1c3
server [cpp] log message improvements
whoenig Mar 6, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 6 additions & 5 deletions crazyflie/launch/launch.py
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading