refactor(ekf_localizer): remove info structs

localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

@@ -47,20 +47,6 @@
 #include <string>
 #include <vector>
 
-struct PoseInfo
-{
-  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr pose;
-  int counter;
-  int smoothing_steps;
-};
-
-struct TwistInfo
-{
-  geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr twist;
-  int counter;
-  int smoothing_steps;
-};
-
 class Simple1DFilter
 {
 public:
@@ -117,6 +103,9 @@ class EKFLocalizer : public rclcpp::Node
   EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & options);
 
 private:
+  using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
+  using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped;
+
   const Warning warning_;
 
   //!< @brief ekf estimated pose publisher
@@ -181,8 +170,14 @@ class EKFLocalizer : public rclcpp::Node
   bool is_activated_;
 
   /* for model prediction */
-  std::queue<TwistInfo> current_twist_info_queue_;    //!< @brief current measured pose
-  std::queue<PoseInfo> current_pose_info_queue_;      //!< @brief current measured pose
+  std::queue<TwistWithCovarianceStamped::SharedPtr>
+    current_twist_queue_;  //!< @brief current measured twist
+  std::queue<int> current_twist_count_queue_;
+
+  std::queue<PoseWithCovarianceStamped::SharedPtr>
+    current_pose_queue_;  //!< @brief current measured pose
+  std::queue<int> current_pose_count_queue_;
+
   geometry_msgs::msg::PoseStamped current_ekf_pose_;  //!< @brief current estimated pose
   geometry_msgs::msg::PoseStamped
     current_biased_ekf_pose_;  //!< @brief current estimated pose without yaw bias correction

localization/ekf_localizer/src/ekf_localizer.cpp

@@ -171,37 +171,45 @@ void EKFLocalizer::timerCallback()
   DEBUG_INFO(get_logger(), "------------------------- end prediction -------------------------\n");
 
   /* pose measurement update */
-  if (!current_pose_info_queue_.empty()) {
+  if (!current_pose_queue_.empty()) {
     DEBUG_INFO(get_logger(), "------------------------- start Pose -------------------------");
     stop_watch_.tic();
 
-    int pose_info_queue_size = static_cast<int>(current_pose_info_queue_.size());
-    for (int i = 0; i < pose_info_queue_size; ++i) {
-      PoseInfo pose_info = current_pose_info_queue_.front();
-      current_pose_info_queue_.pop();
-      measurementUpdatePose(*pose_info.pose);
-      ++pose_info.counter;
-      if (pose_info.counter < pose_info.smoothing_steps) {
-        current_pose_info_queue_.push(pose_info);
+    for (size_t i = 0; i < current_pose_queue_.size(); ++i) {
+      const auto pose = current_pose_queue_.front();
+      current_pose_queue_.pop();
+
+      const int count = current_pose_count_queue_.front();
+      current_pose_count_queue_.pop();
+
+      measurementUpdatePose(*pose);
+
+      if (count + 1 < params_.pose_smoothing_steps) {
+        current_pose_queue_.push(pose);
+        current_pose_count_queue_.push(count + 1);
       }
     }
     DEBUG_INFO(get_logger(), "[EKF] measurementUpdatePose calc time = %f [ms]", stop_watch_.toc());
     DEBUG_INFO(get_logger(), "------------------------- end Pose -------------------------\n");
   }
 
   /* twist measurement update */
-  if (!current_twist_info_queue_.empty()) {
+  if (!current_twist_queue_.empty()) {
     DEBUG_INFO(get_logger(), "------------------------- start Twist -------------------------");
     stop_watch_.tic();
 
-    int twist_info_queue_size = static_cast<int>(current_twist_info_queue_.size());
-    for (int i = 0; i < twist_info_queue_size; ++i) {
-      TwistInfo twist_info = current_twist_info_queue_.front();
-      current_twist_info_queue_.pop();
-      measurementUpdateTwist(*twist_info.twist);
-      ++twist_info.counter;
-      if (twist_info.counter < twist_info.smoothing_steps) {
-        current_twist_info_queue_.push(twist_info);
+    for (size_t i = 0; i < current_twist_queue_.size(); ++i) {
+      const auto twist = current_twist_queue_.front();
+      current_twist_queue_.pop();
+
+      const int count = current_twist_count_queue_.front();
+      current_twist_count_queue_.pop();
+
+      measurementUpdateTwist(*twist);
+
+      if (count + 1 < params_.twist_smoothing_steps) {
+        current_twist_queue_.push(twist);
+        current_twist_count_queue_.push(count + 1);
       }
     }
     DEBUG_INFO(get_logger(), "[EKF] measurementUpdateTwist calc time = %f [ms]", stop_watch_.toc());
@@ -361,9 +369,9 @@ void EKFLocalizer::callbackPoseWithCovariance(
   if (!is_activated_) {
     return;
   }
-  PoseInfo pose_info = {msg, 0, params_.pose_smoothing_steps};
 
-  current_pose_info_queue_.push(pose_info);
+  current_pose_queue_.push(msg);
+  current_pose_count_queue_.push(0);
 
   updateSimple1DFilters(*msg);
 }
@@ -374,8 +382,8 @@ void EKFLocalizer::callbackPoseWithCovariance(
 void EKFLocalizer::callbackTwistWithCovariance(
   geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg)
 {
-  TwistInfo twist_info = {msg, 0, params_.twist_smoothing_steps};
-  current_twist_info_queue_.push(twist_info);
+  current_twist_queue_.push(msg);
+  current_twist_count_queue_.push(0);
 }
 
 /*
@@ -610,17 +618,17 @@ void EKFLocalizer::publishEstimateResult()
   pub_odom_->publish(odometry);
 
   /* debug measured pose */
-  if (!current_pose_info_queue_.empty()) {
+  if (!current_pose_queue_.empty()) {
     geometry_msgs::msg::PoseStamped p;
-    p.pose = current_pose_info_queue_.back().pose->pose.pose;
+    p.pose = current_pose_queue_.back()->pose.pose;
     p.header.stamp = current_time;
     pub_measured_pose_->publish(p);
   }
 
   /* debug publish */
   double pose_yaw = 0.0;
-  if (!current_pose_info_queue_.empty()) {
-    pose_yaw = tf2::getYaw(current_pose_info_queue_.back().pose->pose.pose.orientation);
+  if (!current_pose_queue_.empty()) {
+    pose_yaw = tf2::getYaw(current_pose_queue_.back()->pose.pose.orientation);
   }
 
   tier4_debug_msgs::msg::Float64MultiArrayStamped msg;
@@ -677,8 +685,11 @@ void EKFLocalizer::serviceTriggerNode(
   std_srvs::srv::SetBool::Response::SharedPtr res)
 {
   if (req->data) {
-    while (!current_pose_info_queue_.empty()) current_pose_info_queue_.pop();
-    while (!current_twist_info_queue_.empty()) current_twist_info_queue_.pop();
+    while (!current_pose_queue_.empty()) current_pose_queue_.pop();
+    while (!current_pose_count_queue_.empty()) current_pose_count_queue_.pop();
+
+    while (!current_twist_queue_.empty()) current_twist_queue_.pop();
+    while (!current_twist_count_queue_.empty()) current_twist_count_queue_.pop();
     is_activated_ = true;
   } else {
     is_activated_ = false;