From be0446d759a41c4c8f5750cb5c3c4b59b236b69f Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa Date: Mon, 18 Dec 2023 13:39:02 +0900 Subject: [PATCH] fix(ndt_scan_matcher): delete unmerged feature --- .../src/ndt_scan_matcher_core.cpp | 18 ++++-------------- 1 file changed, 4 insertions(+), 14 deletions(-) diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 50e944f5d163e..dd2ad74e0a875 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -262,12 +262,6 @@ void NDTScanMatcher::publish_diagnostic() diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "Initializing State. "; } - if ( - state_ptr_->count("lidar_topic_delay_time_sec") && - std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > lidar_topic_timeout_sec_) { - diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - diag_status_msg.message += "lidar_topic_delay_time_sec exceed limit. "; - } if ( state_ptr_->count("skipping_publish_num") && std::stoi((*state_ptr_)["skipping_publish_num"]) > 1 && @@ -288,13 +282,6 @@ void NDTScanMatcher::publish_diagnostic() diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "NDT score is unreliably low. "; } - if ( - state_ptr_->count("execution_time") && - std::stod((*state_ptr_)["execution_time"]) >= critical_upper_bound_exe_time_ms_) { - diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - diag_status_msg.message += - "NDT exe time is too long. (took " + (*state_ptr_)["execution_time"] + " [ms])"; - } // Ignore local optimal solution if ( state_ptr_->count("is_local_optimal_solution_oscillation") && @@ -740,7 +727,10 @@ void NDTScanMatcher::service_ndt_align( return; } - res->pose_with_covariance = align_pose(initial_pose_msg_in_map_frame); + // mutex Map + std::lock_guard lock(ndt_ptr_mtx_); + + res->pose_with_covariance = align_using_monte_carlo(ndt_ptr_, initial_pose_msg_in_map_frame); res->success = true; res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance; }