feat(autoware_planning_evaluator): add resampled_relative_angle metri…

…cs (#10020)

* feat(autoware_planning_evaluator): add new large_relative_angle metrics

Signed-off-by: Kasunori-Nakajima <[email protected]>

* fix copyright and vehicle_length_m

Signed-off-by: Kasunori-Nakajima <[email protected]>

* style(pre-commit): autofix

* del: resample trajectory

Signed-off-by: Kasunori-Nakajima <[email protected]>

* del: traj points check

Signed-off-by: Kasunori-Nakajima <[email protected]>

* rename msg and speed optimization

Signed-off-by: Kasunori-Nakajima <[email protected]>

* style(pre-commit): autofix

* add unit_test and fix resample_relative_angle

Signed-off-by: Kasunori-Nakajima <[email protected]>

* style(pre-commit): autofix

* include tuple to test

Signed-off-by: Kasunori-Nakajima <[email protected]>

* target two point, update unit test value

Signed-off-by: Kasunori-Nakajima <[email protected]>

* fix abs

Signed-off-by: Kasunori-Nakajima <[email protected]>

* fix for loop bag and primitive type

Signed-off-by: Kasunori-Nakajima <[email protected]>

---------

Signed-off-by: Kasunori-Nakajima <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml

@@ -6,6 +6,7 @@
       - curvature
       - point_interval
       - relative_angle
+      - resampled_relative_angle
       - length
       - duration
       - velocity

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

@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2025 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -29,6 +29,7 @@ enum class Metric {
   curvature,
   point_interval,
   relative_angle,
+  resampled_relative_angle,
   length,
   duration,
   velocity,
@@ -56,6 +57,7 @@ static const std::unordered_map<std::string, Metric> str_to_metric = {
   {"curvature", Metric::curvature},
   {"point_interval", Metric::point_interval},
   {"relative_angle", Metric::relative_angle},
+  {"resampled_relative_angle", Metric::resampled_relative_angle},
   {"length", Metric::length},
   {"duration", Metric::duration},
   {"velocity", Metric::velocity},
@@ -78,6 +80,7 @@ static const std::unordered_map<Metric, std::string> metric_to_str = {
   {Metric::curvature, "curvature"},
   {Metric::point_interval, "point_interval"},
   {Metric::relative_angle, "relative_angle"},
+  {Metric::resampled_relative_angle, "resampled_relative_angle"},
   {Metric::length, "length"},
   {Metric::duration, "duration"},
   {Metric::velocity, "velocity"},
@@ -101,6 +104,7 @@ static const std::unordered_map<Metric, std::string> metric_descriptions = {
   {Metric::curvature, "Curvature[1/rad]"},
   {Metric::point_interval, "Interval_between_points[m]"},
   {Metric::relative_angle, "Relative_angle[rad]"},
+  {Metric::resampled_relative_angle, "Resampled_relative_angle[rad]"},
   {Metric::length, "Trajectory_length[m]"},
   {Metric::duration, "Trajectory_duration[s]"},
   {Metric::velocity, "Trajectory_velocity[m/s]"},

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

@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2025 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -15,6 +15,9 @@
 #ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__TRAJECTORY_METRICS_HPP_
 #define AUTOWARE__PLANNING_EVALUATOR__METRICS__TRAJECTORY_METRICS_HPP_
 
+#include "autoware/motion_utils/resample/resample.hpp"
+#include "autoware/motion_utils/trajectory/conversion.hpp"
+#include "autoware/motion_utils/trajectory/trajectory.hpp"
 #include "autoware/universe_utils/math/accumulator.hpp"
 
 #include "autoware_planning_msgs/msg/trajectory.hpp"
@@ -37,6 +40,15 @@ using autoware_planning_msgs::msg::TrajectoryPoint;
 Accumulator<double> calcTrajectoryRelativeAngle(
   const Trajectory & traj, const double min_dist_threshold);
 
+/**
+ * @brief calculate large relative angle metric (angle between successive points)
+ * @param [in] traj input trajectory
+ * @param [in] vehicle_length_m input vehicle length
+ * @return calculated statistics
+ */
+Accumulator<double> calcTrajectoryResampledRelativeAngle(
+  const Trajectory & traj, const double vehicle_length_m);
+
 /**
  * @brief calculate metric for the distance between trajectory points
  * @param [in] traj input trajectory

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

@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2025 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -47,9 +47,12 @@ class MetricsCalculator
   /**
    * @brief calculate
    * @param [in] metric Metric enum value
+   * @param [in] traj input trajectory
+   * @param [in] vehicle_length_m input vehicle length
    * @return string describing the requested metric
    */
-  std::optional<Accumulator<double>> calculate(const Metric metric, const Trajectory & traj) const;
+  std::optional<Accumulator<double>> calculate(
+    const Metric metric, const Trajectory & traj, const double vehicle_length_m) const;
   std::optional<Accumulator<double>> calculate(
     const Metric metric, const Pose & base_pose, const Pose & target_pose) const;
 

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

@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2025 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -21,6 +21,8 @@
 #include "tf2_ros/buffer.h"
 #include "tf2_ros/transform_listener.h"
 
+#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
+
 #include "autoware_planning_msgs/msg/trajectory.hpp"
 #include "autoware_planning_msgs/msg/trajectory_point.hpp"
 #include "geometry_msgs/msg/transform_stamped.hpp"
@@ -34,6 +36,7 @@
 namespace planning_diagnostics
 {
 using autoware::universe_utils::Accumulator;
+using autoware::vehicle_info_utils::VehicleInfo;
 using autoware_planning_msgs::msg::Trajectory;
 using autoware_planning_msgs::msg::TrajectoryPoint;
 
@@ -67,6 +70,7 @@ class MotionEvaluatorNode : public rclcpp::Node
   MetricsCalculator metrics_calculator_;
   // Metrics
   std::vector<Metric> metrics_;
+  VehicleInfo vehicle_info_;
   std::deque<rclcpp::Time> stamps_;
   Trajectory accumulated_trajectory_;
 };

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

@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2025 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -24,6 +24,7 @@
 #include <autoware/route_handler/route_handler.hpp>
 #include <autoware/universe_utils/ros/polling_subscriber.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
+#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 
 #include "autoware_perception_msgs/msg/predicted_objects.hpp"
 #include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp"
@@ -44,6 +45,7 @@
 namespace planning_diagnostics
 {
 using autoware::universe_utils::Accumulator;
+using autoware::vehicle_info_utils::VehicleInfo;
 using autoware_perception_msgs::msg::PredictedObjects;
 using autoware_planning_msgs::msg::PoseWithUuidStamped;
 using autoware_planning_msgs::msg::Trajectory;
@@ -167,6 +169,7 @@ class PlanningEvaluatorNode : public rclcpp::Node
     metric_accumulators_;  // 3(min, max, mean) * metric_size
 
   rclcpp::TimerBase::SharedPtr timer_;
+  VehicleInfo vehicle_info_;
   std::optional<AccelWithCovarianceStamped> prev_acc_stamped_{std::nullopt};
 };
 }  // namespace planning_diagnostics

evaluator/autoware_planning_evaluator/package.xml

@@ -19,6 +19,7 @@
   <depend>autoware_planning_msgs</depend>
   <depend>autoware_route_handler</depend>
   <depend>autoware_universe_utils</depend>
+  <depend>autoware_vehicle_info_utils</depend>
   <depend>eigen</depend>
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>

evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json

@@ -26,6 +26,7 @@
             "curvature",
             "point_interval",
             "relative_angle",
+            "resampled_relative_angle",
             "length",
             "duration",
             "velocity",

evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp

@@ -16,6 +16,9 @@
 
 #include "autoware/planning_evaluator/metrics/metrics_utils.hpp"
 #include "autoware/universe_utils/geometry/geometry.hpp"
+
+#include <algorithm>
+
 namespace planning_diagnostics
 {
 namespace metrics
@@ -80,6 +83,38 @@ Accumulator<double> calcTrajectoryRelativeAngle(
   return stat;
 }
 
+Accumulator<double> calcTrajectoryResampledRelativeAngle(
+  const Trajectory & traj, const double vehicle_length_m)
+{
+  Accumulator<double> stat;
+
+  const auto resample_offset = vehicle_length_m / 2;
+  const auto arc_length =
+    autoware::motion_utils::calcSignedArcLengthPartialSum(traj.points, 0, traj.points.size());
+  for (size_t base_id = 0; base_id + 1 < arc_length.size(); ++base_id) {
+    // Get base pose yaw
+    const double base_yaw = tf2::getYaw(traj.points.at(base_id).pose.orientation);
+
+    for (size_t target_id = base_id + 1; target_id < arc_length.size(); ++target_id) {
+      if (arc_length[target_id] >= arc_length[base_id] + resample_offset) {
+        // Get target pose yaw
+        const double front_target_yaw = tf2::getYaw(traj.points.at(target_id).pose.orientation);
+        const double back_target_yaw = tf2::getYaw(traj.points.at(target_id - 1).pose.orientation);
+
+        // Calc diff yaw between base pose yaw and target pose yaw
+        const double front_diff_yaw =
+          std::abs(autoware::universe_utils::normalizeRadian(front_target_yaw - base_yaw));
+        const double back_diff_yaw =
+          std::abs(autoware::universe_utils::normalizeRadian(back_target_yaw - base_yaw));
+
+        stat.add(std::max(front_diff_yaw, back_diff_yaw));
+        break;
+      }
+    }
+  }
+  return stat;
+}
+
 Accumulator<double> calcTrajectoryCurvature(const Trajectory & traj)
 {
   Accumulator<double> stat;

evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp

@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2025 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -24,7 +24,7 @@
 namespace planning_diagnostics
 {
 std::optional<Accumulator<double>> MetricsCalculator::calculate(
-  const Metric metric, const Trajectory & traj) const
+  const Metric metric, const Trajectory & traj, const double vehicle_length_m) const
 {
   // Functions to calculate trajectory metrics
   switch (metric) {
@@ -34,6 +34,8 @@ std::optional<Accumulator<double>> MetricsCalculator::calculate(
       return metrics::calcTrajectoryInterval(traj);
     case Metric::relative_angle:
       return metrics::calcTrajectoryRelativeAngle(traj, parameters.trajectory.min_point_dist_m);
+    case Metric::resampled_relative_angle:
+      return metrics::calcTrajectoryResampledRelativeAngle(traj, vehicle_length_m);
     case Metric::length:
       return metrics::calcTrajectoryLength(traj);
     case Metric::duration:

evaluator/autoware_planning_evaluator/src/motion_evaluator_node.cpp

@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2025 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -27,7 +27,8 @@
 namespace planning_diagnostics
 {
 MotionEvaluatorNode::MotionEvaluatorNode(const rclcpp::NodeOptions & node_options)
-: Node("motion_evaluator", node_options)
+: Node("motion_evaluator", node_options),
+  vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo())
 {
   tf_buffer_ptr_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
   tf_listener_ptr_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_ptr_);
@@ -57,7 +58,8 @@ MotionEvaluatorNode::~MotionEvaluatorNode()
     json j;
     for (Metric metric : metrics_) {
       const std::string base_name = metric_to_str.at(metric) + "/";
-      const auto & stat = metrics_calculator_.calculate(metric, accumulated_trajectory_);
+      const auto & stat = metrics_calculator_.calculate(
+        metric, accumulated_trajectory_, vehicle_info_.vehicle_length_m);
       if (stat) {
         j[base_name + "min"] = stat->min();
         j[base_name + "max"] = stat->max();

evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp

@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2025 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -34,7 +34,8 @@
 namespace planning_diagnostics
 {
 PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_options)
-: Node("planning_evaluator", node_options)
+: Node("planning_evaluator", node_options),
+  vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo())
 {
   using std::placeholders::_1;
   tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
@@ -298,7 +299,8 @@ void PlanningEvaluatorNode::onTrajectory(
   auto start = now();
 
   for (Metric metric : metrics_) {
-    const auto metric_stat = metrics_calculator_.calculate(Metric(metric), *traj_msg);
+    const auto metric_stat =
+      metrics_calculator_.calculate(Metric(metric), *traj_msg, vehicle_info_.vehicle_length_m);
     if (!metric_stat) {
       continue;
     }

evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp

@@ -1,4 +1,4 @@
-// Copyright 2024 Tier IV, Inc.
+// Copyright 2025 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -30,6 +30,7 @@
 #include <iostream>
 #include <memory>
 #include <string>
+#include <tuple>
 #include <utility>
 #include <vector>
 
@@ -53,9 +54,12 @@ class EvalTest : public ::testing::Test
     rclcpp::NodeOptions options;
     const auto share_dir =
       ament_index_cpp::get_package_share_directory("autoware_planning_evaluator");
+    const auto autoware_test_utils_dir =
+      ament_index_cpp::get_package_share_directory("autoware_test_utils");
     options.arguments(
-      {"--ros-args", "--params-file", share_dir + "/config/planning_evaluator.param.yaml", "-p",
-       "output_metrics:=false"});
+      {"--ros-args", "-p", "output_metrics:=false", "--params-file",
+       share_dir + "/config/planning_evaluator.param.yaml", "--params-file",
+       autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml"});
 
     dummy_node = std::make_shared<rclcpp::Node>("planning_evaluator_test_node");
     eval_node = std::make_shared<EvalNode>(options);
@@ -83,6 +87,7 @@ class EvalTest : public ::testing::Test
       dummy_node, "/planning_evaluator/input/modified_goal", 1);
 
     tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(dummy_node);
+    vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*eval_node).getVehicleInfo();
     publishEgoPose(0.0, 0.0, 0.0);
   }
 
@@ -118,6 +123,25 @@ class EvalTest : public ::testing::Test
     return t;
   }
 
+  Trajectory makeTrajectory(const std::vector<std::tuple<double, double, double>> & traj)
+  {
+    Trajectory t;
+    t.header.frame_id = "map";
+    TrajectoryPoint p;
+    for (const std::tuple<double, double, double> & point : traj) {
+      p.pose.position.x = std::get<0>(point);
+      p.pose.position.y = std::get<1>(point);
+      tf2::Quaternion q;
+      q.setRPY(0.0, 0.0, std::get<2>(point));
+      p.pose.orientation.x = q.x();
+      p.pose.orientation.y = q.y();
+      p.pose.orientation.z = q.z();
+      p.pose.orientation.w = q.w();
+      t.points.push_back(p);
+    }
+    return t;
+  }
+
   void publishTrajectory(const Trajectory & traj)
   {
     traj_pub_->publish(traj);
@@ -213,6 +237,9 @@ class EvalTest : public ::testing::Test
   rclcpp::Subscription<MetricArrayMsg>::SharedPtr metric_sub_;
   // TF broadcaster
   std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+
+public:
+  autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
 };
 
 TEST_F(EvalTest, TestCurvature)
@@ -250,6 +277,16 @@ TEST_F(EvalTest, TestRelativeAngle)
   EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 0.0);
 }
 
+TEST_F(EvalTest, TestResampledRelativeAngle)
+{
+  setTargetMetric(planning_diagnostics::Metric::resampled_relative_angle);
+  Trajectory t = makeTrajectory({{0.0, 0.0, 0.0}, {vehicle_info_.vehicle_length_m, 0.0, 0.0}});
+  EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 0.0);
+  t = makeTrajectory(
+    {{0.0, 0.0, 0.0}, {vehicle_info_.vehicle_length_m, vehicle_info_.vehicle_length_m, M_PI_4}});
+  EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), M_PI_4);
+}
+
 TEST_F(EvalTest, TestLength)
 {
   setTargetMetric(planning_diagnostics::Metric::length);