fix(ndt_scan_matcher): check first old_pose_msg for initialization (a…

…utowarefoundation#2660) * fix(ndt_scan_matcher): check first old_pose_msg for initialization Signed-off-by: melike tanrikulu <melike@leodrive.ai> * fix(ndt_scan_matcher): check with old_pose_time_stamp Signed-off-by: melike tanrikulu <melike@leodrive.ai> Signed-off-by: melike tanrikulu <melike@leodrive.ai>
badai-nguyen · Mar 22, 2023 · 608ec3a · 608ec3a
1 parent 2f85a20
commit 608ec3a
Showing 1 changed file with 2 additions and 1 deletion.
diff --git a/localization/ndt_scan_matcher/src/util_func.cpp b/localization/ndt_scan_matcher/src/util_func.cpp
@@ -129,7 +129,8 @@ void get_nearest_timestamp_pose(
     const rclcpp::Time pose_time_stamp = output_new_pose_cov_msg_ptr->header.stamp;
     if (pose_time_stamp > time_stamp) {
       // TODO(Tier IV): refactor
-      if (pose_time_stamp.seconds() == 0.0) {
+      const rclcpp::Time old_pose_time_stamp = output_old_pose_cov_msg_ptr->header.stamp;
+      if (old_pose_time_stamp.seconds() == 0.0) {
         output_old_pose_cov_msg_ptr = output_new_pose_cov_msg_ptr;
       }
       break;