fix to auto project the state forward when meas timeout (cra-ros-pkg#798

)
JoeFrancis7 · Jun 22, 2023 · b0c767f · b0c767f
1 parent ea976f9
commit b0c767f
Showing 1 changed file with 3 additions and 3 deletions.
diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp
@@ -636,7 +636,7 @@ void RosFilter<T>::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<T>::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<T>::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();