feat(ekf_localizer): check whether the initialpose has been set (#9787)

* check set intialpose

Signed-off-by: Yamato Ando <[email protected]>

* update png

Signed-off-by: Yamato Ando <[email protected]>

* style(pre-commit): autofix

---------

Signed-off-by: Yamato Ando <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

localization/autoware_ekf_localizer/README.md

@@ -191,6 +191,7 @@ Note that, although the dimension gets larger since the analytical expansion can
 ### The conditions that result in a WARN state
 
 - The node is not in the activate state.
+- The initial pose is not set.
 - The number of consecutive no measurement update via the Pose/Twist topic exceeds the `pose_no_update_count_threshold_warn`/`twist_no_update_count_threshold_warn`.
 - The timestamp of the Pose/Twist topic is beyond the delay compensation range.
 - The Pose/Twist topic is beyond the range of Mahalanobis distance for covariance estimation.

localization/autoware_ekf_localizer/include/autoware/ekf_localizer/diagnostics.hpp

@@ -24,6 +24,7 @@ namespace autoware::ekf_localizer
 {
 
 diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_activated);
+diagnostic_msgs::msg::DiagnosticStatus check_set_initialpose(const bool is_set_initialpose);
 
 diagnostic_msgs::msg::DiagnosticStatus check_measurement_updated(
   const std::string & measurement_type, const size_t no_update_count,

localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp

@@ -119,6 +119,7 @@ class EKFLocalizer : public rclcpp::Node
   double ekf_dt_;
 
   bool is_activated_;
+  bool is_set_initialpose_;
 
   EKFDiagnosticInfo pose_diag_info_;
   EKFDiagnosticInfo twist_diag_info_;

localization/autoware_ekf_localizer/src/diagnostics.cpp

@@ -41,6 +41,25 @@ diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_act
   return stat;
 }
 
+diagnostic_msgs::msg::DiagnosticStatus check_set_initialpose(const bool is_set_initialpose)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = "is_set_initialpose";
+  key_value.value = is_set_initialpose ? "True" : "False";
+  stat.values.push_back(key_value);
+
+  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat.message = "OK";
+  if (!is_set_initialpose) {
+    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    stat.message = "[WARN]initial pose is not set";
+  }
+
+  return stat;
+}
+
 diagnostic_msgs::msg::DiagnosticStatus check_measurement_updated(
   const std::string & measurement_type, const size_t no_update_count,
   const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error)

localization/autoware_ekf_localizer/src/ekf_localizer.cpp

@@ -56,6 +56,7 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options)
   twist_queue_(params_.twist_smoothing_steps)
 {
   is_activated_ = false;
+  is_set_initialpose_ = false;
 
   /* initialize ros system */
   timer_control_ = rclcpp::create_timer(
@@ -143,6 +144,13 @@ void EKFLocalizer::timer_callback()
     return;
   }
 
+  if (!is_set_initialpose_) {
+    warning_->warn_throttle(
+      "Initial pose is not set. Provide initial pose to pose_initializer", 2000);
+    publish_diagnostics(geometry_msgs::msg::PoseStamped{}, current_time);
+    return;
+  }
+
   DEBUG_INFO(get_logger(), "========================= timer called =========================");
 
   /* update predict frequency with measured timer rate */
@@ -264,6 +272,8 @@ void EKFLocalizer::callback_initial_pose(
       params_.pose_frame_id.c_str(), msg->header.frame_id.c_str());
   }
   ekf_module_->initialize(*msg, transform);
+
+  is_set_initialpose_ = true;
 }
 
 /*
@@ -272,7 +282,7 @@ void EKFLocalizer::callback_initial_pose(
 void EKFLocalizer::callback_pose_with_covariance(
   geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
 {
-  if (!is_activated_) {
+  if (!is_activated_ && !is_set_initialpose_) {
     return;
   }
 
@@ -359,8 +369,9 @@ void EKFLocalizer::publish_diagnostics(
   std::vector<diagnostic_msgs::msg::DiagnosticStatus> diag_status_array;
 
   diag_status_array.push_back(check_process_activated(is_activated_));
+  diag_status_array.push_back(check_set_initialpose(is_set_initialpose_));
 
-  if (is_activated_) {
+  if (is_activated_ && is_set_initialpose_) {
     diag_status_array.push_back(check_measurement_updated(
       "pose", pose_diag_info_.no_update_count, params_.pose_no_update_count_threshold_warn,
       params_.pose_no_update_count_threshold_error));
@@ -439,6 +450,7 @@ void EKFLocalizer::service_trigger_node(
     is_activated_ = true;
   } else {
     is_activated_ = false;
+    is_set_initialpose_ = false;
   }
   res->success = true;
 }

localization/autoware_ekf_localizer/test/test_diagnostics.cpp

@@ -35,6 +35,19 @@ TEST(TestEkfDiagnostics, check_process_activated)
   EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
 }
 
+TEST(TestEkfDiagnostics, check_set_initialpose)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  bool is_set_initialpose = true;
+  stat = check_set_initialpose(is_set_initialpose);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+  is_set_initialpose = false;
+  stat = check_set_initialpose(is_set_initialpose);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+}
+
 TEST(TestEkfDiagnostics, check_measurement_updated)
 {
   diagnostic_msgs::msg::DiagnosticStatus stat;