feat(planning_data_analyzer): add open-loop extended comfort metric

Author: beomseok-kimm

planning/autoware_planning_data_analyzer/CMakeLists.txt

@@ -14,6 +14,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/metrics/history_comfort.cpp
   src/metrics/lane_keeping.cpp
   src/metrics/drivable_area_compliance.cpp
+  src/metrics/extended_comfort.cpp
   src/metrics/trajectory_metrics.cpp
   src/utils/bag_utils.cpp
   src/utils/json_utils.cpp
@@ -34,6 +35,7 @@ if(BUILD_TESTING)
     test/metrics/test_deviation_metrics.cpp
     test/metrics/test_lane_keeping.cpp
     test/metrics/test_drivable_area_compliance.cpp
+    test/metrics/test_extended_comfort.cpp
   )
   target_link_libraries(test_metrics ${PROJECT_NAME})
 

planning/autoware_planning_data_analyzer/config/planning_data_analyzer.param.yaml

@@ -49,6 +49,12 @@
       # ADE/FDE are reported at these horizons plus "full" (entire trajectory).
       # Trajectories shorter than a horizon are excluded from that horizon with 0.1s tolerance.
       evaluation_horizons: [1.0, 2.0, 4.0, 8.0]
+      extended_comfort:
+        max_acceleration_rms: 0.7  # Maximum RMS acceleration discrepancy between consecutive plans [m/s^2]
+        max_jerk_rms: 0.5  # Maximum RMS jerk discrepancy between consecutive plans [m/s^3]
+        max_yaw_rate_rms: 0.1  # Maximum RMS yaw-rate discrepancy between consecutive plans [rad/s]
+        max_yaw_acceleration_rms: 0.1  # Maximum RMS yaw-acceleration discrepancy between consecutive plans [rad/s^2]
+        finite_difference_epsilon: 0.001  # Minimum delta time used for EC finite differences [s]
       lane_keep:
         max_lateral_deviation: 0.5  # Maximum absolute lateral deviation allowed for LK [m]
         max_continuous_violation_time: 2.0  # Continuous over-threshold duration allowed for LK [s]

planning/autoware_planning_data_analyzer/src/autoware_planning_data_analyzer_node.cpp

@@ -134,6 +134,16 @@ AutowarePlanningDataAnalyzerNode::AutowarePlanningDataAnalyzerNode(
     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");
+  extended_comfort_parameters_.max_acceleration_rms =
+    get_or_declare_parameter<double>(*this, "open_loop.extended_comfort.max_acceleration_rms");
+  extended_comfort_parameters_.max_jerk_rms =
+    get_or_declare_parameter<double>(*this, "open_loop.extended_comfort.max_jerk_rms");
+  extended_comfort_parameters_.max_yaw_rate_rms =
+    get_or_declare_parameter<double>(*this, "open_loop.extended_comfort.max_yaw_rate_rms");
+  extended_comfort_parameters_.max_yaw_acceleration_rms =
+    get_or_declare_parameter<double>(*this, "open_loop.extended_comfort.max_yaw_acceleration_rms");
+  extended_comfort_parameters_.finite_difference_epsilon =
+    get_or_declare_parameter<double>(*this, "open_loop.extended_comfort.finite_difference_epsilon");
   lane_keeping_params_.max_lateral_deviation =
     get_or_declare_parameter<double>(*this, "open_loop.lane_keep.max_lateral_deviation");
   lane_keeping_params_.max_continuous_violation_time =
@@ -487,6 +497,7 @@ void AutowarePlanningDataAnalyzerNode::run_evaluation()
       evaluator.set_json_output_dir(output_dir_path.string());
       evaluator.set_metric_variant(open_loop_metric_variant);
       evaluator.set_evaluation_horizons(evaluation_horizons);
+      evaluator.set_extended_comfort_parameters(extended_comfort_parameters_);
       auto times =
         evaluator.run_evaluation_from_bag(bag_path_, evaluation_bag_writer_.get(), topic_names);
       start_time = times.first;

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/extended_comfort.hpp"
 #include "metrics/trajectory_metrics.hpp"
 #include "rosbag2_cpp/reader.hpp"
 #include "rosbag2_cpp/writer.hpp"
@@ -86,6 +87,7 @@ class AutowarePlanningDataAnalyzerNode : public rclcpp::Node
   std::string gt_trajectory_topic_name_;
   double gt_sync_tolerance_ms_ = 200.0;
   metrics::HistoryComfortParameters history_comfort_params_;
+  metrics::ExtendedComfortParameters extended_comfort_parameters_;
   metrics::LaneKeepingParameters lane_keeping_params_;
   autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
   std::string objects_topic_name_;

planning/autoware_planning_data_analyzer/src/metrics/extended_comfort.cpp

@@ -0,0 +1,163 @@
+// 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 "extended_comfort.hpp"
+
+#include <autoware_utils_math/normalization.hpp>
+
+#include <tf2/utils.h>
+
+#include <algorithm>
+#include <cmath>
+#include <vector>
+
+namespace autoware::planning_data_analyzer::metrics
+{
+
+namespace
+{
+
+struct DynamicSignals
+{
+  std::vector<double> accelerations;
+  std::vector<double> jerks;
+  std::vector<double> yaw_rates;
+  std::vector<double> yaw_accelerations;
+};
+
+double get_time_seconds(const builtin_interfaces::msg::Duration & time_from_start)
+{
+  return static_cast<double>(time_from_start.sec) +
+         static_cast<double>(time_from_start.nanosec) * 1e-9;
+}
+
+double compute_rms_difference(const std::vector<double> & lhs, const std::vector<double> & rhs)
+{
+  const auto count = std::min(lhs.size(), rhs.size());
+  if (count == 0U) {
+    return 0.0;
+  }
+
+  double sum_squared_error = 0.0;
+  for (std::size_t i = 0; i < count; ++i) {
+    const double error = lhs.at(i) - rhs.at(i);
+    sum_squared_error += error * error;
+  }
+
+  return std::sqrt(sum_squared_error / static_cast<double>(count));
+}
+
+DynamicSignals calculate_dynamic_signals(
+  const autoware_planning_msgs::msg::Trajectory & trajectory, const double epsilon)
+{
+  DynamicSignals signals;
+  const auto num_points = trajectory.points.size();
+  if (num_points < 2U) {
+    return signals;
+  }
+
+  signals.accelerations.resize(num_points - 1U, 0.0);
+  signals.yaw_rates.resize(num_points - 1U, 0.0);
+
+  for (std::size_t i = 0; i + 1U < num_points; ++i) {
+    const auto & point = trajectory.points.at(i);
+    const auto & next_point = trajectory.points.at(i + 1U);
+    const double dt = std::max(
+      get_time_seconds(next_point.time_from_start) - get_time_seconds(point.time_from_start),
+      epsilon);
+
+    const double speed = std::hypot(point.longitudinal_velocity_mps, point.lateral_velocity_mps);
+    const double next_speed =
+      std::hypot(next_point.longitudinal_velocity_mps, next_point.lateral_velocity_mps);
+    signals.accelerations.at(i) = (next_speed - speed) / dt;
+
+    const double yaw = tf2::getYaw(point.pose.orientation);
+    const double next_yaw = tf2::getYaw(next_point.pose.orientation);
+    const double delta_yaw = autoware_utils_math::normalize_radian(next_yaw - yaw);
+    signals.yaw_rates.at(i) = delta_yaw / dt;
+  }
+
+  if (signals.accelerations.size() >= 2U) {
+    signals.jerks.resize(signals.accelerations.size() - 1U, 0.0);
+    signals.yaw_accelerations.resize(signals.yaw_rates.size() - 1U, 0.0);
+
+    for (std::size_t i = 0; i + 1U < signals.accelerations.size(); ++i) {
+      const auto & point = trajectory.points.at(i);
+      const auto & next_point = trajectory.points.at(i + 1U);
+      const double dt = std::max(
+        get_time_seconds(next_point.time_from_start) - get_time_seconds(point.time_from_start),
+        epsilon);
+      signals.jerks.at(i) = (signals.accelerations.at(i + 1U) - signals.accelerations.at(i)) / dt;
+      signals.yaw_accelerations.at(i) =
+        (signals.yaw_rates.at(i + 1U) - signals.yaw_rates.at(i)) / dt;
+    }
+  }
+
+  return signals;
+}
+
+bool are_parameters_valid(const ExtendedComfortParameters & parameters)
+{
+  return parameters.max_acceleration_rms >= 0.0 && parameters.max_jerk_rms >= 0.0 &&
+         parameters.max_yaw_rate_rms >= 0.0 && parameters.max_yaw_acceleration_rms >= 0.0 &&
+         parameters.finite_difference_epsilon > 0.0;
+}
+
+}  // namespace
+
+ExtendedComfortResult calculate_extended_comfort(
+  const autoware_planning_msgs::msg::Trajectory & previous_trajectory,
+  const autoware_planning_msgs::msg::Trajectory & current_trajectory,
+  const ExtendedComfortParameters & parameters)
+{
+  if (!are_parameters_valid(parameters)) {
+    return {0.0, false, "unavailable_invalid_parameters"};
+  }
+
+  if (previous_trajectory.points.size() < 3U || current_trajectory.points.size() < 3U) {
+    return {0.0, false, "unavailable_short_trajectory"};
+  }
+
+  auto previous_signals =
+    calculate_dynamic_signals(previous_trajectory, parameters.finite_difference_epsilon);
+  auto current_signals =
+    calculate_dynamic_signals(current_trajectory, parameters.finite_difference_epsilon);
+
+  if (
+    previous_signals.accelerations.empty() || current_signals.accelerations.empty() ||
+    previous_signals.jerks.empty() || current_signals.jerks.empty() ||
+    previous_signals.yaw_rates.empty() || current_signals.yaw_rates.empty() ||
+    previous_signals.yaw_accelerations.empty() || current_signals.yaw_accelerations.empty()) {
+    return {0.0, false, "unavailable_short_trajectory"};
+  }
+
+  const double acceleration_rms =
+    compute_rms_difference(previous_signals.accelerations, current_signals.accelerations);
+  const double jerk_rms = compute_rms_difference(previous_signals.jerks, current_signals.jerks);
+  const double yaw_rate_rms =
+    compute_rms_difference(previous_signals.yaw_rates, current_signals.yaw_rates);
+  const double yaw_acceleration_rms =
+    compute_rms_difference(previous_signals.yaw_accelerations, current_signals.yaw_accelerations);
+
+  if (
+    acceleration_rms > parameters.max_acceleration_rms || jerk_rms > parameters.max_jerk_rms ||
+    yaw_rate_rms > parameters.max_yaw_rate_rms ||
+    yaw_acceleration_rms > parameters.max_yaw_acceleration_rms) {
+    return {0.0, true, "available"};
+  }
+
+  return {1.0, true, "available"};
+}
+
+}  // namespace autoware::planning_data_analyzer::metrics

planning/autoware_planning_data_analyzer/src/metrics/extended_comfort.hpp

@@ -0,0 +1,54 @@
+// 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__EXTENDED_COMFORT_HPP_
+#define METRICS__EXTENDED_COMFORT_HPP_
+
+#include <autoware_planning_msgs/msg/trajectory.hpp>
+
+#include <string>
+
+namespace autoware::planning_data_analyzer::metrics
+{
+
+struct ExtendedComfortParameters
+{
+  double max_acceleration_rms{0.7};
+  double max_jerk_rms{0.5};
+  double max_yaw_rate_rms{0.1};
+  double max_yaw_acceleration_rms{0.1};
+  double finite_difference_epsilon{1.0e-3};
+};
+
+struct ExtendedComfortResult
+{
+  double score{0.0};
+  bool available{false};
+  std::string reason{"unavailable"};
+};
+
+/**
+ * @brief Compare consecutive planned trajectories using RMS dynamic-signal discrepancies.
+ *
+ * Returns an available binary subscore only when both trajectories are long enough and the
+ * configured thresholds are valid.
+ */
+ExtendedComfortResult calculate_extended_comfort(
+  const autoware_planning_msgs::msg::Trajectory & previous_trajectory,
+  const autoware_planning_msgs::msg::Trajectory & current_trajectory,
+  const ExtendedComfortParameters & parameters);
+
+}  // namespace autoware::planning_data_analyzer::metrics
+
+#endif  // METRICS__EXTENDED_COMFORT_HPP_

planning/autoware_planning_data_analyzer/src/open_loop_evaluator.cpp

@@ -171,14 +171,35 @@ void OpenLoopEvaluator::evaluate(
   }
 
   // Evaluate each trajectory with its ground truth
-  for (const auto & eval_data : evaluation_data_list) {
+  for (std::size_t i = 0; i < evaluation_data_list.size(); ++i) {
+    const auto & eval_data = evaluation_data_list.at(i);
     // Calculate trajectory point metrics
     auto trajectory_metrics = metrics::calculate_trajectory_point_metrics(
       eval_data.synchronized_data, route_handler_, history_comfort_params_, lane_keeping_params_,
       vehicle_info_);
     // Evaluate trajectory
     auto metrics = evaluate_trajectory(eval_data);
     metrics.history_comfort = trajectory_metrics.history_comfort;
+    if (i == 0U) {
+      metrics.extended_comfort = 0.0;
+      metrics.extended_comfort_available = false;
+      metrics.extended_comfort_reason = "unavailable_no_previous_trajectory";
+    } else {
+      const auto & previous_eval_data = evaluation_data_list.at(i - 1U);
+      const auto & previous_trajectory = previous_eval_data.synchronized_data->trajectory;
+      const auto & current_trajectory = eval_data.synchronized_data->trajectory;
+      if (!previous_trajectory || !current_trajectory) {
+        metrics.extended_comfort = 0.0;
+        metrics.extended_comfort_available = false;
+        metrics.extended_comfort_reason = "unavailable_missing_trajectory";
+      } else {
+        const auto extended_comfort_result = metrics::calculate_extended_comfort(
+          *previous_trajectory, *current_trajectory, extended_comfort_parameters_);
+        metrics.extended_comfort = extended_comfort_result.score;
+        metrics.extended_comfort_available = extended_comfort_result.available;
+        metrics.extended_comfort_reason = extended_comfort_result.reason;
+      }
+    }
     metrics.lane_keeping = trajectory_metrics.lane_keeping;
     metrics.lane_keeping_available = trajectory_metrics.lane_keeping_available;
     metrics.lane_keeping_reason = trajectory_metrics.lane_keeping_reason;
@@ -805,15 +826,21 @@ void OpenLoopEvaluator::save_metrics_to_bag(
   std_msgs::msg::Float64 scalar_msg;
   scalar_msg.data = metrics.history_comfort;
   bag_writer.write(scalar_msg, metric_topic("history_comfort"), message_timestamp);
+  scalar_msg.data = metrics.extended_comfort;
+  bag_writer.write(scalar_msg, metric_topic("extended_comfort"), message_timestamp);
   scalar_msg.data = metrics.lane_keeping;
   bag_writer.write(scalar_msg, metric_topic("lane_keeping"), message_timestamp);
   scalar_msg.data = metrics.drivable_area_compliance;
   bag_writer.write(scalar_msg, metric_topic("drivable_area_compliance"), message_timestamp);
   std_msgs::msg::Bool availability_msg;
+  availability_msg.data = metrics.extended_comfort_available;
+  bag_writer.write(availability_msg, metric_topic("extended_comfort_available"), message_timestamp);
   availability_msg.data = metrics.drivable_area_compliance_available;
   bag_writer.write(
     availability_msg, metric_topic("drivable_area_compliance_available"), message_timestamp);
   std_msgs::msg::String reason_msg;
+  reason_msg.data = metrics.extended_comfort_reason;
+  bag_writer.write(reason_msg, metric_topic("extended_comfort_reason"), message_timestamp);
   reason_msg.data = metrics.drivable_area_compliance_reason;
   bag_writer.write(reason_msg, metric_topic("drivable_area_compliance_reason"), message_timestamp);
 
@@ -1060,6 +1087,25 @@ nlohmann::json OpenLoopEvaluator::get_summary_as_json() const
   emit_metric(
     "aggregate", "history_comfort", "Binary history comfort subscore across trajectories [-]",
     history_comfort_values);
+  std::vector<double> extended_comfort_values;
+  extended_comfort_values.reserve(metrics_list_.size());
+  std::size_t extended_comfort_available_count = 0;
+  std::map<std::string, std::size_t> extended_comfort_reason_counts;
+  for (const auto & m : metrics_list_) {
+    ++extended_comfort_reason_counts[m.extended_comfort_reason];
+    if (m.extended_comfort_available) {
+      extended_comfort_values.push_back(m.extended_comfort);
+      ++extended_comfort_available_count;
+    }
+  }
+  emit_metric(
+    "aggregate", "extended_comfort",
+    "Binary extended comfort subscore across consecutive trajectories [-]",
+    extended_comfort_values);
+  j["aggregate/extended_comfort_available_count"] = extended_comfort_available_count;
+  j["aggregate/extended_comfort_unavailable_count"] =
+    metrics_list_.size() - extended_comfort_available_count;
+  j["aggregate/extended_comfort_reason_counts"] = extended_comfort_reason_counts;
   std::vector<double> lane_keeping_values;
   lane_keeping_values.reserve(metrics_list_.size());
   std::size_t lane_keeping_available_count = 0;
@@ -1146,6 +1192,9 @@ nlohmann::json OpenLoopEvaluator::get_full_results_as_json() const
     traj["longitudinal_deviations"] = m.longitudinal_deviations;
     traj["ttc"] = m.ttc;
     traj["history_comfort"] = m.history_comfort;
+    traj["extended_comfort"] = m.extended_comfort;
+    traj["extended_comfort_available"] = m.extended_comfort_available;
+    traj["extended_comfort_reason"] = m.extended_comfort_reason;
     traj["lane_keeping"] = m.lane_keeping;
     traj["lane_keeping_available"] = m.lane_keeping_available;
     traj["lane_keeping_reason"] = m.lane_keeping_reason;
@@ -1220,6 +1269,9 @@ std::vector<std::pair<std::string, std::string>> OpenLoopEvaluator::get_result_t
     {metric_topic("fhe"), "std_msgs/msg/Float64MultiArray"},
     {metric_topic("ttc"), "std_msgs/msg/Float64MultiArray"},
     {metric_topic("history_comfort"), "std_msgs/msg/Float64"},
+    {metric_topic("extended_comfort"), "std_msgs/msg/Float64"},
+    {metric_topic("extended_comfort_available"), "std_msgs/msg/Bool"},
+    {metric_topic("extended_comfort_reason"), "std_msgs/msg/String"},
     {metric_topic("lane_keeping"), "std_msgs/msg/Float64"},
     {metric_topic("drivable_area_compliance"), "std_msgs/msg/Float64"},
     {metric_topic("drivable_area_compliance_available"), "std_msgs/msg/Bool"},