feat(planning_validator): add yaw deviation metric

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

Author: maxime-clem

planning/autoware_planning_validator/config/planning_validator.param.yaml

@@ -28,6 +28,7 @@
       distance_deviation: 100.0
       longitudinal_distance_deviation: 1.0
       nominal_latency: 1.0
+      yaw_deviation: 1.5708  # (= 90 degrees)
 
     parameters:
       # The required trajectory length is calculated as the distance needed

planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp

@@ -60,6 +60,7 @@ struct ValidationParams
   double distance_deviation_threshold;
   double longitudinal_distance_deviation_threshold;
   double nominal_latency_threshold;
+  double yaw_deviation_threshold;
 
   // parameters
   double forward_trajectory_length_acceleration;
@@ -88,6 +89,7 @@ class PlanningValidator : public rclcpp::Node
   bool checkValidLongitudinalDistanceDeviation(const Trajectory & trajectory);
   bool checkValidForwardTrajectoryLength(const Trajectory & trajectory);
   bool checkValidLatency(const Trajectory & trajectory);
+  bool checkValidYawDeviation(const Trajectory & trajectory);
 
 private:
   void setupDiag();

planning/autoware_planning_validator/msg/PlanningValidatorStatus.msg

@@ -16,6 +16,7 @@ bool is_valid_distance_deviation
 bool is_valid_longitudinal_distance_deviation
 bool is_valid_forward_trajectory_length
 bool is_valid_latency
+bool is_valid_yaw_deviation
 
 # values
 int64 trajectory_size
@@ -33,5 +34,6 @@ float64 longitudinal_distance_deviation
 float64 forward_trajectory_length_required
 float64 forward_trajectory_length_measured
 float64 latency
+float64 yaw_deviation
 
 int64 invalid_count

planning/autoware_planning_validator/src/planning_validator.cpp

@@ -16,9 +16,13 @@
 
 #include "autoware/planning_validator/utils.hpp"
 
+#include <autoware/motion_utils/trajectory/interpolation.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <autoware_utils/geometry/geometry.hpp>
 
+#include <angles/angles/angles.h>
+#include <tf2/utils.h>
+
 #include <memory>
 #include <string>
 #include <utility>
@@ -81,6 +85,7 @@ void PlanningValidator::setupParameters()
     p.longitudinal_distance_deviation_threshold =
       declare_parameter<double>(t + "longitudinal_distance_deviation");
     p.nominal_latency_threshold = declare_parameter<double>(t + "nominal_latency");
+    p.yaw_deviation_threshold = declare_parameter<double>(t + "yaw_deviation");
 
     const std::string ps = "parameters.";
     p.forward_trajectory_length_acceleration =
@@ -172,6 +177,11 @@ void PlanningValidator::setupDiag()
   d->add(ns + "latency", [&](auto & stat) {
     setStatus(stat, validation_status_.is_valid_latency, "latency is larger than expected value.");
   });
+  d->add(ns + "yaw_deviation", [&](auto & stat) {
+    setStatus(
+      stat, validation_status_.is_valid_yaw_deviation,
+      "difference between vehicle yaw and closest trajectory yaw is too large.");
+  });
 }
 
 bool PlanningValidator::isDataReady()
@@ -324,6 +334,7 @@ void PlanningValidator::validate(const Trajectory & trajectory)
   s.is_valid_steering = checkValidSteering(resampled);
   s.is_valid_steering_rate = checkValidSteeringRate(resampled);
   s.is_valid_latency = checkValidLatency(trajectory);
+  s.is_valid_yaw_deviation = checkValidYawDeviation(trajectory);
 
   s.invalid_count = isAllValid(s) ? 0 : s.invalid_count + 1;
 }
@@ -555,14 +566,24 @@ bool PlanningValidator::checkValidLatency(const Trajectory & trajectory)
   return validation_status_.latency < validation_params_.nominal_latency_threshold;
 }
 
+bool PlanningValidator::checkValidYawDeviation(const Trajectory & trajectory)
+{
+  const auto interpolated_trajectory_point =
+    motion_utils::calcInterpolatedPoint(trajectory, current_kinematics_->pose.pose);
+  validation_status_.yaw_deviation = std::abs(angles::shortest_angular_distance(
+    tf2::getYaw(interpolated_trajectory_point.pose.orientation),
+    tf2::getYaw(current_kinematics_->pose.pose.orientation)));
+  return validation_status_.yaw_deviation <= validation_params_.yaw_deviation_threshold;
+}
+
 bool PlanningValidator::isAllValid(const PlanningValidatorStatus & s) const
 {
   return s.is_valid_size && s.is_valid_finite_value && s.is_valid_interval &&
          s.is_valid_relative_angle && s.is_valid_curvature && s.is_valid_lateral_acc &&
          s.is_valid_longitudinal_max_acc && s.is_valid_longitudinal_min_acc &&
          s.is_valid_steering && s.is_valid_steering_rate && s.is_valid_velocity_deviation &&
          s.is_valid_distance_deviation && s.is_valid_longitudinal_distance_deviation &&
-         s.is_valid_forward_trajectory_length && s.is_valid_latency;
+         s.is_valid_forward_trajectory_length && s.is_valid_latency && s.is_valid_yaw_deviation;
 }
 
 void PlanningValidator::displayStatus()
@@ -594,6 +615,7 @@ void PlanningValidator::displayStatus()
     "planning trajectory is too far from ego in longitudinal direction!!");
   warn(s.is_valid_forward_trajectory_length, "planning trajectory forward length is not enough!!");
   warn(s.is_valid_latency, "planning component latency is larger than threshold!!");
+  warn(s.is_valid_yaw_deviation, "planning trajectory yaw difference from ego yaw is too large!!");
 }
 
 }  // namespace autoware::planning_validator