From d6423ac8250a365459f8839a11f7d9438f8b25a0 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 9 Aug 2019 18:04:28 +0000 Subject: [PATCH] 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 --- .../include/pendulum_control/rtt_executor.hpp | 12 ++++++++++-- pendulum_control/src/pendulum_demo.cpp | 5 ++++- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/pendulum_control/include/pendulum_control/rtt_executor.hpp b/pendulum_control/include/pendulum_control/rtt_executor.hpp index dd05204c7..7de072217 100644 --- 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 index 4e9a8a97d..59cbdf965 100644 --- 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); };