feat(pose_initializer): check error initial pose and gnss pose, outpu…

…t diagnostics (autowarefoundation#8947)

* check initial pose error use GNSS pose

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

* add pose_error_check_enabled parameter

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

* fixed key value name

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

* rename diagnostics key value

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

* update README

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

* fixed schema json

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

* fixed type and default in schema json

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

* rename key value

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

---------

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

localization/autoware_pose_initializer/CMakeLists.txt

@@ -9,6 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/gnss_module.cpp
   src/localization_module.cpp
   src/stop_check_module.cpp
+  src/pose_error_check_module.cpp
   src/ekf_localization_trigger_module.cpp
   src/ndt_localization_trigger_module.cpp
 )

localization/autoware_pose_initializer/README.md

@@ -40,6 +40,7 @@ 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`                       | diagnostic_msgs::msg::DiagnosticArray                        | diagnostics                 |
 
 ## Diagnostics
 

localization/autoware_pose_initializer/config/pose_initializer.param.yaml

@@ -6,6 +6,8 @@
 
     gnss_pose_timeout: 3.0 # [sec]
     stop_check_duration: 3.0 # [sec]
+    pose_error_threshold: 5.0 # [m]
+    pose_error_check_enabled: false # check initial pose error with gnss
     ekf_enabled: $(var ekf_enabled)
     gnss_enabled: $(var gnss_enabled)
     yabloc_enabled: $(var yabloc_enabled)

localization/autoware_pose_initializer/schema/pose_initializer.schema.json

@@ -29,6 +29,17 @@
           "default": "3.0",
           "minimum": 0.0
         },
+        "pose_error_check_enabled": {
+          "type": "boolean",
+          "description": "If true, check error between initial pose result and GNSS pose.",
+          "default": "false"
+        },
+        "pose_error_threshold": {
+          "type": "number",
+          "description": "The error threshold between GNSS and initial pose",
+          "default": "5.0",
+          "minimum": 0.0
+        },
         "stop_check_enabled": {
           "type": "string",
           "description": "If true, initialization is accepted only when the vehicle is stopped.",
@@ -75,11 +86,13 @@
         "user_defined_initial_pose",
         "gnss_pose_timeout",
         "stop_check_duration",
+        "pose_error_threshold",
         "ekf_enabled",
         "gnss_enabled",
         "yabloc_enabled",
         "ndt_enabled",
         "stop_check_enabled",
+        "pose_error_check_enabled",
         "gnss_particle_covariance",
         "output_pose_covariance"
       ],

localization/autoware_pose_initializer/src/pose_error_check_module.cpp

@@ -0,0 +1,40 @@
+// Copyright 2022 The Autoware Contributors
+//
+// 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 "pose_error_check_module.hpp"
+
+namespace autoware::pose_initializer
+{
+PoseErrorCheckModule::PoseErrorCheckModule(rclcpp::Node * node) : node_(node)
+{
+  pose_error_threshold_ = node_->declare_parameter<double>("pose_error_threshold");
+}
+
+bool PoseErrorCheckModule::check_pose_error(
+  const geometry_msgs::msg::Pose & reference_pose, const geometry_msgs::msg::Pose & result_pose,
+  double & error_2d)
+{
+  const double diff_pose_x = reference_pose.position.x - result_pose.position.x;
+  const double diff_pose_y = reference_pose.position.y - result_pose.position.y;
+  error_2d = std::sqrt(std::pow(diff_pose_x, 2) + std::pow(diff_pose_y, 2));
+
+  if (pose_error_threshold_ <= error_2d) {
+    RCLCPP_INFO(node_->get_logger(), "Pose Error is Large. Error is %f", error_2d);
+    return false;
+  }
+
+  return true;
+}
+
+}  // namespace autoware::pose_initializer

localization/autoware_pose_initializer/src/pose_error_check_module.hpp

@@ -0,0 +1,38 @@
+// Copyright 2024 The Autoware Contributors
+//
+// 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 POSE_ERROR_CHECK_MODULE_HPP_
+#define POSE_ERROR_CHECK_MODULE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose.hpp>
+
+namespace autoware::pose_initializer
+{
+class PoseErrorCheckModule
+{
+public:
+  explicit PoseErrorCheckModule(rclcpp::Node * node);
+  bool check_pose_error(
+    const geometry_msgs::msg::Pose & reference_pose, const geometry_msgs::msg::Pose & result_pose,
+    double & error_2d);
+
+private:
+  rclcpp::Node * node_;
+  double pose_error_threshold_;
+};
+}  // namespace autoware::pose_initializer
+
+#endif  // POSE_ERROR_CHECK_MODULE_HPP_

localization/autoware_pose_initializer/src/pose_initializer_core.cpp

@@ -19,6 +19,7 @@
 #include "gnss_module.hpp"
 #include "localization_module.hpp"
 #include "ndt_localization_trigger_module.hpp"
+#include "pose_error_check_module.hpp"
 #include "stop_check_module.hpp"
 
 #include <memory>
@@ -60,6 +61,9 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options)
     stop_check_duration_ = declare_parameter<double>("stop_check_duration");
     stop_check_ = std::make_unique<StopCheckModule>(this, stop_check_duration_ + 1.0);
   }
+  if (declare_parameter<bool>("pose_error_check_enabled")) {
+    pose_error_check_ = std::make_unique<PoseErrorCheckModule>(this);
+  }
   logger_configure_ = std::make_unique<autoware::universe_utils::LoggerLevelConfigure>(this);
 
   change_state(State::Message::UNINITIALIZED);
@@ -167,8 +171,26 @@ void PoseInitializer::on_initialize(
 
       diagnostics_pose_reliable_->clear();
 
+      // check pose error between gnss pose and initial pose result
+      if (pose_error_check_ && gnss_) {
+        const auto latest_gnss_pose = get_gnss_pose();
+
+        double gnss_error_2d;
+        const bool is_gnss_pose_error_small = pose_error_check_->check_pose_error(
+          latest_gnss_pose.pose.pose, pose.pose.pose, gnss_error_2d);
+
+        diagnostics_pose_reliable_->add_key_value("gnss_pose_error_2d", gnss_error_2d);
+        diagnostics_pose_reliable_->add_key_value(
+          "is_gnss_pose_error_small", is_gnss_pose_error_small);
+        if (!is_gnss_pose_error_small) {
+          std::stringstream message;
+          message << " Large error between Initial Pose and GNSS Pose.";
+          diagnostics_pose_reliable_->update_level_and_message(
+            diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
+        }
+      }
       // check initial pose result and publish diagnostics
-      diagnostics_pose_reliable_->add_key_value("initial_pose_reliable", reliable);
+      diagnostics_pose_reliable_->add_key_value("is_initial_pose_reliable", reliable);
       if (!reliable) {
         std::stringstream message;
         message << "Initial Pose Estimation is Unstable.";

localization/autoware_pose_initializer/src/pose_initializer_core.hpp

@@ -28,6 +28,7 @@
 
 namespace autoware::pose_initializer
 {
+class PoseErrorCheckModule;
 class StopCheckModule;
 class LocalizationModule;
 class GnssModule;
@@ -56,6 +57,7 @@ class PoseInitializer : public rclcpp::Node
   std::unique_ptr<LocalizationModule> ndt_;
   std::unique_ptr<LocalizationModule> yabloc_;
   std::unique_ptr<StopCheckModule> stop_check_;
+  std::unique_ptr<PoseErrorCheckModule> pose_error_check_;
   std::unique_ptr<EkfLocalizationTriggerModule> ekf_localization_trigger_;
   std::unique_ptr<NdtLocalizationTriggerModule> ndt_localization_trigger_;
   std::unique_ptr<autoware::universe_utils::LoggerLevelConfigure> logger_configure_;