From ddba4f07b685c1b05ff145dbac2744c3c15dbd8f Mon Sep 17 00:00:00 2001 From: melike tanrikulu <41450930+meliketanrikulu@users.noreply.github.com> Date: Tue, 17 Jan 2023 15:09:24 +0300 Subject: [PATCH] fix(ndt_scan_matcher): check first old_pose_msg for initialization (#2660) * fix(ndt_scan_matcher): check first old_pose_msg for initialization Signed-off-by: melike tanrikulu * fix(ndt_scan_matcher): check with old_pose_time_stamp Signed-off-by: melike tanrikulu Signed-off-by: melike tanrikulu --- localization/ndt_scan_matcher/src/util_func.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/src/util_func.cpp b/localization/ndt_scan_matcher/src/util_func.cpp index 237b85ad75e10..2db6b51540d51 100644 --- 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;