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];
   }