Merge pull request #1074 from tier4/fix/ndt_scan_matcher/delete_diagnostics_thread

fix(ndt scan matcher): delete diagnostics thread

Author: 0x126

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<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;
@@ -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;

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<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_sub_;
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::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<geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr>

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<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_(
@@ -146,36 +144,37 @@ 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<pcl::shared_ptr<pcl::PointCloud<PointTarget>>> points_pcl(add_size);
+  for (size_t i = 0; i < add_size; i++) {
+    points_pcl[i] = pcl::make_shared<pcl::PointCloud<PointTarget>>();
+    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<pcl::PointCloud<PointTarget>> map_points_ptr(new pcl::PointCloud<PointTarget>);
-    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 =
     std::chrono::duration_cast<std::chrono::microseconds>(exe_end_time - exe_start_time).count();
   const auto exe_time = static_cast<double>(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();
 }
 

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<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);
   }
 }
 
-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<pcl::PointCloud<PointSource>>();
   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<std::mutex> 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<std::mutex> 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;
 }