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 9641f4f75026a..d1c459d1462e9 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 @@ -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; 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 94fbd53532e7d..e0e04b0a65070 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 @@ -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 diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index c14878a3c6f7e..9fb9d4bd080f7 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/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) { diff --git a/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp index 689291da09f6d..a851eeb410620 100644 --- a/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp +++ b/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