From 85c39ee4a4a7e2bdf75ee823852a2fdba7c81579 Mon Sep 17 00:00:00 2001
From: Marek Piechula <mpiechula@autonomous-systems.pl>
Date: Fri, 11 Mar 2022 17:24:26 +0100
Subject: [PATCH] move updating measurements into new method

---
 include/robot_localization/ros_filter.hpp |  5 ++++
 src/ros_filter.cpp                        | 30 ++++++++++++++---------
 2 files changed, 23 insertions(+), 12 deletions(-)

diff --git a/include/robot_localization/ros_filter.hpp b/include/robot_localization/ros_filter.hpp
index e32ffeddc..dd7cde28f 100644
--- a/include/robot_localization/ros_filter.hpp
+++ b/include/robot_localization/ros_filter.hpp
@@ -353,6 +353,11 @@ class RosFilter : public rclcpp::Node
   //!
   void clearMeasurementQueue();
 
+  //! @brief Update filter with data from measurements queue
+  //! @param[in] time - The time at which to carry out integration
+  //!
+  void updateFilterWithMeasurements(const rclcpp::Time & time);
+
   //! @brief Adds a diagnostic message to the accumulating map and updates the
   //! error level
   //! @param[in] error_level - The error level of the diagnostic
diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp
index efd363aa8..8463ffd90 100644
--- a/src/ros_filter.cpp
+++ b/src/ros_filter.cpp
@@ -2001,18 +2001,7 @@ void RosFilter<T>::periodicUpdate()
 
   rclcpp::Time cur_time = this->now();
 
-  if (toggled_on_) {
-    // Now we'll integrate any measurements we've received
-    integrateMeasurements(cur_time);
-  } else {
-    // Clear out measurements since we're not currently processing new entries
-    clearMeasurementQueue();
-
-    // Reset last measurement time so we don't get a large time delta on toggle
-    if (filter_.getInitializedStatus()) {
-      filter_.setLastMeasurementTime(this->now());
-    }
-  }
+  updateFilterWithMeasurements(cur_time);
 
   // Get latest state and publish it
   auto filtered_position = std::make_unique<nav_msgs::msg::Odometry>();
@@ -2174,6 +2163,23 @@ void RosFilter<T>::periodicUpdate()
   }
 }
 
+template<typename T>
+void RosFilter<T>::updateFilterWithMeasurements(const rclcpp::Time & time)
+{
+  if (toggled_on_) {
+    // Now we'll integrate any measurements we've received
+    integrateMeasurements(time);
+  } else {
+    // Clear out measurements since we're not currently processing new entries
+    clearMeasurementQueue();
+
+    // Reset last measurement time so we don't get a large time delta on toggle
+    if (filter_.getInitializedStatus()) {
+      filter_.setLastMeasurementTime(time);
+    }
+  }
+}
+
 template<typename T>
 void RosFilter<T>::setPoseCallback(
   const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)