fix(localization_error_monitor, system diag): fix to use diagnostics_…

…module in localization_util (#8543)

* fix(localization_error_monitor): fix to use diagnostics_module in localization_util

Signed-off-by: RyuYamamoto <[email protected]>

* fix: update media

Signed-off-by: RyuYamamoto <[email protected]>

* fix: update component name

Signed-off-by: RyuYamamoto <[email protected]>

* fix: rename include file

Signed-off-by: RyuYamamoto <[email protected]>

---------

Signed-off-by: RyuYamamoto <[email protected]>

localization/autoware_localization_error_monitor/CMakeLists.txt

@@ -5,7 +5,6 @@ find_package(autoware_cmake REQUIRED)
 autoware_package()
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
-  src/diagnostics.cpp
   src/localization_error_monitor.cpp
 )
 

localization/autoware_localization_error_monitor/src/diagnostics.cpp → localization/autoware_localization_error_monitor/src/diagnostics_helper.hpp

@@ -12,7 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "diagnostics.hpp"
+#ifndef DIAGNOSTICS_HELPER_HPP_
+#define DIAGNOSTICS_HELPER_HPP_
 
 #include <diagnostic_msgs/msg/diagnostic_status.hpp>
 
@@ -21,16 +22,11 @@
 
 namespace autoware::localization_error_monitor
 {
-diagnostic_msgs::msg::DiagnosticStatus check_localization_accuracy(
+inline diagnostic_msgs::msg::DiagnosticStatus check_localization_accuracy(
   const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size)
 {
   diagnostic_msgs::msg::DiagnosticStatus stat;
 
-  diagnostic_msgs::msg::KeyValue key_value;
-  key_value.key = "localization_error_ellipse";
-  key_value.value = std::to_string(ellipse_size);
-  stat.values.push_back(key_value);
-
   stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
   stat.message = "OK";
   if (ellipse_size >= warn_ellipse_size) {
@@ -45,16 +41,11 @@ diagnostic_msgs::msg::DiagnosticStatus check_localization_accuracy(
   return stat;
 }
 
-diagnostic_msgs::msg::DiagnosticStatus check_localization_accuracy_lateral_direction(
+inline diagnostic_msgs::msg::DiagnosticStatus check_localization_accuracy_lateral_direction(
   const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size)
 {
   diagnostic_msgs::msg::DiagnosticStatus stat;
 
-  diagnostic_msgs::msg::KeyValue key_value;
-  key_value.key = "localization_error_ellipse_lateral_direction";
-  key_value.value = std::to_string(ellipse_size);
-  stat.values.push_back(key_value);
-
   stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
   stat.message = "OK";
   if (ellipse_size >= warn_ellipse_size) {
@@ -69,31 +60,6 @@ diagnostic_msgs::msg::DiagnosticStatus check_localization_accuracy_lateral_direc
   return stat;
 }
 
-// The highest level within the stat_array will be reflected in the merged_stat.
-diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status(
-  const std::vector<diagnostic_msgs::msg::DiagnosticStatus> & stat_array)
-{
-  diagnostic_msgs::msg::DiagnosticStatus merged_stat;
-
-  for (const auto & stat : stat_array) {
-    if ((stat.level > diagnostic_msgs::msg::DiagnosticStatus::OK)) {
-      if (!merged_stat.message.empty()) {
-        merged_stat.message += "; ";
-      }
-      merged_stat.message += stat.message;
-    }
-    if (stat.level > merged_stat.level) {
-      merged_stat.level = stat.level;
-    }
-    for (const auto & value : stat.values) {
-      merged_stat.values.push_back(value);
-    }
-  }
-
-  if (merged_stat.level == diagnostic_msgs::msg::DiagnosticStatus::OK) {
-    merged_stat.message = "OK";
-  }
-
-  return merged_stat;
-}
 }  // namespace autoware::localization_error_monitor
+
+#endif  // DIAGNOSTICS_HELPER_HPP_

localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp

@@ -14,7 +14,7 @@
 
 #include "localization_error_monitor.hpp"
 
-#include "diagnostics.hpp"
+#include "diagnostics_helper.hpp"
 
 #include <Eigen/Dense>
 
@@ -56,36 +56,38 @@ LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & o
   ellipse_marker_pub_ =
     this->create_publisher<visualization_msgs::msg::Marker>("debug/ellipse_marker", durable_qos);
 
-  diag_pub_ = this->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 10);
-
   logger_configure_ = std::make_unique<autoware::universe_utils::LoggerLevelConfigure>(this);
+
+  diagnostics_error_monitor_ = std::make_unique<DiagnosticsModule>(this, "ellipse_error_status");
 }
 
 void LocalizationErrorMonitor::on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg)
 {
+  diagnostics_error_monitor_->clear();
+
   ellipse_ = autoware::localization_util::calculate_xy_ellipse(input_msg->pose, scale_);
 
   const auto ellipse_marker = autoware::localization_util::create_ellipse_marker(
     ellipse_, input_msg->header, input_msg->pose);
   ellipse_marker_pub_->publish(ellipse_marker);
 
-  // diagnostics
-  std::vector<diagnostic_msgs::msg::DiagnosticStatus> diag_status_array;
-  diag_status_array.push_back(
-    check_localization_accuracy(ellipse_.long_radius, warn_ellipse_size_, error_ellipse_size_));
-  diag_status_array.push_back(check_localization_accuracy_lateral_direction(
-    ellipse_.size_lateral_direction, warn_ellipse_size_lateral_direction_,
-    error_ellipse_size_lateral_direction_));
+  // update localization accuracy diagnostics
+  const auto accuracy_status =
+    check_localization_accuracy(ellipse_.long_radius, warn_ellipse_size_, error_ellipse_size_);
+  diagnostics_error_monitor_->add_key_value("localization_error_ellipse", ellipse_.long_radius);
+  diagnostics_error_monitor_->update_level_and_message(
+    accuracy_status.level, accuracy_status.message);
 
-  diagnostic_msgs::msg::DiagnosticStatus diag_merged_status;
-  diag_merged_status = merge_diagnostic_status(diag_status_array);
-  diag_merged_status.name = "localization: " + std::string(this->get_name());
-  diag_merged_status.hardware_id = this->get_name();
+  // update lateral direction error diagnostics
+  const auto lateral_direction_status = check_localization_accuracy_lateral_direction(
+    ellipse_.size_lateral_direction, warn_ellipse_size_lateral_direction_,
+    error_ellipse_size_lateral_direction_);
+  diagnostics_error_monitor_->add_key_value(
+    "localization_error_ellipse_lateral_direction", ellipse_.size_lateral_direction);
+  diagnostics_error_monitor_->update_level_and_message(
+    lateral_direction_status.level, lateral_direction_status.message);
 
-  diagnostic_msgs::msg::DiagnosticArray diag_msg;
-  diag_msg.header.stamp = input_msg->header.stamp;
-  diag_msg.status.push_back(diag_merged_status);
-  diag_pub_->publish(diag_msg);
+  diagnostics_error_monitor_->publish(this->now());
 }
 }  // namespace autoware::localization_error_monitor
 

localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp

@@ -16,12 +16,12 @@
 #define LOCALIZATION_ERROR_MONITOR_HPP_
 
 #include "localization_util/covariance_ellipse.hpp"
+#include "localization_util/diagnostics_module.hpp"
 
 #include <Eigen/Dense>
 #include <autoware/universe_utils/ros/logger_level_configure.hpp>
 #include <rclcpp/rclcpp.hpp>
 
-#include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
@@ -34,12 +34,13 @@ class LocalizationErrorMonitor : public rclcpp::Node
 private:
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
   rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr ellipse_marker_pub_;
-  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diag_pub_;
 
   rclcpp::TimerBase::SharedPtr timer_;
 
   std::unique_ptr<autoware::universe_utils::LoggerLevelConfigure> logger_configure_;
 
+  std::unique_ptr<DiagnosticsModule> diagnostics_error_monitor_;
+
   double scale_;
   double error_ellipse_size_;
   double warn_ellipse_size_;

localization/autoware_localization_error_monitor/test/test_diagnostics.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "diagnostics.hpp"
+#include "diagnostics_helper.hpp"
 
 #include <gtest/gtest.h>
 
@@ -81,65 +81,3 @@ TEST(TestLocalizationErrorMonitorDiagnostics, CheckLocalizationAccuracyLateralDi
     ellipse_size, warn_ellipse_size, error_ellipse_size);
   EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR);
 }
-
-TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus)
-{
-  diagnostic_msgs::msg::DiagnosticStatus merged_stat;
-  std::vector<diagnostic_msgs::msg::DiagnosticStatus> stat_array(2);
-
-  stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK;
-  stat_array.at(0).message = "OK";
-  stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK;
-  stat_array.at(1).message = "OK";
-  merged_stat = autoware::localization_error_monitor::merge_diagnostic_status(stat_array);
-  EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
-  EXPECT_EQ(merged_stat.message, "OK");
-
-  stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
-  stat_array.at(0).message = "WARN0";
-  stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK;
-  stat_array.at(1).message = "OK";
-  merged_stat = autoware::localization_error_monitor::merge_diagnostic_status(stat_array);
-  EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
-  EXPECT_EQ(merged_stat.message, "WARN0");
-
-  stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK;
-  stat_array.at(0).message = "OK";
-  stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
-  stat_array.at(1).message = "WARN1";
-  merged_stat = autoware::localization_error_monitor::merge_diagnostic_status(stat_array);
-  EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
-  EXPECT_EQ(merged_stat.message, "WARN1");
-
-  stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
-  stat_array.at(0).message = "WARN0";
-  stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
-  stat_array.at(1).message = "WARN1";
-  merged_stat = autoware::localization_error_monitor::merge_diagnostic_status(stat_array);
-  EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
-  EXPECT_EQ(merged_stat.message, "WARN0; WARN1");
-
-  stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK;
-  stat_array.at(0).message = "OK";
-  stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
-  stat_array.at(1).message = "ERROR1";
-  merged_stat = autoware::localization_error_monitor::merge_diagnostic_status(stat_array);
-  EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR);
-  EXPECT_EQ(merged_stat.message, "ERROR1");
-
-  stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
-  stat_array.at(0).message = "WARN0";
-  stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
-  stat_array.at(1).message = "ERROR1";
-  merged_stat = autoware::localization_error_monitor::merge_diagnostic_status(stat_array);
-  EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR);
-  EXPECT_EQ(merged_stat.message, "WARN0; ERROR1");
-
-  stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
-  stat_array.at(0).message = "ERROR0";
-  stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
-  stat_array.at(1).message = "ERROR1";
-  merged_stat = autoware::localization_error_monitor::merge_diagnostic_status(stat_array);
-  EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR);
-  EXPECT_EQ(merged_stat.message, "ERROR0; ERROR1");
-}

system/system_diagnostic_monitor/config/localization.yaml

@@ -34,8 +34,8 @@ units:
 
   - path: /autoware/localization/accuracy
     type: diag
-    node: localization
-    name: localization_error_monitor
+    node: localization_error_monitor
+    name: ellipse_error_status
 
   - path: /autoware/localization/sensor_fusion_status
     type: diag

system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml

@@ -27,7 +27,7 @@
             localization_error_ellipse:
               type: diagnostic_aggregator/GenericAnalyzer
               path: localization_error_ellipse
-              contains: ["localization: localization_error_monitor"]
+              contains: ["localization_error_monitor: ellipse_error_status"]
               timeout: 1.0
 
             localization_stability: