feat(autoware_planning_evaluator): new obstacle metrics (autowarefoundation#11761)

* tmp save

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

* remove some draft code

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

* add new implements

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

* polish code, need to update readme

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

* pre-commit

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

* update readme

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

* fix cppcheck

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

* fix unit test bug, and add test cases for ttc, drac.

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

* cry to fix ci building error

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

* refactor code

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

---------

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

Author: xtk8532704

evaluator/autoware_planning_evaluator/README.md

@@ -158,23 +158,48 @@ The following information are used to calculate metrics:
 
 Evaluate the safety of `T(0)` with respect to obstacles.
 
-Metrics are calculated and publish only when the node receives a trajectory.
+Metrics are calculated and published only when the node receives a trajectory and perception results.
 
 The following information are used to calculate metrics:
 
 - the trajectory `T(0)`.
 - the set of objects in the environment.
 
+By default, metrics are published for each object individually using the object's UUID as the identifier,
+along with the worst value across all objects. You can set `obstacle.worst_only` to `true` to publish only the worst value.
+
 #### Implemented metrics
 
-- **`obstacle_distance`**: Statistics of the distance between each object and the closest trajectory point.
-  - Sub-metrics to publish: `/mean`, `/min`, `/max`.
-  - Sub-metrics to output: The same as above but take the published data as data point instead of each trajectory point.
+- **`obstacle_distance`**: Distance from the object's center point to the ego's future trajectory (closest point).
+  - Parameters: None.
+  - Sub-metrics to publish (per object): `/{object_uuid}` or `/worst` (worst value across all objects).
+  - Sub-metrics to output: `/mean`, `/min`, `/max` for the published data.
 
-- **`obstacle_ttc`**: Statistics of the time-to-collision (TTC) of objects and ego. Predicted path of objects and ego trajectory are considered.
-  - Parameters: `obstacle.dist_thr_m` (distance margin to consider a collision occurs between object footprints and ego trajectory footprints).
-  - Sub-metrics to publish: `/mean`, `/min`, `/max`.
-  - Sub-metrics to output: The same as above but take the published data as data point instead of each trajectory point.
+- **`obstacle_ttc`**: Time To Collision (TTC) with the target object. Considers predicted paths of objects and ego trajectory.
+  - TTC is calculated for objects that will actually collide with ego (overlap at the same time).
+  - Parameters:
+    - `obstacle.collision_thr_m`: distance margin to consider a collision occurs between object footprints and ego trajectory footprints.
+    - `obstacle.use_ego_traj_vel`: if true, use planned trajectory velocity; otherwise use current ego velocity.
+    - `obstacle.stop_velocity_mps`: velocity threshold to consider the object or ego as static.
+    - `obstacle.min_time_interval_s`: minimum time interval for ego trajectory resampling.
+    - `obstacle.min_spatial_interval_m`: minimum spatial interval for ego trajectory resampling.
+  - Sub-metrics to publish (per object): `/{object_uuid}` or `/worst` (worst value across all objects).
+  - Sub-metrics to output: `/mean`, `/min`, `/max` for the published data.
+
+- **`obstacle_pet`**: Post Encroachment Time (PET) with the target object.
+  - PET is the time difference between the target object leaving the overlap region and ego entering it.
+  - Only calculated for objects whose future trajectory overlaps with ego's but do not collide at the same time.
+  - An object cannot have both TTC and PET simultaneously (if TTC exists, PET is zero).
+  - Parameters: Same as `obstacle_ttc`.
+  - Sub-metrics to publish (per object): `/{object_uuid}` or `/worst` (worst value across all objects).
+  - Sub-metrics to output: `/mean`, `/min`, `/max` for the published data.
+
+- **`obstacle_drac`**: Deceleration Rate to Avoid Collision (DRAC) with the target object.
+  - The minimum deceleration required to avoid collision with the target object.
+  - Published alongside TTC for potential collision targets.
+  - Parameters: Same as `obstacle_ttc`.
+  - Sub-metrics to publish (per object): `/{object_uuid}` or `/worst` (worst value across all objects).
+  - Sub-metrics to output: `/mean`, `/min`, `/max` for the published data.
 
 ### Modified Goal Metrics
 

evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml

@@ -21,6 +21,8 @@
       - stability_frechet
       - obstacle_distance
       - obstacle_ttc
+      - obstacle_pet
+      - obstacle_drac
       - modified_goal_longitudinal_deviation
       - modified_goal_lateral_deviation
       - modified_goal_yaw_deviation
@@ -48,6 +50,8 @@
       - stability_frechet
       - obstacle_distance
       - obstacle_ttc
+      - obstacle_pet
+      - obstacle_drac
       - modified_goal_longitudinal_deviation
       - modified_goal_lateral_deviation
       - modified_goal_yaw_deviation
@@ -63,8 +67,14 @@
         max_dist_m: 5.0 # [m] maximum distance from ego along the trajectory to use for calculation
         max_time_s: 3.0 # [s] maximum time ahead of ego along the trajectory to use for calculation
 
-    obstacle:
-      dist_thr_m: 0.0 # [m] distance between ego and the obstacle below which a collision is considered
+    obstacle: # parameters for obstacle metrics calculation
+      worst_only: false # if true, only the worst obstacle metric value is published
+      use_ego_traj_vel: false # if true, the planned trajectory velocity is used
+      collision_thr_m: 0.0 # [m] distance threshold to consider a collision occurs between object footprints and ego trajectory footprints
+      stop_velocity_mps: 0.2777 # [m/s] velocity threshold to consider the object or ego is static, used for speed up the calculation
+
+      min_time_interval_s: 0.05 # [s] minimum time interval between two successive points to use for ego trajectory resampling to save calculation.
+      min_spatial_interval_m: 0.1 # [m] minimum spatial interval between two successive points to use for ego trajectory resampling to save calculation.
 
     stop_decision:
       topic_prefix: /planning/planning_factors/ # topic prefix for planning factors

evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp

@@ -46,6 +46,8 @@ enum class Metric {
   stability_frechet,
   obstacle_distance,
   obstacle_ttc,
+  obstacle_pet,
+  obstacle_drac,
   modified_goal_longitudinal_deviation,
   modified_goal_lateral_deviation,
   modified_goal_yaw_deviation,
@@ -78,6 +80,8 @@ static const std::unordered_map<std::string, Metric> str_to_metric = {
   {"stability_frechet", Metric::stability_frechet},
   {"obstacle_distance", Metric::obstacle_distance},
   {"obstacle_ttc", Metric::obstacle_ttc},
+  {"obstacle_pet", Metric::obstacle_pet},
+  {"obstacle_drac", Metric::obstacle_drac},
   {"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},
@@ -105,6 +109,8 @@ static const std::unordered_map<Metric, std::string> metric_to_str = {
   {Metric::stability_frechet, "stability_frechet"},
   {Metric::obstacle_distance, "obstacle_distance"},
   {Metric::obstacle_ttc, "obstacle_ttc"},
+  {Metric::obstacle_pet, "obstacle_pet"},
+  {Metric::obstacle_drac, "obstacle_drac"},
   {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"},
@@ -132,7 +138,9 @@ static const std::unordered_map<Metric, std::string> metric_descriptions = {
   {Metric::stability, "Stability[m]"},
   {Metric::stability_frechet, "StabilityFrechet[m]"},
   {Metric::obstacle_distance, "Obstacle_distance[m]"},
-  {Metric::obstacle_ttc, "Obstacle_time_to_collision[s]"},
+  {Metric::obstacle_ttc, "Obstacle Time To Collision[s]"},
+  {Metric::obstacle_pet, "Obstacle Post Encroaching Time[s]"},
+  {Metric::obstacle_drac, "Deceleration Rate to Avoid Collision[m/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]"},

evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp

@@ -22,6 +22,12 @@
 #include "autoware_perception_msgs/msg/predicted_object.hpp"
 #include "autoware_planning_msgs/msg/trajectory.hpp"
 #include "autoware_planning_msgs/msg/trajectory_point.hpp"
+#include "unique_identifier_msgs/msg/uuid.hpp"
+
+#include <iomanip>
+#include <sstream>
+#include <string>
+#include <utility>
 
 namespace planning_diagnostics
 {
@@ -35,14 +41,30 @@ using autoware_planning_msgs::msg::Trajectory;
 using autoware_planning_msgs::msg::TrajectoryPoint;
 using geometry_msgs::msg::Pose;
 
+/**
+ * @brief convert UUID to string representation
+ * @param [in] uuid UUID message
+ * @return hex string representation of UUID
+ */
+inline std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & uuid)
+{
+  std::stringstream ss;
+  ss << std::hex << std::setfill('0');
+  for (const auto & byte : uuid.uuid) {
+    ss << std::setw(2) << static_cast<int>(byte);
+  }
+  return ss.str();
+}
+
 /**
  * @brief find the index in the trajectory at the given distance of the given index
  * @param [in] traj input trajectory
  * @param [in] curr_id index
  * @param [in] distance distance
  * @return index of the trajectory point at distance ahead of traj[curr_id]
  */
-size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance);
+size_t get_index_after_distance(
+  const Trajectory & traj, const size_t curr_id, const double distance);
 
 /**
  * @brief trim a trajectory from the current ego pose to some fixed time or distance
@@ -68,15 +90,27 @@ Trajectory get_lookahead_trajectory(
 double calc_lookahead_trajectory_distance(const Trajectory & traj, const Pose & ego_pose);
 
 /**
- * @brief calculate the distance between ego vehicle footprint and a predicted object
- * @param [in] local_ego_footprint ego vehicle footprint in local coordinates
- * @param [in] ego_pose current ego vehicle pose in world coordinates
- * @param [in] object predicted object with pose and shape information
- * @return minimum distance between ego footprint and object footprint in meters
+ * @brief Fast polygon intersection check using Separating Axis Theorem (SAT)
+ * @details Efficiently checks if two convex polygons intersect, faster than boost::geometry's
+ *          generic intersection check. Uses SAT with early exit for non-intersecting cases.
+ *
+ * @param [in] poly1 First polygon (typically ego vehicle footprint)
+ * @param [in] poly2 Second polygon (typically obstacle footprint)
+ * @return true if polygons intersect, false otherwise
+ */
+bool polygon_intersects(
+  const autoware_utils::Polygon2d & poly1, const autoware_utils::Polygon2d & poly2);
+
+/**
+ * @brief Calculate min and max distance from a pose to polygon edges
+ * @param [in] pose The reference pose (position is used as the point)
+ * @param [in] polygon The polygon to calculate distance to
+ * @return pair of (min_dist, max_dist) where:
+ *         - min_dist: Minimum distance from pose position to polygon edges (nearest edge segment)
+ *         - max_dist: Maximum distance from pose position to polygon vertices (farthest vertex)
  */
-double calc_ego_object_distance(
-  const autoware_utils::LinearRing2d & local_ego_footprint, const Pose & ego_pose,
-  const PredictedObject & object);
+std::pair<double, double> calculate_point_to_polygon_boundary_distances(
+  const Pose & pose, const autoware_utils::Polygon2d & polygon);
 
 }  // namespace utils
 }  // namespace metrics

evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp

@@ -44,6 +44,8 @@ enum class OutputMetric {
   stability_frechet,
   obstacle_distance,
   obstacle_ttc,
+  obstacle_pet,
+  obstacle_drac,
   modified_goal_longitudinal_deviation,
   modified_goal_lateral_deviation,
   modified_goal_yaw_deviation,
@@ -77,6 +79,8 @@ static const std::unordered_map<std::string, OutputMetric> str_to_output_metric
   {"stability_frechet", OutputMetric::stability_frechet},
   {"obstacle_distance", OutputMetric::obstacle_distance},
   {"obstacle_ttc", OutputMetric::obstacle_ttc},
+  {"obstacle_pet", OutputMetric::obstacle_pet},
+  {"obstacle_drac", OutputMetric::obstacle_drac},
   {"modified_goal_longitudinal_deviation", OutputMetric::modified_goal_longitudinal_deviation},
   {"modified_goal_lateral_deviation", OutputMetric::modified_goal_lateral_deviation},
   {"modified_goal_yaw_deviation", OutputMetric::modified_goal_yaw_deviation},
@@ -105,6 +109,8 @@ static const std::unordered_map<OutputMetric, std::string> output_metric_to_str
   {OutputMetric::stability_frechet, "stability_frechet"},
   {OutputMetric::obstacle_distance, "obstacle_distance"},
   {OutputMetric::obstacle_ttc, "obstacle_ttc"},
+  {OutputMetric::obstacle_pet, "obstacle_pet"},
+  {OutputMetric::obstacle_drac, "obstacle_drac"},
   {OutputMetric::modified_goal_longitudinal_deviation, "modified_goal_longitudinal_deviation"},
   {OutputMetric::modified_goal_lateral_deviation, "modified_goal_lateral_deviation"},
   {OutputMetric::modified_goal_yaw_deviation, "modified_goal_yaw_deviation"},
@@ -133,8 +139,10 @@ static const std::unordered_map<OutputMetric, std::string> output_metric_descrip
    "Statics of published lateral_trajectory_displacement_lookahead metrics"},
   {OutputMetric::stability, "Statics of published stability metrics"},
   {OutputMetric::stability_frechet, "Statics of published stability_frechet metrics"},
-  {OutputMetric::obstacle_distance, "Statics of published obstacle_distance metrics"},
-  {OutputMetric::obstacle_ttc, "Statics of published obstacle_ttc metrics"},
+  {OutputMetric::obstacle_distance, "Statics of worst published obstacle_distance metrics"},
+  {OutputMetric::obstacle_ttc, "Statics of worst published obstacle_ttc metrics"},
+  {OutputMetric::obstacle_pet, "Statics of worst published obstacle_pet metrics"},
+  {OutputMetric::obstacle_drac, "Statics of worst published obstacle_drac metrics"},
   {OutputMetric::modified_goal_longitudinal_deviation,
    "Statics of published modified_goal_longitudinal_deviation metrics"},
   {OutputMetric::modified_goal_lateral_deviation,

evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/output_metric.hpp

@@ -19,7 +19,6 @@
 
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 
-#include "autoware_perception_msgs/msg/predicted_objects.hpp"
 #include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp"
 #include "autoware_planning_msgs/msg/trajectory.hpp"
 #include "autoware_planning_msgs/msg/trajectory_point.hpp"
@@ -31,7 +30,6 @@
 namespace planning_diagnostics
 {
 using autoware::vehicle_info_utils::VehicleInfo;
-using autoware_perception_msgs::msg::PredictedObjects;
 using autoware_planning_msgs::msg::PoseWithUuidStamped;
 using autoware_planning_msgs::msg::Trajectory;
 using autoware_planning_msgs::msg::TrajectoryPoint;
@@ -54,12 +52,6 @@ class MetricsCalculator
         double max_time_s = 3.0;
       } lookahead;
     } trajectory;
-
-    struct
-    {
-      double dist_thr_m = 0.0;
-      double limit_min_accel = -2.5;
-    } obstacle;
   } parameters;  // struct Parameters for those metrics calculated by the MetricsCalculator
 
   MetricsCalculator() = default;
@@ -92,12 +84,6 @@ class MetricsCalculator
    */
   void setPreviousTrajectory(const Trajectory & traj);
 
-  /**
-   * @brief set the dynamic objects used to calculate obstacle metrics
-   * @param [in] traj input previous trajectory
-   */
-  void setPredictedObjects(const PredictedObjects & objects);
-
   /**
    * @brief set the ego pose
    * @param [in] traj input previous trajectory
@@ -115,7 +101,6 @@ class MetricsCalculator
   Trajectory reference_trajectory_lookahead_;
   Trajectory previous_trajectory_;
   Trajectory previous_trajectory_lookahead_;
-  PredictedObjects dynamic_objects_;
   geometry_msgs::msg::Pose ego_pose_;
   nav_msgs::msg::Odometry ego_odometry_;
   PoseWithUuidStamped modified_goal_;