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

…l displacement

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

@@ -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

@@ -14,7 +14,6 @@
 
 #ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_
 #define AUTOWARE__PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_
-
 #include "autoware_planning_msgs/msg/trajectory.hpp"
 #include "autoware_planning_msgs/msg/trajectory_point.hpp"
 
@@ -25,6 +24,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 +35,18 @@ using autoware_planning_msgs::msg::Trajectory;
  */
 size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance);
 
+/**
+ * @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
@@ -100,6 +101,7 @@ class MetricsCalculator
   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,23 @@ size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, cons
   return target_id;
 }
 
+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,45 @@ 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(traj1, interval);
+  double total_lateral_displacement = 0.0;
+  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);
+    total_lateral_displacement += std::abs(dist);
+  }
+  stat.add(total_lateral_displacement);
+  return stat;
+}
+
 }  // namespace metrics
 }  // namespace planning_diagnostics

evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp

@@ -20,6 +20,7 @@
 #include "autoware/planning_evaluator/metrics/stability_metrics.hpp"
 #include "autoware/planning_evaluator/metrics/trajectory_metrics.hpp"
 #include "autoware/universe_utils/geometry/geometry.hpp"
+
 namespace planning_diagnostics
 {
 std::optional<Accumulator<double>> MetricsCalculator::calculate(
@@ -67,6 +68,9 @@ std::optional<Accumulator<double>> MetricsCalculator::calculate(
         getLookaheadTrajectory(
           traj, 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,9 +111,10 @@ 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()

evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp

@@ -50,6 +50,8 @@ PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_op
     declare_parameter<double>("trajectory.lookahead.max_dist_m");
   metrics_calculator_.parameters.trajectory.lookahead.max_time_s =
     declare_parameter<double>("trajectory.lookahead.max_time_s");
+  metrics_calculator_.parameters.trajectory.evaluation_time_s =
+    declare_parameter<double>("trajectory.evaluation_time_s");
   metrics_calculator_.parameters.obstacle.dist_thr_m =
     declare_parameter<double>("obstacle.dist_thr_m");
 
@@ -343,7 +345,7 @@ void PlanningEvaluatorNode::onModifiedGoal(
 void PlanningEvaluatorNode::onOdometry(const Odometry::ConstSharedPtr odometry_msg)
 {
   if (!odometry_msg) return;
-  metrics_calculator_.setEgoPose(odometry_msg->pose.pose);
+  metrics_calculator_.setEgoPose(*odometry_msg);
   {
     getRouteData();
     if (route_handler_.isHandlerReady() && odometry_msg) {