From b0c767fb90947fd5eb4d97162c9e98409a1dda58 Mon Sep 17 00:00:00 2001 From: Joe Francis Date: Thu, 22 Jun 2023 14:34:21 +0200 Subject: [PATCH] fix to auto project the state forward when meas timeout (#798) --- src/ros_filter.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp index b13aa3c0..a859aeef 100644 --- a/src/ros_filter.cpp +++ b/src/ros_filter.cpp @@ -636,7 +636,7 @@ void RosFilter::integrateMeasurements(const rclcpp::Time & current_time) "\n" << measurement_queue_.size() << " measurements in queue.\n"); - bool predict_to_current_time = predict_to_current_time_; + //bool predict_to_current_time = predict_to_current_time_; // If we have any measurements in the queue, process them if (!measurement_queue_.empty()) { @@ -736,7 +736,7 @@ void RosFilter::integrateMeasurements(const rclcpp::Time & current_time) // If we get a large delta, then continuously predict until if (last_update_delta >= filter_.getSensorTimeout()) { - predict_to_current_time = true; + predict_to_current_time_ = true; RF_DEBUG( "Sensor timeout! Last measurement time was " << @@ -749,7 +749,7 @@ void RosFilter::integrateMeasurements(const rclcpp::Time & current_time) RF_DEBUG("Filter not yet initialized.\n"); } - if (filter_.getInitializedStatus() && predict_to_current_time) { + if (filter_.getInitializedStatus() && predict_to_current_time_) { rclcpp::Duration last_update_delta = current_time - filter_.getLastMeasurementTime();