From b0625af68899f8989a49e9e699001acccb8a8d0d Mon Sep 17 00:00:00 2001 From: Kazunori-Nakajima <169115284+Kazunori-Nakajima@users.noreply.github.com> Date: Thu, 19 Dec 2024 19:54:04 +0900 Subject: [PATCH] feat(planning_evaluator): add lateral trajectory displacement metrics (#9674) * feat(planning_evaluator): add nearest pose deviation msg Signed-off-by: Kasunori-Nakajima * update comment contents Signed-off-by: Kasunori-Nakajima * update variable name Signed-off-by: Kasunori-Nakajima * Revert "update variable name" This reverts commit ee427222fcbd2a18ffbc20fecca3ad557f527e37. Signed-off-by: Kasunori-Nakajima * move lateral_trajectory_displacement position Signed-off-by: Kasunori-Nakajima * prev.dist -> prev_lateral_deviation Signed-off-by: Kasunori-Nakajima --------- Signed-off-by: Kasunori-Nakajima --- .../config/planning_evaluator.param.yaml | 1 + .../metrics/deviation_metrics.hpp | 10 ++++++++++ .../planning_evaluator/metrics/metric.hpp | 4 ++++ .../autoware_planning_evaluator.schema.json | 1 + .../src/metrics/deviation_metrics.cpp | 18 ++++++++++++++++++ .../src/metrics_calculator.cpp | 2 ++ 6 files changed, 36 insertions(+) diff --git a/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml b/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml index 14c1bcc6beea4..e00851de63b9c 100644 --- 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/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp index 59866c96ad702..0e08398ffa87e 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp @@ -38,6 +38,16 @@ using geometry_msgs::msg::Pose; */ Accumulator 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 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 index c0313ed0727dd..13dad65b239b1 100644 --- 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 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_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_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 index 1ef3874a0dcbc..263bd374a7e45 100644 --- 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 index 7e2217a49ef87..ffb56baf29f17 100644 --- 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 calcLateralDeviation(const Trajectory & ref, const Trajector return stat; } +Accumulator calcLateralTrajectoryDisplacement( + const Trajectory & prev, const Trajectory & traj, const Pose & ego_pose) +{ + Accumulator 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 calcYawDeviation(const Trajectory & ref, const Trajectory & traj) { Accumulator stat; diff --git a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp index 6e057bcc9fc3d..b7676c2fdab6c 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp @@ -49,6 +49,8 @@ std::optional> 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(