diff --git a/planning/autoware_planning_data_analyzer/CMakeLists.txt b/planning/autoware_planning_data_analyzer/CMakeLists.txt index 7d539d5bd..6376fe031 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/drivable_area_compliance.cpp src/metrics/trajectory_metrics.cpp src/utils/bag_utils.cpp src/utils/json_utils.cpp @@ -27,7 +28,11 @@ rclcpp_components_register_node(${PROJECT_NAME} if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(test_metrics test/metrics/test_deviation_metrics.cpp) + ament_add_gtest( + test_metrics + test/metrics/test_deviation_metrics.cpp + test/metrics/test_drivable_area_compliance.cpp + ) target_link_libraries(test_metrics ${PROJECT_NAME}) ament_add_gtest(test_offline_evaluation diff --git a/planning/autoware_planning_data_analyzer/launch/planning_data_analyzer.launch.xml b/planning/autoware_planning_data_analyzer/launch/planning_data_analyzer.launch.xml index 167101009..31ca66db1 100644 --- a/planning/autoware_planning_data_analyzer/launch/planning_data_analyzer.launch.xml +++ b/planning/autoware_planning_data_analyzer/launch/planning_data_analyzer.launch.xml @@ -1,6 +1,7 @@ + @@ -8,14 +9,30 @@ - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + 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 e0d8d57e9..c8e8eba6f 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 @@ -84,6 +84,16 @@ AutowarePlanningDataAnalyzerNode::AutowarePlanningDataAnalyzerNode( : Node("autoware_planning_data_analyzer", node_options), route_handler_{std::make_shared()} { + try { + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); + } catch (const std::exception & e) { + RCLCPP_WARN( + get_logger(), + "Vehicle info parameters are unavailable. Drivable area compliance will be marked " + "unavailable: %s", + e.what()); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfo{}; + } setup_evaluation_bag_writer(); // Open bag file @@ -468,7 +478,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_, history_comfort_params_); + get_logger(), route_handler_, gt_mode, gt_sync_tolerance_ms_, history_comfort_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 79d9bdaf5..e88366479 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_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; 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/drivable_area_compliance.cpp b/planning/autoware_planning_data_analyzer/src/metrics/drivable_area_compliance.cpp new file mode 100644 index 000000000..60ec8a7d9 --- /dev/null +++ b/planning/autoware_planning_data_analyzer/src/metrics/drivable_area_compliance.cpp @@ -0,0 +1,192 @@ +// 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 "drivable_area_compliance.hpp" + +#include +#include + +#include + +#include + +#include +#include +#include + +namespace autoware::planning_data_analyzer::metrics +{ + +namespace +{ + +bool is_vehicle_info_valid(const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) +{ + return vehicle_info.vehicle_length_m > 0.0 && vehicle_info.vehicle_width_m > 0.0; +} + +using autoware_utils_geometry::LinearRing2d; +using autoware_utils_geometry::MultiPolygon2d; +using autoware_utils_geometry::Point2d; +using autoware_utils_geometry::Polygon2d; + +std::vector create_vehicle_footprints( + const autoware_planning_msgs::msg::Trajectory & trajectory, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) +{ + const auto base_footprint = vehicle_info.createFootprint(0.0); + + std::vector vehicle_footprints; + vehicle_footprints.reserve(trajectory.points.size()); + for (const auto & point : trajectory.points) { + vehicle_footprints.push_back( + autoware_utils_geometry::transform_vector( + base_footprint, autoware_utils_geometry::pose2transform(point.pose))); + } + + return vehicle_footprints; +} + +LinearRing2d create_hull_from_footprints(const std::vector & footprints) +{ + boost::geometry::model::multi_point combined; + for (const auto & footprint : footprints) { + for (const auto & point : footprint) { + combined.push_back(point); + } + } + + LinearRing2d hull; + boost::geometry::convex_hull(combined, hull); + boost::geometry::correct(hull); + return hull; +} + +lanelet::ConstLanelets collect_candidate_lanelets( + const lanelet::ConstLanelets & drivable_lanelets, + const std::vector & vehicle_footprints) +{ + const auto footprint_hull = create_hull_from_footprints(vehicle_footprints); + + lanelet::ConstLanelets candidate_lanelets; + for (const auto & lanelet : drivable_lanelets) { + if (!boost::geometry::disjoint(lanelet.polygon2d().basicPolygon(), footprint_hull)) { + candidate_lanelets.push_back(lanelet); + } + } + + return candidate_lanelets; +} + +Polygon2d to_polygon_2d(const lanelet::BasicPolygon2d & polygon) +{ + Polygon2d converted; + for (const auto & point : polygon) { + converted.outer().push_back(Point2d(point.x(), point.y())); + } + boost::geometry::correct(converted); + return converted; +} + +std::optional fuse_lanelet_polygons( + const lanelet::ConstLanelets & candidate_lanelets) +{ + if (candidate_lanelets.empty()) { + return std::nullopt; + } + + MultiPolygon2d lanelet_unions; + MultiPolygon2d result; + + for (const auto & lanelet : candidate_lanelets) { + const auto polygon = to_polygon_2d(lanelet.polygon2d().basicPolygon()); + boost::geometry::union_(lanelet_unions, polygon, result); + lanelet_unions = result; + result.clear(); + } + + if (lanelet_unions.empty()) { + return std::nullopt; + } + + for (auto & polygon : lanelet_unions) { + boost::geometry::correct(polygon); + } + + return lanelet_unions; +} + +bool is_footprint_inside_any_polygon( + const LinearRing2d & vehicle_footprint, const MultiPolygon2d & fused_polygons) +{ + return std::any_of( + fused_polygons.begin(), fused_polygons.end(), [&vehicle_footprint](const auto & polygon) { + return boost::geometry::within(vehicle_footprint, polygon); + }); +} + +} // namespace + +DrivableAreaComplianceResult calculate_drivable_area_compliance( + const autoware_planning_msgs::msg::Trajectory & trajectory, + const lanelet::ConstLanelets & drivable_lanelets, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) +{ + DrivableAreaComplianceResult result; + if (trajectory.points.empty()) { + result.reason = "unavailable_empty_trajectory"; + return result; + } + if (drivable_lanelets.empty()) { + result.reason = "unavailable_no_drivable_lanelets"; + return result; + } + if (!is_vehicle_info_valid(vehicle_info)) { + result.reason = "unavailable_invalid_vehicle_info"; + return result; + } + + const auto vehicle_footprints = create_vehicle_footprints(trajectory, vehicle_info); + if (vehicle_footprints.empty() || vehicle_footprints.front().empty()) { + result.reason = "unavailable_invalid_footprint"; + return result; + } + + const auto candidate_lanelets = collect_candidate_lanelets(drivable_lanelets, vehicle_footprints); + if (candidate_lanelets.empty()) { + result.reason = "unavailable_no_candidate_lanelets"; + return result; + } + + const auto fused_lanelet_polygons = fuse_lanelet_polygons(candidate_lanelets); + if (!fused_lanelet_polygons) { + result.reason = "unavailable_invalid_drivable_area_polygon"; + return result; + } + + result.available = true; + result.reason = "compliant"; + + for (const auto & footprint : vehicle_footprints) { + if (!is_footprint_inside_any_polygon(footprint, *fused_lanelet_polygons)) { + result.reason = "non_compliant_footprint_outside_drivable_area"; + return result; + } + } + + result.score = 1.0; + return result; +} + +} // namespace autoware::planning_data_analyzer::metrics diff --git a/planning/autoware_planning_data_analyzer/src/metrics/drivable_area_compliance.hpp b/planning/autoware_planning_data_analyzer/src/metrics/drivable_area_compliance.hpp new file mode 100644 index 000000000..76a863db0 --- /dev/null +++ b/planning/autoware_planning_data_analyzer/src/metrics/drivable_area_compliance.hpp @@ -0,0 +1,43 @@ +// 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__DRIVABLE_AREA_COMPLIANCE_HPP_ +#define METRICS__DRIVABLE_AREA_COMPLIANCE_HPP_ + +#include + +#include + +#include + +#include + +namespace autoware::planning_data_analyzer::metrics +{ + +struct DrivableAreaComplianceResult +{ + double score{0.0}; + bool available{false}; + std::string reason{"unavailable"}; +}; + +DrivableAreaComplianceResult calculate_drivable_area_compliance( + const autoware_planning_msgs::msg::Trajectory & trajectory, + const lanelet::ConstLanelets & drivable_lanelets, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); + +} // namespace autoware::planning_data_analyzer::metrics + +#endif // METRICS__DRIVABLE_AREA_COMPLIANCE_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 058d7a9e9..c2ab44a6c 100644 --- a/planning/autoware_planning_data_analyzer/src/metrics/trajectory_metrics.cpp +++ b/planning/autoware_planning_data_analyzer/src/metrics/trajectory_metrics.cpp @@ -14,6 +14,7 @@ #include "trajectory_metrics.hpp" +#include "drivable_area_compliance.hpp" #include "history_comfort.hpp" #include @@ -26,6 +27,8 @@ #include #include #include +#include +#include #include namespace autoware::planning_data_analyzer::metrics @@ -187,12 +190,44 @@ double calculate_time_to_collision( return std::min(ttc, max_ttc_value); } +void append_unique_lanelet( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets & lanelets, + std::unordered_set & seen_ids) +{ + if (seen_ids.insert(lanelet.id()).second) { + lanelets.push_back(lanelet); + } +} + +lanelet::ConstLanelets collect_route_relevant_lanelets_for_trajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, + const std::shared_ptr & route_handler) +{ + lanelet::ConstLanelets drivable_lanelets; + if (!route_handler || !route_handler->isHandlerReady()) { + return drivable_lanelets; + } + + std::unordered_set seen_ids; + for (const auto & point : trajectory.points) { + for (const auto & lanelet : route_handler->getRoadLaneletsAtPose(point.pose)) { + if (!route_handler->isRouteLanelet(lanelet)) { + continue; + } + append_unique_lanelet(lanelet, drivable_lanelets, seen_ids); + } + } + + return drivable_lanelets; +} + } // namespace TrajectoryPointMetrics calculate_trajectory_point_metrics( const std::shared_ptr & sync_data, const std::shared_ptr & route_handler, - const HistoryComfortParameters & history_comfort_params) + const HistoryComfortParameters & history_comfort_params, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { TrajectoryPointMetrics metrics; @@ -213,6 +248,19 @@ TrajectoryPointMetrics calculate_trajectory_point_metrics( } calculate_history_comfort_metrics(trajectory, history_comfort_params, metrics); + if (!route_handler) { + metrics.drivable_area_compliance_reason = "unavailable_no_route_handler"; + } else if (!route_handler->isHandlerReady()) { + metrics.drivable_area_compliance_reason = "unavailable_route_handler_not_ready"; + } else { + const auto drivable_lanelets = + collect_route_relevant_lanelets_for_trajectory(trajectory, route_handler); + const auto drivable_area_compliance = + calculate_drivable_area_compliance(trajectory, drivable_lanelets, vehicle_info); + metrics.drivable_area_compliance = drivable_area_compliance.score; + metrics.drivable_area_compliance_available = drivable_area_compliance.available; + metrics.drivable_area_compliance_reason = drivable_area_compliance.reason; + } // 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 37c301e3a..4fa7ae8b2 100644 --- a/planning/autoware_planning_data_analyzer/src/metrics/trajectory_metrics.hpp +++ b/planning/autoware_planning_data_analyzer/src/metrics/trajectory_metrics.hpp @@ -18,8 +18,10 @@ #include "../data_types.hpp" #include +#include #include +#include #include namespace autoware::planning_data_analyzer::metrics @@ -51,6 +53,9 @@ struct TrajectoryPointMetrics std::vector lateral_deviations; std::vector travel_distances; double history_comfort{0.0}; + double drivable_area_compliance{0.0}; + bool drivable_area_compliance_available{false}; + std::string drivable_area_compliance_reason{"unavailable"}; }; /** @@ -62,7 +67,9 @@ struct TrajectoryPointMetrics 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 HistoryComfortParameters & history_comfort_params = HistoryComfortParameters{}, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info = + autoware::vehicle_info_utils::VehicleInfo{}); } // 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 f7b954cf9..7ab844bcc 100644 --- a/planning/autoware_planning_data_analyzer/src/open_loop_evaluator.cpp +++ b/planning/autoware_planning_data_analyzer/src/open_loop_evaluator.cpp @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -36,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -172,10 +174,14 @@ 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_); + eval_data.synchronized_data, route_handler_, history_comfort_params_, vehicle_info_); // Evaluate trajectory auto metrics = evaluate_trajectory(eval_data); metrics.history_comfort = trajectory_metrics.history_comfort; + metrics.drivable_area_compliance = trajectory_metrics.drivable_area_compliance; + metrics.drivable_area_compliance_available = + trajectory_metrics.drivable_area_compliance_available; + metrics.drivable_area_compliance_reason = trajectory_metrics.drivable_area_compliance_reason; metrics_list_.push_back(metrics); trajectory_point_metrics_list_.push_back(trajectory_metrics); @@ -795,6 +801,15 @@ 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.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.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.drivable_area_compliance_reason; + bag_writer.write(reason_msg, metric_topic("drivable_area_compliance_reason"), message_timestamp); save_dlr_style_result_to_bag(metrics, eval_data, bag_writer); @@ -1039,6 +1054,26 @@ nlohmann::json OpenLoopEvaluator::get_summary_as_json() const emit_metric( "aggregate", "history_comfort", "Binary history comfort subscore across trajectories [-]", history_comfort_values); + std::vector drivable_area_compliance_values; + drivable_area_compliance_values.reserve(metrics_list_.size()); + std::size_t drivable_area_compliance_available_count = 0; + std::map drivable_area_compliance_reason_counts; + for (const auto & m : metrics_list_) { + ++drivable_area_compliance_reason_counts[m.drivable_area_compliance_reason]; + if (m.drivable_area_compliance_available) { + drivable_area_compliance_values.push_back(m.drivable_area_compliance); + ++drivable_area_compliance_available_count; + } + } + emit_metric( + "aggregate", "drivable_area_compliance", + "Binary drivable area compliance subscore across trajectories [-]", + drivable_area_compliance_values); + j["aggregate/drivable_area_compliance_available_count"] = + drivable_area_compliance_available_count; + j["aggregate/drivable_area_compliance_unavailable_count"] = + metrics_list_.size() - drivable_area_compliance_available_count; + j["aggregate/drivable_area_compliance_reason_counts"] = drivable_area_compliance_reason_counts; return j; } @@ -1087,6 +1122,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["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; traj["horizon_results"] = nlohmann::json::object(); fill_horizon_json(traj["horizon_results"], m.horizon_results, m.num_points); @@ -1105,6 +1143,11 @@ 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"]["drivable_area_compliance"] = pm.drivable_area_compliance; + traj["trajectory_point_metrics"]["drivable_area_compliance_available"] = + pm.drivable_area_compliance_available; + traj["trajectory_point_metrics"]["drivable_area_compliance_reason"] = + pm.drivable_area_compliance_reason; } trajectories.push_back(traj); @@ -1147,6 +1190,9 @@ 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("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"}, {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"}, 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 11b30f631..b98a1027d 100644 --- a/planning/autoware_planning_data_analyzer/src/open_loop_evaluator.hpp +++ b/planning/autoware_planning_data_analyzer/src/open_loop_evaluator.hpp @@ -20,6 +20,7 @@ #include "metrics/trajectory_metrics.hpp" #include +#include #include #include #include @@ -62,6 +63,9 @@ 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 drivable_area_compliance{0.0}; // Binary drivable area compliance subscore + bool drivable_area_compliance_available{false}; + std::string drivable_area_compliance_reason{"unavailable"}; // Per-horizon metrics in insertion order: "full" first, then "1s", "2s", ... std::vector> horizon_results; @@ -111,9 +115,11 @@ class OpenLoopEvaluator : public BaseEvaluator std::shared_ptr route_handler = nullptr, GTSourceMode gt_source_mode = GTSourceMode::KINEMATIC_STATE, double gt_sync_tolerance_ms = 200.0, - metrics::HistoryComfortParameters history_comfort_params = {}) + metrics::HistoryComfortParameters history_comfort_params = {}, + autoware::vehicle_info_utils::VehicleInfo vehicle_info = {}) : BaseEvaluator(logger, route_handler), history_comfort_params_(std::move(history_comfort_params)), + vehicle_info_(std::move(vehicle_info)), gt_source_mode_(gt_source_mode), gt_sync_tolerance_ms_(gt_sync_tolerance_ms) { @@ -300,6 +306,7 @@ class OpenLoopEvaluator : public BaseEvaluator std::vector metrics_list_; std::vector trajectory_point_metrics_list_; metrics::HistoryComfortParameters history_comfort_params_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; OpenLoopEvaluationSummary summary_; std::string metric_variant_; GTSourceMode gt_source_mode_; diff --git a/planning/autoware_planning_data_analyzer/test/metrics/test_drivable_area_compliance.cpp b/planning/autoware_planning_data_analyzer/test/metrics/test_drivable_area_compliance.cpp new file mode 100644 index 000000000..3a071558c --- /dev/null +++ b/planning/autoware_planning_data_analyzer/test/metrics/test_drivable_area_compliance.cpp @@ -0,0 +1,96 @@ +// 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/drivable_area_compliance.hpp" + +#include + +#include +#include +#include +#include + +#include + +namespace +{ + +autoware_planning_msgs::msg::Trajectory make_trajectory(const double lateral_offset) +{ + autoware_planning_msgs::msg::Trajectory trajectory; + trajectory.header.frame_id = "map"; + + for (std::size_t i = 0; i < 3; ++i) { + autoware_planning_msgs::msg::TrajectoryPoint point; + point.pose.position.x = 3.0 + static_cast(i) * 2.0; + point.pose.position.y = lateral_offset; + point.pose.orientation.w = 1.0; + point.longitudinal_velocity_mps = 2.0; + point.time_from_start.nanosec = static_cast(i * 100000000UL); + trajectory.points.push_back(point); + } + + return trajectory; +} + +lanelet::ConstLanelets make_drivable_lanelets() +{ + lanelet::LineString3d left_bound{ + lanelet::InvalId, + {lanelet::Point3d{lanelet::InvalId, -5.0, 2.0, 0.0}, + lanelet::Point3d{lanelet::InvalId, 12.0, 2.0, 0.0}}}; + lanelet::LineString3d right_bound{ + lanelet::InvalId, + {lanelet::Point3d{lanelet::InvalId, -5.0, -2.0, 0.0}, + lanelet::Point3d{lanelet::InvalId, 12.0, -2.0, 0.0}}}; + return lanelet::ConstLanelets{lanelet::ConstLanelet{lanelet::InvalId, left_bound, right_bound}}; +} + +autoware::vehicle_info_utils::VehicleInfo make_vehicle_info() +{ + return autoware::vehicle_info_utils::createVehicleInfo( + 0.3, 0.2, 2.5, 1.6, 1.0, 1.0, 0.5, 0.5, 1.8, 0.7); +} + +} // namespace + +TEST(DrivableAreaComplianceTest, ReturnsOneWhenFullFootprintStaysInsideFusedDrivableArea) +{ + const auto result = autoware::planning_data_analyzer::metrics::calculate_drivable_area_compliance( + make_trajectory(0.0), make_drivable_lanelets(), make_vehicle_info()); + + EXPECT_TRUE(result.available); + EXPECT_DOUBLE_EQ(result.score, 1.0); + EXPECT_EQ(result.reason, "compliant"); +} + +TEST(DrivableAreaComplianceTest, ReturnsZeroWhenAnyFootprintLeavesFusedDrivableArea) +{ + const auto result = autoware::planning_data_analyzer::metrics::calculate_drivable_area_compliance( + make_trajectory(1.5), make_drivable_lanelets(), make_vehicle_info()); + + EXPECT_TRUE(result.available); + EXPECT_DOUBLE_EQ(result.score, 0.0); + EXPECT_EQ(result.reason, "non_compliant_footprint_outside_drivable_area"); +} + +TEST(DrivableAreaComplianceTest, ReturnsUnavailableWhenRequiredInputsAreMissing) +{ + const auto result = autoware::planning_data_analyzer::metrics::calculate_drivable_area_compliance( + make_trajectory(0.0), {}, autoware::vehicle_info_utils::VehicleInfo{}); + + EXPECT_FALSE(result.available); + EXPECT_DOUBLE_EQ(result.score, 0.0); + EXPECT_EQ(result.reason, "unavailable_no_drivable_lanelets"); +} 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 5340c71cf..60f3a4ab4 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 @@ -163,6 +163,7 @@ TEST_F(OpenLoopGTSourceModeTest, VariantsNamespaceOpenLoopResultTopics) 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("/open_loop/metrics/raw/drivable_area_compliance")); EXPECT_TRUE(has_topic("/trajectory/raw/lateral_accelerations")); EXPECT_TRUE(has_topic("/trajectory/raw/lateral_jerks")); EXPECT_TRUE(has_topic("/trajectory/raw/jerk_magnitudes")); @@ -218,6 +219,34 @@ TEST_F(OpenLoopGTSourceModeTest, HistoryComfortIsReportedForComfortableAndUncomf EXPECT_DOUBLE_EQ(summary_json["aggregate/history_comfort/max"].get(), 1.0); } +TEST_F(OpenLoopGTSourceModeTest, DACUnavailableReasonIsReportedWhenRouteHandlerIsMissing) +{ + const rclcpp::Time start_time(60, 0); + const auto prediction = make_trajectory(start_time, {0.0, 1.0, 2.0}); + const auto gt = std::make_shared(make_trajectory(start_time, {0.0, 1.0, 2.0})); + std::vector> sync_data_list{make_sync_data(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 full_json = evaluator.get_full_results_as_json(); + ASSERT_EQ(full_json["trajectories"].size(), 1u); + 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/drivable_area_compliance_reason_counts")); + EXPECT_EQ( + summary_json["aggregate/drivable_area_compliance_reason_counts"]["unavailable_no_route_handler"] + .get(), + 1u); +} + TEST_F( OpenLoopGTSourceModeTest, HistoryComfortUsesYawRateForLateralAccelerationWhenLateralVelocityIsZero) @@ -315,4 +344,9 @@ TEST_F(OpenLoopGTSourceModeTest, HeadingMetricsUseWrappedYawErrorPerHorizon) EXPECT_NEAR(summary_json["full/fhe/mean"].get(), 0.4, 1e-6); EXPECT_NEAR(summary_json["0.2s/ahe/mean"].get(), 0.233333333333, 1e-6); EXPECT_NEAR(summary_json["0.2s/fhe/mean"].get(), 0.3, 1e-6); + EXPECT_DOUBLE_EQ(summary_json["aggregate/drivable_area_compliance/mean"].get(), 0.0); + EXPECT_EQ( + summary_json["aggregate/drivable_area_compliance_available_count"].get(), 0u); + EXPECT_EQ( + summary_json["aggregate/drivable_area_compliance_unavailable_count"].get(), 1u); }