feat(planning_data_analyzer): add open-loop lane keeping metric

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})

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]
+      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]
 
     # OR Scene Evaluation Configuration (only used when mode="or_scene")
     or_scene_evaluation:

planning/autoware_planning_data_analyzer/package.xml

@@ -19,6 +19,7 @@
 
   <depend>ament_index_cpp</depend>
   <depend>autoware_lanelet2_extension</depend>
+  <depend>autoware_lanelet2_utils</depend>
   <depend>autoware_map_msgs</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_perception_msgs</depend>

planning/autoware_planning_data_analyzer/src/autoware_planning_data_analyzer_node.cpp

@@ -134,6 +134,10 @@ AutowarePlanningDataAnalyzerNode::AutowarePlanningDataAnalyzerNode(
     get_or_declare_parameter<double>(*this, "open_loop.hc.max_yaw_rate");
   history_comfort_params_.max_yaw_acceleration =
     get_or_declare_parameter<double>(*this, "open_loop.hc.max_yaw_acceleration");
+  lane_keeping_params_.max_lateral_deviation =
+    get_or_declare_parameter<double>(*this, "open_loop.lane_keep.max_lateral_deviation");
+  lane_keeping_params_.max_continuous_violation_time =
+    get_or_declare_parameter<double>(*this, "open_loop.lane_keep.max_continuous_violation_time");
   objects_topic_name_ = get_or_declare_parameter<std::string>(*this, "objects_topic");
   tf_topic_name_ = get_or_declare_parameter<std::string>(*this, "tf_topic");
   acceleration_topic_name_ = get_or_declare_parameter<std::string>(*this, "acceleration_topic");
@@ -479,7 +483,7 @@ void AutowarePlanningDataAnalyzerNode::run_evaluation()
         get_or_declare_parameter<std::vector<double>>(*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);

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_;

planning/autoware_planning_data_analyzer/src/metrics/lane_keeping.cpp

@@ -0,0 +1,61 @@
+// 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 <cmath>
+#include <optional>
+#include <vector>
+
+namespace autoware::planning_data_analyzer::metrics
+{
+
+double calculate_lane_keeping_score(
+  const std::vector<LaneKeepingEvaluationPoint> & evaluation_points,
+  const LaneKeepingParameters & parameters)
+{
+  if (
+    evaluation_points.empty() || parameters.max_lateral_deviation < 0.0 ||
+    parameters.max_continuous_violation_time < 0.0) {
+    return 0.0;
+  }
+
+  std::optional<rclcpp::Duration> violation_start_time;
+
+  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(evaluation_point.lateral_deviation) <= parameters.max_lateral_deviation) {
+      violation_start_time.reset();
+      continue;
+    }
+
+    if (!violation_start_time.has_value()) {
+      violation_start_time = evaluation_point.time_from_start;
+    }
+
+    const double violation_duration =
+      (evaluation_point.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

planning/autoware_planning_data_analyzer/src/metrics/lane_keeping.hpp

@@ -0,0 +1,50 @@
+// 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 <rclcpp/rclcpp.hpp>
+
+#include <vector>
+
+namespace autoware::planning_data_analyzer::metrics
+{
+
+struct LaneKeepingParameters
+{
+  double max_lateral_deviation{0.5};
+  double max_continuous_violation_time{2.0};
+};
+
+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<LaneKeepingEvaluationPoint> & evaluation_points,
+  const LaneKeepingParameters & parameters = LaneKeepingParameters{});
+
+}  // namespace autoware::planning_data_analyzer::metrics
+
+#endif  // METRICS__LANE_KEEPING_HPP_

planning/autoware_planning_data_analyzer/src/metrics/trajectory_metrics.cpp

@@ -18,6 +18,7 @@
 #include "history_comfort.hpp"
 
 #include <autoware/lanelet2_utils/geometry.hpp>
+#include <autoware/lanelet2_utils/intersection.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
 #include <autoware_utils_geometry/geometry.hpp>
@@ -27,6 +28,7 @@
 #include <cmath>
 #include <limits>
 #include <memory>
+#include <optional>
 #include <string>
 #include <unordered_set>
 #include <vector>
@@ -221,12 +223,29 @@ lanelet::ConstLanelets collect_route_relevant_lanelets_for_trajectory(
   return drivable_lanelets;
 }
 
+std::optional<lanelet::ConstLanelet> find_reference_lanelet(
+  const geometry_msgs::msg::Pose & pose,
+  const std::shared_ptr<autoware::route_handler::RouteHandler> & 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;
+  }
+
+  return std::nullopt;
+}
+
 }  // namespace
 
 TrajectoryPointMetrics calculate_trajectory_point_metrics(
   const std::shared_ptr<SynchronizedData> & sync_data,
   const std::shared_ptr<autoware::route_handler::RouteHandler> & route_handler,
   const HistoryComfortParameters & history_comfort_params,
+  const LaneKeepingParameters & lane_keeping_params,
   const autoware::vehicle_info_utils::VehicleInfo & vehicle_info)
 {
   TrajectoryPointMetrics metrics;
@@ -286,17 +305,47 @@ TrajectoryPointMetrics calculate_trajectory_point_metrics(
     }
   }
 
-  // Calculate lateral deviation from preferred lane
-  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;
+  std::vector<LaneKeepingEvaluationPoint> 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) {
+    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<double>::quiet_NaN();
+        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())});
       }
     }
   }
+  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) {

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 <autoware/route_handler/route_handler.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info.hpp>
@@ -53,6 +54,9 @@ struct TrajectoryPointMetrics
   std::vector<double> lateral_deviations;
   std::vector<double> 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"};
@@ -68,6 +72,7 @@ TrajectoryPointMetrics calculate_trajectory_point_metrics(
   const std::shared_ptr<SynchronizedData> & sync_data,
   const std::shared_ptr<autoware::route_handler::RouteHandler> & 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{});
 

planning/autoware_planning_data_analyzer/src/open_loop_evaluator.cpp

@@ -174,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_, 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.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;
@@ -801,6 +805,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 +1060,24 @@ nlohmann::json OpenLoopEvaluator::get_summary_as_json() const
   emit_metric(
     "aggregate", "history_comfort", "Binary history comfort subscore across trajectories [-]",
     history_comfort_values);
+  std::vector<double> lane_keeping_values;
+  lane_keeping_values.reserve(metrics_list_.size());
+  std::size_t lane_keeping_available_count = 0;
+  std::map<std::string, std::size_t> lane_keeping_reason_counts;
+  for (const auto & m : metrics_list_) {
+    ++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<double> drivable_area_compliance_values;
   drivable_area_compliance_values.reserve(metrics_list_.size());
   std::size_t drivable_area_compliance_available_count = 0;
@@ -1122,6 +1146,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["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;
@@ -1143,6 +1170,9 @@ 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"]["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;
@@ -1190,6 +1220,7 @@ std::vector<std::pair<std::string, std::string>> OpenLoopEvaluator::get_result_t
     {metric_topic("fhe"), "std_msgs/msg/Float64MultiArray"},
     {metric_topic("ttc"), "std_msgs/msg/Float64MultiArray"},
     {metric_topic("history_comfort"), "std_msgs/msg/Float64"},
+    {metric_topic("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"},