feat(pose_initializer, ndt_scan_matcher): check initial pose result a…

…nd publish diag (autowarefoundation#8275)

* feat(localization): check initial pose result and publish diag

Signed-off-by: RyuYamamoto <ryu.yamamoto@tier4.jp>

* fix: refactor

Signed-off-by: RyuYamamoto <ryu.yamamoto@tier4.jp>

* feat: update README

Signed-off-by: RyuYamamoto <ryu.yamamoto@tier4.jp>

* fix: rename reliability to reliable

Signed-off-by: RyuYamamoto <ryu.yamamoto@tier4.jp>

* feat: always return true in yabloc module

Signed-off-by: RyuYamamoto <ryu.yamamoto@tier4.jp>

---------

Signed-off-by: RyuYamamoto <ryu.yamamoto@tier4.jp>
Signed-off-by: xtk8532704 <1041084556@qq.com>

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,12 +994,22 @@ void NDTScanMatcher::service_ndt_align_main(
     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);
@@ -1089,13 +1099,6 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose(
     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;
@@ -1104,7 +1107,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_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 @@ LocalizationModule::LocalizationModule(rclcpp::Node * node, const std::string &
   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 @@ PoseWithCovarianceStamped LocalizationModule::align_pose(const PoseWithCovarianc
   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 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options)
   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 @@ void PoseInitializer::on_initialize(
 
       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 @@ void CameraPoseInitializer::on_service(
   // 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 =