autowarefoundation
diff --git a/‎localization/ndt_scan_matcher/CMakeLists.txt
+12-7 b/‎localization/ndt_scan_matcher/CMakeLists.txt
+12-7
diff --git a/‎localization/ndt_scan_matcher/README.md
+100 b/‎localization/ndt_scan_matcher/README.md
+100
diff --git a/‎localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp
+59 b/‎localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp
+59
diff --git a/‎localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp
+6-4 b/‎localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp
+6-4
diff --git a/‎localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp
+14-3 b/‎localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp
+14-3
diff --git a/‎localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
+28-18 b/‎localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
+28-18
diff --git a/‎localization/ndt_scan_matcher/media/diagnostic_initial_pose_subscriber_status.png
23 KB b/‎localization/ndt_scan_matcher/media/diagnostic_initial_pose_subscriber_status.png
23 KB
diff --git a/‎localization/ndt_scan_matcher/media/diagnostic_map_update_status.png
60.3 KB b/‎localization/ndt_scan_matcher/media/diagnostic_map_update_status.png
60.3 KB
diff --git a/‎localization/ndt_scan_matcher/media/diagnostic_ndt_align_service_status.png
64.1 KB b/‎localization/ndt_scan_matcher/media/diagnostic_ndt_align_service_status.png
64.1 KB
diff --git a/‎localization/ndt_scan_matcher/media/diagnostic_regularization_pose_subscriber_status.png
17.5 KB b/‎localization/ndt_scan_matcher/media/diagnostic_regularization_pose_subscriber_status.png
17.5 KB
diff --git a/‎localization/ndt_scan_matcher/media/diagnostic_scan_matching_status.png
72.3 KB b/‎localization/ndt_scan_matcher/media/diagnostic_scan_matching_status.png
72.3 KB
diff --git a/‎localization/ndt_scan_matcher/media/diagnostic_trigger_node_service_status.png
24.2 KB b/‎localization/ndt_scan_matcher/media/diagnostic_trigger_node_service_status.png
24.2 KB
diff --git a/‎localization/ndt_scan_matcher/src/diagnostics_module.cpp
+104 b/‎localization/ndt_scan_matcher/src/diagnostics_module.cpp
+104
@@ -27,10 +27,11 @@ find_package(PCL REQUIRED COMPONENTS common io registration)
 include_directories(${PCL_INCLUDE_DIRS})
 
 ament_auto_add_executable(ndt_scan_matcher
-  src/particle.cpp
-  src/ndt_scan_matcher_node.cpp
-  src/ndt_scan_matcher_core.cpp
+  src/diagnostics_module.cpp
   src/map_update_module.cpp
+  src/ndt_scan_matcher_core.cpp
+  src/ndt_scan_matcher_node.cpp
+  src/particle.cpp
 )
 
 link_directories(${PCL_LIBRARY_DIRS})
@@ -45,15 +46,19 @@ if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
   ament_auto_add_gtest(standard_sequence_for_initial_pose_estimation
     test/test_cases/standard_sequence_for_initial_pose_estimation.cpp
-    src/particle.cpp
-    src/ndt_scan_matcher_core.cpp
+    src/diagnostics_module.cpp
     src/map_update_module.cpp
+    src/ndt_scan_matcher_core.cpp
+#    src/ndt_scan_matcher_node.cpp
+    src/particle.cpp
   )
   ament_auto_add_gtest(once_initialize_at_out_of_map_then_initialize_correctly
     test/test_cases/once_initialize_at_out_of_map_then_initialize_correctly.cpp
-    src/particle.cpp
-    src/ndt_scan_matcher_core.cpp
+    src/diagnostics_module.cpp
     src/map_update_module.cpp
+    src/ndt_scan_matcher_core.cpp
+#    src/ndt_scan_matcher_node.cpp
+    src/particle.cpp
   )
 endif()
 
 
@@ -0,0 +1,59 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NDT_SCAN_MATCHER__DIAGNOSTICS_MODULE_HPP_
+#define NDT_SCAN_MATCHER__DIAGNOSTICS_MODULE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+
+#include <string>
+#include <vector>
+
+class DiagnosticsModule
+{
+public:
+  DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name);
+  void clear();
+  void addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg);
+  template <typename T>
+  void addKeyValue(const std::string & key, const T & value);
+  void updateLevelAndMessage(const int8_t level, const std::string & message);
+  void publish();
+
+private:
+  diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray() const;
+
+  rclcpp::Clock::SharedPtr clock_;
+  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostics_pub_;
+
+  diagnostic_msgs::msg::DiagnosticStatus diagnostics_status_msg_;
+};
+
+template <typename T>
+void DiagnosticsModule::addKeyValue(const std::string & key, const T & value)
+{
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = key;
+  key_value.value = std::to_string(value);
+  addKeyValue(key_value);
+}
+
+template <>
+void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value);
+template <>
+void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value);
+
+#endif  // NDT_SCAN_MATCHER__DIAGNOSTICS_MODULE_HPP_
@@ -20,6 +20,7 @@
 #include <multigrid_pclomp/multigrid_ndt_omp.h>
 
 #include <algorithm>
+#include <sstream>
 #include <string>
 #include <vector>
 
@@ -162,10 +163,11 @@ struct HyperParameters
             initial_pose_offset_model_y[i];
         }
       } else {
-        RCLCPP_WARN(
-          node->get_logger(),
-          "Invalid initial pose offset model parameters. Disable covariance estimation.");
-        covariance.covariance_estimation.enable = false;
+        std::stringstream message;
+        message << "Invalid initial pose offset model parameters."
+                << "Please make sure that the number of elements in "
+                << "initial_pose_offset_model_x and initial_pose_offset_model_y are the same.";
+        throw std::runtime_error(message.str());
       }
     }
 
 
@@ -16,6 +16,7 @@
 #define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_
 
 #include "localization_util/util_func.hpp"
+#include "ndt_scan_matcher/diagnostics_module.hpp"
 #include "ndt_scan_matcher/hyper_parameters.hpp"
 #include "ndt_scan_matcher/particle.hpp"
 
@@ -54,10 +55,20 @@ class MapUpdateModule
 private:
   friend class NDTScanMatcher;
 
+  void callback_timer(
+    const bool is_activated, const std::optional<geometry_msgs::msg::Point> & position,
+    std::unique_ptr<DiagnosticsModule> & diagnostics_ptr);
+
+  [[nodiscard]] bool should_update_map(
+    const geometry_msgs::msg::Point & position,
+    std::unique_ptr<DiagnosticsModule> & diagnostics_ptr);
+  void update_map(
+    const geometry_msgs::msg::Point & position,
+    std::unique_ptr<DiagnosticsModule> & diagnostics_ptr);
   // Update the specified NDT
-  bool update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt);
-  void update_map(const geometry_msgs::msg::Point & position);
-  [[nodiscard]] bool should_update_map(const geometry_msgs::msg::Point & position);
+  bool update_ndt(
+    const geometry_msgs::msg::Point & position, NdtType & ndt,
+    std::unique_ptr<DiagnosticsModule> & diagnostics_ptr);
   void publish_partial_pcd_map();
 
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr loaded_pcd_pub_;
 
@@ -18,6 +18,7 @@
 #define FMT_HEADER_ONLY
 
 #include "localization_util/smart_pose_buffer.hpp"
+#include "ndt_scan_matcher/diagnostics_module.hpp"
 #include "ndt_scan_matcher/hyper_parameters.hpp"
 #include "ndt_scan_matcher/map_update_module.hpp"
 
@@ -62,6 +63,7 @@
 #include <map>
 #include <memory>
 #include <mutex>
+#include <sstream>
 #include <string>
 #include <thread>
 #include <vector>
@@ -77,21 +79,32 @@ class NDTScanMatcher : public rclcpp::Node
   explicit NDTScanMatcher(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
 private:
-  void service_ndt_align(
-    const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req,
-    tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res);
-  void service_trigger_node(
-    const std_srvs::srv::SetBool::Request::SharedPtr req,
-    std_srvs::srv::SetBool::Response::SharedPtr res);
-
   void callback_timer();
-  void callback_sensor_points(
-    sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_msg_in_sensor_frame);
+
   void callback_initial_pose(
     geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr);
+  void callback_initial_pose_main(
+    const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr);
+
   void callback_regularization_pose(
     geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr);
 
+  void callback_sensor_points(
+    sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_msg_in_sensor_frame);
+  bool callback_sensor_points_main(
+    sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_msg_in_sensor_frame);
+
+  void service_trigger_node(
+    const std_srvs::srv::SetBool::Request::SharedPtr req,
+    std_srvs::srv::SetBool::Response::SharedPtr res);
+
+  void service_ndt_align(
+    const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req,
+    tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res);
+  void service_ndt_align_main(
+    const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req,
+    tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res);
+
   geometry_msgs::msg::PoseWithCovarianceStamped align_pose(
     const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov);
 
@@ -116,11 +129,6 @@ class NDTScanMatcher : public rclcpp::Node
     const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_old_msg,
     const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_new_msg);
 
-  bool validate_num_iteration(const int iter_num, const int max_iter_num);
-  bool validate_score(
-    const double score, const double score_threshold, const std::string & score_name);
-  bool validate_converged_param(
-    const double & transform_probability, const double & nearest_voxel_transformation_likelihood);
   static int count_oscillation(const std::vector<geometry_msgs::msg::Pose> & result_pose_msg_array);
 
   std::array<double, 36> rotate_covariance(
@@ -131,8 +139,6 @@ class NDTScanMatcher : public rclcpp::Node
 
   void add_regularization_pose(const rclcpp::Time & sensor_ros_time);
 
-  void publish_diagnostic();
-
   rclcpp::TimerBase::SharedPtr map_update_timer_;
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_sub_;
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sensor_points_sub_;
@@ -168,7 +174,6 @@ class NDTScanMatcher : public rclcpp::Node
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr ndt_marker_pub_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
     ndt_monte_carlo_initial_pose_marker_pub_;
-  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostics_pub_;
 
   rclcpp::Service<tier4_localization_msgs::srv::PoseWithCovarianceStamped>::SharedPtr service_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr service_trigger_node_;
@@ -180,7 +185,6 @@ class NDTScanMatcher : public rclcpp::Node
   rclcpp::CallbackGroup::SharedPtr timer_callback_group_;
 
   std::shared_ptr<NormalDistributionsTransform> ndt_ptr_;
-  std::shared_ptr<std::map<std::string, std::string>> state_ptr_;
 
   Eigen::Matrix4f base_to_sensor_matrix_;
 
@@ -194,6 +198,12 @@ class NDTScanMatcher : public rclcpp::Node
   std::unique_ptr<SmartPoseBuffer> regularization_pose_buffer_;
 
   std::atomic<bool> is_activated_;
+  std::unique_ptr<DiagnosticsModule> diagnostics_scan_points_;
+  std::unique_ptr<DiagnosticsModule> diagnostics_initial_pose_;
+  std::unique_ptr<DiagnosticsModule> diagnostics_regularization_pose_;
+  std::unique_ptr<DiagnosticsModule> diagnostics_map_update_;
+  std::unique_ptr<DiagnosticsModule> diagnostics_ndt_align_;
+  std::unique_ptr<DiagnosticsModule> diagnostics_trigger_node_;
   std::unique_ptr<MapUpdateModule> map_update_module_;
   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
 
 
@@ -0,0 +1,104 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "ndt_scan_matcher/diagnostics_module.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+
+#include <algorithm>
+#include <string>
+
+DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name)
+: clock_(node->get_clock())
+{
+  diagnostics_pub_ =
+    node->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 10);
+
+  diagnostics_status_msg_.name =
+    std::string(node->get_name()) + std::string(": ") + diagnostic_name;
+  diagnostics_status_msg_.hardware_id = node->get_name();
+}
+
+void DiagnosticsModule::clear()
+{
+  diagnostics_status_msg_.values.clear();
+  diagnostics_status_msg_.values.shrink_to_fit();
+
+  diagnostics_status_msg_.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  diagnostics_status_msg_.message = "";
+}
+
+void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg)
+{
+  auto it = std::find_if(
+    std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values),
+    [key_value_msg](const auto & arg) { return arg.key == key_value_msg.key; });
+
+  if (it != std::cend(diagnostics_status_msg_.values)) {
+    it->value = key_value_msg.value;
+  } else {
+    diagnostics_status_msg_.values.push_back(key_value_msg);
+  }
+}
+
+template <>
+void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value)
+{
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = key;
+  key_value.value = value;
+  addKeyValue(key_value);
+}
+
+template <>
+void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value)
+{
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = key;
+  key_value.value = value ? "True" : "False";
+  addKeyValue(key_value);
+}
+
+void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::string & message)
+{
+  if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) {
+    if (!diagnostics_status_msg_.message.empty()) {
+      diagnostics_status_msg_.message += "; ";
+    }
+    diagnostics_status_msg_.message += message;
+  }
+  if (level > diagnostics_status_msg_.level) {
+    diagnostics_status_msg_.level = level;
+  }
+}
+
+void DiagnosticsModule::publish()
+{
+  diagnostics_pub_->publish(createDiagnosticsArray());
+}
+
+diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray() const
+{
+  diagnostic_msgs::msg::DiagnosticArray diagnostics_msg;
+  diagnostics_msg.header.stamp = clock_->now();
+  diagnostics_msg.status.push_back(diagnostics_status_msg_);
+
+  if (diagnostics_msg.status.at(0).level == diagnostic_msgs::msg::DiagnosticStatus::OK) {
+    diagnostics_msg.status.at(0).message = "OK";
+  }
+
+  return diagnostics_msg;
+}