feat(control_evaluator): add goal accuracy longitudinal, lateral, yaw (…

…autowarefoundation#9155)

* feat(control_evaluator): add goal accuracy longitudinal, lateral, yaw

Signed-off-by: Kasunori-Nakajima <kazunori.nakajima@tier4.jp>

* style(pre-commit): autofix

* fix: content of kosuke55-san comments

Signed-off-by: Kasunori-Nakajima <kazunori.nakajima@tier4.jp>

* fix: variable name

Signed-off-by: Kasunori-Nakajima <kazunori.nakajima@tier4.jp>

* fix: variable name

Signed-off-by: Kasunori-Nakajima <kazunori.nakajima@tier4.jp>

---------

Signed-off-by: Kasunori-Nakajima <kazunori.nakajima@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

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

@@ -56,6 +56,9 @@ class ControlEvaluatorNode : public rclcpp::Node
     const Trajectory & traj, const Point & ego_point);
   DiagnosticStatus generateYawDeviationDiagnosticStatus(
     const Trajectory & traj, const Pose & ego_pose);
+  DiagnosticStatus generateGoalLongitudinalDeviationDiagnosticStatus(const Pose & ego_pose);
+  DiagnosticStatus generateGoalLateralDeviationDiagnosticStatus(const Pose & ego_pose);
+  DiagnosticStatus generateGoalYawDeviationDiagnosticStatus(const Pose & ego_pose);
 
   DiagnosticStatus generateAEBDiagnosticStatus(const DiagnosticStatus & diag);
   DiagnosticStatus generateLaneletDiagnosticStatus(const Pose & ego_pose) const;

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

@@ -42,6 +42,30 @@ double calcLateralDeviation(const Trajectory & traj, const Point & point);
  */
 double calcYawDeviation(const Trajectory & traj, const Pose & pose);
 
+/**
+ * @brief calculate longitudinal deviation from target_point to base_pose
+ * @param [in] pose input base_pose
+ * @param [in] point input target_point
+ * @return longitudinal deviation from base_pose to target_point
+ */
+double calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point);
+
+/**
+ * @brief calculate lateral deviation from target_point to base_pose
+ * @param [in] pose input base_pose
+ * @param [in] point input target_point
+ * @return lateral deviation from base_pose to target_point
+ */
+double calcLateralDeviation(const Pose & base_pose, const Point & target_point);
+
+/**
+ * @brief calculate yaw deviation from base_pose to target_pose
+ * @param [in] pose input base_pose
+ * @param [in] pose input target_pose
+ * @return yaw deviation from base_pose to target_pose
+ */
+double calcYawDeviation(const Pose & base_pose, const Pose & target_pose);
+
 }  // namespace metrics
 }  // namespace control_diagnostics
 

evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp

@@ -186,6 +186,53 @@ DiagnosticStatus ControlEvaluatorNode::generateYawDeviationDiagnosticStatus(
   return status;
 }
 
+DiagnosticStatus ControlEvaluatorNode::generateGoalLongitudinalDeviationDiagnosticStatus(
+  const Pose & ego_pose)
+{
+  DiagnosticStatus status;
+  const double longitudinal_deviation =
+    metrics::calcLongitudinalDeviation(route_handler_.getGoalPose(), ego_pose.position);
+
+  status.level = status.OK;
+  status.name = "goal_longitudinal_deviation";
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = "metrics_value";
+  key_value.value = std::to_string(longitudinal_deviation);
+  status.values.push_back(key_value);
+  return status;
+}
+
+DiagnosticStatus ControlEvaluatorNode::generateGoalLateralDeviationDiagnosticStatus(
+  const Pose & ego_pose)
+{
+  DiagnosticStatus status;
+  const double lateral_deviation =
+    metrics::calcLateralDeviation(route_handler_.getGoalPose(), ego_pose.position);
+
+  status.level = status.OK;
+  status.name = "goal_lateral_deviation";
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = "metrics_value";
+  key_value.value = std::to_string(lateral_deviation);
+  status.values.push_back(key_value);
+  return status;
+}
+
+DiagnosticStatus ControlEvaluatorNode::generateGoalYawDeviationDiagnosticStatus(
+  const Pose & ego_pose)
+{
+  DiagnosticStatus status;
+  const double yaw_deviation = metrics::calcYawDeviation(route_handler_.getGoalPose(), ego_pose);
+
+  status.level = status.OK;
+  status.name = "goal_yaw_deviation";
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = "metrics_value";
+  key_value.value = std::to_string(yaw_deviation);
+  status.values.push_back(key_value);
+  return status;
+}
+
 void ControlEvaluatorNode::onTimer()
 {
   DiagnosticArray metrics_msg;
@@ -222,6 +269,10 @@ void ControlEvaluatorNode::onTimer()
   if (odom && route_handler_.isHandlerReady()) {
     const Pose ego_pose = odom->pose.pose;
     metrics_msg.status.push_back(generateLaneletDiagnosticStatus(ego_pose));
+
+    metrics_msg.status.push_back(generateGoalLongitudinalDeviationDiagnosticStatus(ego_pose));
+    metrics_msg.status.push_back(generateGoalLateralDeviationDiagnosticStatus(ego_pose));
+    metrics_msg.status.push_back(generateGoalYawDeviationDiagnosticStatus(ego_pose));
   }
 
   if (odom && acc) {

evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp

@@ -38,5 +38,20 @@ double calcYawDeviation(const Trajectory & traj, const Pose & pose)
     autoware::universe_utils::calcYawDeviation(traj.points[nearest_index].pose, pose));
 }
 
+double calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point)
+{
+  return std::abs(autoware::universe_utils::calcLongitudinalDeviation(base_pose, target_point));
+}
+
+double calcLateralDeviation(const Pose & base_pose, const Point & target_point)
+{
+  return std::abs(autoware::universe_utils::calcLateralDeviation(base_pose, target_point));
+}
+
+double calcYawDeviation(const Pose & base_pose, const Pose & target_pose)
+{
+  return std::abs(autoware::universe_utils::calcYawDeviation(base_pose, target_pose));
+}
+
 }  // namespace metrics
 }  // namespace control_diagnostics