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.

Sign up for GitHub

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

fix(ndt_scan_matcher): delete diagnostics thread (#5532) #1010

Merged
merged 1 commit into from
Nov 9, 2023
Merged

fix(ndt_scan_matcher): delete diagnostics thread (#5532) #1010

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
4 changes: 1 addition & 3 deletions localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,8 +51,7 @@ class MapUpdateModule
rclcpp::Node * node, std::mutex * ndt_ptr_mutex,
std::shared_ptr<NormalDistributionsTransform> ndt_ptr,
std::shared_ptr<Tf2ListenerModule> tf2_listener_module, std::string map_frame,
rclcpp::CallbackGroup::SharedPtr main_callback_group,
std::shared_ptr<std::map<std::string, std::string>> state_ptr);
rclcpp::CallbackGroup::SharedPtr main_callback_group);

private:
friend class NDTScanMatcher;
Expand Down Expand Up @@ -82,7 +81,6 @@ class MapUpdateModule
rclcpp::Logger logger_;
rclcpp::Clock::SharedPtr clock_;
std::shared_ptr<Tf2ListenerModule> tf2_listener_module_;
std::shared_ptr<std::map<std::string, std::string>> state_ptr_;

std::optional<geometry_msgs::msg::Point> last_update_position_ = std::nullopt;
std::optional<geometry_msgs::msg::Point> current_position_ = std::nullopt;
Expand Down
4 changes: 1 addition & 3 deletions localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -132,7 +132,7 @@ class NDTScanMatcher : public rclcpp::Node
const rclcpp::Time & sensor_ros_time);
void add_regularization_pose(const rclcpp::Time & sensor_ros_time);

void timer_diagnostic();
void publish_diagnostic();

rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_sub_;
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sensor_points_sub_;
Expand Down Expand Up @@ -203,8 +203,6 @@ class NDTScanMatcher : public rclcpp::Node
std::mutex ndt_ptr_mtx_;
std::mutex initial_pose_array_mtx_;

std::thread diagnostic_thread_;

// variables for regularization
const bool regularization_enabled_;
std::deque<geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr>
Expand Down
4 changes: 1 addition & 3 deletions localization/ndt_scan_matcher/src/map_update_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,15 +26,13 @@ MapUpdateModule::MapUpdateModule(
rclcpp::Node * node, std::mutex * ndt_ptr_mutex,
std::shared_ptr<NormalDistributionsTransform> ndt_ptr,
std::shared_ptr<Tf2ListenerModule> tf2_listener_module, std::string map_frame,
rclcpp::CallbackGroup::SharedPtr main_callback_group,
std::shared_ptr<std::map<std::string, std::string>> state_ptr)
rclcpp::CallbackGroup::SharedPtr main_callback_group)
: ndt_ptr_(std::move(ndt_ptr)),
ndt_ptr_mutex_(ndt_ptr_mutex),
map_frame_(std::move(map_frame)),
logger_(node->get_logger()),
clock_(node->get_clock()),
tf2_listener_module_(std::move(tf2_listener_module)),
state_ptr_(std::move(state_ptr)),
dynamic_map_loading_update_distance_(
node->declare_parameter<double>("dynamic_map_loading_update_distance")),
dynamic_map_loading_map_radius_(
Expand Down
142 changes: 65 additions & 77 deletions localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -280,93 +280,84 @@ NDTScanMatcher::NDTScanMatcher()
&NDTScanMatcher::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2),
rclcpp::ServicesQoS().get_rmw_qos_profile(), main_callback_group);

diagnostic_thread_ = std::thread(&NDTScanMatcher::timer_diagnostic, this);
diagnostic_thread_.detach();

tf2_listener_module_ = std::make_shared<Tf2ListenerModule>(this);

use_dynamic_map_loading_ = this->declare_parameter<bool>("use_dynamic_map_loading");
if (use_dynamic_map_loading_) {
map_update_module_ = std::make_unique<MapUpdateModule>(
this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group,
state_ptr_);
this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group);
} else {
map_module_ = std::make_unique<MapModule>(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group);
}

logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
}

void NDTScanMatcher::timer_diagnostic()
void NDTScanMatcher::publish_diagnostic()
{
rclcpp::Rate rate(100);
while (rclcpp::ok()) {
diagnostic_msgs::msg::DiagnosticStatus diag_status_msg;
diag_status_msg.name = "ndt_scan_matcher";
diag_status_msg.hardware_id = "";

for (const auto & key_value : (*state_ptr_)) {
diagnostic_msgs::msg::KeyValue key_value_msg;
key_value_msg.key = key_value.first;
key_value_msg.value = key_value.second;
diag_status_msg.values.push_back(key_value_msg);
}

diagnostic_msgs::msg::DiagnosticStatus diag_status_msg;
diag_status_msg.name = "ndt_scan_matcher";
diag_status_msg.hardware_id = "";

for (const auto & key_value : (*state_ptr_)) {
diagnostic_msgs::msg::KeyValue key_value_msg;
key_value_msg.key = key_value.first;
key_value_msg.value = key_value.second;
diag_status_msg.values.push_back(key_value_msg);
}

diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
diag_status_msg.message = "";
if (state_ptr_->count("state") && (*state_ptr_)["state"] == "Initializing") {
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 &&
std::stoi((*state_ptr_)["skipping_publish_num"]) < 5) {
diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
diag_status_msg.message += "skipping_publish_num > 1. ";
}
if (
state_ptr_->count("skipping_publish_num") &&
std::stoi((*state_ptr_)["skipping_publish_num"]) >= 5) {
diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
diag_status_msg.message += "skipping_publish_num exceed limit. ";
}
if (
state_ptr_->count("nearest_voxel_transformation_likelihood") &&
std::stod((*state_ptr_)["nearest_voxel_transformation_likelihood"]) <
converged_param_nearest_voxel_transformation_likelihood_) {
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") &&
std::stoi((*state_ptr_)["is_local_optimal_solution_oscillation"])) {
diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
diag_status_msg.message = "";
if (state_ptr_->count("state") && (*state_ptr_)["state"] == "Initializing") {
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 &&
std::stoi((*state_ptr_)["skipping_publish_num"]) < 5) {
diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
diag_status_msg.message += "skipping_publish_num > 1. ";
}
if (
state_ptr_->count("skipping_publish_num") &&
std::stoi((*state_ptr_)["skipping_publish_num"]) >= 5) {
diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
diag_status_msg.message += "skipping_publish_num exceed limit. ";
}
if (
state_ptr_->count("nearest_voxel_transformation_likelihood") &&
std::stod((*state_ptr_)["nearest_voxel_transformation_likelihood"]) <
converged_param_nearest_voxel_transformation_likelihood_) {
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") &&
std::stoi((*state_ptr_)["is_local_optimal_solution_oscillation"])) {
diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
diag_status_msg.message = "local optimal solution oscillation occurred";
}

diagnostic_msgs::msg::DiagnosticArray diag_msg;
diag_msg.header.stamp = this->now();
diag_msg.status.push_back(diag_status_msg);
diag_status_msg.message = "local optimal solution oscillation occurred";
}

diagnostics_pub_->publish(diag_msg);
diagnostic_msgs::msg::DiagnosticArray diag_msg;
diag_msg.header.stamp = this->now();
diag_msg.status.push_back(diag_status_msg);

rate.sleep();
}
diagnostics_pub_->publish(diag_msg);
}

void NDTScanMatcher::callback_initial_pose(
Expand Down Expand Up @@ -476,13 +467,11 @@ void NDTScanMatcher::callback_sensor_points(
}

// perform ndt scan matching
(*state_ptr_)["state"] = "Aligning";
const Eigen::Matrix4f initial_pose_matrix =
pose_to_matrix4f(interpolator.get_current_pose().pose.pose);
auto output_cloud = std::make_shared<pcl::PointCloud<PointSource>>();
ndt_ptr_->align(*output_cloud, initial_pose_matrix);
const pclomp::NdtResult ndt_result = ndt_ptr_->getResult();
(*state_ptr_)["state"] = "Sleeping";

const geometry_msgs::msg::Pose result_pose_msg = matrix4f_to_pose(ndt_result.pose);
std::vector<geometry_msgs::msg::Pose> transformation_msg_array;
Expand Down Expand Up @@ -573,6 +562,7 @@ void NDTScanMatcher::callback_sensor_points(
make_float32_stamped(sensor_ros_time, no_ground_nearest_voxel_transformation_likelihood));
}

(*state_ptr_)["state"] = "Aligned";
(*state_ptr_)["transform_probability"] = std::to_string(ndt_result.transform_probability);
(*state_ptr_)["nearest_voxel_transformation_likelihood"] =
std::to_string(ndt_result.nearest_voxel_transformation_likelihood);
Expand All @@ -584,6 +574,8 @@ void NDTScanMatcher::callback_sensor_points(
(*state_ptr_)["is_local_optimal_solution_oscillation"] = "0";
}
(*state_ptr_)["execution_time"] = std::to_string(exe_time);

publish_diagnostic();
}

void NDTScanMatcher::transform_sensor_measurement(
Expand Down Expand Up @@ -859,8 +851,6 @@ void NDTScanMatcher::service_trigger_node(
if (is_activated_) {
std::lock_guard<std::mutex> initial_pose_array_lock(initial_pose_array_mtx_);
initial_pose_msg_ptr_array_.clear();
} else {
(*state_ptr_)["state"] = "Initializing";
}
res->success = true;
}
Expand Down Expand Up @@ -897,9 +887,7 @@ void NDTScanMatcher::service_ndt_align(
return;
}

(*state_ptr_)["state"] = "Aligning";
res->pose_with_covariance = align_pose(initial_pose_msg_in_map_frame);
(*state_ptr_)["state"] = "Sleeping";
res->success = true;
res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance;
}
Expand Down
Loading