fix(planning_evaluator): redesign DRAC algorithm

evaluator/autoware_planning_evaluator/src/obstacle_metrics_calculator.cpp

@@ -206,233 +206,286 @@
       std::sqrt(obstacle_twist.x * obstacle_twist.x + obstacle_twist.y * obstacle_twist.y);
     bool is_obstacle_traj_no_overlapping_ego_traj = false;
 
     std::string object_uuid = metrics::utils::uuid_to_string(object.object_id);
-    ;
     // ------------------------------------------------------------------------------------------------
     // 2. roughly check if obstacle trajectory is no overlapping with ego trajectory.
 
     const auto obstacle_polygon = autoware_utils::to_polygon2d(obstacle_pose, object.shape);
     const auto [obstacle_margin_min, obstacle_margin_max] =
       metrics::utils::calculate_point_to_polygon_boundary_distances(
         obstacle_pose, obstacle_polygon);
 
     const auto & ego_initial_pose = ego_trajectory_points_.front().pose;
     const auto & ego_final_time = ego_trajectory_points_.back().time_from_start_s;
 
     const double obstacle_max_reachable_distance = obstacle_velocity * ego_final_time;
     const double obstacle_ego_distance = calc_distance2d(ego_initial_pose, obstacle_pose);
     is_obstacle_traj_no_overlapping_ego_traj =
       (obstacle_ego_distance >= obstacle_max_reachable_distance + ego_max_reachable_distance_ +
                                   ego_margin_max + obstacle_margin_max +
                                   parameters.collision_thr_m);
 
     // ------------------------------------------------------------------------------------------------
     // 3. create obstacle trajectory points:
     //  - It creates the same number of points as the ego trajectory points with the same
     //  `time_from_start_s`.
     //  - But if there is stop point in the ego trajectory, obstacle trajectory points after that
     //  timing are not created.
 
     obstacle_trajectory_points_.emplace_back(obstacle_pose, obstacle_velocity, 0, 0);
     if (!is_obstacle_traj_no_overlapping_ego_traj) {
       if (object.kinematics.predicted_paths.empty()) {
         // Create duplicate obstacle trajectory points with same `time_from_start_s` as ego
         // trajectory points.
         for (size_t i = 1; i < ego_trajectory_points_.size(); ++i) {
           const double ego_time = ego_trajectory_points_[i].time_from_start_s;
           if (!std::isfinite(ego_time)) {
             break;
           }
           obstacle_trajectory_points_.emplace_back(obstacle_pose, obstacle_velocity, ego_time, 0.0);
         }
       } else {
         // Create obstacle trajectory points based on the predicted path with highest confidence.
         const auto & predicted_paths = object.kinematics.predicted_paths;
         auto max_confidence_iter = std::max_element(
           predicted_paths.begin(), predicted_paths.end(),
           [](const auto & a, const auto & b) { return a.confidence < b.confidence; });
         const auto & obstacle_path = max_confidence_iter->path;
 
         // initialize reference point and the point right after next reference point. Here
         // `reference_point` is the point right before the current ego trajectory point.
         ObstacleTrajectoryPoint reference_point = obstacle_trajectory_points_.front();
         size_t next_reference_point_idx = 1;
         double next_reference_point_time_from_start_s = std::numeric_limits<double>::max();
         double next_reference_point_distance_from_start_m = std::numeric_limits<double>::max();
 
         if (next_reference_point_idx < obstacle_path.size()) {
           next_reference_point_distance_from_start_m =
             calc_distance2d(reference_point.pose, obstacle_path[next_reference_point_idx]) +
             reference_point.distance_from_start_m;
           next_reference_point_time_from_start_s =
             next_reference_point_distance_from_start_m / obstacle_velocity;
         }
 
         // create obstacle trajectory points based on the corresponding ego trajectory points.
         for (size_t i = 1; i < ego_trajectory_points_.size(); ++i) {
           const auto & ego_trajectory_point = ego_trajectory_points_[i];
           const double ego_time = ego_trajectory_point.time_from_start_s;
 
           if (!std::isfinite(ego_trajectory_point.time_from_start_s)) {
             break;
           }
 
           // Update the reference point and next reference point.
           while (next_reference_point_idx < obstacle_path.size() &&
                  next_reference_point_time_from_start_s < ego_time) {
             reference_point = ObstacleTrajectoryPoint(
               obstacle_path[next_reference_point_idx], obstacle_velocity,
               next_reference_point_time_from_start_s, next_reference_point_distance_from_start_m);
 
             ++next_reference_point_idx;
 
             if (next_reference_point_idx < obstacle_path.size()) {
               next_reference_point_distance_from_start_m =
                 calc_distance2d(reference_point.pose, obstacle_path[next_reference_point_idx]) +
                 reference_point.distance_from_start_m;
               next_reference_point_time_from_start_s =
                 next_reference_point_distance_from_start_m / obstacle_velocity;
             }
           }
 
           // insert new obstacle trajectory point based on the reference point and
           // `time_from_start_s` of the corresponding ego trajectory point.
           obstacle_trajectory_points_.emplace_back(
             reference_point, ego_time - reference_point.time_from_start_s);
         }
       }
     }
 
     // ------------------------------------------------------------------------------------------------
     // 4. calculate `obstacle_distance` metrics
 
     if (metrics_need_[Metric::obstacle_distance]) {
       double obstacle_distance = std::numeric_limits<double>::max();
       const auto & first_obstacle_point = obstacle_trajectory_points_.front();
 
       for (auto & ego_trajectory_point : ego_trajectory_points_) {
         const double dist = calc_distance2d(ego_trajectory_point.pose, first_obstacle_point.pose);
         obstacle_distance = std::min(obstacle_distance, dist);
       }
 
       // add to metric statistics
       Accumulator<double> obstacle_distance_metric;
       obstacle_distance_metric.add(obstacle_distance);
       obstacle_metrics_[Metric::obstacle_distance].emplace_back(
         object_uuid, obstacle_distance_metric);
     }
 
     // ------------------------------------------------------------------------------------------------
     // 5. check overlap and collision between ego trajectory and obstacle trajectory
 
     for (size_t i = 0; i < obstacle_trajectory_points_.size(); ++i) {
       auto & obstacle_trajectory_point = obstacle_trajectory_points_[i];
 
       for (size_t j = 0; j < ego_trajectory_points_.size(); ++j) {
         const auto & ego_trajectory_point = ego_trajectory_points_[j];
 
         if (!std::isfinite(ego_trajectory_point.time_from_start_s)) {
           break;
         }
 
         const double dist =
           calc_distance2d(ego_trajectory_point.pose, obstacle_trajectory_point.pose);
         bool is_overlapping = false;
         if (dist <= (ego_margin_min + obstacle_margin_min + parameters.collision_thr_m)) {
           is_overlapping = true;
         } else if (dist > ego_margin_max + obstacle_margin_max + parameters.collision_thr_m) {
           is_overlapping = false;
         } else {
           obstacle_trajectory_point.setPolygon(object.shape);
           is_overlapping = metrics::utils::polygon_intersects(
             ego_trajectory_point.polygon.value(), obstacle_trajectory_point.polygon.value());
         }
 
         if (is_overlapping) {
           obstacle_trajectory_point.is_overlapping_with_ego_trajectory = true;
           obstacle_trajectory_point.is_collision_with_ego_trajectory =
             obstacle_trajectory_point.is_collision_with_ego_trajectory ||
             std::abs(
               obstacle_trajectory_point.time_from_start_s -
               ego_trajectory_point.time_from_start_s) < 1e-6;
           obstacle_trajectory_point.first_overlapping_ego_trajectory_index =
             std::min(obstacle_trajectory_point.first_overlapping_ego_trajectory_index, j);
           obstacle_trajectory_point.last_overlapping_ego_trajectory_index =
             std::max(obstacle_trajectory_point.last_overlapping_ego_trajectory_index, j);
         }
       }
     }
 
     // ------------------------------------------------------------------------------------------------
     // 6. get `obstacle_ttc` metrics
 
     if (metrics_need_[Metric::obstacle_ttc]) {
       for (auto & obstacle_trajectory_point : obstacle_trajectory_points_) {
         if (obstacle_trajectory_point.is_collision_with_ego_trajectory) {
           Accumulator<double> obstacle_ttc_metric;
           obstacle_ttc_metric.add(obstacle_trajectory_point.time_from_start_s);
           obstacle_metrics_[Metric::obstacle_ttc].emplace_back(object_uuid, obstacle_ttc_metric);
           break;
         }
       }
     }
 
     // ------------------------------------------------------------------------------------------------
     // 7. get `obstacle_pet` metrics
 
     if (metrics_need_[Metric::obstacle_pet]) {
       double obstacle_pet = std::numeric_limits<double>::max();
       for (size_t i = 0; i < obstacle_trajectory_points_.size(); ++i) {
         const auto & obstacle_trajectory_point = obstacle_trajectory_points_[i];
         if (obstacle_trajectory_point.is_overlapping_with_ego_trajectory) {
           const size_t ego_first_overlap_idx =
             obstacle_trajectory_point.first_overlapping_ego_trajectory_index;
           if (ego_first_overlap_idx == 0) {  // skip if overlap at t=0 (pet = 0)
             continue;
           }
           const double point_pet =
             ego_trajectory_points_[ego_first_overlap_idx - 1].time_from_start_s -
             obstacle_trajectory_point.time_from_start_s;
           if (point_pet >= 0) {  // Only consider the case where obstacle leaves before ego arrives
                                  // （PET > 0） and occlusion occurs （PET = 0）
             obstacle_pet = std::min(obstacle_pet, point_pet);
           }
         }
       }
 
       if (obstacle_pet != std::numeric_limits<double>::max()) {
         Accumulator<double> obstacle_pet_metric;
         obstacle_pet_metric.add(obstacle_pet);
         obstacle_metrics_[Metric::obstacle_pet].emplace_back(object_uuid, obstacle_pet_metric);
       }
     }
 
     // ------------------------------------------------------------------------------------------------
-    // 8. get `obstacle_drac` metrics
-
-    if (metrics_need_[Metric::obstacle_drac]) {
+    // 8. Obstacle DRAC (deceleration rate to avoid a crash)
+    //
+    // Model: from evaluation time t = 0, ego moves along the reference direction with constant
+    // deceleration d (v(t) = v0 - d*t, s(t) = v0*t - 0.5*d*t^2), where v0 is the first ego
+    // trajectory point speed.
+    //
+    // Each collision sample on the obstacle path is anchored to obstacle time t2
+    // (time_from_start_s at that sample). The arc-length cap s_cap is the distance_from_start_m
+    // of the last ego waypoint before the first polygon overlap (ego_before_collision): it is a
+    // discrete proxy for "how far along the ego path ego may reach by t2 without encroaching".
+    //
+    // Required deceleration is the maximum of:
+    //   - d_vel:  enough deceleration that v(t2) <= v2 (obstacle speed projected along ego
+    //   heading).
+    //   - d_pos:  enough deceleration that s(t2) <= s_cap, using the quadratic bound while v(t2) >=
+    //   0,
+    //             otherwise the bound from stopping exactly in distance s_cap.
+
+    if (
+      metrics_need_[Metric::obstacle_drac] &&
+      first_ego_point.velocity_mps > parameters.stop_velocity_mps) {
       double obstacle_drac = 0.0;
       const double ego_start_vel = first_ego_point.velocity_mps;
 
       for (size_t i = 1; i < obstacle_trajectory_points_.size(); ++i) {
         const auto & obstacle_trajectory_point = obstacle_trajectory_points_[i];
-        const auto & ego_trajectory_point = ego_trajectory_points_[i];
         if (!obstacle_trajectory_point.is_collision_with_ego_trajectory) continue;
 
-        // calculate ego_end_vel at the collision point needed for deceleration:
-        //  - ego decelerate max to stop.
-        //  - consider two cases of forward and backward.
-        const double yaw_diff = tf2::getYaw(ego_trajectory_point.pose.orientation) -
+        const size_t ego_first_overlap_idx =
+          obstacle_trajectory_point.first_overlapping_ego_trajectory_index;
+
+        // Skip overlap at t = 0 (ego already at the conflicting pose).
+        if (ego_first_overlap_idx == 0) {
+          continue;
+        }
+        const auto & ego_before_collision = ego_trajectory_points_[ego_first_overlap_idx - 1];
+
+        // Target speed v2 at t2: obstacle speed projected onto ego heading at the last safe ego
+        // pose. Clamp by sign of v0 so forward and reverse driving stay consistent.
+        const double yaw_diff = tf2::getYaw(ego_before_collision.pose.orientation) -
                                 tf2::getYaw(obstacle_trajectory_point.pose.orientation);
         double ego_end_vel = obstacle_trajectory_point.velocity_mps * std::cos(yaw_diff);
         ego_end_vel =
           ego_start_vel >= 0.0 ? std::max(ego_end_vel, 0.0) : std::min(ego_end_vel, 0.0);
 
-        // calculate DRAC
-        const double distance_to_collision = ego_trajectory_point.distance_from_start_m;
-        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));
+        // If the projected obstacle speed exceeds |v0|, slowing ego is not required in this
+        // direction.
+        if (std::abs(ego_end_vel) > std::abs(ego_start_vel)) {
+          continue;
+        }
+
+        // Arc-length cap from the last non-overlapping ego waypoint; obstacle clock t2 is the
+        // evaluation instant for both velocity and position constraints (see block comment above).
+        const double s_cap = ego_before_collision.distance_from_start_m;
+        const double t2 = obstacle_trajectory_point.time_from_start_s;
+
+        const double v0 = ego_start_vel;
+        const double v2 = ego_end_vel;
+
+        // Velocity bound at t2: v0 - d*t2 <= v2  =>  d >= (v0 - v2) / t2
+        const double d_vel = (v0 - v2) / t2;
+
+        // Position bound at t2 while v(t2) >= 0: v0*t2 - 0.5*d*t2^2 <= s_cap
+        //   =>  d >= 2*(v0*t2 - s_cap) / t2^2
+        const double d_quad = 2.0 * (v0 * t2 - s_cap) / (t2 * t2);
+
+        // Largest d for which v(t2) = v0 - d*t2 is still non-negative: d <= v0 / t2
+        const double d_time_stop_bound = v0 / t2;
+
+        // If d_quad implies stopping before t2 under the same constant d, use stopping in s_cap:
+        // 0 = v0^2 - 2*d*s_cap  =>  d = v0^2 / (2*s_cap)
+        const double d_stop_in_distance = (v0 * v0) / (2.0 * s_cap);
+
+        const double d_pos = d_quad <= d_time_stop_bound ? d_quad : d_stop_in_distance;
+
+        // Smallest constant deceleration from t = 0 satisfying both bounds at obstacle time t2.
+        const double point_drac = std::max(0.0, std::max(d_vel, d_pos));
+
+        obstacle_drac = std::max(obstacle_drac, point_drac);
       }
 
       // Add to metric statistics if we found any valid DRAC value