refactor(planning_evaluator): rename and add lateral trajectory displ…

…acement metrics

Signed-off-by: kyoichi-sugahara <[email protected]>

evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml

@@ -14,7 +14,8 @@
       - lateral_deviation
       - yaw_deviation
       - velocity_deviation
-      - trajectory_lateral_displacement
+      - lateral_trajectory_displacement_local
+      - lateral_trajectory_displacement_lookahead
       - stability
       - stability_frechet
       - obstacle_distance

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

@@ -45,7 +45,7 @@ Accumulator<double> calcLateralDeviation(const Trajectory & ref, const Trajector
  * @param [in] base_pose base pose
  * @return calculated statistics
  */
-Accumulator<double> calcLateralTrajectoryDisplacement(
+Accumulator<double> calcLocalLateralTrajectoryDisplacement(
   const Trajectory & prev, const Trajectory & traj, const Pose & base_pose);
 
 /**

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

@@ -37,10 +37,10 @@ enum class Metric {
   lateral_deviation,
   yaw_deviation,
   velocity_deviation,
-  lateral_trajectory_displacement,
+  lateral_trajectory_displacement_local,
+  lateral_trajectory_displacement_lookahead,
   stability,
   stability_frechet,
-  trajectory_lateral_displacement,
   obstacle_distance,
   obstacle_ttc,
   modified_goal_longitudinal_deviation,
@@ -64,10 +64,10 @@ static const std::unordered_map<std::string, Metric> str_to_metric = {
   {"lateral_deviation", Metric::lateral_deviation},
   {"yaw_deviation", Metric::yaw_deviation},
   {"velocity_deviation", Metric::velocity_deviation},
-  {"lateral_trajectory_displacement", Metric::lateral_trajectory_displacement},
+  {"lateral_trajectory_displacement_local", Metric::lateral_trajectory_displacement_local},
+  {"lateral_trajectory_displacement_lookahead", Metric::lateral_trajectory_displacement_lookahead},
   {"stability", Metric::stability},
   {"stability_frechet", Metric::stability_frechet},
-  {"trajectory_lateral_displacement", Metric::trajectory_lateral_displacement},
   {"obstacle_distance", Metric::obstacle_distance},
   {"obstacle_ttc", Metric::obstacle_ttc},
   {"modified_goal_longitudinal_deviation", Metric::modified_goal_longitudinal_deviation},
@@ -86,10 +86,10 @@ static const std::unordered_map<Metric, std::string> metric_to_str = {
   {Metric::lateral_deviation, "lateral_deviation"},
   {Metric::yaw_deviation, "yaw_deviation"},
   {Metric::velocity_deviation, "velocity_deviation"},
-  {Metric::lateral_trajectory_displacement, "lateral_trajectory_displacement"},
+  {Metric::lateral_trajectory_displacement_local, "lateral_trajectory_displacement_local"},
+  {Metric::lateral_trajectory_displacement_lookahead, "lateral_trajectory_displacement_lookahead"},
   {Metric::stability, "stability"},
   {Metric::stability_frechet, "stability_frechet"},
-  {Metric::trajectory_lateral_displacement, "trajectory_lateral_displacement"},
   {Metric::obstacle_distance, "obstacle_distance"},
   {Metric::obstacle_ttc, "obstacle_ttc"},
   {Metric::modified_goal_longitudinal_deviation, "modified_goal_longitudinal_deviation"},
@@ -109,10 +109,10 @@ static const std::unordered_map<Metric, std::string> metric_descriptions = {
   {Metric::lateral_deviation, "Lateral_deviation[m]"},
   {Metric::yaw_deviation, "Yaw_deviation[rad]"},
   {Metric::velocity_deviation, "Velocity_deviation[m/s]"},
-  {Metric::lateral_trajectory_displacement, "Nearest Pose Lateral Deviation[m]"},
+  {Metric::lateral_trajectory_displacement_local, "Nearest Pose Lateral Deviation[m]"},
+  {Metric::lateral_trajectory_displacement_lookahead, "Kateral_Offset_Over_Distance_Ahead[m]"},
   {Metric::stability, "Stability[m]"},
   {Metric::stability_frechet, "StabilityFrechet[m]"},
-  {Metric::trajectory_lateral_displacement, "Trajectory_lateral_displacement[m]"},
   {Metric::obstacle_distance, "Obstacle_distance[m]"},
   {Metric::obstacle_ttc, "Obstacle_time_to_collision[s]"},
   {Metric::modified_goal_longitudinal_deviation, "Modified_goal_longitudinal_deviation[m]"},

evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp

@@ -45,7 +45,7 @@ Accumulator<double> calcLateralDeviation(const Trajectory & ref, const Trajector
   return stat;
 }
 
-Accumulator<double> calcLateralTrajectoryDisplacement(
+Accumulator<double> calcLocalLateralTrajectoryDisplacement(
   const Trajectory & prev, const Trajectory & traj, const Pose & ego_pose)
 {
   Accumulator<double> stat;

evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp

@@ -50,8 +50,11 @@ std::optional<Accumulator<double>> MetricsCalculator::calculate(
       return metrics::calcYawDeviation(reference_trajectory_, traj);
     case Metric::velocity_deviation:
       return metrics::calcVelocityDeviation(reference_trajectory_, traj);
-    case Metric::lateral_trajectory_displacement:
-      return metrics::calcLateralTrajectoryDisplacement(previous_trajectory_, traj, ego_pose_);
+    case Metric::lateral_trajectory_displacement_local:
+      return metrics::calcLocalLateralTrajectoryDisplacement(previous_trajectory_, traj, ego_pose_);
+    case Metric::trajectory_lateral_displacement_lookahead:
+      return metrics::calcTrajectoryLateralDisplacement(
+        previous_trajectory_, traj, ego_odometry_, parameters.trajectory.evaluation_time_s);
     case Metric::stability_frechet:
       return metrics::calcFrechetDistance(
         metrics::utils::get_lookahead_trajectory(
@@ -68,9 +71,6 @@ std::optional<Accumulator<double>> MetricsCalculator::calculate(
         metrics::utils::get_lookahead_trajectory(
           traj, ego_pose_, parameters.trajectory.lookahead.max_dist_m,
           parameters.trajectory.lookahead.max_time_s));
-    case Metric::trajectory_lateral_displacement:
-      return metrics::calcTrajectoryLateralDisplacement(
-        previous_trajectory_, traj, ego_odometry_, parameters.trajectory.evaluation_time_s);
     case Metric::obstacle_distance:
       return metrics::calcDistanceToObstacle(dynamic_objects_, traj);
     case Metric::obstacle_ttc: