tmp save.

Author: t4-adc

Diff for: evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml

@@ -24,6 +24,10 @@
       - modified_goal_longitudinal_deviation
       - modified_goal_lateral_deviation
       - modified_goal_yaw_deviation
+    
+    selected_counters:
+      - stop_count
+      - abnormal_stop_count
 
     trajectory:
       min_point_dist_m: 0.1 # [m] minimum distance between two successive points to use for angle calculation
@@ -34,3 +38,49 @@
 
     obstacle:
       dist_thr_m: 1.0 # [m] distance between ego and the obstacle below which a collision is considered
+
+    planning_factor_counters:
+      topic_prefix: /planning/planning_factors/
+      stop_count:
+        time_threshold_s: 5.0 # [s] time threshold for a stop to be considered as the new one
+        dist_threshold_m: 5.0 # [m] distance threshold for a stop to be considered as the new one
+        abnormal_deceleration_threshold: 2.0 # [m/s^2] 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
+
+# TODO：
+#  用一个metrics_counter.cpp/hpp，写一个大类，包含各种validate(或者自己counter，返回有效和无效即可)和所需参数。
+#  在metrics_counter中为所有类以及两种stop，制造两个列表:一个列表：里面是structure保存上一个stop的时间地点和计数值。另一个列表是int，保存计数值。
+
+#  写一个onPlanningFactor的类，收集各factor并用metrics_counter验证记入成功或失败。
+#  ？是否需要publish当前的counter?或者publish counter成功/失败?

Diff for: evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp

@@ -47,6 +47,8 @@ enum class Metric {
   modified_goal_longitudinal_deviation,
   modified_goal_lateral_deviation,
   modified_goal_yaw_deviation,
+  stop_count,
+  abnormal_stop_count,
   SIZE,
 };
 
@@ -74,7 +76,10 @@ static const std::unordered_map<std::string, Metric> str_to_metric = {
   {"obstacle_ttc", Metric::obstacle_ttc},
   {"modified_goal_longitudinal_deviation", Metric::modified_goal_longitudinal_deviation},
   {"modified_goal_lateral_deviation", Metric::modified_goal_lateral_deviation},
-  {"modified_goal_yaw_deviation", Metric::modified_goal_yaw_deviation}};
+  {"modified_goal_yaw_deviation", Metric::modified_goal_yaw_deviation},
+  {"stop_count", Metric::stop_count},
+  {"abnormal_stop_count", Metric::abnormal_stop_count}
+  };
 
 static const std::unordered_map<Metric, std::string> metric_to_str = {
   {Metric::curvature, "curvature"},
@@ -97,7 +102,10 @@ static const std::unordered_map<Metric, std::string> metric_to_str = {
   {Metric::obstacle_ttc, "obstacle_ttc"},
   {Metric::modified_goal_longitudinal_deviation, "modified_goal_longitudinal_deviation"},
   {Metric::modified_goal_lateral_deviation, "modified_goal_lateral_deviation"},
-  {Metric::modified_goal_yaw_deviation, "modified_goal_yaw_deviation"}};
+  {Metric::modified_goal_yaw_deviation, "modified_goal_yaw_deviation"},
+  {Metric::stop_count, "stop_count"},
+  {Metric::abnormal_stop_count, "abnormal_stop_count"}
+  };
 
 // Metrics descriptions
 static const std::unordered_map<Metric, std::string> metric_descriptions = {
@@ -121,7 +129,10 @@ static const std::unordered_map<Metric, std::string> metric_descriptions = {
   {Metric::obstacle_ttc, "Obstacle_time_to_collision[s]"},
   {Metric::modified_goal_longitudinal_deviation, "Modified_goal_longitudinal_deviation[m]"},
   {Metric::modified_goal_lateral_deviation, "Modified_goal_lateral_deviation[m]"},
-  {Metric::modified_goal_yaw_deviation, "Modified_goal_yaw_deviation[rad]"}};
+  {Metric::modified_goal_yaw_deviation, "Modified_goal_yaw_deviation[rad]"},
+  {Metric::stop_count, "Count of stop decisions made by each module"},
+  {Metric::abnormal_stop_count, "Count of abnormal stop decisions made by each module"}
+  };
 
 namespace details
 {

Diff for: evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_counter.hpp

@@ -0,0 +1,31 @@
+// 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__METRICS_COUNTER_HPP_
+#define AUTOWARE__PLANNING_EVALUATOR__METRICS_COUNTER_HPP_
+#include "autoware/planning_evaluator/metrics/metric.hpp"
+#include "autoware/planning_evaluator/parameters.hpp"
+#include "autoware_utils/math/accumulator.hpp"
+
+namespace planning_diagnostics
+{
+using autoware_utils::Accumulator;
+
+class MetricsCounter
+{
+}
+
+}  // namespace planning_diagnostics
+
+#endif  // AUTOWARE__PLANNING_EVALUATOR__METRICS_COUNTER_HPP_

Diff for: evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp

@@ -33,6 +33,8 @@
 #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include <autoware_internal_debug_msgs/msg/float64_stamped.hpp>
+#include <autoware_internal_planning_msgs/msg/planning_factor.hpp>
+#include <autoware_internal_planning_msgs/msg/planning_factor_array.hpp>
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
 #include <tier4_metric_msgs/msg/metric.hpp>
 #include <tier4_metric_msgs/msg/metric_array.hpp>
@@ -41,6 +43,8 @@
 #include <deque>
 #include <memory>
 #include <string>
+#include <unordered_map>
+#include <unordered_set>
 #include <vector>
 namespace planning_diagnostics
 {
@@ -54,6 +58,8 @@ using MetricMsg = tier4_metric_msgs::msg::Metric;
 using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray;
 using nav_msgs::msg::Odometry;
 using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin;
+using autoware_internal_planning_msgs::msg::PlanningFactor;
+using autoware_internal_planning_msgs::msg::PlanningFactorArray;
 using autoware_planning_msgs::msg::LaneletRoute;
 using geometry_msgs::msg::AccelWithCovarianceStamped;
 /**
@@ -114,6 +120,10 @@ class PlanningEvaluatorNode : public rclcpp::Node
   void AddKinematicStateMetricMsg(
     const AccelWithCovarianceStamped & accel_stamped, const Odometry::ConstSharedPtr ego_state_ptr);
 
+  void AddStopCountMetricMsg(
+    const PlanningFactorArray::ConstSharedPtr & planning_factors,
+    const Odometry::ConstSharedPtr ego_state_ptr, const std::string & module_name);
+
 private:
   static bool isFinite(const TrajectoryPoint & p);
 

Diff for: evaluator/autoware_planning_evaluator/package.xml

@@ -14,9 +14,11 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>autoware_internal_planning_msgs</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_planning_msgs</depend>
+  <depend>autoware_planning_factor_interface</depend>
   <depend>autoware_route_handler</depend>
   <depend>autoware_utils</depend>
   <depend>autoware_vehicle_info_utils</depend>

Diff for: evaluator/autoware_planning_evaluator/src/metrics_counter.cpp

@@ -0,0 +1,15 @@
+// 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.
+
+#include "autoware/planning_evaluator/metrics_counter.hpp"