Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat: keep stop policy v0.11.2 #1125

Merged
merged 16 commits into from
Mar 11, 2024
Merged
Changes from 1 commit
Commits
Show all changes
16 commits
Select commit Hold shift + click to select a range
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
fix(ndt_scan_matcher): delete unmerged feature
  • Loading branch information
0x126 authored and saka1-s committed Feb 2, 2024
commit 64b3492caa6221c7a973b35faca0cab06e04d403
19 changes: 4 additions & 15 deletions localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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 &&
Expand All @@ -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") &&
Expand Down Expand Up @@ -509,7 +496,6 @@ void NDTScanMatcher::callback_sensor_points(
} else {
(*state_ptr_)["is_local_optimal_solution_oscillation"] = "0";
}
(*state_ptr_)["execution_time"] = std::to_string(exe_time);

publish_diagnostic();
}
Expand Down Expand Up @@ -740,7 +726,10 @@ void NDTScanMatcher::service_ndt_align(
return;
}

res->pose_with_covariance = align_pose(initial_pose_msg_in_map_frame);
// mutex Map
std::lock_guard<std::mutex> 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;
}
Expand Down