feat(control_evaluator): add a new stop_deviation metric

evaluator/autoware_control_evaluator/CMakeLists.txt

@@ -27,6 +27,6 @@ endif()
 
 ament_auto_package(
   INSTALL_TO_SHARE
-    param
     launch
+    config
 )

evaluator/autoware_control_evaluator/config/control_evaluator.param.yaml

@@ -0,0 +1,17 @@
+/**:
+  ros__parameters:
+    planning_factor_metrics:
+      topic_prefix: /planning/planning_factors/
+      stop_deviation:
+        module_list: # list of modules to be checked for stop deviation. Here we assume that the `module` inside the planning_factors should be the same as the topic name.
+          - blind_spot
+          - crosswalk
+          - detection_area
+          - intersection
+          - merge_from_private
+          - no_drivable_lane
+          - no_stopping_area
+          - stop_line
+          - traffic_light
+          - virtual_traffic_light
+          - walkway

evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp

@@ -30,6 +30,8 @@
 #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
 #include <autoware_internal_debug_msgs/msg/float64_stamped.hpp>
 #include <autoware_internal_planning_msgs/msg/path_with_lane_id.hpp>
+#include <autoware_internal_planning_msgs/msg/planning_factor.hpp>
+#include <autoware_internal_planning_msgs/msg/planning_factor_array.hpp>
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <tier4_metric_msgs/msg/metric.hpp>
@@ -38,6 +40,8 @@
 #include <deque>
 #include <optional>
 #include <string>
+#include <unordered_map>
+#include <unordered_set>
 #include <vector>
 
 namespace control_diagnostics
@@ -57,6 +61,8 @@ using geometry_msgs::msg::AccelWithCovarianceStamped;
 using MetricMsg = tier4_metric_msgs::msg::Metric;
 using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray;
 using autoware_internal_planning_msgs::msg::PathWithLaneId;
+using autoware_internal_planning_msgs::msg::PlanningFactor;
+using autoware_internal_planning_msgs::msg::PlanningFactorArray;
 
 /**
  * @brief Node for control evaluation
@@ -79,7 +85,8 @@ class ControlEvaluatorNode : public rclcpp::Node
   void AddKinematicStateMetricMsg(
     const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped);
   void AddSteeringMetricMsg(const SteeringReport & steering_report);
-
+  void AddStopDeviationMetricMsg(
+    const PlanningFactorArray::ConstSharedPtr & planning_factors, const std::string & module_name);
   void onTimer();
 
 private:
@@ -98,6 +105,11 @@ class ControlEvaluatorNode : public rclcpp::Node
   autoware_utils::InterProcessPollingSubscriber<SteeringReport> steering_sub_{
     this, "~/input/steering_status"};
 
+  std::unordered_map<
+    std::string, autoware_utils::InterProcessPollingSubscriber<PlanningFactorArray>>
+    planning_factors_sub_;
+  std::unordered_set<std::string> stop_deviation_modules_;
+
   rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float64Stamped>::SharedPtr
     processing_time_pub_;
   rclcpp::Publisher<MetricArrayMsg>::SharedPtr metrics_pub_;

evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp

@@ -36,6 +36,7 @@ enum class Metric {
   steering_angle,
   steering_rate,
   steering_acceleration,
+  stop_deviation,
   SIZE,
 };
 
@@ -50,6 +51,7 @@ static const std::unordered_map<std::string, Metric> str_to_metric = {
   {"steering_angle", Metric::steering_angle},
   {"steering_rate", Metric::steering_rate},
   {"steering_acceleration", Metric::steering_acceleration},
+  {"stop_deviation", Metric::stop_deviation},
 };
 
 static const std::unordered_map<Metric, std::string> metric_to_str = {
@@ -63,6 +65,7 @@ static const std::unordered_map<Metric, std::string> metric_to_str = {
   {Metric::steering_angle, "steering_angle"},
   {Metric::steering_rate, "steering_rate"},
   {Metric::steering_acceleration, "steering_acceleration"},
+  {Metric::stop_deviation, "stop_deviation"},
 };
 
 // Metrics descriptions
@@ -77,6 +80,7 @@ static const std::unordered_map<Metric, std::string> metric_descriptions = {
   {Metric::steering_angle, "Steering angle[rad]"},
   {Metric::steering_rate, "Steering angle rate[rad/s]"},
   {Metric::steering_acceleration, "Steering angle acceleration[rad/s^2]"},
+  {Metric::stop_deviation, "Deviation to the stop line when the ego stop by a module[m]"},
 };
 
 namespace details

evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml

@@ -11,6 +11,7 @@
   <!-- control evaluator -->
   <group>
     <node name="control_evaluator" exec="control_evaluator" pkg="autoware_control_evaluator">
+      <param from="$(find-pkg-share autoware_control_evaluator)/config/control_evaluator.param.yaml"/>
       <param name="output_metrics" value="$(var output_metrics)"/>
       <remap from="~/input/odometry" to="$(var input/odometry)"/>
       <remap from="~/input/acceleration" to="$(var input/acceleration)"/>

evaluator/autoware_control_evaluator/package.xml

@@ -16,7 +16,9 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>autoware_internal_planning_msgs</depend>
   <depend>autoware_motion_utils</depend>
+  <depend>autoware_planning_factor_interface</depend>
   <depend>autoware_planning_msgs</depend>
   <depend>autoware_route_handler</depend>
   <depend>autoware_test_utils</depend>
@@ -31,7 +33,6 @@
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>tier4_metric_msgs</depend>
-  <depend>tier4_planning_msgs</depend>
 
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>

evaluator/autoware_control_evaluator/schema/autoware_control_evaluator.schema.json

@@ -0,0 +1,62 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for Autoware Control Evaluator Nodes",
+  "type": "object",
+  "definitions": {
+    "autoware_control_evaluator": {
+      "type": "object",
+      "properties": {
+        "planning_factor_metrics": {
+          "description": "metrics' parameters for planning_factor evaluation",
+          "type": "object",
+          "properties": {
+            "topic_prefix": {
+              "description": "prefix of the topics to subscribe to",
+              "type": "string",
+              "default": "/planning/planning_factors/"
+            },
+            "stop_deviation": {
+              "description": "stop_deviation metrics' parameters",
+              "type": "object",
+              "properties": {
+                "module_list": {
+                  "description": "list of modules' name to evaluate",
+                  "type": "array",
+                  "items": {
+                    "type": "string"
+                  },
+                  "default": [
+                    "blind_spot",
+                    "crosswalk",
+                    "detection_area",
+                    "intersection",
+                    "merge_from_private",
+                    "no_drivable_lane",
+                    "no_stopping_area",
+                    "stop_line",
+                    "traffic_light",
+                    "virtual_traffic_light",
+                    "walkway"
+                  ]
+                }
+              }
+            }
+          }
+        }
+      },
+      "required": ["planning_factor_metrics"]
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/autoware_control_evaluator"
+        }
+      },
+      "required": ["ros__parameters"]
+    }
+  },
+  "required": ["/**"]
+}

evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp

@@ -1,4 +1,4 @@
 // Copyright 2025 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.
@@ -24,9 +24,12 @@
 #include <chrono>
 #include <filesystem>
 #include <fstream>
+#include <functional>
 #include <iostream>
 #include <limits>
+#include <optional>
 #include <string>
+#include <unordered_set>
 #include <vector>
 
 namespace control_diagnostics
@@ -37,6 +40,21 @@
 {
   using std::placeholders::_1;
 
+  // planning_factor subscribers
+  std::vector<std::string> stop_deviation_modules_list =
+    declare_parameter<std::vector<std::string>>(
+      "planning_factor_metrics.stop_deviation.module_list");
+  stop_deviation_modules_ = std::unordered_set<std::string>(
+    stop_deviation_modules_list.begin(), stop_deviation_modules_list.end());
+
+  const std::string topic_prefix =
+    declare_parameter<std::string>("planning_factor_metrics.topic_prefix");
+  for (const auto & module_name : stop_deviation_modules_) {
+    planning_factors_sub_.emplace(
+      module_name, autoware_utils::InterProcessPollingSubscriber<PlanningFactorArray>(
+                     this, topic_prefix + module_name));
+  }
+
   // Publisher
   processing_time_pub_ = this->create_publisher<autoware_internal_debug_msgs::msg::Float64Stamped>(
     "~/debug/processing_time_ms", 1);
@@ -328,6 +346,29 @@
   AddMetricMsg(metric, metric_value);
 }
 
+void ControlEvaluatorNode::AddStopDeviationMetricMsg(
+  const PlanningFactorArray::ConstSharedPtr & planning_factors, const std::string & module_name)
+{
+  std::optional<double> min_distance = std::nullopt;
+  for (const auto & factor : planning_factors->factors) {
+    if (factor.behavior == PlanningFactor::STOP) {
+      for (const auto & control_point : factor.control_points) {
+        if (!min_distance || std::abs(control_point.distance) < std::abs(*min_distance)) {
+          min_distance = control_point.distance;
+        }
+      }
+    }
+  }
+  if (!min_distance) {
+    return;
+  }
+
+  MetricMsg metric_msg;
+  metric_msg.name = "stop_deviation/" + module_name;
+  metric_msg.value = std::to_string(*min_distance);
+  metrics_msg_.metric_array.push_back(metric_msg);
+}
+
 void ControlEvaluatorNode::onTimer()
 {
   autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch;
@@ -337,13 +378,14 @@
   const auto behavior_path = behavior_path_subscriber_.take_data();
   const auto steering_status = steering_sub_.take_data();
 
-  // calculate deviation metrics
+  // add deviation metrics
   if (odom && traj && !traj->points.empty()) {
     const Pose ego_pose = odom->pose.pose;
     AddLateralDeviationMetricMsg(*traj, ego_pose.position);
     AddYawDeviationMetricMsg(*traj, ego_pose);
   }
 
+  // add goal deviation metrics
   getRouteData();
   if (odom && route_handler_.isHandlerReady()) {
     const Pose ego_pose = odom->pose.pose;
@@ -353,15 +395,30 @@
     AddGoalYawDeviationMetricMsg(ego_pose);
   }
 
+  // add planning_factor related metrics
+  for (auto & [module_name, planning_factor_sub_] : planning_factors_sub_) {
+    const auto planning_factors = planning_factor_sub_.take_data();
+    if (!planning_factors || planning_factors->factors.empty()) {
+      continue;
+    }
+
+    if (stop_deviation_modules_.count(module_name) > 0) {
+      AddStopDeviationMetricMsg(planning_factors, module_name);
+    }
+  }
+
+  // add kinematic info
   if (odom && acc) {
     AddKinematicStateMetricMsg(*odom, *acc);
   }
 
+  // add boundary distance metrics
   if (odom && behavior_path) {
     const Pose ego_pose = odom->pose.pose;
     AddBoundaryDistanceMetricMsg(*behavior_path, ego_pose);
   }
 
+  // add steering metrics
   if (steering_status) {
     AddSteeringMetricMsg(*steering_status);
   }

evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp

@@ -19,6 +19,7 @@
 #include "tf2_ros/transform_broadcaster.h"
 
 #include <autoware/control_evaluator/control_evaluator_node.hpp>
+#include <autoware/planning_factor_interface/planning_factor_interface.hpp>
 
 #include "autoware_planning_msgs/msg/trajectory.hpp"
 #include "geometry_msgs/msg/transform_stamped.hpp"
@@ -38,7 +39,10 @@ using EvalNode = control_diagnostics::ControlEvaluatorNode;
 using Trajectory = autoware_planning_msgs::msg::Trajectory;
 using TrajectoryPoint = autoware_planning_msgs::msg::TrajectoryPoint;
 using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray;
+using autoware_internal_planning_msgs::msg::PlanningFactor;
+using autoware_internal_planning_msgs::msg::SafetyFactorArray;
 using geometry_msgs::msg::AccelWithCovarianceStamped;
+using geometry_msgs::msg::Pose;
 using nav_msgs::msg::Odometry;
 
 constexpr double epsilon = 1e-6;
@@ -51,10 +55,13 @@ class EvalTest : public ::testing::Test
     rclcpp::init(0, nullptr);
 
     rclcpp::NodeOptions options;
+    const auto share_dir =
+      ament_index_cpp::get_package_share_directory("autoware_control_evaluator");
     const auto autoware_test_utils_dir =
       ament_index_cpp::get_package_share_directory("autoware_test_utils");
     options.arguments(
-      {"--ros-args", "-p", "output_metrics:=false", "--params-file",
+      {"--ros-args", "-p", "output_metrics:=true", "--params-file",
+       share_dir + "/config/control_evaluator.param.yaml", "--params-file",
        autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml"});
 
     dummy_node = std::make_shared<rclcpp::Node>("control_evaluator_test_node", options);
@@ -76,6 +83,9 @@ class EvalTest : public ::testing::Test
       rclcpp::create_publisher<Odometry>(dummy_node, "/control_evaluator/input/odometry", 1);
     acc_pub_ = rclcpp::create_publisher<AccelWithCovarianceStamped>(
       dummy_node, "/control_evaluator/input/acceleration", 1);
+    planning_factor_interface_ =
+      std::make_unique<autoware::planning_factor_interface::PlanningFactorInterface>(
+        dummy_node.get(), "stop_line");
     tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(dummy_node);
     publishEgoPose(0.0, 0.0, 0.0);
   }
@@ -169,6 +179,23 @@ class EvalTest : public ::testing::Test
     }
     return metric_value_;
   }
+
+  double publishPlanningFactorAndGetStopDeviationMetric(
+    const double distance, const double stop_point_x, const double stop_point_y)
+  {
+    Pose stop_point = Pose();
+    stop_point.position.x = stop_point_x;
+    stop_point.position.y = stop_point_y;
+
+    planning_factor_interface_->add(
+      distance, stop_point, PlanningFactor::STOP, SafetyFactorArray({}));
+    planning_factor_interface_->publish();
+    while (!metric_updated_) {
+      spin_some();
+    }
+    return metric_value_;
+  }
+
   void spin_some()
   {
     rclcpp::spin_some(eval_node);
@@ -186,6 +213,9 @@ class EvalTest : public ::testing::Test
   rclcpp::Publisher<Trajectory>::SharedPtr traj_pub_;
   rclcpp::Publisher<Odometry>::SharedPtr odom_pub_;
   rclcpp::Publisher<AccelWithCovarianceStamped>::SharedPtr acc_pub_;
+  std::unique_ptr<autoware::planning_factor_interface::PlanningFactorInterface>
+    planning_factor_interface_;
+
   rclcpp::Subscription<MetricArrayMsg>::SharedPtr metric_sub_;
   // TF broadcaster
   std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
@@ -248,3 +278,9 @@ TEST_F(EvalTest, TestKinematicStateJerk)
   publishEgoAcc(0.0);
   EXPECT_NEAR(publishEgoAccAndGetMetric(1), 10.0, 0.5);
 }
+
+TEST_F(EvalTest, TestStopDeviation)
+{
+  setTargetMetric("stop_deviation/stop_line");
+  EXPECT_NEAR(publishPlanningFactorAndGetStopDeviationMetric(5.0, 4.0, 3.0), 5.0, epsilon);
+}