Only publish statistics if they are available.

It is possible to try to collect statistics from the underlying realtime_tools before they are available. If we detect that is the case, skip publishing the data and wait until the data is available. Signed-off-by: Chris Lalancette <[email protected]>
ros2 · Sep 20, 2019 · d6423ac · d6423ac
1 parent d0cbff3
commit d6423ac
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 3 deletions.
diff --git a/pendulum_control/include/pendulum_control/rtt_executor.hpp b/pendulum_control/include/pendulum_control/rtt_executor.hpp
@@ -59,8 +59,11 @@ class RttExecutor : public rclcpp::executor::Executor
     return rclcpp::ok() && running;
   }
 
-  void set_rtt_results_message(pendulum_msgs::msg::RttestResults & msg) const
+  bool set_rtt_results_message(pendulum_msgs::msg::RttestResults & msg) const
   {
+    if (!results_available) {
+      return false;
+    }
     msg.cur_latency = last_sample;
     msg.mean_latency = results.mean_latency;
     msg.min_latency = results.min_latency;
@@ -71,6 +74,8 @@ class RttExecutor : public rclcpp::executor::Executor
     clock_gettime(CLOCK_MONOTONIC, &curtime);
     msg.stamp.sec = curtime.tv_sec;
     msg.stamp.nanosec = curtime.tv_nsec;
+
+    return true;
   }
 
   /// Wrap executor::spin into rttest_spin.
@@ -105,7 +110,9 @@ class RttExecutor : public rclcpp::executor::Executor
     executor->spin_some();
 
     // Retrieve rttest statistics accumulated so far and store them in the executor.
-    rttest_get_statistics(&executor->results);
+    if (rttest_get_statistics(&executor->results) >= 0) {
+      executor->results_available = true;
+    }
     rttest_get_sample_at(executor->results.iteration, &executor->last_sample);
     // In case this boolean wasn't set, notify that we've recently run the callback.
     executor->running = true;
@@ -114,6 +121,7 @@ class RttExecutor : public rclcpp::executor::Executor
 
   // For storing accumulated performance statistics.
   rttest_results results;
+  bool results_available{false};
   // True if the executor is spinning.
   bool running;
   // True if rttest has initialized and hasn't been stopped yet.

diff --git a/pendulum_control/src/pendulum_demo.cpp b/pendulum_control/src/pendulum_demo.cpp
@@ -249,9 +249,12 @@ int main(int argc, char * argv[])
   auto logger_publish_callback =
     [&logger_pub, &executor, &pendulum_motor, &pendulum_controller]() {
       pendulum_msgs::msg::RttestResults results_msg;
+      if (!executor->set_rtt_results_message(results_msg)) {
+        // No data is available, just get out instead of publishing bogus data.
+        return;
+      }
       results_msg.command = pendulum_controller->get_next_command_message();
       results_msg.state = pendulum_motor->get_next_sensor_message();
-      executor->set_rtt_results_message(results_msg);
       logger_pub->publish(results_msg);
     };