feat(planning_evaluator): add evaluation feature of trajectory latera…

…l displacement (autowarefoundation#9718)

* feat(planning_evaluator): add evaluation feature of trajectory lateral displacement

Signed-off-by: Kyoichi Sugahara <[email protected]>

* feat(metrics_calculator): implement lookahead trajectory calculation and remove deprecated method

Signed-off-by: Kyoichi Sugahara <[email protected]>

* fix(planning_evaluator): rename lateral_trajectory_displacement to trajectory_lateral_displacement for consistency

Signed-off-by: Kyoichi Sugahara <[email protected]>

---------

Signed-off-by: Kyoichi Sugahara <[email protected]>

evaluator/autoware_planning_evaluator/README.md

@@ -13,6 +13,7 @@ Metrics are calculated using the following information:
 - the previous trajectory `T(-1)`.
 - the _reference_ trajectory assumed to be used as the reference to plan `T(0)`.
 - the current ego pose.
+- the current ego odometry.
 - the set of objects in the environment.
 
 These information are maintained by an instance of class `MetricsCalculator`

evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml

@@ -14,7 +14,7 @@
       - lateral_deviation
       - yaw_deviation
       - velocity_deviation
-      - lateral_trajectory_displacement
+      - trajectory_lateral_displacement
       - stability
       - stability_frechet
       - obstacle_distance
@@ -25,6 +25,7 @@
 
     trajectory:
       min_point_dist_m: 0.1 # [m] minimum distance between two successive points to use for angle calculation
+      evaluation_time_s: 5.0 # [s] time duration for trajectory evaluation in seconds
       lookahead:
         max_dist_m: 5.0 # [m] maximum distance from ego along the trajectory to use for calculation
         max_time_s: 3.0 # [s] maximum time ahead of ego along the trajectory to use for calculation

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

@@ -40,6 +40,7 @@ enum class Metric {
   lateral_trajectory_displacement,
   stability,
   stability_frechet,
+  trajectory_lateral_displacement,
   obstacle_distance,
   obstacle_ttc,
   modified_goal_longitudinal_deviation,
@@ -66,6 +67,7 @@ static const std::unordered_map<std::string, Metric> str_to_metric = {
   {"lateral_trajectory_displacement", Metric::lateral_trajectory_displacement},
   {"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},
@@ -87,6 +89,7 @@ static const std::unordered_map<Metric, std::string> metric_to_str = {
   {Metric::lateral_trajectory_displacement, "lateral_trajectory_displacement"},
   {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,6 +112,7 @@ static const std::unordered_map<Metric, std::string> metric_descriptions = {
   {Metric::lateral_trajectory_displacement, "Nearest Pose Lateral Deviation[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/include/autoware/planning_evaluator/metrics/metrics_utils.hpp

@@ -25,6 +25,7 @@ namespace metrics
 namespace utils
 {
 using autoware_planning_msgs::msg::Trajectory;
+using geometry_msgs::msg::Pose;
 
 /**
  * @brief find the index in the trajectory at the given distance of the given index
@@ -35,6 +36,29 @@ using autoware_planning_msgs::msg::Trajectory;
  */
 size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance);
 
+/**
+ * @brief trim a trajectory from the current ego pose to some fixed time or distance
+ * @param [in] traj input trajectory to trim
+ * @param [in] max_dist_m [m] maximum distance ahead of the ego pose
+ * @param [in] max_time_s [s] maximum time ahead of the ego pose
+ * @return sub-trajectory starting from the ego pose and of maximum length max_dist_m, maximum
+ * duration max_time_s
+ */
+Trajectory get_lookahead_trajectory(
+  const Trajectory & traj, const Pose & ego_pose, const double max_dist_m, const double max_time_s);
+
+/**
+ * @brief calculate the total distance from ego position to the end of trajectory
+ * @details finds the nearest point to ego position on the trajectory and calculates
+ *          the cumulative distance by summing up the distances between consecutive points
+ *          from that position to the end of the trajectory.
+ *
+ * @param [in] traj input trajectory to calculate distance along
+ * @param [in] ego_pose current ego vehicle pose
+ * @return total distance from ego position to trajectory end in meters
+ */
+double calc_lookahead_trajectory_distance(const Trajectory & traj, const Pose & ego_pose);
+
 }  // namespace utils
 }  // namespace metrics
 }  // namespace planning_diagnostics

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

@@ -18,6 +18,7 @@
 #include "autoware/universe_utils/math/accumulator.hpp"
 
 #include "autoware_planning_msgs/msg/trajectory.hpp"
+#include <nav_msgs/msg/odometry.hpp>
 
 namespace planning_diagnostics
 {
@@ -42,6 +43,23 @@ Accumulator<double> calcFrechetDistance(const Trajectory & traj1, const Trajecto
  */
 Accumulator<double> calcLateralDistance(const Trajectory & traj1, const Trajectory & traj2);
 
+/**
+ * @brief calculate the total lateral displacement between two trajectories
+ * @details Evaluates the cumulative absolute lateral displacement by sampling points
+ *          along the first trajectory and measuring their offset from the second trajectory.
+ *          The evaluation section length is determined by the ego vehicle's velocity and
+ *          the specified evaluation time.
+ *
+ * @param traj1 first trajectory to compare
+ * @param traj2 second trajectory to compare against
+ * @param [in] ego_odom current ego vehicle odometry containing pose and velocity
+ * @param [in]  trajectory_eval_time_s time duration for trajectory evaluation in seconds
+ * @return statistical accumulator containing the total lateral displacement
+ */
+Accumulator<double> calcTrajectoryLateralDisplacement(
+  const Trajectory traj1, const Trajectory traj2, const nav_msgs::msg::Odometry & ego_odom,
+  const double trajectory_eval_time_s);
+
 }  // namespace metrics
 }  // namespace planning_diagnostics
 

evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp

@@ -23,6 +23,7 @@
 #include "autoware_planning_msgs/msg/trajectory.hpp"
 #include "autoware_planning_msgs/msg/trajectory_point.hpp"
 #include "geometry_msgs/msg/pose.hpp"
+#include <nav_msgs/msg/odometry.hpp>
 
 #include <optional>
 
@@ -74,7 +75,7 @@ class MetricsCalculator
    * @brief set the ego pose
    * @param [in] traj input previous trajectory
    */
-  void setEgoPose(const geometry_msgs::msg::Pose & pose);
+  void setEgoPose(const nav_msgs::msg::Odometry & ego_odometry);
 
   /**
    * @brief get the ego pose
@@ -83,23 +84,13 @@ class MetricsCalculator
   Pose getEgoPose();
 
 private:
-  /**
-   * @brief trim a trajectory from the current ego pose to some fixed time or distance
-   * @param [in] traj input trajectory to trim
-   * @param [in] max_dist_m [m] maximum distance ahead of the ego pose
-   * @param [in] max_time_s [s] maximum time ahead of the ego pose
-   * @return sub-trajectory starting from the ego pose and of maximum length max_dist_m, maximum
-   * duration max_time_s
-   */
-  Trajectory getLookaheadTrajectory(
-    const Trajectory & traj, const double max_dist_m, const double max_time_s) const;
-
   Trajectory reference_trajectory_;
   Trajectory reference_trajectory_lookahead_;
   Trajectory previous_trajectory_;
   Trajectory previous_trajectory_lookahead_;
   PredictedObjects dynamic_objects_;
   geometry_msgs::msg::Pose ego_pose_;
+  nav_msgs::msg::Odometry ego_odometry_;
   PoseWithUuidStamped modified_goal_;
 };  // class MetricsCalculator
 

evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/parameters.hpp

@@ -31,6 +31,7 @@ struct Parameters
   struct
   {
     double min_point_dist_m = 0.1;
+    double evaluation_time_s = 5.0;
     struct
     {
       double max_dist_m = 5.0;

evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp

@@ -12,8 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "autoware/motion_utils/trajectory/trajectory.hpp"
 #include "autoware/planning_evaluator/metrics/trajectory_metrics.hpp"
 #include "autoware/universe_utils/geometry/geometry.hpp"
+
 namespace planning_diagnostics
 {
 namespace metrics
@@ -23,6 +25,7 @@ namespace utils
 using autoware::universe_utils::calcDistance2d;
 using autoware_planning_msgs::msg::Trajectory;
 using autoware_planning_msgs::msg::TrajectoryPoint;
+using geometry_msgs::msg::Pose;
 
 size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance)
 {
@@ -41,6 +44,52 @@ size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, cons
   return target_id;
 }
 
+Trajectory get_lookahead_trajectory(
+  const Trajectory & traj, const Pose & ego_pose, const double max_dist_m, const double max_time_s)
+{
+  if (traj.points.empty()) {
+    return traj;
+  }
+
+  const auto ego_index =
+    autoware::motion_utils::findNearestSegmentIndex(traj.points, ego_pose.position);
+  Trajectory lookahead_traj;
+  lookahead_traj.header = traj.header;
+  double dist = 0.0;
+  double time = 0.0;
+  auto curr_point_it = std::next(traj.points.begin(), ego_index);
+  auto prev_point_it = curr_point_it;
+  while (curr_point_it != traj.points.end() && dist <= max_dist_m && time <= max_time_s) {
+    lookahead_traj.points.push_back(*curr_point_it);
+    const auto d = autoware::universe_utils::calcDistance2d(
+      prev_point_it->pose.position, curr_point_it->pose.position);
+    dist += d;
+    if (prev_point_it->longitudinal_velocity_mps != 0.0) {
+      time += d / std::abs(prev_point_it->longitudinal_velocity_mps);
+    }
+    prev_point_it = curr_point_it;
+    ++curr_point_it;
+  }
+  return lookahead_traj;
+}
+
+double calc_lookahead_trajectory_distance(const Trajectory & traj, const Pose & ego_pose)
+{
+  const auto ego_index =
+    autoware::motion_utils::findNearestSegmentIndex(traj.points, ego_pose.position);
+  double dist = 0.0;
+  auto curr_point_it = std::next(traj.points.begin(), ego_index);
+  auto prev_point_it = curr_point_it;
+  for (size_t i = 0; i < traj.points.size(); ++i) {
+    const auto d = autoware::universe_utils::calcDistance2d(
+      prev_point_it->pose.position, curr_point_it->pose.position);
+    dist += d;
+    prev_point_it = curr_point_it;
+    ++curr_point_it;
+  }
+
+  return dist;
+}
 }  // namespace utils
 }  // namespace metrics
 }  // namespace planning_diagnostics

evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp

@@ -14,7 +14,9 @@
 
 #include "autoware/planning_evaluator/metrics/stability_metrics.hpp"
 
+#include "autoware/motion_utils/resample/resample.hpp"
 #include "autoware/motion_utils/trajectory/trajectory.hpp"
+#include "autoware/planning_evaluator/metrics/metrics_utils.hpp"
 #include "autoware/universe_utils/geometry/geometry.hpp"
 
 #include <Eigen/Core>
@@ -96,5 +98,47 @@ Accumulator<double> calcLateralDistance(const Trajectory & traj1, const Trajecto
   return stat;
 }
 
+Accumulator<double> calcTrajectoryLateralDisplacement(
+  const Trajectory traj1, const Trajectory traj2, const nav_msgs::msg::Odometry & ego_odom,
+  const double trajectory_eval_time_s)
+{
+  Accumulator<double> stat;
+
+  if (traj1.points.empty() || traj2.points.empty()) {
+    return stat;
+  }
+
+  const double ego_velocity =
+    std::hypot(ego_odom.twist.twist.linear.x, ego_odom.twist.twist.linear.y);
+
+  const double evaluation_section_length = trajectory_eval_time_s * std::abs(ego_velocity);
+
+  const double traj1_lookahead_distance =
+    utils::calc_lookahead_trajectory_distance(traj1, ego_odom.pose.pose);
+  const double traj2_lookahead_distance =
+    utils::calc_lookahead_trajectory_distance(traj2, ego_odom.pose.pose);
+
+  if (
+    traj1_lookahead_distance < evaluation_section_length ||
+    traj2_lookahead_distance < evaluation_section_length) {
+    return stat;
+  }
+
+  constexpr double num_evaluation_points = 10.0;
+  const double interval = evaluation_section_length / num_evaluation_points;
+
+  const auto resampled_traj1 = autoware::motion_utils::resampleTrajectory(
+    utils::get_lookahead_trajectory(
+      traj1, ego_odom.pose.pose, evaluation_section_length, trajectory_eval_time_s),
+    interval);
+
+  for (const auto & point : resampled_traj1.points) {
+    const auto p0 = autoware::universe_utils::getPoint(point);
+    const double dist = autoware::motion_utils::calcLateralOffset(traj2.points, p0);
+    stat.add(std::abs(dist));
+  }
+  return stat;
+}
+
 }  // namespace metrics
 }  // namespace planning_diagnostics

evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp

@@ -16,6 +16,7 @@
 
 #include "autoware/motion_utils/trajectory/trajectory.hpp"
 #include "autoware/planning_evaluator/metrics/deviation_metrics.hpp"
+#include "autoware/planning_evaluator/metrics/metrics_utils.hpp"
 #include "autoware/planning_evaluator/metrics/obstacle_metrics.hpp"
 #include "autoware/planning_evaluator/metrics/stability_metrics.hpp"
 #include "autoware/planning_evaluator/metrics/trajectory_metrics.hpp"
@@ -53,20 +54,23 @@ std::optional<Accumulator<double>> MetricsCalculator::calculate(
       return metrics::calcLateralTrajectoryDisplacement(previous_trajectory_, traj, ego_pose_);
     case Metric::stability_frechet:
       return metrics::calcFrechetDistance(
-        getLookaheadTrajectory(
-          previous_trajectory_, parameters.trajectory.lookahead.max_dist_m,
+        metrics::utils::get_lookahead_trajectory(
+          previous_trajectory_, ego_pose_, parameters.trajectory.lookahead.max_dist_m,
           parameters.trajectory.lookahead.max_time_s),
-        getLookaheadTrajectory(
-          traj, parameters.trajectory.lookahead.max_dist_m,
+        metrics::utils::get_lookahead_trajectory(
+          traj, ego_pose_, parameters.trajectory.lookahead.max_dist_m,
           parameters.trajectory.lookahead.max_time_s));
     case Metric::stability:
       return metrics::calcLateralDistance(
-        getLookaheadTrajectory(
-          previous_trajectory_, parameters.trajectory.lookahead.max_dist_m,
+        metrics::utils::get_lookahead_trajectory(
+          previous_trajectory_, ego_pose_, parameters.trajectory.lookahead.max_dist_m,
           parameters.trajectory.lookahead.max_time_s),
-        getLookaheadTrajectory(
-          traj, parameters.trajectory.lookahead.max_dist_m,
+        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:
@@ -107,43 +111,15 @@ void MetricsCalculator::setPredictedObjects(const PredictedObjects & objects)
   dynamic_objects_ = objects;
 }
 
-void MetricsCalculator::setEgoPose(const geometry_msgs::msg::Pose & pose)
+void MetricsCalculator::setEgoPose(const nav_msgs::msg::Odometry & ego_odometry)
 {
-  ego_pose_ = pose;
+  ego_pose_ = ego_odometry.pose.pose;
+  ego_odometry_ = ego_odometry;
 }
 
 Pose MetricsCalculator::getEgoPose()
 {
   return ego_pose_;
 }
 
-Trajectory MetricsCalculator::getLookaheadTrajectory(
-  const Trajectory & traj, const double max_dist_m, const double max_time_s) const
-{
-  if (traj.points.empty()) {
-    return traj;
-  }
-
-  const auto ego_index =
-    autoware::motion_utils::findNearestSegmentIndex(traj.points, ego_pose_.position);
-  Trajectory lookahead_traj;
-  lookahead_traj.header = traj.header;
-  double dist = 0.0;
-  double time = 0.0;
-  auto curr_point_it = std::next(traj.points.begin(), ego_index);
-  auto prev_point_it = curr_point_it;
-  while (curr_point_it != traj.points.end() && dist <= max_dist_m && time <= max_time_s) {
-    lookahead_traj.points.push_back(*curr_point_it);
-    const auto d = autoware::universe_utils::calcDistance2d(
-      prev_point_it->pose.position, curr_point_it->pose.position);
-    dist += d;
-    if (prev_point_it->longitudinal_velocity_mps != 0.0) {
-      time += d / std::abs(prev_point_it->longitudinal_velocity_mps);
-    }
-    prev_point_it = curr_point_it;
-    ++curr_point_it;
-  }
-  return lookahead_traj;
-}
-
 }  // namespace planning_diagnostics