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 <[email protected]> * fix(ndt_scan_matcher): check with old_pose_time_stamp Signed-off-by: melike tanrikulu <[email protected]> Signed-off-by: melike tanrikulu <[email protected]>
badai-nguyen · Apr 4, 2023 · ddba4f0 · ddba4f0
1 parent 207367f
commit ddba4f0
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;