feat(planning_evaluator): add lateral trajectory displacement metrics (…

…#9674) * feat(planning_evaluator): add nearest pose deviation msg Signed-off-by: Kasunori-Nakajima <[email protected]> * update comment contents Signed-off-by: Kasunori-Nakajima <[email protected]> * update variable name Signed-off-by: Kasunori-Nakajima <[email protected]> * Revert "update variable name" This reverts commit ee42722. Signed-off-by: Kasunori-Nakajima <[email protected]> * move lateral_trajectory_displacement position Signed-off-by: Kasunori-Nakajima <[email protected]> * prev.dist -> prev_lateral_deviation Signed-off-by: Kasunori-Nakajima <[email protected]> --------- Signed-off-by: Kasunori-Nakajima <[email protected]>
autowarefoundation · Dec 19, 2024 · b0625af · b0625af
1 parent eef298b
commit b0625af
Show file tree

Hide file tree

Showing 6 changed files with 36 additions and 0 deletions.
diff --git a/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml b/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml
@@ -14,6 +14,7 @@
       - lateral_deviation
       - yaw_deviation
       - velocity_deviation
+      - lateral_trajectory_displacement
       - stability
       - stability_frechet
       - obstacle_distance

diff --git a/...ware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp b/...ware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp
@@ -38,6 +38,16 @@ using geometry_msgs::msg::Pose;
  */
 Accumulator<double> calcLateralDeviation(const Trajectory & ref, const Trajectory & traj);
 
+/**
+ * @brief calculate lateral trajectory displacement from the previous trajectory and the trajectory
+ * @param [in] prev previous trajectory
+ * @param [in] traj input trajectory
+ * @param [in] base_pose base pose
+ * @return calculated statistics
+ */
+Accumulator<double> calcLateralTrajectoryDisplacement(
+  const Trajectory & prev, const Trajectory & traj, const Pose & base_pose);
+
 /**
  * @brief calculate yaw deviation of the given trajectory from the reference trajectory
  * @param [in] ref reference trajectory

diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp
@@ -37,6 +37,7 @@ enum class Metric {
   lateral_deviation,
   yaw_deviation,
   velocity_deviation,
+  lateral_trajectory_displacement,
   stability,
   stability_frechet,
   obstacle_distance,
@@ -62,6 +63,7 @@ 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},
   {"stability", Metric::stability},
   {"stability_frechet", Metric::stability_frechet},
   {"obstacle_distance", Metric::obstacle_distance},
@@ -82,6 +84,7 @@ 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::stability, "stability"},
   {Metric::stability_frechet, "stability_frechet"},
   {Metric::obstacle_distance, "obstacle_distance"},
@@ -103,6 +106,7 @@ 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::stability, "Stability[m]"},
   {Metric::stability_frechet, "StabilityFrechet[m]"},
   {Metric::obstacle_distance, "Obstacle_distance[m]"},

diff --git a/evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json b/evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json
@@ -34,6 +34,7 @@
             "lateral_deviation",
             "yaw_deviation",
             "velocity_deviation",
+            "lateral_trajectory_displacement",
             "stability",
             "stability_frechet",
             "obstacle_distance",

diff --git a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp
@@ -45,6 +45,24 @@ Accumulator<double> calcLateralDeviation(const Trajectory & ref, const Trajector
   return stat;
 }
 
+Accumulator<double> calcLateralTrajectoryDisplacement(
+  const Trajectory & prev, const Trajectory & traj, const Pose & ego_pose)
+{
+  Accumulator<double> stat;
+
+  if (prev.points.empty() || traj.points.empty()) {
+    return stat;
+  }
+
+  const auto prev_lateral_deviation =
+    autoware::motion_utils::calcLateralOffset(prev.points, ego_pose.position);
+  const auto traj_lateral_deviation =
+    autoware::motion_utils::calcLateralOffset(traj.points, ego_pose.position);
+  const auto lateral_trajectory_displacement = traj_lateral_deviation - prev_lateral_deviation;
+  stat.add(lateral_trajectory_displacement);
+  return stat;
+}
+
 Accumulator<double> calcYawDeviation(const Trajectory & ref, const Trajectory & traj)
 {
   Accumulator<double> stat;

diff --git a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp
@@ -49,6 +49,8 @@ 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::stability_frechet:
       return metrics::calcFrechetDistance(
         getLookaheadTrajectory(