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 4f630bb8c5898..2af5f15aacd1c 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 8657e354c728a..ed6af89412961 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 @@ -127,7 +127,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_; @@ -190,8 +190,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 9c94467dc94c0..783368aac813a 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_( @@ -146,21 +144,30 @@ void MapUpdateModule::update_ndt( } const auto exe_start_time = std::chrono::system_clock::now(); - NormalDistributionsTransform backup_ndt = *ndt_ptr_; + const size_t add_size = maps_to_add.size(); + + // Perform heavy processing outside of the lock scope + std::vector>> points_pcl(add_size); + for (size_t i = 0; i < add_size; i++) { + points_pcl[i] = pcl::make_shared>(); + pcl::fromROSMsg(maps_to_add[i].pointcloud, *points_pcl[i]); + } + + (*ndt_ptr_mutex_).lock(); // Add pcd - for (const auto & map_to_add : maps_to_add) { - pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); - pcl::fromROSMsg(map_to_add.pointcloud, *map_points_ptr); - backup_ndt.addTarget(map_points_ptr, map_to_add.cell_id); + for (size_t i = 0; i < add_size; i++) { + ndt_ptr_->addTarget(points_pcl[i], maps_to_add[i].cell_id); } // Remove pcd for (const std::string & map_id_to_remove : map_ids_to_remove) { - backup_ndt.removeTarget(map_id_to_remove); + ndt_ptr_->removeTarget(map_id_to_remove); } - backup_ndt.createVoxelKdtree(); + ndt_ptr_->createVoxelKdtree(); + + (*ndt_ptr_mutex_).unlock(); const auto exe_end_time = std::chrono::system_clock::now(); const auto duration_micro_sec = @@ -168,14 +175,6 @@ void MapUpdateModule::update_ndt( const auto exe_time = static_cast(duration_micro_sec) / 1000.0; RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time); - // swap - (*ndt_ptr_mutex_).lock(); - // ToDo (kminoda): Here negligible NDT copy occurs during the new map loading phase, which should - // ideally be avoided. But I will leave this for now since I cannot come up with a solution other - // than using pointer of pointer. - *ndt_ptr_ = backup_ndt; - (*ndt_ptr_mutex_).unlock(); - publish_partial_pcd_map(); } 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 667fd59196df3..28ab075ef0cc5 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -232,70 +232,69 @@ 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); } } -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("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. "; + } + // 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("skipping_publish_num") && - std::stoi((*state_ptr_)["skipping_publish_num"]) > 1) { - 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. "; - } - // 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( @@ -387,13 +386,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 auto exe_end_time = std::chrono::system_clock::now(); const auto duration_micro_sec = @@ -488,6 +485,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); @@ -498,6 +496,8 @@ void NDTScanMatcher::callback_sensor_points( } else { (*state_ptr_)["is_local_optimal_solution_oscillation"] = "0"; } + + publish_diagnostic(); } void NDTScanMatcher::transform_sensor_measurement( @@ -694,8 +694,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; } @@ -731,9 +729,7 @@ void NDTScanMatcher::service_ndt_align( // mutex Map std::lock_guard lock(ndt_ptr_mtx_); - (*state_ptr_)["state"] = "Aligning"; res->pose_with_covariance = align_using_monte_carlo(ndt_ptr_, 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; }