fix(planning_evaluator): incorrect DRAC formula (autowarefoundation#12124)

* fix drac calculation

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

* fix unit test

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

---------

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

Author: xtk8532704

evaluator/autoware_planning_evaluator/src/obstacle_metrics_calculator.cpp

@@ -429,8 +429,8 @@ void ObstacleMetricsCalculator::ProcessObstaclesTrajectory()
 
         // calculate DRAC
         const double distance_to_collision = ego_trajectory_point.distance_from_start_m;
-        const double point_drac = (ego_end_vel * ego_end_vel - ego_start_vel * ego_start_vel) /
-                                  (2.0 * distance_to_collision + 1e-6);
+        const double point_drac =
+          std::pow(ego_end_vel - ego_start_vel, 2) / (2.0 * distance_to_collision + 1e-6);
         obstacle_drac = std::max(obstacle_drac, std::abs(point_drac));
       }
 

evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp

@@ -662,8 +662,8 @@ TEST_F(EvalTest, TestObstacleDRAC)
   const double ego_baselink_at_collision = ego_initial_x + ego_velocity * t_collision;
   const double ego_travel_distance = ego_baselink_at_collision - ego_initial_x;
 
-  const double expected_drac = std::abs(
-    (obj_velocity * obj_velocity - ego_velocity * ego_velocity) / (2.0 * ego_travel_distance));
+  const double expected_drac =
+    std::abs(std::pow(obj_velocity - ego_velocity, 2) / (2.0 * ego_travel_distance));
 
   // Create a moving obstacle (same direction as ego, but slower)
   Objects objs;