feat(autoware_control_evaluator): add new boundary_distance metrics (#…

…9984)

* add boundary_distance metric

Signed-off-by: xtk8532704 <[email protected]>

* pre-commit

Signed-off-by: xtk8532704 <[email protected]>

* use path topic instead of lanenet.

Signed-off-by: xtk8532704 <[email protected]>

* remove unused import

Signed-off-by: xtk8532704 <[email protected]>

* apply is_point_left_of_line

Signed-off-by: xtk8532704 <[email protected]>

* fix typo

Signed-off-by: xtk8532704 <[email protected]>

* fix test bug

Signed-off-by: xtk8532704 <[email protected]>

* manual pre-commit

Signed-off-by: xtk8532704 <[email protected]>

---------

Signed-off-by: xtk8532704 <[email protected]>
Co-authored-by: t4-adc <[email protected]>

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

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"
@@ -30,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>
@@ -39,6 +41,9 @@
 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;
 using geometry_msgs::msg::Pose;
@@ -48,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
@@ -64,8 +70,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 AddBoundaryDistanceMetricMsg(const PathWithLaneId & behavior_path, 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);
 
@@ -84,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_;
@@ -106,6 +115,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};

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.

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 distance to the right boundary[m]"},
+};
 
 namespace details
 {

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

@@ -0,0 +1,62 @@
+// 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>
+
+#include <vector>
+namespace control_diagnostics
+{
+namespace metrics
+{
+namespace utils
+{
+using autoware::route_handler::RouteHandler;
+using geometry_msgs::msg::Point;
+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 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 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
+}  // namespace control_diagnostics
+#endif  // AUTOWARE__CONTROL_EVALUATOR__METRICS__METRICS_UTILS_HPP_

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>

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>
@@ -29,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>

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,43 @@ 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;
+  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));
+
+  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);
+
+    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;
+    AddMetricMsg(metric_left, distance_to_left_boundary);
+  }
+
+  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);
+
+    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;
+    AddMetricMsg(metric_right, distance_to_right_boundary);
+  }
+}
+
 void ControlEvaluatorNode::AddKinematicStateMetricMsg(
   const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped)
 {
@@ -260,6 +293,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()) {
@@ -271,8 +305,7 @@ void ControlEvaluatorNode::onTimer()
   getRouteData();
   if (odom && route_handler_.isHandlerReady()) {
     const Pose ego_pose = odom->pose.pose;
-    AddLaneletMetricMsg(ego_pose);
-
+    AddLaneletInfoMsg(ego_pose);
     AddGoalLongitudinalDeviationMetricMsg(ego_pose);
     AddGoalLateralDeviationMetricMsg(ego_pose);
     AddGoalYawDeviationMetricMsg(ego_pose);
@@ -282,6 +315,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_);

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.