feat: subdivide diagnostics items by check content

Signed-off-by: Motsu-san <83898149+Motsu-san@users.noreply.github.com>

Author: Motsu-san

localization/autoware_ekf_localizer/src/ekf_localizer.cpp

@@ -373,84 +373,135 @@ void EKFLocalizer::publish_diagnostics(
   diagnostic_msgs::msg::DiagnosticArray diag_msg;
   diag_msg.header.stamp = current_time;
 
-  // 1. General status (その他)
+  // 1. Activation state diagnostic
   {
-    std::vector<diagnostic_msgs::msg::DiagnosticStatus> general_diag_array;
-    general_diag_array.push_back(check_process_activated(is_activated_));
-    general_diag_array.push_back(check_set_initialpose(is_set_initialpose_));
-
-    diagnostic_msgs::msg::DiagnosticStatus general_status =
-      merge_diagnostic_status(general_diag_array);
-    general_status.name = "localization: " + std::string(this->get_name()) + ": general_status";
-    general_status.hardware_id = this->get_name();
-    diag_msg.status.push_back(general_status);
+    diagnostic_msgs::msg::DiagnosticStatus status = check_process_activated(is_activated_);
+    status.name = "localization: " + std::string(this->get_name()) + ": is_activated";
+    status.hardware_id = this->get_name();
+    diag_msg.status.push_back(status);
+  }
+
+  // 2. Initial pose setting diagnostic
+  {
+    diagnostic_msgs::msg::DiagnosticStatus status = check_set_initialpose(is_set_initialpose_);
+    status.name = "localization: " + std::string(this->get_name()) + ": is_set_initialpose";
+    status.hardware_id = this->get_name();
+    diag_msg.status.push_back(status);
   }
 
   if (is_activated_ && is_set_initialpose_) {
-    // 2. Pose diagnostics (pose_...)
+    // 3. Pose no update count diagnostic
     {
-      std::vector<diagnostic_msgs::msg::DiagnosticStatus> pose_diag_array;
-      pose_diag_array.push_back(check_measurement_updated(
+      diagnostic_msgs::msg::DiagnosticStatus status = check_measurement_updated(
         "pose", pose_diag_info_.no_update_count, params_.pose_no_update_count_threshold_warn,
-        params_.pose_no_update_count_threshold_error));
-      pose_diag_array.push_back(check_measurement_queue_size("pose", pose_diag_info_.queue_size));
-      pose_diag_array.push_back(check_measurement_delay_gate(
+        params_.pose_no_update_count_threshold_error);
+      status.name = "localization: " + std::string(this->get_name()) + ": pose_no_update_count";
+      status.hardware_id = this->get_name();
+      diag_msg.status.push_back(status);
+    }
+
+    // 4. Pose queue size diagnostic
+    {
+      diagnostic_msgs::msg::DiagnosticStatus status =
+        check_measurement_queue_size("pose", pose_diag_info_.queue_size);
+      status.name = "localization: " + std::string(this->get_name()) + ": pose_queue_size";
+      status.hardware_id = this->get_name();
+      diag_msg.status.push_back(status);
+    }
+
+    // 5. Pose delay time diagnostic
+    {
+      diagnostic_msgs::msg::DiagnosticStatus status = check_measurement_delay_gate(
         "pose", pose_diag_info_.is_passed_delay_gate, pose_diag_info_.delay_time,
-        pose_diag_info_.delay_time_threshold));
-      pose_diag_array.push_back(check_measurement_mahalanobis_gate(
-        "pose", pose_diag_info_.is_passed_mahalanobis_gate, pose_diag_info_.mahalanobis_distance,
-        params_.pose_gate_dist));
+        pose_diag_info_.delay_time_threshold);
+      status.name = "localization: " + std::string(this->get_name()) + ": pose_delay_time";
+      status.hardware_id = this->get_name();
+      diag_msg.status.push_back(status);
+    }
 
-      diagnostic_msgs::msg::DiagnosticStatus pose_status = merge_diagnostic_status(pose_diag_array);
-      pose_status.name = "localization: " + std::string(this->get_name()) + ": pose_measurement";
-      pose_status.hardware_id = this->get_name();
-      diag_msg.status.push_back(pose_status);
+    // 6. Pose mahalanobis distance diagnostic
+    {
+      diagnostic_msgs::msg::DiagnosticStatus status = check_measurement_mahalanobis_gate(
+        "pose", pose_diag_info_.is_passed_mahalanobis_gate, pose_diag_info_.mahalanobis_distance,
+        params_.pose_gate_dist);
+      status.name =
+        "localization: " + std::string(this->get_name()) + ": pose_mahalanobis_distance";
+      status.hardware_id = this->get_name();
+      diag_msg.status.push_back(status);
     }
 
-    // 3. Twist diagnostics (twist_...)
+    // 7. Twist no update count diagnostic
     {
-      std::vector<diagnostic_msgs::msg::DiagnosticStatus> twist_diag_array;
-      twist_diag_array.push_back(check_measurement_updated(
+      diagnostic_msgs::msg::DiagnosticStatus status = check_measurement_updated(
         "twist", twist_diag_info_.no_update_count, params_.twist_no_update_count_threshold_warn,
-        params_.twist_no_update_count_threshold_error));
-      twist_diag_array.push_back(
-        check_measurement_queue_size("twist", twist_diag_info_.queue_size));
-      twist_diag_array.push_back(check_measurement_delay_gate(
+        params_.twist_no_update_count_threshold_error);
+      status.name = "localization: " + std::string(this->get_name()) + ": twist_no_update_count";
+      status.hardware_id = this->get_name();
+      diag_msg.status.push_back(status);
+    }
+
+    // 8. Twist queue size diagnostic
+    {
+      diagnostic_msgs::msg::DiagnosticStatus status =
+        check_measurement_queue_size("twist", twist_diag_info_.queue_size);
+      status.name = "localization: " + std::string(this->get_name()) + ": twist_queue_size";
+      status.hardware_id = this->get_name();
+      diag_msg.status.push_back(status);
+    }
+
+    // 9. Twist delay time diagnostic
+    {
+      diagnostic_msgs::msg::DiagnosticStatus status = check_measurement_delay_gate(
         "twist", twist_diag_info_.is_passed_delay_gate, twist_diag_info_.delay_time,
-        twist_diag_info_.delay_time_threshold));
-      twist_diag_array.push_back(check_measurement_mahalanobis_gate(
-        "twist", twist_diag_info_.is_passed_mahalanobis_gate, twist_diag_info_.mahalanobis_distance,
-        params_.twist_gate_dist));
+        twist_diag_info_.delay_time_threshold);
+      status.name = "localization: " + std::string(this->get_name()) + ": twist_delay_time";
+      status.hardware_id = this->get_name();
+      diag_msg.status.push_back(status);
+    }
 
-      diagnostic_msgs::msg::DiagnosticStatus twist_status =
-        merge_diagnostic_status(twist_diag_array);
-      twist_status.name = "localization: " + std::string(this->get_name()) + ": twist_measurement";
-      twist_status.hardware_id = this->get_name();
-      diag_msg.status.push_back(twist_status);
+    // 10. Twist mahalanobis distance diagnostic
+    {
+      diagnostic_msgs::msg::DiagnosticStatus status = check_measurement_mahalanobis_gate(
+        "twist", twist_diag_info_.is_passed_mahalanobis_gate, twist_diag_info_.mahalanobis_distance,
+        params_.twist_gate_dist);
+      status.name =
+        "localization: " + std::string(this->get_name()) + ": twist_mahalanobis_distance";
+      status.hardware_id = this->get_name();
+      diag_msg.status.push_back(status);
     }
 
-    // 4. Covariance ellipse diagnostics (cov_ellipse_...)
+    // 11. Covariance ellipse long axis diagnostic
     {
       geometry_msgs::msg::PoseWithCovariance pose_cov;
       pose_cov.pose = current_ekf_pose.pose;
       pose_cov.covariance = ekf_module_->get_current_pose_covariance();
       const autoware::localization_util::Ellipse ellipse =
         autoware::localization_util::calculate_xy_ellipse(pose_cov, params_.ellipse_scale);
 
-      std::vector<diagnostic_msgs::msg::DiagnosticStatus> covariance_diag_array;
-      covariance_diag_array.push_back(check_covariance_ellipse(
+      diagnostic_msgs::msg::DiagnosticStatus status = check_covariance_ellipse(
         "cov_ellipse_long_axis", ellipse.long_radius, params_.warn_ellipse_size,
-        params_.error_ellipse_size));
-      covariance_diag_array.push_back(check_covariance_ellipse(
+        params_.error_ellipse_size);
+      status.name =
+        "localization: " + std::string(this->get_name()) + ": cov_ellipse_long_axis_size";
+      status.hardware_id = this->get_name();
+      diag_msg.status.push_back(status);
+    }
+
+    // 12. Covariance ellipse lateral direction diagnostic
+    {
+      geometry_msgs::msg::PoseWithCovariance pose_cov;
+      pose_cov.pose = current_ekf_pose.pose;
+      pose_cov.covariance = ekf_module_->get_current_pose_covariance();
+      const autoware::localization_util::Ellipse ellipse =
+        autoware::localization_util::calculate_xy_ellipse(pose_cov, params_.ellipse_scale);
+
+      diagnostic_msgs::msg::DiagnosticStatus status = check_covariance_ellipse(
         "cov_ellipse_lateral_direction", ellipse.size_lateral_direction,
-        params_.warn_ellipse_size_lateral_direction, params_.error_ellipse_size_lateral_direction));
-
-      diagnostic_msgs::msg::DiagnosticStatus covariance_status =
-        merge_diagnostic_status(covariance_diag_array);
-      covariance_status.name =
-        "localization: " + std::string(this->get_name()) + ": covariance_ellipse";
-      covariance_status.hardware_id = this->get_name();
-      diag_msg.status.push_back(covariance_status);
+        params_.warn_ellipse_size_lateral_direction, params_.error_ellipse_size_lateral_direction);
+      status.name =
+        "localization: " + std::string(this->get_name()) + ": cov_ellipse_lateral_direction_size";
+      status.hardware_id = this->get_name();
+      diag_msg.status.push_back(status);
     }
   }