fix(trajectory_validator): replace is_feasible boolean to risk level … (#3358)

* fix(trajectory_validator): replace is_feasible boolean to risk level check

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* use value comparison

Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com>

* fix: rename function name

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix: docstring

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

---------

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com>

Author: zulfaqar-azmi-t4

control/autoware_control_validator/src/control_validator.cpp

@@ -174,7 +174,7 @@ void VelocityValidator::validate(
       if (is_stopped || std::abs(v_vel) < params_.rolling_back_velocity) {
         return false;
       }
-  
+
       if (std::abs(t_vel) < stopped_vel_th) {
         return std::signbit(v_vel);
       }

planning/autoware_trajectory_validator/README.md

@@ -9,7 +9,7 @@ This package provides a pluginlib-based C++ library for evaluating candidate tra
 For each input `CandidateTrajectories` message the library runs every loaded plugin against every trajectory:
 
 1. **Plugin evaluation**: each plugin's `is_feasible()` is called with the trajectory points and the current `ValidatorContext` (odometry, predicted objects, acceleration, HD map, traffic light states).
-2. **Feasibility decision**: a trajectory survives if every plugin listed in `filter_names` returns `is_feasible = true`. Plugins listed in `shadow_mode_filter_names` are evaluated and reported but never remove a trajectory.
+2. **Feasibility decision**: each plugin reports metrics with a risk level. A plugin accepts the trajectory when the worst risk level of its metrics is `SAFE`, `LOW_CAUTION`, or `HIGH_CAUTION`, and rejects it on `DANGER` or `FATAL`. A trajectory survives if every plugin listed in `filter_names` accepts it. Plugins listed in `shadow_mode_filter_names` are evaluated and reported but never remove a trajectory.
 3. **Diagnostics**: `TrajectoryValidatorDiagnostic` selects the best candidate (the one with the least severe action across all active filters), resolves the `(filter_name, action)` pair to a named `DiagnosticStatus` via the `configured_actions` parameter, and publishes every tracked status every cycle (active ones at their level, inactive ones at `OK` to prevent staleness). Shadow-mode filters never contribute to the action aggregation. See [docs/TrajectoryValidatorDiagnostic.md](docs/TrajectoryValidatorDiagnostic.md) for details.
 
 ## Built-in Plugins

planning/autoware_trajectory_validator/include/autoware/trajectory_validator/detail/risk_utils.hpp

@@ -54,5 +54,15 @@ inline RiskLevelType worst_risk_level(const std::vector<MetricReport> & metrics)
   }
   return worst;
 }
+
+/**
+ * @brief Returns a boolean indicating the trajectory is feasible based on risk level.
+ * @param metrics Worst risk level.
+ * @return True if risk level is less or equal to HIGH_CAUTION.
+ */
+inline bool is_feasible(const RiskLevelType risk_level)
+{
+  return risk_level <= RiskLevel::HIGH_CAUTION;
+}
 }  // namespace autoware::trajectory_validator
 #endif  // AUTOWARE__TRAJECTORY_VALIDATOR__DETAIL__RISK_UTILS_HPP_

planning/autoware_trajectory_validator/include/autoware/trajectory_validator/validator_interface.hpp

@@ -45,7 +45,6 @@ using autoware_trajectory_validator::msg::RiskLevel;
 /** @brief Result of a single plugin's feasibility check. */
 struct ValidationResult
 {
-  bool is_feasible{true};
   std::vector<MetricReport> metrics;
   PlanningFactorArray planning_factors{};
 };

planning/autoware_trajectory_validator/src/detail/trajectory_validator.cpp

@@ -169,12 +169,13 @@ std::pair<PluginEvaluation, RiskLevel> TrajectoryValidator::summarize_feasibilit
   }
 
   const auto & val = res.value();
-  evaluation.is_feasible = val.is_feasible;
+  risk_level.level = worst_risk_level(val.metrics);
+  evaluation.is_feasible = is_feasible(risk_level.level);
+
   if (!evaluation.is_feasible) {
     evaluation.reason = "Found failed metrics";
   }
 
-  risk_level.level = worst_risk_level(val.metrics);
   return {evaluation, risk_level};
 }
 

planning/autoware_trajectory_validator/src/filters/safety/collision_check_filter/collision_check_filter.cpp

@@ -157,7 +157,6 @@ CollisionCheckFilter::result_t CollisionCheckFilter::is_feasible(
     debug_markers_, global_params_.time_resolution);
 
   return ValidationResult{
-    calc_worst_risk({drac_artifact.risk, rss_artifact.risk}) < RiskLevel::DANGER,
     generate_metric_reports(drac_artifact, rss_artifact), std::move(planning_factors)};
 }
 

planning/autoware_trajectory_validator/src/filters/safety/point_cloud_collision_check/point_cloud_collision_check_filter.cpp

@@ -14,6 +14,7 @@
 
 #include "point_cloud_collision_check_filter.hpp"
 
+#include <utility>
 #include <vector>
 
 namespace autoware::trajectory_validator::plugin::safety
@@ -47,13 +48,14 @@ std::vector<pcc::StopObstacle> PointCloudCollisionCheckFilter::calc_obstacle_sto
   return {};
 }
 
-bool PointCloudCollisionCheckFilter::judge_stop_feasibility(
+RiskLevel PointCloudCollisionCheckFilter::judge_stop_risk(
   [[maybe_unused]] const std::vector<pcc::StopObstacle> & stop_obstacles,
   [[maybe_unused]] const geometry_msgs::msg::Twist & twist) const
 {
-  // It is not currently implemented. always return true.
-  bool is_feasible = true;
-  return is_feasible;
+  // It is not currently implemented. always return SAFE.
+  RiskLevel risk_level;
+  risk_level.level = RiskLevel::SAFE;
+  return risk_level;
 }
 
 PointCloudCollisionCheckFilter::result_t PointCloudCollisionCheckFilter::is_feasible(
@@ -70,10 +72,15 @@ PointCloudCollisionCheckFilter::result_t PointCloudCollisionCheckFilter::is_feas
 
   const auto stop_obstacles = calc_obstacle_stop(candidate_trajectory.points);
 
-  ValidationResult result{};
-  result.is_feasible = judge_stop_feasibility(stop_obstacles, context.odometry->twist.twist);
+  std::vector<MetricReport> metrics{
+    autoware_trajectory_validator::build<MetricReport>()
+      .validator_name(get_name())
+      .validator_category(category())
+      .metric_name("point_cloud_stop_feasibility")
+      .metric_value(static_cast<double>(stop_obstacles.size()))
+      .risk(judge_stop_risk(stop_obstacles, context.odometry->twist.twist))};
 
-  return result;
+  return ValidationResult{std::move(metrics)};
 }
 
 void PointCloudCollisionCheckFilter::update_parameters(const validator::Params & params)

planning/autoware_trajectory_validator/src/filters/safety/point_cloud_collision_check/point_cloud_collision_check_filter.hpp

@@ -43,7 +43,7 @@ class PointCloudCollisionCheckFilter final : public plugin::ValidatorInterface
 
 private:
   /// @brief 評価に必要な入力が揃っているかを判定する。
-  /// false のとき is_feasible は評価せず feasible（ValidationResult{}）を返す。
+  /// false のとき is_feasible は評価せず、メトリクスなし（ValidationResult{}）を返す。
   bool is_available_data(
     const CandidateTrajectory & candidate_trajectory, const FilterContext & context) const;
 
@@ -60,8 +60,8 @@ class PointCloudCollisionCheckFilter final : public plugin::ValidatorInterface
   std::vector<point_cloud_collision_check::StopObstacle> calc_obstacle_stop(
     const std::vector<TrajectoryPoint> & raw_trajectory_points);
 
-  /// @brief 停止対象から停止可否を判定する。未実装のため現状は常に true を返す。
-  bool judge_stop_feasibility(
+  /// @brief 停止対象から停止可否のリスクレベルを判定する。未実装のため現状は常に SAFE を返す。
+  RiskLevel judge_stop_risk(
     const std::vector<point_cloud_collision_check::StopObstacle> & stop_obstacles,
     const geometry_msgs::msg::Twist & twist) const;
 

planning/autoware_trajectory_validator/src/filters/safety/trajectory_feasibility_filter.cpp

@@ -177,17 +177,16 @@ TrajectoryFeasibilityFilter::result_t TrajectoryFeasibilityFilter::is_feasible(
     return tl::make_unexpected("Vehicle info not set");
   }
 
-  // NOTE: Feasibility decision logic might be more complex in the future, but for now we just
-  // check all constraints and return false if any are violated
-  bool is_feasible = true;
+  // Each checker reports its own risk level. The core derives feasibility from the worst risk
+  // level of these metrics, so a violated constraint reports HIGH_CAUTION and does not reject the
+  // trajectory on its own.
   std::vector<MetricReport> metrics;
   for (const auto & checker : checkers_) {
     auto report = (this->*checker)(traj_points, context);
-    is_feasible &= report.risk.level == RiskLevel::SAFE;
     metrics.push_back(report);
   }
 
-  return ValidationResult{is_feasible, std::move(metrics)};
+  return ValidationResult{std::move(metrics)};
 }
 
 MetricReport TrajectoryFeasibilityFilter::check_speed(

planning/autoware_trajectory_validator/src/filters/safety/uncrossable_boundary_departure.cpp

@@ -75,8 +75,9 @@ UncrossableBoundaryDepartureFilter::result_t UncrossableBoundaryDepartureFilter:
                                       .metric_value(status.lat_dist_to_uncrossable_bound)
                                       .risk(risk_level)};
 
-  // Only a CRITICAL departure rejects the trajectory. NEAR_BOUNDARY is advisory.
-  return ValidationResult{!is_critical_departure, std::move(metrics)};
+  // Only a CRITICAL departure rejects the trajectory, because it reports DANGER. NEAR_BOUNDARY
+  // reports LOW_CAUTION and is advisory.
+  return ValidationResult{std::move(metrics)};
 }
 
 void UncrossableBoundaryDepartureFilter::update_parameters(const validator::Params & params)