feat(planning_validator): add yaw deviation metric (autowarefoundation#10258)

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

Author: maxime-clem

planning/autoware_planning_validator/README.md

@@ -20,6 +20,7 @@ The following features are supported for trajectory validation and can have thre
 - **Distance deviation** : invalid if the ego is too far from the trajectory
 - **Longitudinal distance deviation** : invalid if the trajectory is too far from ego in longitudinal direction
 - **Forward trajectory length** : invalid if the trajectory length is not enough to stop within a given deceleration
+- **Yaw difference** : invalid if the difference between the ego yaw and closest trajectory yaw is too large
 
 The following features are to be implemented.
 
@@ -77,6 +78,7 @@ The input trajectory is detected as invalid if the index exceeds the following t
 | `thresholds.steering_rate`                   | double | invalid threshold of the steering angle rate in each trajectory point [rad/s]                                      | 10.0          |
 | `thresholds.velocity_deviation`              | double | invalid threshold of the velocity deviation between the ego velocity and the trajectory point closest to ego [m/s] | 100.0         |
 | `thresholds.distance_deviation`              | double | invalid threshold of the distance deviation between the ego position and the trajectory point closest to ego [m]   | 100.0         |
+| `thresholds.yaw_deviation`                   | double | invalid threshold of the difference between the ego yaw and the closest trajectory yaw [rad]                       | 1.5708        |
 | `parameters.longitudinal_distance_deviation` | double | invalid threshold of the longitudinal distance deviation between the ego position and the trajectory [m]           | 2.0           |
 
 #### Parameters

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

planning/autoware_planning_validator/test/src/test_parameter.hpp

@@ -33,6 +33,7 @@ constexpr double THRESHOLD_VELOCITY_DEVIATION = 15.0 * kmph2mps;
 constexpr double THRESHOLD_DISTANCE_DEVIATION = 3.0;
 constexpr double THRESHOLD_LONGITUDINAL_DISTANCE_DEVIATION = 2.0;
 constexpr double THRESHOLD_NOMINAL_LATENCY = 1.0;
+constexpr double THRESHOLD_YAW_DEVIATION = 1.5708;
 constexpr double PARAMETER_FORWARD_TRAJECTORY_LENGTH_ACCELERATION = -5.0;
 constexpr double PARAMETER_FORWARD_TRAJECTORY_LENGTH_MARGIN = 2.0;
 

planning/autoware_planning_validator/test/src/test_planning_validator_helper.cpp

@@ -211,6 +211,7 @@ rclcpp::NodeOptions getNodeOptionsWithDefaultParams()
   node_options.append_parameter_override(
     "thresholds.longitudinal_distance_deviation", THRESHOLD_LONGITUDINAL_DISTANCE_DEVIATION);
   node_options.append_parameter_override("thresholds.nominal_latency", THRESHOLD_NOMINAL_LATENCY);
+  node_options.append_parameter_override("thresholds.yaw_deviation", THRESHOLD_YAW_DEVIATION);
   node_options.append_parameter_override(
     "parameters.forward_trajectory_length_acceleration",
     PARAMETER_FORWARD_TRAJECTORY_LENGTH_ACCELERATION);

planning/autoware_planning_validator/test/src/test_planning_validator_pubsub.cpp

@@ -16,6 +16,7 @@
 #include "test_parameter.hpp"
 #include "test_planning_validator_helper.hpp"
 
+#include <autoware_utils/geometry/geometry.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <nav_msgs/msg/odometry.hpp>
@@ -514,3 +515,28 @@ TEST(PlanningValidator, DiagCheckForwardTrajectoryLength)
     runWithBadTrajectory(bad_trajectory, ego_odom, diag_name);
   }
 }
+
+TEST(PlanningValidator, DiagCheckYawDeviation)
+{
+  const auto diag_name = "planning_validator: trajectory_validation_yaw_deviation";
+  const auto straight_trajectory = generateTrajectory(1.0, 0.0, 0.0, 10);
+
+  // Ego with yaw deviation smaller than threshold -> must be OK
+  {
+    auto ego_odom = generateDefaultOdometry(0.0, 0.0, 0.0);
+    for (auto yaw = 0.0; yaw <= THRESHOLD_YAW_DEVIATION; yaw += 0.1) {
+      ego_odom.pose.pose.orientation = autoware_utils::create_quaternion_from_yaw(yaw);
+      runWithOKTrajectory(straight_trajectory, ego_odom, diag_name);
+    }
+  }
+  // Ego with yaw deviation larger than threshold -> must be NG
+  {
+    auto ego_odom = generateDefaultOdometry(0.0, 0.0, 0.0);
+    for (auto yaw = THRESHOLD_YAW_DEVIATION + 1e-3; yaw < M_PI; yaw += 0.1) {
+      ego_odom.pose.pose.orientation = autoware_utils::create_quaternion_from_yaw(yaw);
+      runWithBadTrajectory(straight_trajectory, ego_odom, diag_name);
+      ego_odom.pose.pose.orientation = autoware_utils::create_quaternion_from_yaw(-yaw);
+      runWithBadTrajectory(straight_trajectory, ego_odom, diag_name);
+    }
+  }
+}