Merge pull request #342 from IMRCLab/issue209

server (cpp): continous unicast latency monitoring

crazyflie/CMakeLists.txt

@@ -85,6 +85,7 @@ ament_target_dependencies(crazyflie_server
 # Install C++ executables
 install(TARGETS
  # crazyflie_tools
+ comCheck
  scan
  listParams
  listLogVariables

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

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;
@@ -147,6 +148,18 @@ class CrazyflieROS
  std::chrono::milliseconds(1),
  std::bind(&CrazyflieROS::spin_once, this), callback_group_cf_srv);
 
+ // 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>();
+
+ 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();
 
@@ -684,6 +697,25 @@ class CrazyflieROS
  (*pub)->publish(msg);
  }
 
+ void on_link_statistics_timer()
+ {
+ cf_.triggerLatencyMeasurement();
+
+ auto now = std::chrono::steady_clock::now();
+ std::chrono::duration<double> 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_;
@@ -720,7 +752,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<std::chrono::steady_clock> last_on_latency_;
+ float warning_freq_;
+ float max_latency_;
+
 };
 
 class CrazyflieServer : public rclcpp::Node
@@ -772,6 +810,19 @@ class CrazyflieServer : public rclcpp::Node
  broadcasts_delay_between_repeats_ms_ = this->get_parameter("all.broadcasts.delay_between_repeats_ms").get_parameter_value().get<int>();
  mocap_enabled_ = false;
 
+ // Warnings
+ this->declare_parameter("warnings.frequency", 1.0);
+ float freq = this->get_parameter("warnings.frequency").get_parameter_value().get<float>();
+ 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<double>({80.0, 120.0}));
+ auto rate_range = this->get_parameter("warnings.motion_capture.warning_if_rate_outside").get_parameter_value().get<std::vector<double>>();
+ 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<std::string, rclcpp::ParameterValue> &parameter_overrides =
@@ -841,16 +892,6 @@ class CrazyflieServer : public rclcpp::Node
  param_subscriber_ = std::make_shared<rclcpp::ParameterEventHandler>(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<float>();
- 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<double>({80.0, 120.0}));
- auto rate_range = this->get_parameter("warnings.motion_capture.warning_if_rate_outside").get_parameter_value().get<std::vector<double>>();
- mocap_min_rate_ = rate_range[0];
- mocap_max_rate_ = rate_range[1];
  }