From 772c768fe937ee500385c502fcac420ef7e9e9a4 Mon Sep 17 00:00:00 2001 From: Kim Date: Tue, 31 Mar 2026 13:38:44 +0900 Subject: [PATCH 1/2] feat(planning_data_analyzer): add open-loop lane keeping metric --- .../CMakeLists.txt | 2 + .../config/planning_data_analyzer.param.yaml | 3 + .../package.xml | 1 + .../autoware_planning_data_analyzer_node.cpp | 6 +- .../autoware_planning_data_analyzer_node.hpp | 1 + .../src/metrics/lane_keeping.cpp | 59 +++++++++++++ .../src/metrics/lane_keeping.hpp | 44 ++++++++++ .../src/metrics/trajectory_metrics.cpp | 52 +++++++++-- .../src/metrics/trajectory_metrics.hpp | 3 + .../src/open_loop_evaluator.cpp | 17 +++- .../src/open_loop_evaluator.hpp | 5 ++ .../test/metrics/test_lane_keeping.cpp | 88 +++++++++++++++++++ .../test/test_open_loop_gt_source_mode.cpp | 2 + 13 files changed, 274 insertions(+), 9 deletions(-) create mode 100644 planning/autoware_planning_data_analyzer/src/metrics/lane_keeping.cpp create mode 100644 planning/autoware_planning_data_analyzer/src/metrics/lane_keeping.hpp create mode 100644 planning/autoware_planning_data_analyzer/test/metrics/test_lane_keeping.cpp diff --git a/planning/autoware_planning_data_analyzer/CMakeLists.txt b/planning/autoware_planning_data_analyzer/CMakeLists.txt index 6376fe031..2c4b50121 100644 --- a/planning/autoware_planning_data_analyzer/CMakeLists.txt +++ b/planning/autoware_planning_data_analyzer/CMakeLists.txt @@ -12,6 +12,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/or_scene_evaluator.cpp src/metrics/deviation_metrics.cpp src/metrics/history_comfort.cpp + src/metrics/lane_keeping.cpp src/metrics/drivable_area_compliance.cpp src/metrics/trajectory_metrics.cpp src/utils/bag_utils.cpp @@ -31,6 +32,7 @@ if(BUILD_TESTING) ament_add_gtest( test_metrics test/metrics/test_deviation_metrics.cpp + test/metrics/test_lane_keeping.cpp test/metrics/test_drivable_area_compliance.cpp ) target_link_libraries(test_metrics ${PROJECT_NAME}) 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 e123d223d..bf5d7660e 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 @@ -49,6 +49,9 @@ # 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] + lk: + 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] # OR Scene Evaluation Configuration (only used when mode="or_scene") or_scene_evaluation: diff --git a/planning/autoware_planning_data_analyzer/package.xml b/planning/autoware_planning_data_analyzer/package.xml index 9d2dd04e8..711981fd2 100644 --- a/planning/autoware_planning_data_analyzer/package.xml +++ b/planning/autoware_planning_data_analyzer/package.xml @@ -18,6 +18,7 @@ eigen3_cmake_module ament_index_cpp + autoware_lanelet2_utils autoware_lanelet2_extension autoware_map_msgs autoware_motion_utils 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 c8e8eba6f..ba21c51b5 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 @@ -134,6 +134,10 @@ AutowarePlanningDataAnalyzerNode::AutowarePlanningDataAnalyzerNode( 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"); + lane_keeping_params_.max_lateral_deviation = + get_or_declare_parameter(*this, "open_loop.lk.max_lateral_deviation"); + lane_keeping_params_.max_continuous_violation_time = + get_or_declare_parameter(*this, "open_loop.lk.max_continuous_violation_time"); 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"); @@ -479,7 +483,7 @@ void AutowarePlanningDataAnalyzerNode::run_evaluation() get_or_declare_parameter>(*this, "open_loop.evaluation_horizons"); OpenLoopEvaluator evaluator( get_logger(), route_handler_, gt_mode, gt_sync_tolerance_ms_, history_comfort_params_, - vehicle_info_); + lane_keeping_params_, vehicle_info_); 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 e88366479..858508dd7 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 @@ -86,6 +86,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::LaneKeepingParameters lane_keeping_params_; autoware::vehicle_info_utils::VehicleInfo vehicle_info_; std::string objects_topic_name_; std::string tf_topic_name_; diff --git a/planning/autoware_planning_data_analyzer/src/metrics/lane_keeping.cpp b/planning/autoware_planning_data_analyzer/src/metrics/lane_keeping.cpp new file mode 100644 index 000000000..f619fe60a --- /dev/null +++ b/planning/autoware_planning_data_analyzer/src/metrics/lane_keeping.cpp @@ -0,0 +1,59 @@ +// 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 "lane_keeping.hpp" + +#include +#include + +namespace autoware::planning_data_analyzer::metrics +{ + +double calculate_lane_keeping_score( + const std::vector & samples, const LaneKeepingParameters & parameters) +{ + if ( + samples.empty() || parameters.max_lateral_deviation < 0.0 || + parameters.max_continuous_violation_time < 0.0) { + return 0.0; + } + + std::optional violation_start_time; + + for (const auto & sample : samples) { + if (sample.is_in_intersection || !std::isfinite(sample.lateral_deviation)) { + violation_start_time.reset(); + continue; + } + + if (std::abs(sample.lateral_deviation) <= parameters.max_lateral_deviation) { + violation_start_time.reset(); + continue; + } + + if (!violation_start_time.has_value()) { + violation_start_time = sample.time_from_start; + } + + const double violation_duration = + (sample.time_from_start - violation_start_time.value()).seconds(); + if (violation_duration >= parameters.max_continuous_violation_time) { + return 0.0; + } + } + + return 1.0; +} + +} // namespace autoware::planning_data_analyzer::metrics diff --git a/planning/autoware_planning_data_analyzer/src/metrics/lane_keeping.hpp b/planning/autoware_planning_data_analyzer/src/metrics/lane_keeping.hpp new file mode 100644 index 000000000..330ad79e8 --- /dev/null +++ b/planning/autoware_planning_data_analyzer/src/metrics/lane_keeping.hpp @@ -0,0 +1,44 @@ +// 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__LANE_KEEPING_HPP_ +#define METRICS__LANE_KEEPING_HPP_ + +#include + +#include + +namespace autoware::planning_data_analyzer::metrics +{ + +struct LaneKeepingParameters +{ + double max_lateral_deviation{0.5}; + double max_continuous_violation_time{2.0}; +}; + +struct LaneKeepingSample +{ + rclcpp::Duration time_from_start{0, 0}; + double lateral_deviation{0.0}; + bool is_in_intersection{false}; +}; + +double calculate_lane_keeping_score( + const std::vector & samples, + const LaneKeepingParameters & parameters = LaneKeepingParameters{}); + +} // namespace autoware::planning_data_analyzer::metrics + +#endif // METRICS__LANE_KEEPING_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 c2ab44a6c..b5e7cb494 100644 --- a/planning/autoware_planning_data_analyzer/src/metrics/trajectory_metrics.cpp +++ b/planning/autoware_planning_data_analyzer/src/metrics/trajectory_metrics.cpp @@ -18,6 +18,7 @@ #include "history_comfort.hpp" #include +#include #include #include #include @@ -27,6 +28,7 @@ #include #include #include +#include #include #include #include @@ -221,12 +223,36 @@ lanelet::ConstLanelets collect_route_relevant_lanelets_for_trajectory( return drivable_lanelets; } +std::optional find_reference_lanelet( + const geometry_msgs::msg::Pose & pose, + const std::shared_ptr & route_handler) +{ + if (!route_handler || !route_handler->isHandlerReady()) { + return std::nullopt; + } + + lanelet::ConstLanelet closest_lanelet; + if (route_handler->getClosestLaneletWithinRoute(pose, &closest_lanelet)) { + return closest_lanelet; + } + + const auto road_lanelets = route_handler->getRoadLaneletsAtPose(pose); + for (const auto & lanelet : road_lanelets) { + if (route_handler->isRouteLanelet(lanelet)) { + return lanelet; + } + } + + return std::nullopt; +} + } // namespace TrajectoryPointMetrics calculate_trajectory_point_metrics( const std::shared_ptr & sync_data, const std::shared_ptr & route_handler, const HistoryComfortParameters & history_comfort_params, + const LaneKeepingParameters & lane_keeping_params, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { TrajectoryPointMetrics metrics; @@ -286,17 +312,29 @@ TrajectoryPointMetrics calculate_trajectory_point_metrics( } } - // Calculate lateral deviation from preferred lane + std::vector lane_keeping_samples; + lane_keeping_samples.reserve(num_points); + + // Calculate lateral deviation from the local route lane at each pose. if (route_handler && route_handler->isHandlerReady()) { - const auto preferred_lanes = route_handler->getPreferredLanelets(); - if (!preferred_lanes.empty()) { - for (size_t i = 0; i < num_points; ++i) { - const auto arc_coordinates = autoware::experimental::lanelet2_utils::get_arc_coordinates( - preferred_lanes, trajectory.points[i].pose); - metrics.lateral_deviations[i] = arc_coordinates.distance; + for (size_t i = 0; i < num_points; ++i) { + const auto & point = trajectory.points[i]; + const auto reference_lanelet = find_reference_lanelet(point.pose, route_handler); + if (!reference_lanelet.has_value()) { + metrics.lateral_deviations[i] = std::numeric_limits::quiet_NaN(); + continue; } + + metrics.lateral_deviations[i] = + autoware::experimental::lanelet2_utils::get_lateral_distance_to_centerline( + reference_lanelet.value(), point.pose); + lane_keeping_samples.push_back(LaneKeepingSample{ + point.time_from_start, metrics.lateral_deviations[i], + autoware::experimental::lanelet2_utils::is_intersection_lanelet( + reference_lanelet.value())}); } } + metrics.lane_keeping = calculate_lane_keeping_score(lane_keeping_samples, lane_keeping_params); // Calculate travel distances using motion_utils for (size_t i = 0; i < num_points; ++i) { 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 4fa7ae8b2..10f107a28 100644 --- a/planning/autoware_planning_data_analyzer/src/metrics/trajectory_metrics.hpp +++ b/planning/autoware_planning_data_analyzer/src/metrics/trajectory_metrics.hpp @@ -16,6 +16,7 @@ #define METRICS__TRAJECTORY_METRICS_HPP_ #include "../data_types.hpp" +#include "lane_keeping.hpp" #include #include @@ -53,6 +54,7 @@ struct TrajectoryPointMetrics std::vector lateral_deviations; std::vector travel_distances; double history_comfort{0.0}; + double lane_keeping{0.0}; double drivable_area_compliance{0.0}; bool drivable_area_compliance_available{false}; std::string drivable_area_compliance_reason{"unavailable"}; @@ -68,6 +70,7 @@ TrajectoryPointMetrics calculate_trajectory_point_metrics( const std::shared_ptr & sync_data, const std::shared_ptr & route_handler = nullptr, const HistoryComfortParameters & history_comfort_params = HistoryComfortParameters{}, + const LaneKeepingParameters & lane_keeping_params = LaneKeepingParameters{}, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info = autoware::vehicle_info_utils::VehicleInfo{}); 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 7ab844bcc..98fbbad66 100644 --- a/planning/autoware_planning_data_analyzer/src/open_loop_evaluator.cpp +++ b/planning/autoware_planning_data_analyzer/src/open_loop_evaluator.cpp @@ -174,10 +174,12 @@ void OpenLoopEvaluator::evaluate( 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_, vehicle_info_); + 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; + metrics.lane_keeping = trajectory_metrics.lane_keeping; metrics.drivable_area_compliance = trajectory_metrics.drivable_area_compliance; metrics.drivable_area_compliance_available = trajectory_metrics.drivable_area_compliance_available; @@ -801,6 +803,8 @@ 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.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; @@ -1054,6 +1058,14 @@ nlohmann::json OpenLoopEvaluator::get_summary_as_json() const emit_metric( "aggregate", "history_comfort", "Binary history comfort subscore across trajectories [-]", history_comfort_values); + std::vector lane_keeping_values; + lane_keeping_values.reserve(metrics_list_.size()); + for (const auto & m : metrics_list_) { + lane_keeping_values.push_back(m.lane_keeping); + } + emit_metric( + "aggregate", "lane_keeping", "Binary lane keeping subscore across trajectories [-]", + lane_keeping_values); std::vector drivable_area_compliance_values; drivable_area_compliance_values.reserve(metrics_list_.size()); std::size_t drivable_area_compliance_available_count = 0; @@ -1122,6 +1134,7 @@ 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["lane_keeping"] = m.lane_keeping; traj["drivable_area_compliance"] = m.drivable_area_compliance; traj["drivable_area_compliance_available"] = m.drivable_area_compliance_available; traj["drivable_area_compliance_reason"] = m.drivable_area_compliance_reason; @@ -1143,6 +1156,7 @@ nlohmann::json OpenLoopEvaluator::get_full_results_as_json() const 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; + traj["trajectory_point_metrics"]["lane_keeping"] = pm.lane_keeping; traj["trajectory_point_metrics"]["drivable_area_compliance"] = pm.drivable_area_compliance; traj["trajectory_point_metrics"]["drivable_area_compliance_available"] = pm.drivable_area_compliance_available; @@ -1190,6 +1204,7 @@ std::vector> 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("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"}, {metric_topic("drivable_area_compliance_reason"), "std_msgs/msg/String"}, 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 b98a1027d..7be4b726d 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/lane_keeping.hpp" #include "metrics/trajectory_metrics.hpp" #include @@ -63,6 +64,7 @@ struct OpenLoopTrajectoryMetrics 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 + double lane_keeping{0.0}; // Binary lane keeping subscore for the trajectory double drivable_area_compliance{0.0}; // Binary drivable area compliance subscore bool drivable_area_compliance_available{false}; std::string drivable_area_compliance_reason{"unavailable"}; @@ -116,9 +118,11 @@ class OpenLoopEvaluator : public BaseEvaluator GTSourceMode gt_source_mode = GTSourceMode::KINEMATIC_STATE, double gt_sync_tolerance_ms = 200.0, metrics::HistoryComfortParameters history_comfort_params = {}, + metrics::LaneKeepingParameters lane_keeping_params = {}, autoware::vehicle_info_utils::VehicleInfo vehicle_info = {}) : BaseEvaluator(logger, route_handler), history_comfort_params_(std::move(history_comfort_params)), + lane_keeping_params_(std::move(lane_keeping_params)), vehicle_info_(std::move(vehicle_info)), gt_source_mode_(gt_source_mode), gt_sync_tolerance_ms_(gt_sync_tolerance_ms) @@ -306,6 +310,7 @@ class OpenLoopEvaluator : public BaseEvaluator std::vector metrics_list_; std::vector trajectory_point_metrics_list_; metrics::HistoryComfortParameters history_comfort_params_; + metrics::LaneKeepingParameters lane_keeping_params_; autoware::vehicle_info_utils::VehicleInfo vehicle_info_; OpenLoopEvaluationSummary summary_; std::string metric_variant_; diff --git a/planning/autoware_planning_data_analyzer/test/metrics/test_lane_keeping.cpp b/planning/autoware_planning_data_analyzer/test/metrics/test_lane_keeping.cpp new file mode 100644 index 000000000..3f7af717d --- /dev/null +++ b/planning/autoware_planning_data_analyzer/test/metrics/test_lane_keeping.cpp @@ -0,0 +1,88 @@ +// 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 "../../src/metrics/lane_keeping.hpp" + +#include + +#include + +using autoware::planning_data_analyzer::metrics::LaneKeepingParameters; +using autoware::planning_data_analyzer::metrics::LaneKeepingSample; + +namespace +{ + +LaneKeepingSample make_sample( + const double seconds, const double lateral_deviation, const bool is_in_intersection = false) +{ + return LaneKeepingSample{ + rclcpp::Duration::from_seconds(seconds), lateral_deviation, is_in_intersection}; +} + +} // namespace + +TEST(LaneKeepingTest, ReturnsOneWhenAllDeviationsStayWithinThreshold) +{ + const std::vector samples{ + make_sample(0.0, 0.1), make_sample(1.0, -0.3), make_sample(2.0, 0.5)}; + + const auto score = autoware::planning_data_analyzer::metrics::calculate_lane_keeping_score( + samples, LaneKeepingParameters{0.5, 2.0}); + + EXPECT_DOUBLE_EQ(score, 1.0); +} + +TEST(LaneKeepingTest, ReturnsOneForShortThresholdSpike) +{ + const std::vector samples{ + make_sample(0.0, 0.0), make_sample(0.5, 0.8), make_sample(1.0, 0.1)}; + + const auto score = autoware::planning_data_analyzer::metrics::calculate_lane_keeping_score( + samples, LaneKeepingParameters{0.5, 2.0}); + + EXPECT_DOUBLE_EQ(score, 1.0); +} + +TEST(LaneKeepingTest, ReturnsZeroForContinuousViolationLongerThanWindow) +{ + const std::vector samples{ + make_sample(0.0, 0.7), make_sample(1.0, 0.8), make_sample(2.1, 0.9)}; + + const auto score = autoware::planning_data_analyzer::metrics::calculate_lane_keeping_score( + samples, LaneKeepingParameters{0.5, 2.0}); + + EXPECT_DOUBLE_EQ(score, 0.0); +} + +TEST(LaneKeepingTest, IgnoresViolationsInsideIntersections) +{ + const std::vector samples{ + make_sample(0.0, 0.8, true), make_sample(1.0, 0.9, true), make_sample(2.5, 0.1, false)}; + + const auto score = autoware::planning_data_analyzer::metrics::calculate_lane_keeping_score( + samples, LaneKeepingParameters{0.5, 2.0}); + + EXPECT_DOUBLE_EQ(score, 1.0); +} + +TEST(LaneKeepingTest, ReturnsZeroWhenSamplesAreEmpty) +{ + const std::vector samples; + + const auto score = autoware::planning_data_analyzer::metrics::calculate_lane_keeping_score( + samples, LaneKeepingParameters{0.5, 2.0}); + + EXPECT_DOUBLE_EQ(score, 0.0); +} 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 60f3a4ab4..b65658e08 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 @@ -162,6 +162,7 @@ TEST_F(OpenLoopGTSourceModeTest, VariantsNamespaceOpenLoopResultTopics) 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("/open_loop/metrics/raw/lane_keeping")); EXPECT_TRUE(has_topic("/trajectory/raw/longitudinal_accelerations")); EXPECT_TRUE(has_topic("/open_loop/metrics/raw/drivable_area_compliance")); EXPECT_TRUE(has_topic("/trajectory/raw/lateral_accelerations")); @@ -217,6 +218,7 @@ TEST_F(OpenLoopGTSourceModeTest, HistoryComfortIsReportedForComfortableAndUncomf 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); + EXPECT_DOUBLE_EQ(summary_json["aggregate/lane_keeping/mean"].get(), 0.0); } TEST_F(OpenLoopGTSourceModeTest, DACUnavailableReasonIsReportedWhenRouteHandlerIsMissing) From 1cb4555efd62975e63988e08068ce21bec65391c Mon Sep 17 00:00:00 2001 From: Kim Date: Tue, 31 Mar 2026 14:29:44 +0900 Subject: [PATCH 2/2] fix(planning_data_analyzer): address lane keeping review feedback --- .../config/planning_data_analyzer.param.yaml | 2 +- .../package.xml | 2 +- .../autoware_planning_data_analyzer_node.cpp | 4 +- .../src/metrics/lane_keeping.cpp | 16 +++--- .../src/metrics/lane_keeping.hpp | 10 +++- .../src/metrics/trajectory_metrics.cpp | 51 +++++++++++------- .../src/metrics/trajectory_metrics.hpp | 2 + .../src/open_loop_evaluator.cpp | 18 ++++++- .../src/open_loop_evaluator.hpp | 4 +- .../test/metrics/test_lane_keeping.cpp | 52 +++++++++++++------ .../test/test_open_loop_gt_source_mode.cpp | 12 ++++- 11 files changed, 120 insertions(+), 53 deletions(-) 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 bf5d7660e..53a745dfa 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 @@ -49,7 +49,7 @@ # 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] - lk: + 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] diff --git a/planning/autoware_planning_data_analyzer/package.xml b/planning/autoware_planning_data_analyzer/package.xml index 711981fd2..161441d6d 100644 --- a/planning/autoware_planning_data_analyzer/package.xml +++ b/planning/autoware_planning_data_analyzer/package.xml @@ -18,8 +18,8 @@ eigen3_cmake_module ament_index_cpp - autoware_lanelet2_utils autoware_lanelet2_extension + autoware_lanelet2_utils autoware_map_msgs autoware_motion_utils autoware_perception_msgs 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 ba21c51b5..3b28d6ee2 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 @@ -135,9 +135,9 @@ AutowarePlanningDataAnalyzerNode::AutowarePlanningDataAnalyzerNode( history_comfort_params_.max_yaw_acceleration = get_or_declare_parameter(*this, "open_loop.hc.max_yaw_acceleration"); lane_keeping_params_.max_lateral_deviation = - get_or_declare_parameter(*this, "open_loop.lk.max_lateral_deviation"); + get_or_declare_parameter(*this, "open_loop.lane_keep.max_lateral_deviation"); lane_keeping_params_.max_continuous_violation_time = - get_or_declare_parameter(*this, "open_loop.lk.max_continuous_violation_time"); + get_or_declare_parameter(*this, "open_loop.lane_keep.max_continuous_violation_time"); 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"); diff --git a/planning/autoware_planning_data_analyzer/src/metrics/lane_keeping.cpp b/planning/autoware_planning_data_analyzer/src/metrics/lane_keeping.cpp index f619fe60a..b9ba5bc20 100644 --- a/planning/autoware_planning_data_analyzer/src/metrics/lane_keeping.cpp +++ b/planning/autoware_planning_data_analyzer/src/metrics/lane_keeping.cpp @@ -16,38 +16,40 @@ #include #include +#include namespace autoware::planning_data_analyzer::metrics { double calculate_lane_keeping_score( - const std::vector & samples, const LaneKeepingParameters & parameters) + const std::vector & evaluation_points, + const LaneKeepingParameters & parameters) { if ( - samples.empty() || parameters.max_lateral_deviation < 0.0 || + evaluation_points.empty() || parameters.max_lateral_deviation < 0.0 || parameters.max_continuous_violation_time < 0.0) { return 0.0; } std::optional violation_start_time; - for (const auto & sample : samples) { - if (sample.is_in_intersection || !std::isfinite(sample.lateral_deviation)) { + for (const auto & evaluation_point : evaluation_points) { + if (evaluation_point.is_in_intersection || !std::isfinite(evaluation_point.lateral_deviation)) { violation_start_time.reset(); continue; } - if (std::abs(sample.lateral_deviation) <= parameters.max_lateral_deviation) { + if (std::abs(evaluation_point.lateral_deviation) <= parameters.max_lateral_deviation) { violation_start_time.reset(); continue; } if (!violation_start_time.has_value()) { - violation_start_time = sample.time_from_start; + violation_start_time = evaluation_point.time_from_start; } const double violation_duration = - (sample.time_from_start - violation_start_time.value()).seconds(); + (evaluation_point.time_from_start - violation_start_time.value()).seconds(); if (violation_duration >= parameters.max_continuous_violation_time) { return 0.0; } diff --git a/planning/autoware_planning_data_analyzer/src/metrics/lane_keeping.hpp b/planning/autoware_planning_data_analyzer/src/metrics/lane_keeping.hpp index 330ad79e8..909a48388 100644 --- a/planning/autoware_planning_data_analyzer/src/metrics/lane_keeping.hpp +++ b/planning/autoware_planning_data_analyzer/src/metrics/lane_keeping.hpp @@ -28,15 +28,21 @@ struct LaneKeepingParameters double max_continuous_violation_time{2.0}; }; -struct LaneKeepingSample +struct LaneKeepingEvaluationPoint { rclcpp::Duration time_from_start{0, 0}; double lateral_deviation{0.0}; bool is_in_intersection{false}; }; +/** + * @brief Evaluate the binary lane-keeping subscore for one trajectory. + * + * Returns `0.0` when the continuous over-threshold duration reaches the configured limit, + * otherwise returns `1.0`. + */ double calculate_lane_keeping_score( - const std::vector & samples, + const std::vector & evaluation_points, const LaneKeepingParameters & parameters = LaneKeepingParameters{}); } // namespace autoware::planning_data_analyzer::metrics 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 b5e7cb494..58917121f 100644 --- a/planning/autoware_planning_data_analyzer/src/metrics/trajectory_metrics.cpp +++ b/planning/autoware_planning_data_analyzer/src/metrics/trajectory_metrics.cpp @@ -236,13 +236,6 @@ std::optional find_reference_lanelet( return closest_lanelet; } - const auto road_lanelets = route_handler->getRoadLaneletsAtPose(pose); - for (const auto & lanelet : road_lanelets) { - if (route_handler->isRouteLanelet(lanelet)) { - return lanelet; - } - } - return std::nullopt; } @@ -312,29 +305,47 @@ TrajectoryPointMetrics calculate_trajectory_point_metrics( } } - std::vector lane_keeping_samples; - lane_keeping_samples.reserve(num_points); + std::vector lane_keeping_evaluation_points; + lane_keeping_evaluation_points.reserve(num_points); // Calculate lateral deviation from the local route lane at each pose. - if (route_handler && route_handler->isHandlerReady()) { + if (!route_handler) { + metrics.lane_keeping_reason = "unavailable_no_route_handler"; + } else if (!route_handler->isHandlerReady()) { + metrics.lane_keeping_reason = "unavailable_route_handler_not_ready"; + } else { for (size_t i = 0; i < num_points; ++i) { const auto & point = trajectory.points[i]; const auto reference_lanelet = find_reference_lanelet(point.pose, route_handler); if (!reference_lanelet.has_value()) { metrics.lateral_deviations[i] = std::numeric_limits::quiet_NaN(); - continue; + lane_keeping_evaluation_points.push_back( + LaneKeepingEvaluationPoint{point.time_from_start, metrics.lateral_deviations[i], false}); + } else { + metrics.lateral_deviations[i] = + autoware::experimental::lanelet2_utils::get_lateral_distance_to_centerline( + reference_lanelet.value(), point.pose); + lane_keeping_evaluation_points.push_back( + LaneKeepingEvaluationPoint{ + point.time_from_start, metrics.lateral_deviations[i], + autoware::experimental::lanelet2_utils::is_intersection_lanelet( + reference_lanelet.value())}); } - - metrics.lateral_deviations[i] = - autoware::experimental::lanelet2_utils::get_lateral_distance_to_centerline( - reference_lanelet.value(), point.pose); - lane_keeping_samples.push_back(LaneKeepingSample{ - point.time_from_start, metrics.lateral_deviations[i], - autoware::experimental::lanelet2_utils::is_intersection_lanelet( - reference_lanelet.value())}); } } - metrics.lane_keeping = calculate_lane_keeping_score(lane_keeping_samples, lane_keeping_params); + const auto has_finite_lane_keeping_sample = std::any_of( + lane_keeping_evaluation_points.begin(), lane_keeping_evaluation_points.end(), + [](const auto & evaluation_point) { + return std::isfinite(evaluation_point.lateral_deviation); + }); + if (has_finite_lane_keeping_sample) { + metrics.lane_keeping = + calculate_lane_keeping_score(lane_keeping_evaluation_points, lane_keeping_params); + metrics.lane_keeping_available = true; + metrics.lane_keeping_reason = "available"; + } else if (metrics.lane_keeping_reason == "unavailable") { + metrics.lane_keeping_reason = "unavailable_no_reference_lanelet"; + } // Calculate travel distances using motion_utils for (size_t i = 0; i < num_points; ++i) { 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 10f107a28..6471a1909 100644 --- a/planning/autoware_planning_data_analyzer/src/metrics/trajectory_metrics.hpp +++ b/planning/autoware_planning_data_analyzer/src/metrics/trajectory_metrics.hpp @@ -55,6 +55,8 @@ struct TrajectoryPointMetrics std::vector travel_distances; double history_comfort{0.0}; double lane_keeping{0.0}; + bool lane_keeping_available{false}; + std::string lane_keeping_reason{"unavailable"}; double drivable_area_compliance{0.0}; bool drivable_area_compliance_available{false}; std::string drivable_area_compliance_reason{"unavailable"}; 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 98fbbad66..169689613 100644 --- a/planning/autoware_planning_data_analyzer/src/open_loop_evaluator.cpp +++ b/planning/autoware_planning_data_analyzer/src/open_loop_evaluator.cpp @@ -180,6 +180,8 @@ void OpenLoopEvaluator::evaluate( auto metrics = evaluate_trajectory(eval_data); metrics.history_comfort = trajectory_metrics.history_comfort; 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; metrics.drivable_area_compliance = trajectory_metrics.drivable_area_compliance; metrics.drivable_area_compliance_available = trajectory_metrics.drivable_area_compliance_available; @@ -1060,12 +1062,22 @@ nlohmann::json OpenLoopEvaluator::get_summary_as_json() const history_comfort_values); std::vector lane_keeping_values; lane_keeping_values.reserve(metrics_list_.size()); + std::size_t lane_keeping_available_count = 0; + std::map lane_keeping_reason_counts; for (const auto & m : metrics_list_) { - lane_keeping_values.push_back(m.lane_keeping); + ++lane_keeping_reason_counts[m.lane_keeping_reason]; + if (m.lane_keeping_available) { + lane_keeping_values.push_back(m.lane_keeping); + ++lane_keeping_available_count; + } } emit_metric( "aggregate", "lane_keeping", "Binary lane keeping subscore across trajectories [-]", lane_keeping_values); + j["aggregate/lane_keeping_available_count"] = lane_keeping_available_count; + j["aggregate/lane_keeping_unavailable_count"] = + metrics_list_.size() - lane_keeping_available_count; + j["aggregate/lane_keeping_reason_counts"] = lane_keeping_reason_counts; std::vector drivable_area_compliance_values; drivable_area_compliance_values.reserve(metrics_list_.size()); std::size_t drivable_area_compliance_available_count = 0; @@ -1135,6 +1147,8 @@ nlohmann::json OpenLoopEvaluator::get_full_results_as_json() const traj["ttc"] = m.ttc; traj["history_comfort"] = m.history_comfort; traj["lane_keeping"] = m.lane_keeping; + traj["lane_keeping_available"] = m.lane_keeping_available; + traj["lane_keeping_reason"] = m.lane_keeping_reason; traj["drivable_area_compliance"] = m.drivable_area_compliance; traj["drivable_area_compliance_available"] = m.drivable_area_compliance_available; traj["drivable_area_compliance_reason"] = m.drivable_area_compliance_reason; @@ -1157,6 +1171,8 @@ nlohmann::json OpenLoopEvaluator::get_full_results_as_json() const traj["trajectory_point_metrics"]["travel_distances"] = pm.travel_distances; traj["trajectory_point_metrics"]["history_comfort"] = pm.history_comfort; traj["trajectory_point_metrics"]["lane_keeping"] = pm.lane_keeping; + traj["trajectory_point_metrics"]["lane_keeping_available"] = pm.lane_keeping_available; + traj["trajectory_point_metrics"]["lane_keeping_reason"] = pm.lane_keeping_reason; traj["trajectory_point_metrics"]["drivable_area_compliance"] = pm.drivable_area_compliance; traj["trajectory_point_metrics"]["drivable_area_compliance_available"] = pm.drivable_area_compliance_available; 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 7be4b726d..b7d7dc697 100644 --- a/planning/autoware_planning_data_analyzer/src/open_loop_evaluator.hpp +++ b/planning/autoware_planning_data_analyzer/src/open_loop_evaluator.hpp @@ -65,7 +65,9 @@ struct OpenLoopTrajectoryMetrics std::vector ttc; // Time To Collision at each trajectory point double history_comfort{0.0}; // Binary comfort subscore for the trajectory double lane_keeping{0.0}; // Binary lane keeping subscore for the trajectory - double drivable_area_compliance{0.0}; // Binary drivable area compliance subscore + bool lane_keeping_available{false}; + std::string lane_keeping_reason{"unavailable"}; + double drivable_area_compliance{0.0}; // Binary drivable area compliance subscore bool drivable_area_compliance_available{false}; std::string drivable_area_compliance_reason{"unavailable"}; diff --git a/planning/autoware_planning_data_analyzer/test/metrics/test_lane_keeping.cpp b/planning/autoware_planning_data_analyzer/test/metrics/test_lane_keeping.cpp index 3f7af717d..b2a945f08 100644 --- a/planning/autoware_planning_data_analyzer/test/metrics/test_lane_keeping.cpp +++ b/planning/autoware_planning_data_analyzer/test/metrics/test_lane_keeping.cpp @@ -16,18 +16,19 @@ #include +#include #include +using autoware::planning_data_analyzer::metrics::LaneKeepingEvaluationPoint; using autoware::planning_data_analyzer::metrics::LaneKeepingParameters; -using autoware::planning_data_analyzer::metrics::LaneKeepingSample; namespace { -LaneKeepingSample make_sample( +LaneKeepingEvaluationPoint make_evaluation_point( const double seconds, const double lateral_deviation, const bool is_in_intersection = false) { - return LaneKeepingSample{ + return LaneKeepingEvaluationPoint{ rclcpp::Duration::from_seconds(seconds), lateral_deviation, is_in_intersection}; } @@ -35,54 +36,71 @@ LaneKeepingSample make_sample( TEST(LaneKeepingTest, ReturnsOneWhenAllDeviationsStayWithinThreshold) { - const std::vector samples{ - make_sample(0.0, 0.1), make_sample(1.0, -0.3), make_sample(2.0, 0.5)}; + const std::vector evaluation_points{ + make_evaluation_point(0.0, 0.1), make_evaluation_point(1.0, -0.3), + make_evaluation_point(2.0, 0.5)}; const auto score = autoware::planning_data_analyzer::metrics::calculate_lane_keeping_score( - samples, LaneKeepingParameters{0.5, 2.0}); + evaluation_points, LaneKeepingParameters{0.5, 2.0}); EXPECT_DOUBLE_EQ(score, 1.0); } TEST(LaneKeepingTest, ReturnsOneForShortThresholdSpike) { - const std::vector samples{ - make_sample(0.0, 0.0), make_sample(0.5, 0.8), make_sample(1.0, 0.1)}; + const std::vector evaluation_points{ + make_evaluation_point(0.0, 0.0), make_evaluation_point(0.5, 0.8), + make_evaluation_point(1.0, 0.1)}; const auto score = autoware::planning_data_analyzer::metrics::calculate_lane_keeping_score( - samples, LaneKeepingParameters{0.5, 2.0}); + evaluation_points, LaneKeepingParameters{0.5, 2.0}); EXPECT_DOUBLE_EQ(score, 1.0); } TEST(LaneKeepingTest, ReturnsZeroForContinuousViolationLongerThanWindow) { - const std::vector samples{ - make_sample(0.0, 0.7), make_sample(1.0, 0.8), make_sample(2.1, 0.9)}; + const std::vector evaluation_points{ + make_evaluation_point(0.0, 0.7), make_evaluation_point(1.0, 0.8), + make_evaluation_point(2.1, 0.9)}; const auto score = autoware::planning_data_analyzer::metrics::calculate_lane_keeping_score( - samples, LaneKeepingParameters{0.5, 2.0}); + evaluation_points, LaneKeepingParameters{0.5, 2.0}); EXPECT_DOUBLE_EQ(score, 0.0); } TEST(LaneKeepingTest, IgnoresViolationsInsideIntersections) { - const std::vector samples{ - make_sample(0.0, 0.8, true), make_sample(1.0, 0.9, true), make_sample(2.5, 0.1, false)}; + const std::vector evaluation_points{ + make_evaluation_point(0.0, 0.8, true), make_evaluation_point(1.0, 0.9, true), + make_evaluation_point(2.5, 0.1, false)}; const auto score = autoware::planning_data_analyzer::metrics::calculate_lane_keeping_score( - samples, LaneKeepingParameters{0.5, 2.0}); + evaluation_points, LaneKeepingParameters{0.5, 2.0}); EXPECT_DOUBLE_EQ(score, 1.0); } TEST(LaneKeepingTest, ReturnsZeroWhenSamplesAreEmpty) { - const std::vector samples; + const std::vector evaluation_points; const auto score = autoware::planning_data_analyzer::metrics::calculate_lane_keeping_score( - samples, LaneKeepingParameters{0.5, 2.0}); + evaluation_points, LaneKeepingParameters{0.5, 2.0}); EXPECT_DOUBLE_EQ(score, 0.0); } + +TEST(LaneKeepingTest, NonFiniteGapBreaksContinuousViolationWindow) +{ + const std::vector evaluation_points{ + make_evaluation_point(0.0, 0.7), + make_evaluation_point(1.0, std::numeric_limits::quiet_NaN()), + make_evaluation_point(2.1, 0.9)}; + + const auto score = autoware::planning_data_analyzer::metrics::calculate_lane_keeping_score( + evaluation_points, LaneKeepingParameters{0.5, 2.0}); + + EXPECT_DOUBLE_EQ(score, 1.0); +} 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 b65658e08..dc4fb31ac 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 @@ -218,7 +218,8 @@ TEST_F(OpenLoopGTSourceModeTest, HistoryComfortIsReportedForComfortableAndUncomf 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); - EXPECT_DOUBLE_EQ(summary_json["aggregate/lane_keeping/mean"].get(), 0.0); + EXPECT_EQ(summary_json["aggregate/lane_keeping_available_count"].get(), 0u); + EXPECT_EQ(summary_json["aggregate/lane_keeping_unavailable_count"].get(), 2u); } TEST_F(OpenLoopGTSourceModeTest, DACUnavailableReasonIsReportedWhenRouteHandlerIsMissing) @@ -236,12 +237,21 @@ TEST_F(OpenLoopGTSourceModeTest, DACUnavailableReasonIsReportedWhenRouteHandlerI const auto full_json = evaluator.get_full_results_as_json(); ASSERT_EQ(full_json["trajectories"].size(), 1u); + EXPECT_FALSE(full_json["trajectories"][0]["lane_keeping_available"].get()); + EXPECT_EQ( + full_json["trajectories"][0]["lane_keeping_reason"].get(), + "unavailable_no_route_handler"); EXPECT_FALSE(full_json["trajectories"][0]["drivable_area_compliance_available"].get()); EXPECT_EQ( full_json["trajectories"][0]["drivable_area_compliance_reason"].get(), "unavailable_no_route_handler"); const auto summary_json = evaluator.get_summary_as_json(); + ASSERT_TRUE(summary_json.contains("aggregate/lane_keeping_reason_counts")); + EXPECT_EQ( + summary_json["aggregate/lane_keeping_reason_counts"]["unavailable_no_route_handler"] + .get(), + 1u); ASSERT_TRUE(summary_json.contains("aggregate/drivable_area_compliance_reason_counts")); EXPECT_EQ( summary_json["aggregate/drivable_area_compliance_reason_counts"]["unavailable_no_route_handler"]