feat(planning_data_analyzer): add open-loop history comfort metric (#379)

* feat(planning_data_analyzer): add open-loop history comfort metric

* chore(planning_data_analyzer): drop local implementation note from PR

* refactor(planning_data_analyzer): extract history comfort metric utility

* style(planning_data_analyzer): apply clang-format to history comfort utility

* fix(planning_data_analyzer): address history comfort review comments

Author: beomseok-kimm

planning/autoware_planning_data_analyzer/CMakeLists.txt

@@ -11,6 +11,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/or_event_extractor.cpp
   src/or_scene_evaluator.cpp
   src/metrics/deviation_metrics.cpp
+  src/metrics/history_comfort.cpp
   src/metrics/trajectory_metrics.cpp
   src/utils/bag_utils.cpp
   src/utils/json_utils.cpp

planning/autoware_planning_data_analyzer/config/planning_data_analyzer.param.yaml

@@ -34,6 +34,16 @@
       gt_source_mode: "kinematic_state"
       gt_trajectory_topic: "/planning/ground_truth_trajectory"
       gt_sync_tolerance_ms: 200.0
+      hc:
+        # NAVSIM-aligned default thresholds for the History Comfort (HC) subscore.
+        finite_difference_epsilon: 0.001
+        max_longitudinal_acceleration: 2.40
+        min_longitudinal_acceleration: -4.05
+        max_lateral_acceleration: 4.89
+        max_jerk_magnitude: 8.37
+        max_longitudinal_jerk: 4.13
+        max_yaw_rate: 0.95
+        max_yaw_acceleration: 1.93
       metric_variant: ""
       # DLR result horizon checkpoints in seconds.
       # ADE/FDE are reported at these horizons plus "full" (entire trajectory).

planning/autoware_planning_data_analyzer/src/autoware_planning_data_analyzer_node.cpp

@@ -108,6 +108,22 @@ AutowarePlanningDataAnalyzerNode::AutowarePlanningDataAnalyzerNode(
   gt_trajectory_topic_name_ =
     get_or_declare_parameter<std::string>(*this, "open_loop.gt_trajectory_topic");
   gt_sync_tolerance_ms_ = get_or_declare_parameter<double>(*this, "open_loop.gt_sync_tolerance_ms");
+  history_comfort_params_.finite_difference_epsilon =
+    get_or_declare_parameter<double>(*this, "open_loop.hc.finite_difference_epsilon");
+  history_comfort_params_.max_longitudinal_acceleration =
+    get_or_declare_parameter<double>(*this, "open_loop.hc.max_longitudinal_acceleration");
+  history_comfort_params_.min_longitudinal_acceleration =
+    get_or_declare_parameter<double>(*this, "open_loop.hc.min_longitudinal_acceleration");
+  history_comfort_params_.max_lateral_acceleration =
+    get_or_declare_parameter<double>(*this, "open_loop.hc.max_lateral_acceleration");
+  history_comfort_params_.max_jerk_magnitude =
+    get_or_declare_parameter<double>(*this, "open_loop.hc.max_jerk_magnitude");
+  history_comfort_params_.max_longitudinal_jerk =
+    get_or_declare_parameter<double>(*this, "open_loop.hc.max_longitudinal_jerk");
+  history_comfort_params_.max_yaw_rate =
+    get_or_declare_parameter<double>(*this, "open_loop.hc.max_yaw_rate");
+  history_comfort_params_.max_yaw_acceleration =
+    get_or_declare_parameter<double>(*this, "open_loop.hc.max_yaw_acceleration");
   objects_topic_name_ = get_or_declare_parameter<std::string>(*this, "objects_topic");
   tf_topic_name_ = get_or_declare_parameter<std::string>(*this, "tf_topic");
   acceleration_topic_name_ = get_or_declare_parameter<std::string>(*this, "acceleration_topic");
@@ -451,7 +467,8 @@ void AutowarePlanningDataAnalyzerNode::run_evaluation()
       }
       const auto evaluation_horizons =
         get_or_declare_parameter<std::vector<double>>(*this, "open_loop.evaluation_horizons");
-      OpenLoopEvaluator evaluator(get_logger(), route_handler_, gt_mode, gt_sync_tolerance_ms_);
+      OpenLoopEvaluator evaluator(
+        get_logger(), route_handler_, gt_mode, gt_sync_tolerance_ms_, history_comfort_params_);
       evaluator.set_json_output_dir(output_dir_path.string());
       evaluator.set_metric_variant(open_loop_metric_variant);
       evaluator.set_evaluation_horizons(evaluation_horizons);

planning/autoware_planning_data_analyzer/src/autoware_planning_data_analyzer_node.hpp

@@ -16,6 +16,7 @@
 #define AUTOWARE_PLANNING_DATA_ANALYZER_NODE_HPP_
 
 #include "bag_handler.hpp"
+#include "metrics/trajectory_metrics.hpp"
 #include "rosbag2_cpp/reader.hpp"
 #include "rosbag2_cpp/writer.hpp"
 #include "serialized_bag_message.hpp"
@@ -84,6 +85,7 @@ class AutowarePlanningDataAnalyzerNode : public rclcpp::Node
   std::string gt_source_mode_;
   std::string gt_trajectory_topic_name_;
   double gt_sync_tolerance_ms_ = 200.0;
+  metrics::HistoryComfortParameters history_comfort_params_;
   std::string objects_topic_name_;
   std::string tf_topic_name_;
   std::string acceleration_topic_name_;

planning/autoware_planning_data_analyzer/src/metrics/history_comfort.cpp

@@ -0,0 +1,141 @@
+// Copyright 2026 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.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "history_comfort.hpp"
+
+#include <autoware_utils_math/normalization.hpp>
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include <tf2/utils.h>
+
+#include <algorithm>
+#include <cmath>
+#include <vector>
+
+namespace autoware::planning_data_analyzer::metrics
+{
+
+namespace
+{
+
+double calculate_time_resolution(
+  const autoware_planning_msgs::msg::TrajectoryPoint & point,
+  const autoware_planning_msgs::msg::TrajectoryPoint & next_point, const double epsilon)
+{
+  const double time_diff = rclcpp::Duration(next_point.time_from_start).seconds() -
+                           rclcpp::Duration(point.time_from_start).seconds();
+  return time_diff > epsilon ? time_diff : epsilon;
+}
+
+void backfill_last_value_from_previous(std::vector<double> & values)
+{
+  if (values.size() > 1U) {
+    values.back() = values[values.size() - 2];
+  }
+}
+
+bool is_within(const std::vector<double> & values, const double min_value, const double max_value)
+{
+  return std::all_of(values.begin(), values.end(), [min_value, max_value](const double value) {
+    return min_value < value && value < max_value;
+  });
+}
+
+bool is_abs_within(const std::vector<double> & values, const double max_abs_value)
+{
+  return std::all_of(values.begin(), values.end(), [max_abs_value](const double value) {
+    return std::abs(value) < max_abs_value;
+  });
+}
+
+}  // namespace
+
+void calculate_history_comfort_metrics(
+  const autoware_planning_msgs::msg::Trajectory & trajectory,
+  const HistoryComfortParameters & history_comfort_params, TrajectoryPointMetrics & metrics)
+{
+  const size_t num_points = trajectory.points.size();
+  if (num_points == 0U) {
+    return;
+  }
+
+  metrics.longitudinal_accelerations.resize(num_points, 0.0);
+  metrics.lateral_accelerations.resize(num_points, 0.0);
+  metrics.lateral_jerks.resize(num_points, 0.0);
+  metrics.jerk_magnitudes.resize(num_points, 0.0);
+  metrics.longitudinal_jerks.resize(num_points, 0.0);
+  metrics.yaw_rates.resize(num_points, 0.0);
+  metrics.yaw_accelerations.resize(num_points, 0.0);
+
+  for (size_t i = 0; i < num_points - 1; ++i) {
+    const double time_resolution = calculate_time_resolution(
+      trajectory.points[i], trajectory.points[i + 1],
+      history_comfort_params.finite_difference_epsilon);
+
+    const double yaw_delta = autoware_utils_math::normalize_radian(
+      tf2::getYaw(trajectory.points[i + 1].pose.orientation) -
+      tf2::getYaw(trajectory.points[i].pose.orientation));
+    metrics.yaw_rates[i] = yaw_delta / time_resolution;
+    metrics.longitudinal_accelerations[i] = (trajectory.points[i + 1].longitudinal_velocity_mps -
+                                             trajectory.points[i].longitudinal_velocity_mps) /
+                                            time_resolution;
+    metrics.lateral_accelerations[i] =
+      trajectory.points[i].longitudinal_velocity_mps * metrics.yaw_rates[i];
+  }
+  backfill_last_value_from_previous(metrics.longitudinal_accelerations);
+  backfill_last_value_from_previous(metrics.lateral_accelerations);
+  backfill_last_value_from_previous(metrics.yaw_rates);
+
+  for (size_t i = 0; i < num_points - 1; ++i) {
+    const double time_resolution = calculate_time_resolution(
+      trajectory.points[i], trajectory.points[i + 1],
+      history_comfort_params.finite_difference_epsilon);
+
+    metrics.longitudinal_jerks[i] =
+      (metrics.longitudinal_accelerations[i + 1] - metrics.longitudinal_accelerations[i]) /
+      time_resolution;
+    metrics.lateral_jerks[i] =
+      (metrics.lateral_accelerations[i + 1] - metrics.lateral_accelerations[i]) / time_resolution;
+    metrics.jerk_magnitudes[i] =
+      std::hypot(metrics.longitudinal_jerks[i], metrics.lateral_jerks[i]);
+    metrics.yaw_accelerations[i] =
+      (metrics.yaw_rates[i + 1] - metrics.yaw_rates[i]) / time_resolution;
+  }
+  backfill_last_value_from_previous(metrics.longitudinal_jerks);
+  backfill_last_value_from_previous(metrics.lateral_jerks);
+  backfill_last_value_from_previous(metrics.jerk_magnitudes);
+  backfill_last_value_from_previous(metrics.yaw_accelerations);
+
+  const bool longitudinal_acceleration_ok = is_within(
+    metrics.longitudinal_accelerations, history_comfort_params.min_longitudinal_acceleration,
+    history_comfort_params.max_longitudinal_acceleration);
+  const bool lateral_acceleration_ok =
+    is_abs_within(metrics.lateral_accelerations, history_comfort_params.max_lateral_acceleration);
+  const bool jerk_magnitude_ok =
+    is_abs_within(metrics.jerk_magnitudes, history_comfort_params.max_jerk_magnitude);
+  const bool longitudinal_jerk_ok =
+    is_abs_within(metrics.longitudinal_jerks, history_comfort_params.max_longitudinal_jerk);
+  const bool yaw_rate_ok = is_abs_within(metrics.yaw_rates, history_comfort_params.max_yaw_rate);
+  const bool yaw_acceleration_ok =
+    is_abs_within(metrics.yaw_accelerations, history_comfort_params.max_yaw_acceleration);
+
+  metrics.history_comfort = longitudinal_acceleration_ok && lateral_acceleration_ok &&
+                                jerk_magnitude_ok && longitudinal_jerk_ok && yaw_rate_ok &&
+                                yaw_acceleration_ok
+                              ? 1.0
+                              : 0.0;
+}
+
+}  // namespace autoware::planning_data_analyzer::metrics

planning/autoware_planning_data_analyzer/src/metrics/history_comfort.hpp

@@ -0,0 +1,31 @@
+// Copyright 2026 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.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef METRICS__HISTORY_COMFORT_HPP_
+#define METRICS__HISTORY_COMFORT_HPP_
+
+#include "trajectory_metrics.hpp"
+
+#include <autoware_planning_msgs/msg/trajectory.hpp>
+
+namespace autoware::planning_data_analyzer::metrics
+{
+
+void calculate_history_comfort_metrics(
+  const autoware_planning_msgs::msg::Trajectory & trajectory,
+  const HistoryComfortParameters & history_comfort_params, TrajectoryPointMetrics & metrics);
+
+}  // namespace autoware::planning_data_analyzer::metrics
+
+#endif  // METRICS__HISTORY_COMFORT_HPP_

planning/autoware_planning_data_analyzer/src/metrics/trajectory_metrics.cpp

@@ -14,14 +14,14 @@
 
 #include "trajectory_metrics.hpp"
 
+#include "history_comfort.hpp"
+
 #include <autoware/lanelet2_utils/geometry.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
 #include <autoware_utils_geometry/geometry.hpp>
 #include <tf2/LinearMath/Vector3.hpp>
 
-#include <tf2/utils.h>
-
 #include <algorithm>
 #include <cmath>
 #include <limits>
@@ -191,7 +191,8 @@ double calculate_time_to_collision(
 
 TrajectoryPointMetrics calculate_trajectory_point_metrics(
   const std::shared_ptr<SynchronizedData> & sync_data,
-  const std::shared_ptr<autoware::route_handler::RouteHandler> & route_handler)
+  const std::shared_ptr<autoware::route_handler::RouteHandler> & route_handler,
+  const HistoryComfortParameters & history_comfort_params)
 {
   TrajectoryPointMetrics metrics;
 
@@ -203,56 +204,15 @@ TrajectoryPointMetrics calculate_trajectory_point_metrics(
   const size_t num_points = trajectory.points.size();
 
   // Initialize vectors
-  metrics.lateral_accelerations.resize(num_points, 0.0);
-  metrics.longitudinal_jerks.resize(num_points, 0.0);
   metrics.ttc_values.resize(num_points, std::numeric_limits<double>::max());
   metrics.lateral_deviations.resize(num_points, 0.0);
   metrics.travel_distances.resize(num_points, 0.0);
 
-  // Calculate lateral acceleration from lateral velocity difference
-  constexpr double epsilon = 1.0e-3;
-  for (size_t i = 0; i < num_points - 1; ++i) {
-    const double time_diff = rclcpp::Duration(trajectory.points[i + 1].time_from_start).seconds() -
-                             rclcpp::Duration(trajectory.points[i].time_from_start).seconds();
-    const double time_resolution = time_diff > epsilon ? time_diff : epsilon;
-
-    const double lateral_acc =
-      (trajectory.points[i + 1].lateral_velocity_mps - trajectory.points[i].lateral_velocity_mps) /
-      time_resolution;
-    metrics.lateral_accelerations[i] = lateral_acc;
-  }
-  if (num_points > 0) {
-    metrics.lateral_accelerations[num_points - 1] = metrics.lateral_accelerations[num_points - 2];
-  }
-
-  // Calculate longitudinal jerk from velocity differences
-  std::vector<double> longitudinal_accelerations(num_points, 0.0);
-  for (size_t i = 0; i < num_points - 1; ++i) {
-    const double time_diff = rclcpp::Duration(trajectory.points[i + 1].time_from_start).seconds() -
-                             rclcpp::Duration(trajectory.points[i].time_from_start).seconds();
-    const double time_resolution = time_diff > epsilon ? time_diff : epsilon;
-
-    longitudinal_accelerations[i] = (trajectory.points[i + 1].longitudinal_velocity_mps -
-                                     trajectory.points[i].longitudinal_velocity_mps) /
-                                    time_resolution;
-  }
-  if (num_points > 0) {
-    longitudinal_accelerations[num_points - 1] = longitudinal_accelerations[num_points - 2];
+  if (num_points == 0U) {
+    return metrics;
   }
 
-  // Calculate jerk from acceleration differences
-  for (size_t i = 0; i < num_points - 1; ++i) {
-    const double time_diff = rclcpp::Duration(trajectory.points[i + 1].time_from_start).seconds() -
-                             rclcpp::Duration(trajectory.points[i].time_from_start).seconds();
-    const double time_resolution = time_diff > epsilon ? time_diff : epsilon;
-
-    const double jerk =
-      (longitudinal_accelerations[i + 1] - longitudinal_accelerations[i]) / time_resolution;
-    metrics.longitudinal_jerks[i] = jerk;
-  }
-  if (num_points > 0) {
-    metrics.longitudinal_jerks[num_points - 1] = metrics.longitudinal_jerks[num_points - 2];
-  }
+  calculate_history_comfort_metrics(trajectory, history_comfort_params, metrics);
 
   // Calculate TTC for each point (based on autoware_trajectory_ranker implementation)
   constexpr double max_ttc_value = 10.0;  // Maximum TTC value in seconds

planning/autoware_planning_data_analyzer/src/metrics/trajectory_metrics.hpp

@@ -25,14 +25,32 @@
 namespace autoware::planning_data_analyzer::metrics
 {
 
+struct HistoryComfortParameters
+{
+  double finite_difference_epsilon{1.0e-3};
+  double max_longitudinal_acceleration{2.40};
+  double min_longitudinal_acceleration{-4.05};
+  double max_lateral_acceleration{4.89};
+  double max_jerk_magnitude{8.37};
+  double max_longitudinal_jerk{4.13};
+  double max_yaw_rate{0.95};
+  double max_yaw_acceleration{1.93};
+};
+
 // Structure for trajectory point-wise metrics
 struct TrajectoryPointMetrics
 {
+  std::vector<double> longitudinal_accelerations;
   std::vector<double> lateral_accelerations;
+  std::vector<double> lateral_jerks;
+  std::vector<double> jerk_magnitudes;
   std::vector<double> longitudinal_jerks;
+  std::vector<double> yaw_rates;
+  std::vector<double> yaw_accelerations;
   std::vector<double> ttc_values;
   std::vector<double> lateral_deviations;
   std::vector<double> travel_distances;
+  double history_comfort{0.0};
 };
 
 /**
@@ -43,7 +61,8 @@ struct TrajectoryPointMetrics
  */
 TrajectoryPointMetrics calculate_trajectory_point_metrics(
   const std::shared_ptr<SynchronizedData> & sync_data,
-  const std::shared_ptr<autoware::route_handler::RouteHandler> & route_handler = nullptr);
+  const std::shared_ptr<autoware::route_handler::RouteHandler> & route_handler = nullptr,
+  const HistoryComfortParameters & history_comfort_params = HistoryComfortParameters{});
 
 }  // namespace autoware::planning_data_analyzer::metrics