From d595785aca975a1c437ddf12446f268d3a8aece5 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Thu, 9 Nov 2023 18:23:52 +0900 Subject: [PATCH] fix(ndt_scan_matcher): delete diagnostics thread (#5532) Signed-off-by: yamato-ando Co-authored-by: yamato-ando --- .../ndt_scan_matcher/map_update_module.hpp | 4 +- .../ndt_scan_matcher_core.hpp | 4 +- .../src/map_update_module.cpp | 4 +- .../src/ndt_scan_matcher_core.cpp | 142 ++++++++---------- 4 files changed, 68 insertions(+), 86 deletions(-) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index 13721c03ca22e..78052cb8198a3 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -51,8 +51,7 @@ class MapUpdateModule rclcpp::Node * node, std::mutex * ndt_ptr_mutex, std::shared_ptr ndt_ptr, std::shared_ptr tf2_listener_module, std::string map_frame, - rclcpp::CallbackGroup::SharedPtr main_callback_group, - std::shared_ptr> state_ptr); + rclcpp::CallbackGroup::SharedPtr main_callback_group); private: friend class NDTScanMatcher; @@ -82,7 +81,6 @@ class MapUpdateModule rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; std::shared_ptr tf2_listener_module_; - std::shared_ptr> state_ptr_; std::optional last_update_position_ = std::nullopt; std::optional current_position_ = std::nullopt; diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 9eb62230dad9e..f0d90fdfa0e08 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -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::SharedPtr initial_pose_sub_; rclcpp::Subscription::SharedPtr sensor_points_sub_; @@ -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 diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 7512ae2e77a21..f0a583417fb76 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -26,15 +26,13 @@ MapUpdateModule::MapUpdateModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, std::shared_ptr ndt_ptr, std::shared_ptr tf2_listener_module, std::string map_frame, - rclcpp::CallbackGroup::SharedPtr main_callback_group, - std::shared_ptr> 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("dynamic_map_loading_update_distance")), dynamic_map_loading_map_radius_( 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 cb66998147220..a2e89540a172f 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -280,16 +280,12 @@ 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(this); use_dynamic_map_loading_ = this->declare_parameter("use_dynamic_map_loading"); if (use_dynamic_map_loading_) { map_update_module_ = std::make_unique( - 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(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group); } @@ -297,76 +293,71 @@ NDTScanMatcher::NDTScanMatcher() logger_configure_ = std::make_unique(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( @@ -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>(); 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 transformation_msg_array; @@ -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); @@ -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( @@ -859,8 +851,6 @@ void NDTScanMatcher::service_trigger_node( if (is_activated_) { std::lock_guard initial_pose_array_lock(initial_pose_array_mtx_); initial_pose_msg_ptr_array_.clear(); - } else { - (*state_ptr_)["state"] = "Initializing"; } res->success = true; } @@ -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; }