feat(pose_initializer, ndt_scan_matcher): check initial pose result and publish diag

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

@@ -66,6 +66,7 @@
 #include <sstream>
 #include <string>
 #include <thread>
+#include <tuple>
 #include <vector>
 
 class NDTScanMatcher : public rclcpp::Node
@@ -105,7 +106,7 @@ class NDTScanMatcher : public rclcpp::Node
     const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req,
     tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res);
 
-  geometry_msgs::msg::PoseWithCovarianceStamped align_pose(
+  std::tuple<geometry_msgs::msg::PoseWithCovarianceStamped, double> align_pose(
     const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov);
 
   void transform_sensor_measurement(

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -994,117 +994,120 @@
     return;
   }
 
-  res->pose_with_covariance = align_pose(initial_pose_msg_in_map_frame);
+  // estimate initial pose
+  const auto [pose_with_covariance, score] = align_pose(initial_pose_msg_in_map_frame);
+
+  // check reliability of initial pose result
+  res->reliable =
+    (param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood < score);
+  if (!res->reliable) {
+    RCLCPP_WARN_STREAM(
+      this->get_logger(), "Initial Pose Estimation is Unstable. Score is " << score);
+  }
   res->success = true;
+  res->pose_with_covariance = pose_with_covariance;
   res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance;
 }
 
-geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose(
+std::tuple<geometry_msgs::msg::PoseWithCovarianceStamped, double> NDTScanMatcher::align_pose(
   const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov)
 {
   output_pose_with_cov_to_log(get_logger(), "align_pose_input", initial_pose_with_cov);
 
   const auto base_rpy = get_rpy(initial_pose_with_cov);
   const Eigen::Map<const RowMatrixXd> covariance = {
     initial_pose_with_cov.pose.covariance.data(), 6, 6};
   const double stddev_x = std::sqrt(covariance(0, 0));
   const double stddev_y = std::sqrt(covariance(1, 1));
   const double stddev_z = std::sqrt(covariance(2, 2));
   const double stddev_roll = std::sqrt(covariance(3, 3));
   const double stddev_pitch = std::sqrt(covariance(4, 4));
 
   // Since only yaw is uniformly sampled, we define the mean and standard deviation for the others.
   const std::vector<double> sample_mean{
     initial_pose_with_cov.pose.pose.position.x,  // trans_x
     initial_pose_with_cov.pose.pose.position.y,  // trans_y
     initial_pose_with_cov.pose.pose.position.z,  // trans_z
     base_rpy.x,                                  // angle_x
     base_rpy.y                                   // angle_y
   };
   const std::vector<double> sample_stddev{stddev_x, stddev_y, stddev_z, stddev_roll, stddev_pitch};
 
   // Optimizing (x, y, z, roll, pitch, yaw) 6 dimensions.
   TreeStructuredParzenEstimator tpe(
     TreeStructuredParzenEstimator::Direction::MAXIMIZE,
     param_.initial_pose_estimation.n_startup_trials, sample_mean, sample_stddev);
 
   std::vector<Particle> particle_array;
   auto output_cloud = std::make_shared<pcl::PointCloud<PointSource>>();
 
   // publish the estimated poses in 20 times to see the progress and to avoid dropping data
   visualization_msgs::msg::MarkerArray marker_array;
   constexpr int64_t publish_num = 20;
   const int64_t publish_interval = param_.initial_pose_estimation.particles_num / publish_num;
 
   for (int64_t i = 0; i < param_.initial_pose_estimation.particles_num; i++) {
     const TreeStructuredParzenEstimator::Input input = tpe.get_next_input();
 
     geometry_msgs::msg::Pose initial_pose;
     initial_pose.position.x = input[0];
     initial_pose.position.y = input[1];
     initial_pose.position.z = input[2];
     geometry_msgs::msg::Vector3 init_rpy;
     init_rpy.x = input[3];
     init_rpy.y = input[4];
     init_rpy.z = input[5];
     tf2::Quaternion tf_quaternion;
     tf_quaternion.setRPY(init_rpy.x, init_rpy.y, init_rpy.z);
     initial_pose.orientation = tf2::toMsg(tf_quaternion);
 
     const Eigen::Matrix4f initial_pose_matrix = pose_to_matrix4f(initial_pose);
     ndt_ptr_->align(*output_cloud, initial_pose_matrix);
     const pclomp::NdtResult ndt_result = ndt_ptr_->getResult();
 
     Particle particle(
       initial_pose, matrix4f_to_pose(ndt_result.pose),
       ndt_result.nearest_voxel_transformation_likelihood, ndt_result.iteration_num);
     particle_array.push_back(particle);
     push_debug_markers(marker_array, get_clock()->now(), param_.frame.map_frame, particle, i);
     if (
       (i + 1) % publish_interval == 0 || (i + 1) == param_.initial_pose_estimation.particles_num) {
       ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array);
       marker_array.markers.clear();
     }
 
     const geometry_msgs::msg::Pose pose = matrix4f_to_pose(ndt_result.pose);
     const geometry_msgs::msg::Vector3 rpy = get_rpy(pose);
 
     TreeStructuredParzenEstimator::Input result(6);
     result[0] = pose.position.x;
     result[1] = pose.position.y;
     result[2] = pose.position.z;
     result[3] = rpy.x;
     result[4] = rpy.y;
     result[5] = rpy.z;
     tpe.add_trial(TreeStructuredParzenEstimator::Trial{result, ndt_result.transform_probability});
 
     auto sensor_points_in_map_ptr = std::make_shared<pcl::PointCloud<PointSource>>();
     autoware::universe_utils::transformPointCloud(
       *ndt_ptr_->getInputSource(), *sensor_points_in_map_ptr, ndt_result.pose);
     publish_point_cloud(
       initial_pose_with_cov.header.stamp, param_.frame.map_frame, sensor_points_in_map_ptr);
   }
 
   auto best_particle_ptr = std::max_element(
     std::begin(particle_array), std::end(particle_array),
     [](const Particle & lhs, const Particle & rhs) { return lhs.score < rhs.score; });
 
-  if (
-    best_particle_ptr->score <
-    param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood)
-    RCLCPP_WARN_STREAM(
-      this->get_logger(),
-      "Initial Pose Estimation is Unstable. Score is " << best_particle_ptr->score);
-
   geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg;
   result_pose_with_cov_msg.header.stamp = initial_pose_with_cov.header.stamp;
   result_pose_with_cov_msg.header.frame_id = param_.frame.map_frame;
   result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose;
 
   output_pose_with_cov_to_log(get_logger(), "align_pose_output", result_pose_with_cov_msg);
   diagnostics_ndt_align_->add_key_value("best_particle_score", best_particle_ptr->score);
 
-  return result_pose_with_cov_msg;
+  return std::make_tuple(result_pose_with_cov_msg, best_particle_ptr->score);
 }
 
 #include <rclcpp_components/register_node_macro.hpp>

localization/pose_initializer/README.md

@@ -41,6 +41,14 @@ This node depends on the map height fitter library.
 | `/localization/initialization_state` | autoware_adapi_v1_msgs::msg::LocalizationInitializationState | pose initialization state   |
 | `/initialpose3d`                     | geometry_msgs::msg::PoseWithCovarianceStamped                | calculated initial ego pose |
 
+## Diagnostics
+
+### pose_initializer_status
+
+If the score of initial pose estimation result is lower than score threshold, ERROR message is output to the `/diagnostics` topic.
+
+<img src="./media/diagnostic_pose_reliability.png" alt="drawing" width="400"/>
+
 ## Connection with Default AD API
 
 This `pose_initializer` is used via default AD API. For detailed description of the API description, please refer to [the description of `default_ad_api`](https://github.com/autowarefoundation/autoware.universe/blob/main/system/default_ad_api/document/localization.md).

localization/pose_initializer/package.xml

@@ -24,6 +24,7 @@
   <depend>component_interface_specs</depend>
   <depend>component_interface_utils</depend>
   <depend>geometry_msgs</depend>
+  <depend>localization_util</depend>
   <depend>map_height_fitter</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>

localization/pose_initializer/src/pose_initializer/localization_module.cpp

@@ -30,7 +30,8 @@
   cli_align_ = node->create_client<RequestPoseAlignment>(service_name);
 }
 
-PoseWithCovarianceStamped LocalizationModule::align_pose(const PoseWithCovarianceStamped & pose)
+std::tuple<PoseWithCovarianceStamped, bool> LocalizationModule::align_pose(
+  const PoseWithCovarianceStamped & pose)
 {
   const auto req = std::make_shared<RequestPoseAlignment::Request>();
   req->pose_with_covariance = pose;
@@ -47,5 +48,5 @@
   RCLCPP_INFO(logger_, "align server succeeded.");
 
   // Overwrite the covariance.
-  return res->pose_with_covariance;
+  return std::make_tuple(res->pose_with_covariance, res->reliable);
 }

localization/pose_initializer/src/pose_initializer/localization_module.hpp

@@ -21,6 +21,7 @@
 #include <tier4_localization_msgs/srv/pose_with_covariance_stamped.hpp>
 
 #include <string>
+#include <tuple>
 
 class LocalizationModule
 {
@@ -30,7 +31,7 @@ class LocalizationModule
 
 public:
   LocalizationModule(rclcpp::Node * node, const std::string & service_name);
-  PoseWithCovarianceStamped align_pose(const PoseWithCovarianceStamped & pose);
+  std::tuple<PoseWithCovarianceStamped, bool> align_pose(const PoseWithCovarianceStamped & pose);
 
 private:
   rclcpp::Logger logger_;

localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp

@@ -37,6 +37,8 @@
   output_pose_covariance_ = get_covariance_parameter(this, "output_pose_covariance");
   gnss_particle_covariance_ = get_covariance_parameter(this, "gnss_particle_covariance");
 
+  diagnostics_pose_reliable_ = std::make_unique<DiagnosticsModule>(this, "pose_initializer_status");
+
   if (declare_parameter<bool>("ekf_enabled")) {
     ekf_localization_trigger_ = std::make_unique<EkfLocalizationTriggerModule>(this);
   }
@@ -151,13 +153,27 @@
 
       auto pose =
         req->pose_with_covariance.empty() ? get_gnss_pose() : req->pose_with_covariance.front();
+      bool reliable = true;
       if (ndt_) {
-        pose = ndt_->align_pose(pose);
+        std::tie(pose, reliable) = ndt_->align_pose(pose);
       } else if (yabloc_) {
         // If both the NDT and YabLoc initializer are enabled, prioritize NDT as it offers more
         // accuracy pose.
-        pose = yabloc_->align_pose(pose);
+        std::tie(pose, reliable) = yabloc_->align_pose(pose);
+      }
+
+      diagnostics_pose_reliable_->clear();
+
+      // check initial pose result and publish diagnostics
+      diagnostics_pose_reliable_->add_key_value("initial_pose_reliable", reliable);
+      if (!reliable) {
+        std::stringstream message;
+        message << "Initial Pose Estimation is Unstable.";
+        diagnostics_pose_reliable_->update_level_and_message(
+          diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str());
       }
+      diagnostics_pose_reliable_->publish(this->now());
+
       pose.pose.covariance = output_pose_covariance_;
       pub_reset_->publish(pose);
 

localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp

@@ -15,6 +15,8 @@
 #ifndef POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_
 #define POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_
 
+#include "localization_util/diagnostics_module.hpp"
+
 #include <autoware/universe_utils/ros/logger_level_configure.hpp>
 #include <component_interface_specs/localization.hpp>
 #include <component_interface_utils/rclcpp.hpp>
@@ -55,6 +57,7 @@ class PoseInitializer : public rclcpp::Node
   std::unique_ptr<EkfLocalizationTriggerModule> ekf_localization_trigger_;
   std::unique_ptr<NdtLocalizationTriggerModule> ndt_localization_trigger_;
   std::unique_ptr<autoware::universe_utils::LoggerLevelConfigure> logger_configure_;
+  std::unique_ptr<DiagnosticsModule> diagnostics_pose_reliable_;
   double stop_check_duration_;
 
   void change_node_trigger(bool flag, bool need_spin = false);

localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp

@@ -199,6 +199,8 @@
   // Estimate orientation
   const double initial_yaw_angle_rad = 2 * std::atan2(orientation.z, orientation.w);
   const auto yaw_angle_rad_opt = estimate_pose(pos_vec3f, initial_yaw_angle_rad, yaw_std_rad);
+  // TODO(localization/mapping team): judge reliability of estimate pose
+  response->reliable = true;
   if (yaw_angle_rad_opt.has_value()) {
     response->success = true;
     response->pose_with_covariance =