Fixed bug in calculating delay_step

Signed-off-by: TaikiYamada4 <[email protected]>
TaikiYamada4 · Oct 27, 2023 · 9b54ade · 9b54ade
1 parent 648f510
commit 9b54ade
Showing 1 changed file with 4 additions and 2 deletions.
diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp
@@ -207,7 +207,9 @@ void EKFLocalizer::timerCallback()
       }
 
       // Get measurement_residual 
-      int delay_step = std::roundf(pose_delay_time_ / ekf_dt_);
+      const rclcpp::Time t_curr = this->now();
+      double delay_time = (t_curr - pose->header.stamp).seconds() + params_.pose_additional_delay;
+      int delay_step = std::roundf(delay_time / params_.ekf_dt);
       if(delay_step >= 0 && delay_step < params_.extend_state_step){
         predicted_ekf_pose_.position.x = ekf_.getXelement(delay_step * dim_x_ + IDX::X);
         predicted_ekf_pose_.position.y = ekf_.getXelement(delay_step * dim_x_ + IDX::Y);
@@ -224,7 +226,7 @@ void EKFLocalizer::timerCallback()
         pub_measurement_residual_pose_->publish(relative_pose);
 
         geometry_msgs::msg::PoseStamped predicted_pose_msg;
-        predicted_pose_msg.header.stamp = this->now();
+        predicted_pose_msg.header.stamp = t_curr;
         predicted_pose_msg.pose = predicted_ekf_pose_;
         pub_predicted_pose_->publish(predicted_pose_msg);
       }