fix(control_evaluator, planning_evaluator): fix goal-related metrics calculation (autowarefoundation#11337)

* fix stop condition

Signed-off-by: xtk8532704 <1041084556@qq.com>

* fix include

Signed-off-by: xtk8532704 <1041084556@qq.com>

---------

Signed-off-by: xtk8532704 <1041084556@qq.com>

Author: xtk8532704

evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp

@@ -437,18 +437,20 @@ void ControlEvaluatorNode::AddYawDeviationMetricMsg(const Trajectory & traj, con
 void ControlEvaluatorNode::AddGoalDeviationMetricMsg(const Odometry & odom)
 {
   const Pose ego_pose = odom.pose.pose;
+  const auto & goal_pose = route_handler_.getGoalPose();
+
   const double longitudinal_deviation_value =
-    metrics::calcLongitudinalDeviation(route_handler_.getGoalPose(), ego_pose.position);
+    metrics::calcLongitudinalDeviation(goal_pose, ego_pose.position);
   const double longitudinal_deviation_value_abs = std::abs(longitudinal_deviation_value);
   const double lateral_deviation_value =
-    metrics::calcLateralDeviation(route_handler_.getGoalPose(), ego_pose.position);
+    metrics::calcLateralDeviation(goal_pose, ego_pose.position);
   const double lateral_deviation_value_abs = std::abs(lateral_deviation_value);
-  const double yaw_deviation_value =
-    metrics::calcYawDeviation(route_handler_.getGoalPose(), ego_pose);
+  const double yaw_deviation_value = metrics::calcYawDeviation(goal_pose, ego_pose);
   const double yaw_deviation_value_abs = std::abs(yaw_deviation_value);
 
   const bool is_ego_stopped_near_goal =
-    std::abs(longitudinal_deviation_value) < 3.0 && ego_speed_ < 0.001;
+    autoware_utils::calc_distance2d(ego_pose.position, goal_pose.position) < 3.0 &&
+    ego_speed_ < 0.001;
 
   AddMetricMsg(
     Metric::goal_longitudinal_deviation, longitudinal_deviation_value, is_ego_stopped_near_goal);

evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp

@@ -19,6 +19,7 @@
 
 #include <autoware_lanelet2_extension/utility/query.hpp>
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
+#include <autoware_utils/geometry/geometry.hpp>
 #include <nlohmann/json.hpp>
 
 #include <diagnostic_msgs/msg/detail/diagnostic_status__struct.hpp>
@@ -380,6 +381,10 @@ void PlanningEvaluatorNode::onModifiedGoal(
     return;
   }
   auto start = now();
+  const auto is_ego_stopped_near_goal =
+    std::abs(ego_state_ptr->twist.twist.linear.x) < 0.001 &&
+    autoware_utils::calc_distance2d(
+      ego_state_ptr->pose.pose.position, modified_goal_msg->pose.position) < 3.0;
 
   for (Metric metric : metrics_for_publish_) {
     const auto metric_stat = metrics_calculator_.calculate(
@@ -388,9 +393,8 @@ void PlanningEvaluatorNode::onModifiedGoal(
       continue;
     }
     AddMetricMsg(metric, *metric_stat);
-    if (
-      output_metrics_ && std::abs(ego_state_ptr->twist.twist.linear.x) < 0.001 &&
-      metric_stat->mean() < 3.0) {  // only record when ego stop close to the target in 3m
+    if (output_metrics_ && is_ego_stopped_near_goal) {  // only record when ego stop close to the
+                                                        // target in 3m
       const OutputMetric output_metric = str_to_output_metric.at(metric_to_str.at(metric));
       metrics_accumulator_.accumulate(output_metric, *metric_stat);
     }