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