feat(tier4_metric_msgs): apply tier4_metric_msgs for scenario_simulat…

…or_v2_adapter, control_evaluator, planning_evaluator, autonomous_emergency_braking, obstacle_cruise_planner, motion_velocity_planner, processing_time_checker (#9180)

* first commit

Signed-off-by: xtk8532704 <1041084556@qq.com>

* fix building errs.

Signed-off-by: xtk8532704 <1041084556@qq.com>

* change diagnostic messages to metric messages for publishing decision.

Signed-off-by: xtk8532704 <1041084556@qq.com>

* fix bug about motion_velocity_planner

Signed-off-by: xtk8532704 <1041084556@qq.com>

* change the diagnostic msg to metric msg in autoware_obstacle_cruise_planner.

Signed-off-by: xtk8532704 <1041084556@qq.com>

* tmp save for planning_evaluator

Signed-off-by: xtk8532704 <1041084556@qq.com>

* change the topic to which metrics published to.

Signed-off-by: xtk8532704 <1041084556@qq.com>

* fix typo.

Signed-off-by: xtk8532704 <1041084556@qq.com>

* remove unnesessary publishing of metrics.

Signed-off-by: xtk8532704 <1041084556@qq.com>

* mke planning_evaluator publish msg of MetricArray instead of Diags.

Signed-off-by: xtk8532704 <1041084556@qq.com>

* update aeb with metric type for decision.

Signed-off-by: xtk8532704 <1041084556@qq.com>

* fix some bug

Signed-off-by: xtk8532704 <1041084556@qq.com>

* remove autoware_evaluator_utils package.

Signed-off-by: xtk8532704 <1041084556@qq.com>

* remove diagnostic_msgs dependency of planning_evaluator

Signed-off-by: xtk8532704 <1041084556@qq.com>

* use metric_msgs for autoware_processing_time_checker.

Signed-off-by: xtk8532704 <1041084556@qq.com>

* rewrite diagnostic_convertor to scenario_simulator_v2_adapter, supporting metric_msgs.

Signed-off-by: xtk8532704 <1041084556@qq.com>

* pre-commit and fix typo

Signed-off-by: xtk8532704 <1041084556@qq.com>

* publish metrics even if there is no metric in the MetricArray.

Signed-off-by: xtk8532704 <1041084556@qq.com>

* modify the metric name of processing_time.

Signed-off-by: xtk8532704 <1041084556@qq.com>

* update unit test for test_planning/control_evaluator

Signed-off-by: xtk8532704 <1041084556@qq.com>

* manual pre-commit

Signed-off-by: xtk8532704 <1041084556@qq.com>

---------

Signed-off-by: xtk8532704 <1041084556@qq.com>

.github/CODEOWNERS

@@ -64,7 +64,7 @@ control/predicted_path_checker/** berkay@leodrive.ai
 evaluator/autoware_control_evaluator/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp temkei.kem@tier4.jp
 evaluator/autoware_evaluator_utils/** daniel.sanchez@tier4.jp takayuki.murooka@tier4.jp
 evaluator/autoware_planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp
-evaluator/diagnostic_converter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp
+evaluator/scenario_simulator_v2_adapter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp
 evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
 evaluator/localization_evaluator/** dominik.jargot@robotec.ai koji.minoda@tier4.jp
 evaluator/perception_online_evaluator/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp

control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp

@@ -34,6 +34,8 @@
 #include <sensor_msgs/msg/imu.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <tier4_debug_msgs/msg/float32_stamped.hpp>
+#include <tier4_metric_msgs/msg/metric.hpp>
+#include <tier4_metric_msgs/msg/metric_array.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
@@ -77,6 +79,8 @@ using Vector3 = geometry_msgs::msg::Vector3;
 using autoware_perception_msgs::msg::PredictedObject;
 using autoware_perception_msgs::msg::PredictedObjects;
 using colorTuple = std::tuple<double, double, double, double>;
+using Metric = tier4_metric_msgs::msg::Metric;
+using MetricArray = tier4_metric_msgs::msg::MetricArray;
 
 /**
  * @brief Struct to store object data
@@ -345,6 +349,7 @@ class AEB : public rclcpp::Node
   rclcpp::Publisher<autoware::universe_utils::ProcessingTimeDetail>::SharedPtr
     debug_processing_time_detail_pub_;
   rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr debug_rss_distance_publisher_;
+  rclcpp::Publisher<MetricArray>::SharedPtr metrics_pub_;
   // timer
   rclcpp::TimerBase::SharedPtr timer_;
   mutable std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper_{nullptr};

control/autoware_autonomous_emergency_braking/package.xml

@@ -42,6 +42,7 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>tier4_debug_msgs</depend>
+  <depend>tier4_metric_msgs</depend>
   <depend>visualization_msgs</depend>
 
   <export>

control/autoware_autonomous_emergency_braking/src/node.cpp

@@ -142,6 +142,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
     virtual_wall_publisher_ = this->create_publisher<MarkerArray>("~/virtual_wall", 1);
     debug_rss_distance_publisher_ =
       this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>("~/debug/rss_distance", 1);
+    metrics_pub_ = this->create_publisher<MetricArray>("~/metrics", 1);
   }
   // Diagnostics
   {
@@ -400,6 +401,7 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat)
 {
   MarkerArray debug_markers;
   MarkerArray virtual_wall_marker;
+  auto metrics = MetricArray();
   checkCollision(debug_markers);
 
   if (!collision_data_keeper_.checkCollisionExpired()) {
@@ -416,6 +418,14 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat)
       }
     }
     addVirtualStopWallMarker(virtual_wall_marker);
+
+    {
+      auto metric = Metric();
+      metric.name = "decision";
+      metric.value = "brake";
+      metrics.metric_array.push_back(metric);
+    }
+
   } else {
     const std::string error_msg = "[AEB]: No Collision";
     const auto diag_level = DiagnosticStatus::OK;
@@ -425,6 +435,9 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat)
   // publish debug markers
   debug_marker_publisher_->publish(debug_markers);
   virtual_wall_publisher_->publish(virtual_wall_marker);
+  // publish metrics
+  metrics.stamp = get_clock()->now();
+  metrics_pub_->publish(metrics);
 }
 
 bool AEB::checkCollision(MarkerArray & debug_markers)

evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp

@@ -23,27 +23,26 @@
 
 #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
-#include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <tier4_metric_msgs/msg/metric.hpp>
+#include <tier4_metric_msgs/msg/metric_array.hpp>
 
 #include <deque>
 #include <optional>
 #include <string>
-#include <utility>
-#include <vector>
 
 namespace control_diagnostics
 {
 
 using autoware_planning_msgs::msg::Trajectory;
-using diagnostic_msgs::msg::DiagnosticArray;
-using diagnostic_msgs::msg::DiagnosticStatus;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using nav_msgs::msg::Odometry;
 using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin;
 using autoware_planning_msgs::msg::LaneletRoute;
 using geometry_msgs::msg::AccelWithCovarianceStamped;
+using MetricMsg = tier4_metric_msgs::msg::Metric;
+using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray;
 
 /**
  * @brief Node for control evaluation
@@ -52,28 +51,19 @@ class ControlEvaluatorNode : public rclcpp::Node
 {
 public:
   explicit ControlEvaluatorNode(const rclcpp::NodeOptions & node_options);
-  DiagnosticStatus generateLateralDeviationDiagnosticStatus(
-    const Trajectory & traj, const Point & ego_point);
-  DiagnosticStatus generateYawDeviationDiagnosticStatus(
-    const Trajectory & traj, const Pose & ego_pose);
-  DiagnosticStatus generateGoalLongitudinalDeviationDiagnosticStatus(const Pose & ego_pose);
-  DiagnosticStatus generateGoalLateralDeviationDiagnosticStatus(const Pose & ego_pose);
-  DiagnosticStatus generateGoalYawDeviationDiagnosticStatus(const Pose & ego_pose);
-
-  DiagnosticStatus generateAEBDiagnosticStatus(const DiagnosticStatus & diag);
-  DiagnosticStatus generateLaneletDiagnosticStatus(const Pose & ego_pose) const;
-  DiagnosticStatus generateKinematicStateDiagnosticStatus(
+  void AddLateralDeviationMetricMsg(const Trajectory & traj, const Point & ego_point);
+  void AddYawDeviationMetricMsg(const Trajectory & traj, const Pose & ego_pose);
+  void AddGoalLongitudinalDeviationMetricMsg(const Pose & ego_pose);
+  void AddGoalLateralDeviationMetricMsg(const Pose & ego_pose);
+  void AddGoalYawDeviationMetricMsg(const Pose & ego_pose);
+
+  void AddLaneletMetricMsg(const Pose & ego_pose);
+  void AddKinematicStateMetricMsg(
     const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped);
 
-  void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg);
   void onTimer();
 
 private:
-  // The diagnostics cycle is faster than timer, and each node publishes diagnostic separately.
-  // takeData() in onTimer() with a polling subscriber will miss a topic, so save all topics with
-  // onDiagnostics().
-  rclcpp::Subscription<DiagnosticArray>::SharedPtr control_diag_sub_;
-
   autoware::universe_utils::InterProcessPollingSubscriber<Odometry> odometry_sub_{
     this, "~/input/odometry"};
   autoware::universe_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped> accel_sub_{
@@ -87,7 +77,7 @@ class ControlEvaluatorNode : public rclcpp::Node
     LaneletMapBin, autoware::universe_utils::polling_policy::Newest>
     vector_map_subscriber_{this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()};
 
-  rclcpp::Publisher<DiagnosticArray>::SharedPtr metrics_pub_;
+  rclcpp::Publisher<MetricArrayMsg>::SharedPtr metrics_pub_;
 
   // update Route Handler
   void getRouteData();
@@ -96,10 +86,7 @@ class ControlEvaluatorNode : public rclcpp::Node
   // Metrics
   std::deque<rclcpp::Time> stamps_;
 
-  // queue for diagnostics and time stamp
-  std::deque<std::pair<DiagnosticStatus, rclcpp::Time>> diag_queue_;
-  const std::vector<std::string> target_functions_ = {"autonomous_emergency_braking"};
-
+  MetricArrayMsg metrics_msg_;
   autoware::route_handler::RouteHandler route_handler_;
   rclcpp::TimerBase::SharedPtr timer_;
   std::optional<AccelWithCovarianceStamped> prev_acc_stamped_{std::nullopt};

evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml

@@ -13,7 +13,6 @@
       <remap from="~/input/odometry" to="$(var input/odometry)"/>
       <remap from="~/input/acceleration" to="$(var input/acceleration)"/>
       <remap from="~/input/trajectory" to="$(var input/trajectory)"/>
-      <remap from="~/metrics" to="/control/control_evaluator/metrics"/>
       <remap from="~/input/vector_map" to="$(var map_topic_name)"/>
       <remap from="~/input/route" to="$(var route_topic_name)"/>
     </node>

evaluator/autoware_control_evaluator/package.xml

@@ -16,19 +16,18 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
-  <depend>autoware_evaluator_utils</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_planning_msgs</depend>
   <depend>autoware_route_handler</depend>
   <depend>autoware_test_utils</depend>
   <depend>autoware_universe_utils</depend>
-  <depend>diagnostic_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
+  <depend>tier4_metric_msgs</depend>
 
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>