From 28faf90afb170008d2ccbafe5297189d720e5189 Mon Sep 17 00:00:00 2001
From: t4-adc <grp-rd-1-adc-admin@tier4.jp>
Date: Fri, 17 Jan 2025 18:25:43 +0900
Subject: [PATCH 1/8] add boundary_distance metric

Signed-off-by: xtk8532704 <1041084556@qq.com>
---
 .../autoware_control_evaluator/CMakeLists.txt |   3 +-
 .../control_evaluator_node.hpp                |   8 +-
 .../metrics/deviation_metrics.hpp             |   2 +-
 .../control_evaluator/metrics/metric.hpp      |  11 +-
 .../metrics/metrics_utils.hpp                 |  72 ++++++++++++
 .../autoware_control_evaluator/package.xml    |   1 +
 .../src/control_evaluator_node.cpp            |  60 +++++++---
 .../src/metrics/deviation_metrics.cpp         |   2 +-
 .../src/metrics/metrics_utils.cpp             | 105 ++++++++++++++++++
 .../test/test_control_evaluator_node.cpp      |   2 +-
 10 files changed, 245 insertions(+), 21 deletions(-)
 create mode 100644 evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp
 create mode 100644 evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp

diff --git a/evaluator/autoware_control_evaluator/CMakeLists.txt b/evaluator/autoware_control_evaluator/CMakeLists.txt
index 5d6474de88015..e01f1d2bba1f8 100644
--- a/evaluator/autoware_control_evaluator/CMakeLists.txt
+++ b/evaluator/autoware_control_evaluator/CMakeLists.txt
@@ -7,8 +7,7 @@ autoware_package()
 find_package(pluginlib REQUIRED)
 
 ament_auto_add_library(control_evaluator_node SHARED
-  src/control_evaluator_node.cpp
-  src/metrics/deviation_metrics.cpp
+  DIRECTORY src
 )
 
 rclcpp_components_register_node(control_evaluator_node
diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp
index 9606dea577bd0..91398ddfa94ea 100644
--- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp
+++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp
@@ -1,4 +1,4 @@
-// Copyright 2024 Tier IV, Inc.
+// 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.
@@ -22,6 +22,7 @@
 #include <autoware/route_handler/route_handler.hpp>
 #include <autoware/universe_utils/ros/polling_subscriber.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
+#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
@@ -39,6 +40,7 @@
 namespace control_diagnostics
 {
 using autoware::universe_utils::Accumulator;
+using autoware::vehicle_info_utils::VehicleInfo;
 using autoware_planning_msgs::msg::Trajectory;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
@@ -64,8 +66,9 @@ class ControlEvaluatorNode : public rclcpp::Node
   void AddGoalLongitudinalDeviationMetricMsg(const Pose & ego_pose);
   void AddGoalLateralDeviationMetricMsg(const Pose & ego_pose);
   void AddGoalYawDeviationMetricMsg(const Pose & ego_pose);
-
   void AddLaneletMetricMsg(const Pose & ego_pose);
+
+  void AddLaneletInfoMsg(const Pose & ego_pose);
   void AddKinematicStateMetricMsg(
     const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped);
 
@@ -106,6 +109,7 @@ class ControlEvaluatorNode : public rclcpp::Node
     metric_accumulators_;  // 3(min, max, mean) * metric_size
 
   MetricArrayMsg metrics_msg_;
+  VehicleInfo vehicle_info_;
   autoware::route_handler::RouteHandler route_handler_;
   rclcpp::TimerBase::SharedPtr timer_;
   std::optional<AccelWithCovarianceStamped> prev_acc_stamped_{std::nullopt};
diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp
index e0e04b0a65070..514dfcc69ae35 100644
--- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp
+++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp
@@ -1,4 +1,4 @@
-// Copyright 2024 TIER IV, Inc.
+// 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.
diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp
index be2f3135d7f7e..dddf07a6710b3 100644
--- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp
+++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp
@@ -31,6 +31,8 @@ enum class Metric {
   goal_longitudinal_deviation,
   goal_lateral_deviation,
   goal_yaw_deviation,
+  left_boundary_distance,
+  right_boundary_distance,
   SIZE,
 };
 
@@ -40,6 +42,8 @@ static const std::unordered_map<std::string, Metric> str_to_metric = {
   {"goal_longitudinal_deviation", Metric::goal_longitudinal_deviation},
   {"goal_lateral_deviation", Metric::goal_lateral_deviation},
   {"goal_yaw_deviation", Metric::goal_yaw_deviation},
+  {"left_boundary_distance", Metric::left_boundary_distance},
+  {"right_boundary_distance", Metric::right_boundary_distance},
 };
 
 static const std::unordered_map<Metric, std::string> metric_to_str = {
@@ -48,6 +52,8 @@ static const std::unordered_map<Metric, std::string> metric_to_str = {
   {Metric::goal_longitudinal_deviation, "goal_longitudinal_deviation"},
   {Metric::goal_lateral_deviation, "goal_lateral_deviation"},
   {Metric::goal_yaw_deviation, "goal_yaw_deviation"},
+  {Metric::left_boundary_distance, "left_boundary_distance"},
+  {Metric::right_boundary_distance, "right_boundary_distance"},
 };
 
 // Metrics descriptions
@@ -56,7 +62,10 @@ static const std::unordered_map<Metric, std::string> metric_descriptions = {
   {Metric::yaw_deviation, "Yaw deviation from the reference trajectory[rad]"},
   {Metric::goal_longitudinal_deviation, "Longitudinal deviation from the goal point[m]"},
   {Metric::goal_lateral_deviation, "Lateral deviation from the goal point[m]"},
-  {Metric::goal_yaw_deviation, "Yaw deviation from the goal point[rad]"}};
+  {Metric::goal_yaw_deviation, "Yaw deviation from the goal point[rad]"},
+  {Metric::left_boundary_distance, "Signed distance to the left boundary[m]"},
+  {Metric::right_boundary_distance, "Signed istance to the right boundary[m]"},
+};
 
 namespace details
 {
diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp
new file mode 100644
index 0000000000000..f2f2b2d71fc79
--- /dev/null
+++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp
@@ -0,0 +1,72 @@
+// 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.
+// 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 AUTOWARE__CONTROL_EVALUATOR__METRICS__METRICS_UTILS_HPP_
+#define AUTOWARE__CONTROL_EVALUATOR__METRICS__METRICS_UTILS_HPP_
+
+#include <autoware/route_handler/route_handler.hpp>
+#include <autoware/universe_utils/geometry/boost_geometry.hpp>
+
+namespace control_diagnostics
+{
+namespace metrics
+{
+namespace utils
+{
+using autoware::route_handler::RouteHandler;
+using autoware::universe_utils::Point2d;
+using geometry_msgs::msg::Pose;
+
+/**
+ * @brief Get the closest lanelets to the ego vehicle, considering shoulder lanelets.
+ * @param [in] route_handler route handler
+ * @param [in] ego_pose ego vehicle pose
+ * @return closest lanelets to the ego vehicle
+ **/
+lanelet::ConstLanelets get_current_lanes(const RouteHandler & route_handler, const Pose & ego_pose);
+
+/**
+ * @brief Get the most side boundary of the lanelet.
+ * @param [in] route_handler route handler
+ * @param [in] lanelet lanelet to get the most side boundary
+ * @param [in] left_side true to get left side boundary, false to get right side boundary
+ * @param [in] enable_same_root true if the same root lanelets are considered, false otherwise
+ * @param [in] get_shoulder_lane true if shoulder lanelets are considered, false otherwise
+ * @return most side boundary of the lanelet
+ **/
+lanelet::ConstLineString3d get_most_side_boundary(
+  const RouteHandler & route_handler, const lanelet::ConstLanelet & lanelet, const bool left_side,
+  const bool enable_same_root = false, const bool get_shoulder_lane = true);
+
+/**
+ * @brief Calculate the Euler distance between the vehicle and the lanelet.
+ * @param [in] vehicle_footprint vehicle footprint
+ * @param [in] line lanelet line
+ * @return distance between the vehicle footprint and the lanelet, 0.0 if the vehicle intersects
+ *with the line
+ **/
+double calc_distance_to_line(const autoware::universe_utils::LinearRing2d & vehicle_footprint, const lanelet::ConstLineString3d & line);
+
+/**
+ * @brief Calculate the yaw deviation between the ego's orientation and the vector from the ego position to the closest point on the line.
+ * @param [in] ego_pose The pose of the ego vehicle.
+ * @param [in] line The line to which the yaw deviation is calculated.
+ * @return The yaw deviation in radians, normalized to the range [-π, π].
+ */
+double calc_yaw_to_line(const Pose & ego_pose, const lanelet::ConstLineString3d & line);
+
+}  // namespace utils
+}  // namespace metrics
+}  // namespace control_diagnostics
+#endif  // AUTOWARE__CONTROL_EVALUATOR__METRICS__METRICS_UTILS_HPP_
\ No newline at end of file
diff --git a/evaluator/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml
index a636a55bc5efd..24c33ceb56b2b 100644
--- a/evaluator/autoware_control_evaluator/package.xml
+++ b/evaluator/autoware_control_evaluator/package.xml
@@ -21,6 +21,7 @@
   <depend>autoware_route_handler</depend>
   <depend>autoware_test_utils</depend>
   <depend>autoware_universe_utils</depend>
+  <depend>autoware_vehicle_info_utils</depend>
   <depend>nav_msgs</depend>
   <depend>nlohmann-json-dev</depend>
   <depend>pluginlib</depend>
diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp
index 79e89423af6dd..d840a293c3dd5 100644
--- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp
+++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp
@@ -1,4 +1,4 @@
-// Copyright 2024 Tier IV, Inc.
+// 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.
@@ -14,6 +14,9 @@
 
 #include "autoware/control_evaluator/control_evaluator_node.hpp"
 
+#include "autoware/control_evaluator/metrics/metrics_utils.hpp"
+
+#include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware_lanelet2_extension/utility/query.hpp>
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
 #include <nlohmann/json.hpp>
@@ -29,7 +32,8 @@
 namespace control_diagnostics
 {
 ControlEvaluatorNode::ControlEvaluatorNode(const rclcpp::NodeOptions & node_options)
-: Node("control_evaluator", node_options)
+: Node("control_evaluator", node_options),
+  vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo())
 {
   using std::placeholders::_1;
 
@@ -136,17 +140,9 @@ void ControlEvaluatorNode::AddMetricMsg(const Metric & metric, const double & me
   }
 }
 
-void ControlEvaluatorNode::AddLaneletMetricMsg(const Pose & ego_pose)
+void ControlEvaluatorNode::AddLaneletInfoMsg(const Pose & ego_pose)
 {
-  const auto current_lanelets = [&]() {
-    lanelet::ConstLanelet closest_route_lanelet;
-    route_handler_.getClosestLaneletWithinRoute(ego_pose, &closest_route_lanelet);
-    const auto shoulder_lanelets = route_handler_.getShoulderLaneletsAtPose(ego_pose);
-    lanelet::ConstLanelets closest_lanelets{closest_route_lanelet};
-    closest_lanelets.insert(
-      closest_lanelets.end(), shoulder_lanelets.begin(), shoulder_lanelets.end());
-    return closest_lanelets;
-  }();
+  const auto current_lanelets = metrics::utils::get_current_lanes(route_handler_, ego_pose);
   const auto arc_coordinates = lanelet::utils::getArcCoordinates(current_lanelets, ego_pose);
   lanelet::ConstLanelet current_lane;
   lanelet::utils::query::getClosestLanelet(current_lanelets, ego_pose, &current_lane);
@@ -173,6 +169,44 @@ void ControlEvaluatorNode::AddLaneletMetricMsg(const Pose & ego_pose)
   }
 }
 
+void ControlEvaluatorNode::AddLaneletMetricMsg(const Pose & ego_pose)
+{
+  const auto current_lanelets = metrics::utils::get_current_lanes(route_handler_, ego_pose);
+  lanelet::ConstLanelet current_lane;
+  lanelet::utils::query::getClosestLanelet(current_lanelets, ego_pose, &current_lane);
+  const auto local_vehicle_footprint = vehicle_info_.createFootprint();
+  const auto current_vehicle_footprint =
+    transformVector(local_vehicle_footprint, autoware::universe_utils::pose2transform(ego_pose));
+
+  // calculate signed distance to the left boundary
+  const auto most_left_boundary =
+    metrics::utils::get_most_side_boundary(route_handler_, current_lane, true);
+
+  double distance_to_left_boundary =
+    metrics::utils::calc_distance_to_line(current_vehicle_footprint, most_left_boundary);
+  const double yaw_to_left_boundary =
+    metrics::utils::calc_yaw_to_line(ego_pose, most_left_boundary);
+  if (yaw_to_left_boundary < 0.0) {
+    distance_to_left_boundary *= -1.0;
+  }
+  const Metric metric = Metric::left_boundary_distance;
+  AddMetricMsg(metric, distance_to_left_boundary);
+
+  // calculate signed distance to the right boundary
+  const auto most_right_boundary =
+    metrics::utils::get_most_side_boundary(route_handler_, current_lane, false);
+
+  double distance_to_right_boundary =
+    metrics::utils::calc_distance_to_line(current_vehicle_footprint, most_right_boundary);
+  const double angle_to_right_boundary =
+    metrics::utils::calc_yaw_to_line(ego_pose, most_right_boundary);
+  if (angle_to_right_boundary > 0.0) {
+    distance_to_right_boundary *= -1.0;
+  }
+  const Metric metric_right = Metric::right_boundary_distance;
+  AddMetricMsg(metric_right, distance_to_right_boundary);
+}
+
 void ControlEvaluatorNode::AddKinematicStateMetricMsg(
   const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped)
 {
@@ -271,8 +305,8 @@ void ControlEvaluatorNode::onTimer()
   getRouteData();
   if (odom && route_handler_.isHandlerReady()) {
     const Pose ego_pose = odom->pose.pose;
+    AddLaneletInfoMsg(ego_pose);
     AddLaneletMetricMsg(ego_pose);
-
     AddGoalLongitudinalDeviationMetricMsg(ego_pose);
     AddGoalLateralDeviationMetricMsg(ego_pose);
     AddGoalYawDeviationMetricMsg(ego_pose);
diff --git a/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp
index a851eeb410620..aa5b47ad31a4b 100644
--- a/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp
+++ b/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp
@@ -1,4 +1,4 @@
-// Copyright 2024 TIER IV, Inc.
+// 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.
diff --git a/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp b/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp
new file mode 100644
index 0000000000000..7450f786567cd
--- /dev/null
+++ b/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp
@@ -0,0 +1,105 @@
+// 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.
+// 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 "autoware/control_evaluator/metrics/metrics_utils.hpp"
+
+#include "autoware/motion_utils/trajectory/trajectory.hpp"
+
+#include <autoware/universe_utils/geometry/boost_geometry.hpp>
+#include <autoware_lanelet2_extension/utility/message_conversion.hpp>
+#include <autoware_lanelet2_extension/utility/query.hpp>
+#include <autoware_lanelet2_extension/utility/utilities.hpp>
+
+#include <boost/geometry.hpp>
+// #include <boost/geometry/algorithms/detail/distance/interface.hpp>
+
+#include <lanelet2_core/Forward.h>
+namespace control_diagnostics
+{
+namespace metrics
+{
+namespace utils
+{
+using autoware::route_handler::RouteHandler;
+using autoware::universe_utils::Point2d;
+using autoware::universe_utils::Point3d;
+using geometry_msgs::msg::Pose;
+
+lanelet::ConstLanelets get_current_lanes(const RouteHandler & route_handler, const Pose & ego_pose)
+{
+  lanelet::ConstLanelet closest_route_lanelet;
+  route_handler.getClosestLaneletWithinRoute(ego_pose, &closest_route_lanelet);
+  const auto shoulder_lanelets = route_handler.getShoulderLaneletsAtPose(ego_pose);
+  lanelet::ConstLanelets closest_lanelets{closest_route_lanelet};
+  closest_lanelets.insert(
+    closest_lanelets.end(), shoulder_lanelets.begin(), shoulder_lanelets.end());
+  return closest_lanelets;
+}
+
+lanelet::ConstLineString3d get_most_side_boundary(
+  const RouteHandler & route_handler, const lanelet::ConstLanelet & lanelet, const bool left_side,
+  const bool enable_same_root, const bool get_shoulder_lane)
+{
+  lanelet::ConstLineString3d most_side_lanelet;
+  if (left_side) {
+    most_side_lanelet =
+      route_handler.getMostLeftLanelet(lanelet, enable_same_root, get_shoulder_lane).leftBound3d();
+  } else {
+    most_side_lanelet =
+      route_handler.getMostRightLanelet(lanelet, enable_same_root, get_shoulder_lane)
+        .rightBound3d();
+  }
+  return most_side_lanelet;
+}
+
+double calc_distance_to_line(
+  const autoware::universe_utils::LinearRing2d & vehicle_footprint,
+  const lanelet::ConstLineString3d & line)
+{
+  // convert ConstLineString3d to LineString2d
+  autoware::universe_utils::LineString2d line_2d;
+  for (const auto & p : line) line_2d.push_back(Point2d{p.x(), p.y()});
+  return boost::geometry::distance(vehicle_footprint, line_2d);
+}
+
+double calc_yaw_to_line(const Pose & ego_pose, const lanelet::ConstLineString3d & line)
+{
+  const double ego_yaw = tf2::getYaw(ego_pose.orientation);
+
+  // find nearest point on the line.
+  double nearest_pt_x = ego_pose.position.x;
+  double nearest_pt_y = ego_pose.position.y;
+  double min_dist = std::numeric_limits<double>::max();
+  for (const auto & pt : line) {
+    const double dist = std::hypot(pt.x() - ego_pose.position.x, pt.y() - ego_pose.position.y);
+    if (dist < min_dist) {
+      min_dist = dist;
+      nearest_pt_x = pt.x();
+      nearest_pt_y = pt.y();
+    }
+  }
+
+  const double ego2line_yaw =
+    std::atan2(nearest_pt_y - ego_pose.position.y, nearest_pt_x - ego_pose.position.x);
+
+  double yaw_diff = ego2line_yaw - ego_yaw;
+  while (yaw_diff > M_PI) yaw_diff -= 2.0 * M_PI;
+  while (yaw_diff < -M_PI) yaw_diff += 2.0 * M_PI;
+
+  return yaw_diff;
+}
+
+}  // namespace utils
+}  // namespace metrics
+}  // namespace control_diagnostics
\ No newline at end of file
diff --git a/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp
index 9098ce5667424..7bf0c206f38e7 100644
--- a/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp
+++ b/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp
@@ -1,4 +1,4 @@
-// Copyright 2024 Tier IV, Inc.
+// 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.

From c04a38542d0a6d50606e573078e1319d8c7bf266 Mon Sep 17 00:00:00 2001
From: t4-adc <grp-rd-1-adc-admin@tier4.jp>
Date: Mon, 20 Jan 2025 16:25:37 +0900
Subject: [PATCH 2/8] pre-commit

Signed-off-by: xtk8532704 <1041084556@qq.com>
---
 .../autoware/control_evaluator/metrics/metrics_utils.hpp | 9 ++++++---
 .../src/metrics/metrics_utils.cpp                        | 2 +-
 2 files changed, 7 insertions(+), 4 deletions(-)

diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp
index f2f2b2d71fc79..1698eb1160f48 100644
--- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp
+++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp
@@ -56,10 +56,13 @@ lanelet::ConstLineString3d get_most_side_boundary(
  * @return distance between the vehicle footprint and the lanelet, 0.0 if the vehicle intersects
  *with the line
  **/
-double calc_distance_to_line(const autoware::universe_utils::LinearRing2d & vehicle_footprint, const lanelet::ConstLineString3d & line);
+double calc_distance_to_line(
+  const autoware::universe_utils::LinearRing2d & vehicle_footprint,
+  const lanelet::ConstLineString3d & line);
 
 /**
- * @brief Calculate the yaw deviation between the ego's orientation and the vector from the ego position to the closest point on the line.
+ * @brief Calculate the yaw deviation between the ego's orientation and the vector from the ego
+ * position to the closest point on the line.
  * @param [in] ego_pose The pose of the ego vehicle.
  * @param [in] line The line to which the yaw deviation is calculated.
  * @return The yaw deviation in radians, normalized to the range [-π, π].
@@ -69,4 +72,4 @@ double calc_yaw_to_line(const Pose & ego_pose, const lanelet::ConstLineString3d
 }  // namespace utils
 }  // namespace metrics
 }  // namespace control_diagnostics
-#endif  // AUTOWARE__CONTROL_EVALUATOR__METRICS__METRICS_UTILS_HPP_
\ No newline at end of file
+#endif  // AUTOWARE__CONTROL_EVALUATOR__METRICS__METRICS_UTILS_HPP_
diff --git a/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp b/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp
index 7450f786567cd..9c763fac78352 100644
--- a/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp
+++ b/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp
@@ -102,4 +102,4 @@ double calc_yaw_to_line(const Pose & ego_pose, const lanelet::ConstLineString3d
 
 }  // namespace utils
 }  // namespace metrics
-}  // namespace control_diagnostics
\ No newline at end of file
+}  // namespace control_diagnostics

From d0bc2b82717bbf829a945d1379933013b105250b Mon Sep 17 00:00:00 2001
From: xtk8532704 <1041084556@qq.com>
Date: Mon, 20 Jan 2025 19:41:31 +0900
Subject: [PATCH 3/8] use path topic instead of lanenet.

Signed-off-by: xtk8532704 <1041084556@qq.com>
---
 .../control_evaluator_node.hpp                |  8 ++-
 .../control_evaluator/metrics/metric.hpp      |  2 +-
 .../metrics/metrics_utils.hpp                 | 18 +-----
 .../launch/control_evaluator.launch.xml       |  2 +
 .../autoware_control_evaluator/package.xml    |  1 +
 .../src/control_evaluator_node.cpp            | 59 +++++++++++--------
 .../src/metrics/metrics_utils.cpp             | 27 +--------
 7 files changed, 49 insertions(+), 68 deletions(-)

diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp
index 91398ddfa94ea..663da9e9ebd87 100644
--- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp
+++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp
@@ -31,6 +31,7 @@
 #include <nav_msgs/msg/odometry.hpp>
 #include <tier4_metric_msgs/msg/metric.hpp>
 #include <tier4_metric_msgs/msg/metric_array.hpp>
+#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
 
 #include <deque>
 #include <optional>
@@ -40,6 +41,8 @@
 namespace control_diagnostics
 {
 using autoware::universe_utils::Accumulator;
+using autoware::universe_utils::LineString2d;
+using autoware::universe_utils::Point2d;
 using autoware::vehicle_info_utils::VehicleInfo;
 using autoware_planning_msgs::msg::Trajectory;
 using geometry_msgs::msg::Point;
@@ -50,6 +53,7 @@ using autoware_planning_msgs::msg::LaneletRoute;
 using geometry_msgs::msg::AccelWithCovarianceStamped;
 using MetricMsg = tier4_metric_msgs::msg::Metric;
 using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray;
+using tier4_planning_msgs::msg::PathWithLaneId;
 
 /**
  * @brief Node for control evaluation
@@ -66,7 +70,7 @@ class ControlEvaluatorNode : public rclcpp::Node
   void AddGoalLongitudinalDeviationMetricMsg(const Pose & ego_pose);
   void AddGoalLateralDeviationMetricMsg(const Pose & ego_pose);
   void AddGoalYawDeviationMetricMsg(const Pose & ego_pose);
-  void AddLaneletMetricMsg(const Pose & ego_pose);
+  void AddBoundaryDistanceMetricMsg(const PathWithLaneId & behavior_path, const Pose & ego_pose);
 
   void AddLaneletInfoMsg(const Pose & ego_pose);
   void AddKinematicStateMetricMsg(
@@ -87,6 +91,8 @@ class ControlEvaluatorNode : public rclcpp::Node
   autoware::universe_utils::InterProcessPollingSubscriber<
     LaneletMapBin, autoware::universe_utils::polling_policy::Newest>
     vector_map_subscriber_{this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()};
+  autoware::universe_utils::InterProcessPollingSubscriber<PathWithLaneId> behavior_path_subscriber_{
+    this, "~/input/behavior_path"};
 
   rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float64Stamped>::SharedPtr
     processing_time_pub_;
diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp
index dddf07a6710b3..ab2533d0f6ed3 100644
--- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp
+++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp
@@ -64,7 +64,7 @@ static const std::unordered_map<Metric, std::string> metric_descriptions = {
   {Metric::goal_lateral_deviation, "Lateral deviation from the goal point[m]"},
   {Metric::goal_yaw_deviation, "Yaw deviation from the goal point[rad]"},
   {Metric::left_boundary_distance, "Signed distance to the left boundary[m]"},
-  {Metric::right_boundary_distance, "Signed istance to the right boundary[m]"},
+  {Metric::right_boundary_distance, "Signed distance to the right boundary[m]"},
 };
 
 namespace details
diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp
index 1698eb1160f48..91e3a31ec8304 100644
--- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp
+++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp
@@ -25,7 +25,6 @@ namespace metrics
 namespace utils
 {
 using autoware::route_handler::RouteHandler;
-using autoware::universe_utils::Point2d;
 using geometry_msgs::msg::Pose;
 
 /**
@@ -36,19 +35,6 @@ using geometry_msgs::msg::Pose;
  **/
 lanelet::ConstLanelets get_current_lanes(const RouteHandler & route_handler, const Pose & ego_pose);
 
-/**
- * @brief Get the most side boundary of the lanelet.
- * @param [in] route_handler route handler
- * @param [in] lanelet lanelet to get the most side boundary
- * @param [in] left_side true to get left side boundary, false to get right side boundary
- * @param [in] enable_same_root true if the same root lanelets are considered, false otherwise
- * @param [in] get_shoulder_lane true if shoulder lanelets are considered, false otherwise
- * @return most side boundary of the lanelet
- **/
-lanelet::ConstLineString3d get_most_side_boundary(
-  const RouteHandler & route_handler, const lanelet::ConstLanelet & lanelet, const bool left_side,
-  const bool enable_same_root = false, const bool get_shoulder_lane = true);
-
 /**
  * @brief Calculate the Euler distance between the vehicle and the lanelet.
  * @param [in] vehicle_footprint vehicle footprint
@@ -58,7 +44,7 @@ lanelet::ConstLineString3d get_most_side_boundary(
  **/
 double calc_distance_to_line(
   const autoware::universe_utils::LinearRing2d & vehicle_footprint,
-  const lanelet::ConstLineString3d & line);
+  const autoware::universe_utils::LineString2d & line);
 
 /**
  * @brief Calculate the yaw deviation between the ego's orientation and the vector from the ego
@@ -67,7 +53,7 @@ double calc_distance_to_line(
  * @param [in] line The line to which the yaw deviation is calculated.
  * @return The yaw deviation in radians, normalized to the range [-π, π].
  */
-double calc_yaw_to_line(const Pose & ego_pose, const lanelet::ConstLineString3d & line);
+double calc_yaw_to_line(const Pose & ego_pose, const autoware::universe_utils::LineString2d & line);
 
 }  // namespace utils
 }  // namespace metrics
diff --git a/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml
index 4cf795afb5a7d..7d26a58367fa1 100644
--- a/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml
+++ b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml
@@ -5,6 +5,7 @@
   <arg name="input/trajectory" default="/planning/scenario_planning/trajectory"/>
   <arg name="input/vector_map" default="/map/vector_map"/>
   <arg name="input/route" default="/planning/mission_planning/route"/>
+  <arg name="input/behavior_path" default="/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id"/>
 
   <!-- control evaluator -->
   <group>
@@ -15,6 +16,7 @@
       <remap from="~/input/trajectory" to="$(var input/trajectory)"/>
       <remap from="~/input/vector_map" to="$(var input/vector_map)"/>
       <remap from="~/input/route" to="$(var input/route)"/>
+      <remap from="~/input/behavior_path" to="$(var input/behavior_path)"/>
     </node>
   </group>
 </launch>
diff --git a/evaluator/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml
index 24c33ceb56b2b..ea8f0875855d1 100644
--- a/evaluator/autoware_control_evaluator/package.xml
+++ b/evaluator/autoware_control_evaluator/package.xml
@@ -30,6 +30,7 @@
   <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>
diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp
index d840a293c3dd5..de5abc231c3fd 100644
--- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp
+++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp
@@ -20,6 +20,7 @@
 #include <autoware_lanelet2_extension/utility/query.hpp>
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
 #include <nlohmann/json.hpp>
+#include <rclcpp/logging.hpp>
 
 #include <chrono>
 #include <filesystem>
@@ -169,7 +170,8 @@ void ControlEvaluatorNode::AddLaneletInfoMsg(const Pose & ego_pose)
   }
 }
 
-void ControlEvaluatorNode::AddLaneletMetricMsg(const Pose & ego_pose)
+void ControlEvaluatorNode::AddBoundaryDistanceMetricMsg(
+  const PathWithLaneId & behavior_path, const Pose & ego_pose)
 {
   const auto current_lanelets = metrics::utils::get_current_lanes(route_handler_, ego_pose);
   lanelet::ConstLanelet current_lane;
@@ -178,33 +180,33 @@ void ControlEvaluatorNode::AddLaneletMetricMsg(const Pose & ego_pose)
   const auto current_vehicle_footprint =
     transformVector(local_vehicle_footprint, autoware::universe_utils::pose2transform(ego_pose));
 
-  // calculate signed distance to the left boundary
-  const auto most_left_boundary =
-    metrics::utils::get_most_side_boundary(route_handler_, current_lane, true);
+  if (behavior_path.left_bound.size() >= 1) {
+    LineString2d left_boundary;
+    for (const auto & p : behavior_path.left_bound) left_boundary.push_back(Point2d(p.x, p.y));
+    double distance_to_left_boundary =
+      metrics::utils::calc_distance_to_line(current_vehicle_footprint, left_boundary);
 
-  double distance_to_left_boundary =
-    metrics::utils::calc_distance_to_line(current_vehicle_footprint, most_left_boundary);
-  const double yaw_to_left_boundary =
-    metrics::utils::calc_yaw_to_line(ego_pose, most_left_boundary);
-  if (yaw_to_left_boundary < 0.0) {
-    distance_to_left_boundary *= -1.0;
+    const double yaw_to_left_boundary = metrics::utils::calc_yaw_to_line(ego_pose, left_boundary);
+    if (yaw_to_left_boundary < 0.0) {
+      distance_to_left_boundary *= -1.0;
+    }
+    const Metric metric_left = Metric::left_boundary_distance;
+    AddMetricMsg(metric_left, distance_to_left_boundary);
   }
-  const Metric metric = Metric::left_boundary_distance;
-  AddMetricMsg(metric, distance_to_left_boundary);
-
-  // calculate signed distance to the right boundary
-  const auto most_right_boundary =
-    metrics::utils::get_most_side_boundary(route_handler_, current_lane, false);
-
-  double distance_to_right_boundary =
-    metrics::utils::calc_distance_to_line(current_vehicle_footprint, most_right_boundary);
-  const double angle_to_right_boundary =
-    metrics::utils::calc_yaw_to_line(ego_pose, most_right_boundary);
-  if (angle_to_right_boundary > 0.0) {
-    distance_to_right_boundary *= -1.0;
+
+  if (behavior_path.right_bound.size() >= 1) {
+    LineString2d right_boundary;
+    for (const auto & p : behavior_path.right_bound) right_boundary.push_back(Point2d(p.x, p.y));
+    double distance_to_right_boundary =
+      metrics::utils::calc_distance_to_line(current_vehicle_footprint, right_boundary);
+
+    const double yaw_to_right_boundary = metrics::utils::calc_yaw_to_line(ego_pose, right_boundary);
+    if (yaw_to_right_boundary > 0.0) {
+      distance_to_right_boundary *= -1.0;
+    }
+    const Metric metric_right = Metric::right_boundary_distance;
+    AddMetricMsg(metric_right, distance_to_right_boundary);
   }
-  const Metric metric_right = Metric::right_boundary_distance;
-  AddMetricMsg(metric_right, distance_to_right_boundary);
 }
 
 void ControlEvaluatorNode::AddKinematicStateMetricMsg(
@@ -294,6 +296,7 @@ void ControlEvaluatorNode::onTimer()
   const auto traj = traj_sub_.takeData();
   const auto odom = odometry_sub_.takeData();
   const auto acc = accel_sub_.takeData();
+  const auto behavior_path = behavior_path_subscriber_.takeData();
 
   // calculate deviation metrics
   if (odom && traj && !traj->points.empty()) {
@@ -306,7 +309,6 @@ void ControlEvaluatorNode::onTimer()
   if (odom && route_handler_.isHandlerReady()) {
     const Pose ego_pose = odom->pose.pose;
     AddLaneletInfoMsg(ego_pose);
-    AddLaneletMetricMsg(ego_pose);
     AddGoalLongitudinalDeviationMetricMsg(ego_pose);
     AddGoalLateralDeviationMetricMsg(ego_pose);
     AddGoalYawDeviationMetricMsg(ego_pose);
@@ -316,6 +318,11 @@ void ControlEvaluatorNode::onTimer()
     AddKinematicStateMetricMsg(*odom, *acc);
   }
 
+  if (odom && behavior_path) {
+    const Pose ego_pose = odom->pose.pose;
+    AddBoundaryDistanceMetricMsg(*behavior_path, ego_pose);
+  }
+
   // Publish metrics
   metrics_msg_.stamp = now();
   metrics_pub_->publish(metrics_msg_);
diff --git a/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp b/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp
index 9c763fac78352..51312e51b907b 100644
--- a/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp
+++ b/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp
@@ -32,8 +32,6 @@ namespace metrics
 namespace utils
 {
 using autoware::route_handler::RouteHandler;
-using autoware::universe_utils::Point2d;
-using autoware::universe_utils::Point3d;
 using geometry_msgs::msg::Pose;
 
 lanelet::ConstLanelets get_current_lanes(const RouteHandler & route_handler, const Pose & ego_pose)
@@ -47,33 +45,14 @@ lanelet::ConstLanelets get_current_lanes(const RouteHandler & route_handler, con
   return closest_lanelets;
 }
 
-lanelet::ConstLineString3d get_most_side_boundary(
-  const RouteHandler & route_handler, const lanelet::ConstLanelet & lanelet, const bool left_side,
-  const bool enable_same_root, const bool get_shoulder_lane)
-{
-  lanelet::ConstLineString3d most_side_lanelet;
-  if (left_side) {
-    most_side_lanelet =
-      route_handler.getMostLeftLanelet(lanelet, enable_same_root, get_shoulder_lane).leftBound3d();
-  } else {
-    most_side_lanelet =
-      route_handler.getMostRightLanelet(lanelet, enable_same_root, get_shoulder_lane)
-        .rightBound3d();
-  }
-  return most_side_lanelet;
-}
-
 double calc_distance_to_line(
   const autoware::universe_utils::LinearRing2d & vehicle_footprint,
-  const lanelet::ConstLineString3d & line)
+  const autoware::universe_utils::LineString2d & line)
 {
-  // convert ConstLineString3d to LineString2d
-  autoware::universe_utils::LineString2d line_2d;
-  for (const auto & p : line) line_2d.push_back(Point2d{p.x(), p.y()});
-  return boost::geometry::distance(vehicle_footprint, line_2d);
+  return boost::geometry::distance(vehicle_footprint, line);
 }
 
-double calc_yaw_to_line(const Pose & ego_pose, const lanelet::ConstLineString3d & line)
+double calc_yaw_to_line(const Pose & ego_pose, const autoware::universe_utils::LineString2d & line)
 {
   const double ego_yaw = tf2::getYaw(ego_pose.orientation);
 

From 1ef92a023804c078643f357159fa5bfe8f1c1734 Mon Sep 17 00:00:00 2001
From: xtk8532704 <1041084556@qq.com>
Date: Mon, 20 Jan 2025 19:44:54 +0900
Subject: [PATCH 4/8] remove unused import

Signed-off-by: xtk8532704 <1041084556@qq.com>
---
 .../autoware_control_evaluator/src/control_evaluator_node.cpp    | 1 -
 1 file changed, 1 deletion(-)

diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp
index de5abc231c3fd..65b794fb51212 100644
--- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp
+++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp
@@ -20,7 +20,6 @@
 #include <autoware_lanelet2_extension/utility/query.hpp>
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
 #include <nlohmann/json.hpp>
-#include <rclcpp/logging.hpp>
 
 #include <chrono>
 #include <filesystem>

From 74a61ee1a00705f947c621c382c5de1ed5e52421 Mon Sep 17 00:00:00 2001
From: xtk8532704 <1041084556@qq.com>
Date: Wed, 22 Jan 2025 00:42:28 +0900
Subject: [PATCH 5/8] apply is_point_left_of_line

Signed-off-by: xtk8532704 <1041084556@qq.com>
---
 .../metrics/metrics_utils.hpp                 | 14 ++++----
 .../src/control_evaluator_node.cpp            |  6 ++--
 .../src/metrics/metrics_utils.cpp             | 32 +++++--------------
 3 files changed, 17 insertions(+), 35 deletions(-)

diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp
index 91e3a31ec8304..0e9e89f764db7 100644
--- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp
+++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp
@@ -25,6 +25,7 @@ namespace metrics
 namespace utils
 {
 using autoware::route_handler::RouteHandler;
+using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 
 /**
@@ -47,13 +48,12 @@ double calc_distance_to_line(
   const autoware::universe_utils::LineString2d & line);
 
 /**
- * @brief Calculate the yaw deviation between the ego's orientation and the vector from the ego
- * position to the closest point on the line.
- * @param [in] ego_pose The pose of the ego vehicle.
- * @param [in] line The line to which the yaw deviation is calculated.
- * @return The yaw deviation in radians, normalized to the range [-π, π].
- */
-double calc_yaw_to_line(const Pose & ego_pose, const autoware::universe_utils::LineString2d & line);
+ * @brief Check if the point is on the left side of the line.
+ * @param [in] point point
+ * @param [in] line line
+ * @return true if the ego vehicle is on the left side of the lanelet line, false otherwise
+ **/
+bool is_point_left_of_line(const Point & point, const std::vector<Point> & line);
 
 }  // namespace utils
 }  // namespace metrics
diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp
index 65b794fb51212..4abfb25afeee8 100644
--- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp
+++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp
@@ -185,8 +185,7 @@ void ControlEvaluatorNode::AddBoundaryDistanceMetricMsg(
     double distance_to_left_boundary =
       metrics::utils::calc_distance_to_line(current_vehicle_footprint, left_boundary);
 
-    const double yaw_to_left_boundary = metrics::utils::calc_yaw_to_line(ego_pose, left_boundary);
-    if (yaw_to_left_boundary < 0.0) {
+    if (metrics::utils::is_point_left_of_line(ego_pose.position, behavior_path.left_bound)) {
       distance_to_left_boundary *= -1.0;
     }
     const Metric metric_left = Metric::left_boundary_distance;
@@ -199,8 +198,7 @@ void ControlEvaluatorNode::AddBoundaryDistanceMetricMsg(
     double distance_to_right_boundary =
       metrics::utils::calc_distance_to_line(current_vehicle_footprint, right_boundary);
 
-    const double yaw_to_right_boundary = metrics::utils::calc_yaw_to_line(ego_pose, right_boundary);
-    if (yaw_to_right_boundary > 0.0) {
+    if (!metrics::utils::is_point_left_of_line(ego_pose.position, behavior_path.right_bound)) {
       distance_to_right_boundary *= -1.0;
     }
     const Metric metric_right = Metric::right_boundary_distance;
diff --git a/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp b/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp
index 51312e51b907b..1bd9eb97cde97 100644
--- a/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp
+++ b/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp
@@ -25,6 +25,8 @@
 // #include <boost/geometry/algorithms/detail/distance/interface.hpp>
 
 #include <lanelet2_core/Forward.h>
+
+#include <cstddef>
 namespace control_diagnostics
 {
 namespace metrics
@@ -32,6 +34,7 @@ namespace metrics
 namespace utils
 {
 using autoware::route_handler::RouteHandler;
+using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 
 lanelet::ConstLanelets get_current_lanes(const RouteHandler & route_handler, const Pose & ego_pose)
@@ -52,31 +55,12 @@ double calc_distance_to_line(
   return boost::geometry::distance(vehicle_footprint, line);
 }
 
-double calc_yaw_to_line(const Pose & ego_pose, const autoware::universe_utils::LineString2d & line)
+bool is_point_left_of_line(const Point & point, const std::vector<Point> & line)
 {
-  const double ego_yaw = tf2::getYaw(ego_pose.orientation);
-
-  // find nearest point on the line.
-  double nearest_pt_x = ego_pose.position.x;
-  double nearest_pt_y = ego_pose.position.y;
-  double min_dist = std::numeric_limits<double>::max();
-  for (const auto & pt : line) {
-    const double dist = std::hypot(pt.x() - ego_pose.position.x, pt.y() - ego_pose.position.y);
-    if (dist < min_dist) {
-      min_dist = dist;
-      nearest_pt_x = pt.x();
-      nearest_pt_y = pt.y();
-    }
-  }
-
-  const double ego2line_yaw =
-    std::atan2(nearest_pt_y - ego_pose.position.y, nearest_pt_x - ego_pose.position.x);
-
-  double yaw_diff = ego2line_yaw - ego_yaw;
-  while (yaw_diff > M_PI) yaw_diff -= 2.0 * M_PI;
-  while (yaw_diff < -M_PI) yaw_diff += 2.0 * M_PI;
-
-  return yaw_diff;
+  const size_t clost_idx = autoware::motion_utils::findNearestSegmentIndex(line, point);
+  const auto & p1 = line[clost_idx];
+  const auto & p2 = line[clost_idx + 1];
+  return ((p2.x - p1.x) * (point.y - p1.y) - (p2.y - p1.y) * (point.x - p1.x)) > 0;
 }
 
 }  // namespace utils

From f4451420b3ff83c1f656a9583e86311b0ec2f531 Mon Sep 17 00:00:00 2001
From: xtk8532704 <1041084556@qq.com>
Date: Wed, 22 Jan 2025 00:47:41 +0900
Subject: [PATCH 6/8] fix typo

Signed-off-by: xtk8532704 <1041084556@qq.com>
---
 .../src/metrics/metrics_utils.cpp                           | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp b/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp
index 1bd9eb97cde97..02f69b6563108 100644
--- a/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp
+++ b/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp
@@ -57,9 +57,9 @@ double calc_distance_to_line(
 
 bool is_point_left_of_line(const Point & point, const std::vector<Point> & line)
 {
-  const size_t clost_idx = autoware::motion_utils::findNearestSegmentIndex(line, point);
-  const auto & p1 = line[clost_idx];
-  const auto & p2 = line[clost_idx + 1];
+  const size_t closest_idx = autoware::motion_utils::findNearestSegmentIndex(line, point);
+  const auto & p1 = line[closest_idx];
+  const auto & p2 = line[closest_idx + 1];
   return ((p2.x - p1.x) * (point.y - p1.y) - (p2.y - p1.y) * (point.x - p1.x)) > 0;
 }
 

From 0947ee797d34bf98f9419ca65026d6fcb37d228b Mon Sep 17 00:00:00 2001
From: xtk8532704 <1041084556@qq.com>
Date: Wed, 22 Jan 2025 15:50:12 +0900
Subject: [PATCH 7/8] fix test bug

Signed-off-by: xtk8532704 <1041084556@qq.com>
---
 .../test/test_control_evaluator_node.cpp               | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp
index 7bf0c206f38e7..b2dcc93855c0f 100644
--- a/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp
+++ b/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp
@@ -51,11 +51,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");
-    options.arguments({"--ros-args", "-p", "output_metrics:=false"});
+    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",
+       autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml"});
 
-    dummy_node = std::make_shared<rclcpp::Node>("control_evaluator_test_node");
+    dummy_node = std::make_shared<rclcpp::Node>("control_evaluator_test_node", options);
     eval_node = std::make_shared<EvalNode>(options);
     // Enable all logging in the node
     auto ret = rcutils_logging_set_logger_level(

From b49aa573b086ed7ef87866642a4063361ccb82f5 Mon Sep 17 00:00:00 2001
From: xtk8532704 <1041084556@qq.com>
Date: Thu, 23 Jan 2025 17:16:15 +0900
Subject: [PATCH 8/8] manual pre-commit

Signed-off-by: xtk8532704 <1041084556@qq.com>
---
 .../include/autoware/control_evaluator/metrics/metrics_utils.hpp | 1 +
 .../autoware_control_evaluator/src/metrics/metrics_utils.cpp     | 1 +
 2 files changed, 2 insertions(+)

diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp
index 0e9e89f764db7..9527b00aa3b9d 100644
--- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp
+++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp
@@ -18,6 +18,7 @@
 #include <autoware/route_handler/route_handler.hpp>
 #include <autoware/universe_utils/geometry/boost_geometry.hpp>
 
+#include <vector>
 namespace control_diagnostics
 {
 namespace metrics
diff --git a/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp b/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp
index 02f69b6563108..3fe95d4decb8d 100644
--- a/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp
+++ b/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp
@@ -27,6 +27,7 @@
 #include <lanelet2_core/Forward.h>
 
 #include <cstddef>
+#include <vector>
 namespace control_diagnostics
 {
 namespace metrics