[WIP] refactoring planning_evaluator

.github/sync-files.yaml

@@ -14,7 +14,6 @@
     - source: .github/workflows/comment-on-pr.yaml
     - source: .github/workflows/delete-closed-pr-docs.yaml
     - source: .github/workflows/deploy-docs.yaml
-    - source: .github/workflows/github-release.yaml
     - source: .github/workflows/pre-commit.yaml
     - source: .github/workflows/pre-commit-optional.yaml
     - source: .github/workflows/pre-commit-optional-autoupdate.yaml

evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml

@@ -2,7 +2,7 @@
   ros__parameters:
     ego_frame: base_link # reference frame of ego
 
-    selected_metrics:
+    metrics_for_publish:
       - curvature
       - point_interval
       - relative_angle
@@ -24,6 +24,37 @@
       - modified_goal_longitudinal_deviation
       - modified_goal_lateral_deviation
       - modified_goal_yaw_deviation
+      - stop_decision
+      - abnormal_stop_decision
+      - blinker_change_count
+      - steer_change_count
+
+    metrics_for_output:
+      - curvature
+      - point_interval
+      - relative_angle
+      - resampled_relative_angle
+      - length
+      - duration
+      - velocity
+      - acceleration
+      - jerk
+      - lateral_deviation
+      - yaw_deviation
+      - velocity_deviation
+      - lateral_trajectory_displacement_local
+      - lateral_trajectory_displacement_lookahead
+      - stability
+      - stability_frechet
+      - obstacle_distance
+      - obstacle_ttc
+      - modified_goal_longitudinal_deviation
+      - modified_goal_lateral_deviation
+      - modified_goal_yaw_deviation
+      - stop_decision
+      - abnormal_stop_decision
+      - blinker_change_count
+      - steer_change_count
 
     trajectory:
       min_point_dist_m: 0.1 # [m] minimum distance between two successive points to use for angle calculation
@@ -34,3 +65,52 @@
 
     obstacle:
       dist_thr_m: 1.0 # [m] distance between ego and the obstacle below which a collision is considered
+
+    stop_decision:
+      topic_prefix: /planning/planning_factors/ # topic prefix for planning factors
+      time_count_threshold_s: 60.0 # [s] time threshold to count a stop as a new one
+      dist_count_threshold_m: 5.0 # [m] distance threshold to count a stop as a new one
+      abnormal_deceleration_threshold_mps2: 2.0 # [m/s^2] abnormal deceleration threshold for a stop to be considered as abnormal
+      module_list:  # list of modules to be checked for stop deciation.
+        - avoidance_by_lane_change
+        - behavior_path_planner
+        - blind_spot
+        - crosswalk
+        - detection_area
+        - dynamic_obstacle_avoidance
+        - dynamic_obstacle_stop
+        - goal_planner
+        - intersection
+        - lane_change_left
+        - lane_change_right
+        - motion_velocity_planner
+        - merge_from_private
+        - no_drivable_lane
+        - no_stopping_area
+        - obstacle_cruise
+        - obstacle_cruise_planner
+        - obstacle_slow_down
+        - obstacle_stop
+        - obstacle_stop_planner
+        - occlusion_spot
+        - out_of_lane
+        - run_out
+        - side_shift
+        - start_planner
+        - static_obstacle_avoidance
+        - stop_line
+        - surround_obstacle_checker
+        - traffic_light
+        - virtual_traffic_light
+        - walkway
+
+    blinker_change_count:
+      window_duration_s: 5.0 # [s] window duration of counting blinker change for publishing
+
+    steer_change_count:
+      window_duration_s: 5.0 # [s] window duration of counting steer change for publishing
+      steer_rate_margin_radps: 0.1 # [rad/s] margin of steer_rate around 0 to count as steer change
+
+# TODO：
+#  整理READEME文档。
+#  写测试，截图，做PR。

evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metric_accumulators/blinker_accumulator.hpp

@@ -0,0 +1,84 @@
+// Copyright 2025 Tier IV, Inc.
+//
+// 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 AUTOWARE__PLANNING_EVALUATOR__METRIC_ACCUMULATORS__BLINKER_ACCUMULATOR_HPP_
+#define AUTOWARE__PLANNING_EVALUATOR__METRIC_ACCUMULATORS__BLINKER_ACCUMULATOR_HPP_
+
+#include "autoware/planning_evaluator/metrics/metric.hpp"
+#include "autoware/planning_evaluator/metrics/output_metric.hpp"
+
+#include <autoware_utils/math/accumulator.hpp>
+#include <nlohmann/json.hpp>
+
+#include <autoware_vehicle_msgs/msg/turn_indicators_report.hpp>
+#include <tier4_metric_msgs/msg/metric.hpp>
+#include <tier4_metric_msgs/msg/metric_array.hpp>
+
+#include <cstdint>
+#include <deque>
+
+namespace planning_diagnostics
+{
+using autoware_utils::Accumulator;
+using autoware_vehicle_msgs::msg::TurnIndicatorsReport;
+using MetricMsg = tier4_metric_msgs::msg::Metric;
+using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray;
+using json = nlohmann::json;
+
+/**
+ * @class BlinkerAccumulator
+ * @brief Accumulator to generate blinker-related metrics from TurnIndicatorsReport messages.
+ */
+class BlinkerAccumulator
+{
+public:
+  struct Parameters
+  {
+    double window_duration_s = 5.0;  // [s] Duration to count blinker changes for publishing
+  } parameters;
+
+  BlinkerAccumulator() = default;
+  ~BlinkerAccumulator() = default;
+
+  /**
+   * @brief update the accumulator with new blinker data
+   * @param msg new TurnIndicatorsReport msg to update the accumulator state
+   */
+  void update(const TurnIndicatorsReport & msg);
+
+  /**
+   * @brief get the output json data for the OutputMetric
+   * @return json data
+   */
+  json getOutputJson(const OutputMetric & output_metric) const;
+
+  /**
+   * @brief Add metrics to MetricArray for the given Metric
+   * @param metric Metric to add to the MetricArrayMsg
+   * @param metrics_msg MetricArrayMsg to add the metric to
+   */
+  bool addMetricMsg(const Metric & metric, MetricArrayMsg & metrics_msg) const;
+
+private:
+  // statistics
+  uint8_t prev_blinker_ = TurnIndicatorsReport::DISABLE;
+  long blinker_change_count_total_ = 0;
+  long blinker_change_count_in_window_ = 0;
+  std::deque<double> blinker_change_window_;
+  Accumulator<double> blinker_change_count_accumulator_;
+};
+
+}  // namespace planning_diagnostics
+
+#endif  // AUTOWARE__PLANNING_EVALUATOR__METRIC_ACCUMULATORS__BLINKER_ACCUMULATOR_HPP_

evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metric_accumulators/common_accumulator.hpp

@@ -0,0 +1,72 @@
+// Copyright 2025 Tier IV, Inc.
+//
+// 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 AUTOWARE__PLANNING_EVALUATOR__METRIC_ACCUMULATORS__COMMON_ACCUMULATOR_HPP_
+#define AUTOWARE__PLANNING_EVALUATOR__METRIC_ACCUMULATORS__COMMON_ACCUMULATOR_HPP_
+
+#include "autoware/planning_evaluator/metrics/output_metric.hpp"
+
+#include <autoware_utils/math/accumulator.hpp>
+#include <nlohmann/json.hpp>
+
+namespace planning_diagnostics
+{
+using autoware_utils::Accumulator;
+using json = nlohmann::json;
+
+/**
+ * @class CommonAccumulator
+ * @brief Accumulator to generate OutputMetric json result from normal Metric.
+ */
+class CommonAccumulator
+{
+public:
+  CommonAccumulator() = default;
+  ~CommonAccumulator() = default;
+
+  /**
+   * @brief update the accumulator with new statistics-based metrics with count of data points
+   * @param accumulator new metric data to add to the accumulator
+   * @param count number of data points to count
+   */
+  void update(const Accumulator<double> & accumulator, const int count);
+
+  /**
+   * @brief update the accumulator with new statistics-based metrics with count of update times
+   * @param accumulator new metric data to add to the accumulator
+   */
+  void update(const Accumulator<double> & accumulator) { update(accumulator, 1); }
+
+  /**
+   * @brief update the accumulator with new value-based metrics
+   * @param value new metric data to add to the accumulator
+   */
+  void update(const double value);
+
+  /**
+   * @brief get the output json data for the OutputMetric
+   * @return json data
+   */
+  json getOutputJson(const OutputMetric & output_metric) const;
+
+private:
+  Accumulator<double> min_accumulator_;
+  Accumulator<double> max_accumulator_;
+  Accumulator<double> mean_accumulator_;
+  int count_ = 0;
+};
+
+}  // namespace planning_diagnostics
+
+#endif  // AUTOWARE__PLANNING_EVALUATOR__METRIC_ACCUMULATORS__COMMON_ACCUMULATOR_HPP_

evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metric_accumulators/planning_factor_accumulator.hpp

@@ -0,0 +1,112 @@
+// Copyright 2025 Tier IV, Inc.
+//
+// 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 AUTOWARE__PLANNING_EVALUATOR__METRIC_ACCUMULATORS__PLANNING_FACTOR_ACCUMULATOR_HPP_
+#define AUTOWARE__PLANNING_EVALUATOR__METRIC_ACCUMULATORS__PLANNING_FACTOR_ACCUMULATOR_HPP_
+
+#include "autoware/planning_evaluator/metrics/metric.hpp"
+#include "autoware/planning_evaluator/metrics/output_metric.hpp"
+
+#include <autoware_utils/geometry/boost_geometry.hpp>
+#include <autoware_utils/math/accumulator.hpp>
+#include <nlohmann/json.hpp>
+
+#include <autoware_internal_planning_msgs/msg/control_point.hpp>
+#include <autoware_internal_planning_msgs/msg/planning_factor.hpp>
+#include <autoware_internal_planning_msgs/msg/planning_factor_array.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <tier4_metric_msgs/msg/metric.hpp>
+#include <tier4_metric_msgs/msg/metric_array.hpp>
+
+#include <unordered_map>
+#include <unordered_set>
+
+namespace planning_diagnostics
+{
+using autoware_utils::Accumulator;
+using MetricMsg = tier4_metric_msgs::msg::Metric;
+using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray;
+using autoware_internal_planning_msgs::msg::ControlPoint;
+using autoware_internal_planning_msgs::msg::PlanningFactor;
+using autoware_internal_planning_msgs::msg::PlanningFactorArray;
+using json = nlohmann::json;
+using nav_msgs::msg::Odometry;
+
+/**
+ * @class PlanningFactorAccumulator
+ * @brief Accumulator to generate planning factor-related metrics from PlanningFactorArray messages.
+ */
+class PlanningFactorAccumulator
+{
+public:
+  struct Parameters
+  {
+    double time_count_threshold_s = 60.0;  // [s] time threshold to count a stop as a new one
+    double dist_count_threshold_m = 5.0;   // [m] distance threshold to count a stop as a new one
+    double abnormal_deceleration_threshold_mps2 =
+      2.0;       // [m/s^2] deceleration threshold for a stop to be considered as abnormal
+  } parameters;  // struct Parameters
+
+  PlanningFactorAccumulator() = default;
+  ~PlanningFactorAccumulator() = default;
+
+  /**
+   * @brief update the status with new planning factor data
+   * @param module_name the module name of the planning factor data
+   * @param planning_factors the planning factor data
+   * @param ego_state the ego odometry data
+   */
+  void update(
+    const std::string & module_name, const PlanningFactorArray & planning_factors,
+    const Odometry & ego_state);
+
+  /**
+   * @brief add the metric message to the MetricArrayMsg for the given Metric and module name
+   * @param metric the metric to add to the MetricArrayMsg
+   * @param metrics_msg the MetricArrayMsg to add the metric to
+   */
+  bool addMetricMsg(const Metric & metric, MetricArrayMsg & metrics_msg) const;
+
+  /**
+   * @brief get the output json data for the OutputMetric
+   */
+  json getOutputJson(const OutputMetric & output_metric);
+
+private:
+  // Stop decision's state
+  struct StopDecisionState
+  {
+    double last_stop_time, last_stop_x, last_stop_y, last_stop_z;
+    double stop_decision_keep_time, distance_to_stop;
+    bool is_stop_decision;
+    Accumulator<double> stop_decision_keep_time_accumulator;
+
+    explicit StopDecisionState()
+    : last_stop_time(0.0),
+      last_stop_x(0.0),
+      last_stop_y(0.0),
+      last_stop_z(0.0),
+      stop_decision_keep_time(std::numeric_limits<double>::quiet_NaN()),
+      distance_to_stop(std::numeric_limits<double>::quiet_NaN()),
+      is_stop_decision(false),
+      stop_decision_keep_time_accumulator()
+    {
+    }
+  };
+  std::unordered_map<std::string, StopDecisionState> stop_decision_state_,
+    abnormal_stop_decision_state_;
+  std::unordered_set<std::string> stop_decision_modules_;
+};
+}  // namespace planning_diagnostics
+#endif  // AUTOWARE__PLANNING_EVALUATOR__METRIC_ACCUMULATORS__PLANNING_FACTOR_ACCUMULATOR_HPP_