diff --git a/planning/autoware_planning_data_analyzer/CMakeLists.txt b/planning/autoware_planning_data_analyzer/CMakeLists.txt index 029911be3..7d539d5bd 100644 --- a/planning/autoware_planning_data_analyzer/CMakeLists.txt +++ b/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 diff --git a/planning/autoware_planning_data_analyzer/config/planning_data_analyzer.param.yaml b/planning/autoware_planning_data_analyzer/config/planning_data_analyzer.param.yaml index 9d15f81e7..e123d223d 100644 --- a/planning/autoware_planning_data_analyzer/config/planning_data_analyzer.param.yaml +++ b/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). diff --git a/planning/autoware_planning_data_analyzer/src/autoware_planning_data_analyzer_node.cpp b/planning/autoware_planning_data_analyzer/src/autoware_planning_data_analyzer_node.cpp index 0862bb1f0..e0d8d57e9 100644 --- a/planning/autoware_planning_data_analyzer/src/autoware_planning_data_analyzer_node.cpp +++ b/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(*this, "open_loop.gt_trajectory_topic"); gt_sync_tolerance_ms_ = get_or_declare_parameter(*this, "open_loop.gt_sync_tolerance_ms"); + history_comfort_params_.finite_difference_epsilon = + get_or_declare_parameter(*this, "open_loop.hc.finite_difference_epsilon"); + history_comfort_params_.max_longitudinal_acceleration = + get_or_declare_parameter(*this, "open_loop.hc.max_longitudinal_acceleration"); + history_comfort_params_.min_longitudinal_acceleration = + get_or_declare_parameter(*this, "open_loop.hc.min_longitudinal_acceleration"); + history_comfort_params_.max_lateral_acceleration = + get_or_declare_parameter(*this, "open_loop.hc.max_lateral_acceleration"); + history_comfort_params_.max_jerk_magnitude = + get_or_declare_parameter(*this, "open_loop.hc.max_jerk_magnitude"); + history_comfort_params_.max_longitudinal_jerk = + get_or_declare_parameter(*this, "open_loop.hc.max_longitudinal_jerk"); + history_comfort_params_.max_yaw_rate = + get_or_declare_parameter(*this, "open_loop.hc.max_yaw_rate"); + history_comfort_params_.max_yaw_acceleration = + get_or_declare_parameter(*this, "open_loop.hc.max_yaw_acceleration"); objects_topic_name_ = get_or_declare_parameter(*this, "objects_topic"); tf_topic_name_ = get_or_declare_parameter(*this, "tf_topic"); acceleration_topic_name_ = get_or_declare_parameter(*this, "acceleration_topic"); @@ -451,7 +467,8 @@ void AutowarePlanningDataAnalyzerNode::run_evaluation() } const auto evaluation_horizons = get_or_declare_parameter>(*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); diff --git a/planning/autoware_planning_data_analyzer/src/autoware_planning_data_analyzer_node.hpp b/planning/autoware_planning_data_analyzer/src/autoware_planning_data_analyzer_node.hpp index 92d9bab31..79d9bdaf5 100644 --- a/planning/autoware_planning_data_analyzer/src/autoware_planning_data_analyzer_node.hpp +++ b/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_; diff --git a/planning/autoware_planning_data_analyzer/src/metrics/history_comfort.cpp b/planning/autoware_planning_data_analyzer/src/metrics/history_comfort.cpp new file mode 100644 index 000000000..b77b4cedc --- /dev/null +++ b/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 + +#include + +#include + +#include +#include +#include + +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 & values) +{ + if (values.size() > 1U) { + values.back() = values[values.size() - 2]; + } +} + +bool is_within(const std::vector & 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 & 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 diff --git a/planning/autoware_planning_data_analyzer/src/metrics/history_comfort.hpp b/planning/autoware_planning_data_analyzer/src/metrics/history_comfort.hpp new file mode 100644 index 000000000..7b8a2e13f --- /dev/null +++ b/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 + +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_ diff --git a/planning/autoware_planning_data_analyzer/src/metrics/trajectory_metrics.cpp b/planning/autoware_planning_data_analyzer/src/metrics/trajectory_metrics.cpp index ebcc4cb5c..058d7a9e9 100644 --- a/planning/autoware_planning_data_analyzer/src/metrics/trajectory_metrics.cpp +++ b/planning/autoware_planning_data_analyzer/src/metrics/trajectory_metrics.cpp @@ -14,14 +14,14 @@ #include "trajectory_metrics.hpp" +#include "history_comfort.hpp" + #include #include #include #include #include -#include - #include #include #include @@ -191,7 +191,8 @@ double calculate_time_to_collision( TrajectoryPointMetrics calculate_trajectory_point_metrics( const std::shared_ptr & sync_data, - const std::shared_ptr & route_handler) + const std::shared_ptr & 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::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 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 diff --git a/planning/autoware_planning_data_analyzer/src/metrics/trajectory_metrics.hpp b/planning/autoware_planning_data_analyzer/src/metrics/trajectory_metrics.hpp index 29a2ecab3..37c301e3a 100644 --- a/planning/autoware_planning_data_analyzer/src/metrics/trajectory_metrics.hpp +++ b/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 longitudinal_accelerations; std::vector lateral_accelerations; + std::vector lateral_jerks; + std::vector jerk_magnitudes; std::vector longitudinal_jerks; + std::vector yaw_rates; + std::vector yaw_accelerations; std::vector ttc_values; std::vector lateral_deviations; std::vector travel_distances; + double history_comfort{0.0}; }; /** @@ -43,7 +61,8 @@ struct TrajectoryPointMetrics */ TrajectoryPointMetrics calculate_trajectory_point_metrics( const std::shared_ptr & sync_data, - const std::shared_ptr & route_handler = nullptr); + const std::shared_ptr & route_handler = nullptr, + const HistoryComfortParameters & history_comfort_params = HistoryComfortParameters{}); } // namespace autoware::planning_data_analyzer::metrics diff --git a/planning/autoware_planning_data_analyzer/src/open_loop_evaluator.cpp b/planning/autoware_planning_data_analyzer/src/open_loop_evaluator.cpp index b2fe3a98c..f7b954cf9 100644 --- a/planning/autoware_planning_data_analyzer/src/open_loop_evaluator.cpp +++ b/planning/autoware_planning_data_analyzer/src/open_loop_evaluator.cpp @@ -170,13 +170,13 @@ void OpenLoopEvaluator::evaluate( // Evaluate each trajectory with its ground truth for (const auto & eval_data : evaluation_data_list) { + // Calculate trajectory point metrics + auto trajectory_metrics = metrics::calculate_trajectory_point_metrics( + eval_data.synchronized_data, route_handler_, history_comfort_params_); // Evaluate trajectory auto metrics = evaluate_trajectory(eval_data); + metrics.history_comfort = trajectory_metrics.history_comfort; metrics_list_.push_back(metrics); - - // Calculate trajectory point metrics - auto trajectory_metrics = - metrics::calculate_trajectory_point_metrics(eval_data.synchronized_data, route_handler_); trajectory_point_metrics_list_.push_back(trajectory_metrics); // Save to bag if writer provided @@ -792,6 +792,10 @@ void OpenLoopEvaluator::save_metrics_to_bag( array_msg.data = metrics.ttc; bag_writer.write(array_msg, metric_topic("ttc"), message_timestamp); + std_msgs::msg::Float64 scalar_msg; + scalar_msg.data = metrics.history_comfort; + bag_writer.write(scalar_msg, metric_topic("history_comfort"), message_timestamp); + save_dlr_style_result_to_bag(metrics, eval_data, bag_writer); // Save the precomputed ground truth trajectory directly @@ -807,18 +811,49 @@ void OpenLoopEvaluator::save_trajectory_point_metrics_to_bag_with_variant( const metrics::TrajectoryPointMetrics & metrics, rosbag2_cpp::Writer & bag_writer, const rclcpp::Time & normalized_timestamp) const { + { + std_msgs::msg::Float64MultiArray msg; + msg.data = metrics.longitudinal_accelerations; + bag_writer.write( + msg, trajectory_metric_topic("longitudinal_accelerations"), normalized_timestamp); + } + { std_msgs::msg::Float64MultiArray msg; msg.data = metrics.lateral_accelerations; bag_writer.write(msg, trajectory_metric_topic("lateral_accelerations"), normalized_timestamp); } + { + std_msgs::msg::Float64MultiArray msg; + msg.data = metrics.lateral_jerks; + bag_writer.write(msg, trajectory_metric_topic("lateral_jerks"), normalized_timestamp); + } + + { + std_msgs::msg::Float64MultiArray msg; + msg.data = metrics.jerk_magnitudes; + bag_writer.write(msg, trajectory_metric_topic("jerk_magnitudes"), normalized_timestamp); + } + { std_msgs::msg::Float64MultiArray msg; msg.data = metrics.longitudinal_jerks; bag_writer.write(msg, trajectory_metric_topic("longitudinal_jerks"), normalized_timestamp); } + { + std_msgs::msg::Float64MultiArray msg; + msg.data = metrics.yaw_rates; + bag_writer.write(msg, trajectory_metric_topic("yaw_rates"), normalized_timestamp); + } + + { + std_msgs::msg::Float64MultiArray msg; + msg.data = metrics.yaw_accelerations; + bag_writer.write(msg, trajectory_metric_topic("yaw_accelerations"), normalized_timestamp); + } + { std_msgs::msg::Float64MultiArray msg; msg.data = metrics.lateral_deviations; @@ -996,6 +1031,15 @@ nlohmann::json OpenLoopEvaluator::get_summary_as_json() const tag, "min_ttc", "Minimum Time To Collision within " + tag + " horizon [s]", min_ttc_vals); } + std::vector history_comfort_values; + history_comfort_values.reserve(metrics_list_.size()); + for (const auto & m : metrics_list_) { + history_comfort_values.push_back(m.history_comfort); + } + emit_metric( + "aggregate", "history_comfort", "Binary history comfort subscore across trajectories [-]", + history_comfort_values); + return j; } @@ -1042,17 +1086,25 @@ nlohmann::json OpenLoopEvaluator::get_full_results_as_json() const traj["lateral_deviations"] = m.lateral_deviations; traj["longitudinal_deviations"] = m.longitudinal_deviations; traj["ttc"] = m.ttc; + traj["history_comfort"] = m.history_comfort; traj["horizon_results"] = nlohmann::json::object(); fill_horizon_json(traj["horizon_results"], m.horizon_results, m.num_points); if (i < trajectory_point_metrics_list_.size()) { const auto & pm = trajectory_point_metrics_list_[i]; + traj["trajectory_point_metrics"]["longitudinal_accelerations"] = + pm.longitudinal_accelerations; traj["trajectory_point_metrics"]["lateral_accelerations"] = pm.lateral_accelerations; + traj["trajectory_point_metrics"]["lateral_jerks"] = pm.lateral_jerks; + traj["trajectory_point_metrics"]["jerk_magnitudes"] = pm.jerk_magnitudes; traj["trajectory_point_metrics"]["longitudinal_jerks"] = pm.longitudinal_jerks; + traj["trajectory_point_metrics"]["yaw_rates"] = pm.yaw_rates; + traj["trajectory_point_metrics"]["yaw_accelerations"] = pm.yaw_accelerations; traj["trajectory_point_metrics"]["ttc_values"] = pm.ttc_values; traj["trajectory_point_metrics"]["lateral_deviations"] = pm.lateral_deviations; traj["trajectory_point_metrics"]["travel_distances"] = pm.travel_distances; + traj["trajectory_point_metrics"]["history_comfort"] = pm.history_comfort; } trajectories.push_back(traj); @@ -1094,10 +1146,16 @@ std::vector> OpenLoopEvaluator::get_result_t {metric_topic("ahe"), "std_msgs/msg/Float64MultiArray"}, {metric_topic("fhe"), "std_msgs/msg/Float64MultiArray"}, {metric_topic("ttc"), "std_msgs/msg/Float64MultiArray"}, + {metric_topic("history_comfort"), "std_msgs/msg/Float64"}, {metric_topic("lateral_deviation"), "std_msgs/msg/Float64MultiArray"}, {metric_topic("longitudinal_deviation"), "std_msgs/msg/Float64MultiArray"}, + {trajectory_metric_topic("longitudinal_accelerations"), "std_msgs/msg/Float64MultiArray"}, {trajectory_metric_topic("lateral_accelerations"), "std_msgs/msg/Float64MultiArray"}, + {trajectory_metric_topic("lateral_jerks"), "std_msgs/msg/Float64MultiArray"}, + {trajectory_metric_topic("jerk_magnitudes"), "std_msgs/msg/Float64MultiArray"}, {trajectory_metric_topic("longitudinal_jerks"), "std_msgs/msg/Float64MultiArray"}, + {trajectory_metric_topic("yaw_rates"), "std_msgs/msg/Float64MultiArray"}, + {trajectory_metric_topic("yaw_accelerations"), "std_msgs/msg/Float64MultiArray"}, {trajectory_metric_topic("ttc_values"), "std_msgs/msg/Float64MultiArray"}, {trajectory_metric_topic("lateral_deviations"), "std_msgs/msg/Float64MultiArray"}, {trajectory_metric_topic("travel_distances"), "std_msgs/msg/Float64MultiArray"}, diff --git a/planning/autoware_planning_data_analyzer/src/open_loop_evaluator.hpp b/planning/autoware_planning_data_analyzer/src/open_loop_evaluator.hpp index cbea71c04..11b30f631 100644 --- a/planning/autoware_planning_data_analyzer/src/open_loop_evaluator.hpp +++ b/planning/autoware_planning_data_analyzer/src/open_loop_evaluator.hpp @@ -17,6 +17,7 @@ #include "bag_handler.hpp" #include "base_evaluator.hpp" +#include "metrics/trajectory_metrics.hpp" #include #include @@ -60,6 +61,7 @@ struct OpenLoopTrajectoryMetrics std::vector ahe; // Average heading error at each trajectory point [rad] std::vector heading_errors; // Absolute heading error at each trajectory point [rad] std::vector ttc; // Time To Collision at each trajectory point + double history_comfort{0.0}; // Binary comfort subscore for the trajectory // Per-horizon metrics in insertion order: "full" first, then "1s", "2s", ... std::vector> horizon_results; @@ -108,8 +110,10 @@ class OpenLoopEvaluator : public BaseEvaluator rclcpp::Logger logger, std::shared_ptr route_handler = nullptr, GTSourceMode gt_source_mode = GTSourceMode::KINEMATIC_STATE, - double gt_sync_tolerance_ms = 200.0) + double gt_sync_tolerance_ms = 200.0, + metrics::HistoryComfortParameters history_comfort_params = {}) : BaseEvaluator(logger, route_handler), + history_comfort_params_(std::move(history_comfort_params)), gt_source_mode_(gt_source_mode), gt_sync_tolerance_ms_(gt_sync_tolerance_ms) { @@ -295,6 +299,7 @@ class OpenLoopEvaluator : public BaseEvaluator std::vector metrics_list_; std::vector trajectory_point_metrics_list_; + metrics::HistoryComfortParameters history_comfort_params_; OpenLoopEvaluationSummary summary_; std::string metric_variant_; GTSourceMode gt_source_mode_; diff --git a/planning/autoware_planning_data_analyzer/test/test_open_loop_gt_source_mode.cpp b/planning/autoware_planning_data_analyzer/test/test_open_loop_gt_source_mode.cpp index bb9ebb3d1..5340c71cf 100644 --- a/planning/autoware_planning_data_analyzer/test/test_open_loop_gt_source_mode.cpp +++ b/planning/autoware_planning_data_analyzer/test/test_open_loop_gt_source_mode.cpp @@ -161,11 +161,101 @@ TEST_F(OpenLoopGTSourceModeTest, VariantsNamespaceOpenLoopResultTopics) EXPECT_TRUE(has_topic("/open_loop/metrics/raw/ade")); EXPECT_TRUE(has_topic("/open_loop/metrics/raw/ahe")); EXPECT_TRUE(has_topic("/open_loop/metrics/raw/fhe")); + EXPECT_TRUE(has_topic("/open_loop/metrics/raw/history_comfort")); + EXPECT_TRUE(has_topic("/trajectory/raw/longitudinal_accelerations")); EXPECT_TRUE(has_topic("/trajectory/raw/lateral_accelerations")); + EXPECT_TRUE(has_topic("/trajectory/raw/lateral_jerks")); + EXPECT_TRUE(has_topic("/trajectory/raw/jerk_magnitudes")); + EXPECT_TRUE(has_topic("/trajectory/raw/yaw_rates")); + EXPECT_TRUE(has_topic("/trajectory/raw/yaw_accelerations")); EXPECT_TRUE(has_topic("/evaluation/compared_trajectory/raw")); EXPECT_TRUE(has_topic("/driving_log_replayer/time_step_based_trajectory/raw/results")); } +TEST_F(OpenLoopGTSourceModeTest, HistoryComfortIsReportedForComfortableAndUncomfortableTrajectories) +{ + const rclcpp::Time start_time(50, 0); + + auto comfortable_prediction = make_trajectory(start_time, {0.0, 0.5, 1.0, 1.5}); + auto uncomfortable_prediction = make_trajectory(start_time, {0.0, 0.5, 1.0, 1.5}); + const auto gt = std::make_shared(make_trajectory(start_time, {0.0, 0.5, 1.0, 1.5})); + + for (size_t i = 0; i < comfortable_prediction.points.size(); ++i) { + comfortable_prediction.points[i].longitudinal_velocity_mps = 2.0; + uncomfortable_prediction.points[i].longitudinal_velocity_mps = 2.0; + } + + uncomfortable_prediction.points[1].longitudinal_velocity_mps = 3.0; + uncomfortable_prediction.points[2].longitudinal_velocity_mps = 5.0; + uncomfortable_prediction.points[3].longitudinal_velocity_mps = 6.0; + + std::vector> sync_data_list{ + make_sync_data(comfortable_prediction, gt), make_sync_data(uncomfortable_prediction, gt)}; + + OpenLoopEvaluator evaluator( + rclcpp::get_logger("open_loop_gt_source_test"), nullptr, + OpenLoopEvaluator::GTSourceMode::GT_TRAJECTORY, 200.0); + + evaluator.evaluate(sync_data_list, nullptr); + + const auto metrics = evaluator.get_metrics(); + ASSERT_EQ(metrics.size(), 2u); + EXPECT_DOUBLE_EQ(metrics[0].history_comfort, 1.0); + EXPECT_DOUBLE_EQ(metrics[1].history_comfort, 0.0); + + const auto full_json = evaluator.get_full_results_as_json(); + ASSERT_EQ(full_json["trajectories"].size(), 2u); + EXPECT_DOUBLE_EQ(full_json["trajectories"][0]["history_comfort"].get(), 1.0); + EXPECT_DOUBLE_EQ(full_json["trajectories"][1]["history_comfort"].get(), 0.0); + EXPECT_DOUBLE_EQ( + full_json["trajectories"][0]["trajectory_point_metrics"]["history_comfort"].get(), 1.0); + EXPECT_DOUBLE_EQ( + full_json["trajectories"][1]["trajectory_point_metrics"]["history_comfort"].get(), 0.0); + + const auto summary_json = evaluator.get_summary_as_json(); + EXPECT_DOUBLE_EQ(summary_json["aggregate/history_comfort/mean"].get(), 0.5); + EXPECT_DOUBLE_EQ(summary_json["aggregate/history_comfort/min"].get(), 0.0); + EXPECT_DOUBLE_EQ(summary_json["aggregate/history_comfort/max"].get(), 1.0); +} + +TEST_F( + OpenLoopGTSourceModeTest, + HistoryComfortUsesYawRateForLateralAccelerationWhenLateralVelocityIsZero) +{ + const rclcpp::Time start_time(55, 0); + + auto turning_prediction = make_trajectory(start_time, {0.0, 1.0, 2.0, 3.0}); + const auto gt = std::make_shared(make_trajectory(start_time, {0.0, 1.0, 2.0, 3.0})); + + for (size_t i = 0; i < turning_prediction.points.size(); ++i) { + turning_prediction.points[i].longitudinal_velocity_mps = 10.0; + turning_prediction.points[i].lateral_velocity_mps = 0.0; + } + + for (size_t i = 0; i < turning_prediction.points.size(); ++i) { + const double yaw = 0.05 * static_cast(i); + turning_prediction.points[i].pose.orientation.z = std::sin(yaw * 0.5); + turning_prediction.points[i].pose.orientation.w = std::cos(yaw * 0.5); + } + + std::vector> sync_data_list{ + make_sync_data(turning_prediction, gt)}; + + OpenLoopEvaluator evaluator( + rclcpp::get_logger("open_loop_gt_source_test"), nullptr, + OpenLoopEvaluator::GTSourceMode::GT_TRAJECTORY, 200.0); + + evaluator.evaluate(sync_data_list, nullptr); + + const auto metrics = evaluator.get_metrics(); + ASSERT_EQ(metrics.size(), 1u); + EXPECT_DOUBLE_EQ(metrics[0].history_comfort, 0.0); + + const auto full_json = evaluator.get_full_results_as_json(); + ASSERT_EQ(full_json["trajectories"].size(), 1u); + EXPECT_DOUBLE_EQ(full_json["trajectories"][0]["history_comfort"].get(), 0.0); +} + TEST_F(OpenLoopGTSourceModeTest, HeadingMetricsUseWrappedYawErrorPerHorizon) { const rclcpp::Time start_time(40, 0);