feat(planning_validator): add diag to check planning component latency

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/autoware_planning_validator/config/planning_validator.param.yaml

@@ -27,6 +27,7 @@
       velocity_deviation: 100.0
       distance_deviation: 100.0
       longitudinal_distance_deviation: 1.0
+      nominal_latency: 0.1
 
     parameters:
       # The required trajectory length is calculated as the distance needed

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

@@ -59,6 +59,7 @@ struct ValidationParams
   double velocity_deviation_threshold;
   double distance_deviation_threshold;
   double longitudinal_distance_deviation_threshold;
+  double nominal_latency_threshold;
 
   // parameters
   double forward_trajectory_length_acceleration;
@@ -86,6 +87,7 @@ class PlanningValidator : public rclcpp::Node
   bool checkValidDistanceDeviation(const Trajectory & trajectory);
   bool checkValidLongitudinalDistanceDeviation(const Trajectory & trajectory);
   bool checkValidForwardTrajectoryLength(const Trajectory & trajectory);
+  bool checkValidLatency(const Trajectory & trajectory);
 
 private:
   void setupDiag();

planning/autoware_planning_validator/msg/PlanningValidatorStatus.msg

@@ -15,6 +15,7 @@ bool is_valid_velocity_deviation
 bool is_valid_distance_deviation
 bool is_valid_longitudinal_distance_deviation
 bool is_valid_forward_trajectory_length
+bool is_valid_latency
 
 # values
 int64 trajectory_size
@@ -31,5 +32,6 @@ float64 distance_deviation
 float64 longitudinal_distance_deviation
 float64 forward_trajectory_length_required
 float64 forward_trajectory_length_measured
+float64 latency
 
 int64 invalid_count

planning/autoware_planning_validator/src/planning_validator.cpp

@@ -80,6 +80,7 @@ void PlanningValidator::setupParameters()
     p.distance_deviation_threshold = declare_parameter<double>(t + "distance_deviation");
     p.longitudinal_distance_deviation_threshold =
       declare_parameter<double>(t + "longitudinal_distance_deviation");
+    p.nominal_latency_threshold = declare_parameter<double>(t + "nominal_latency");
 
     const std::string ps = "parameters.";
     p.forward_trajectory_length_acceleration =
@@ -168,6 +169,9 @@ void PlanningValidator::setupDiag()
       stat, validation_status_.is_valid_forward_trajectory_length,
       "trajectory length is too short");
   });
+  d->add(ns + "latency", [&](auto & stat) {
+    setStatus(stat, validation_status_.is_valid_latency, "latency is larger than expected value.");
+  });
 }
 
 bool PlanningValidator::isDataReady()
@@ -319,6 +323,7 @@ void PlanningValidator::validate(const Trajectory & trajectory)
   s.is_valid_lateral_acc = checkValidLateralAcceleration(resampled);
   s.is_valid_steering = checkValidSteering(resampled);
   s.is_valid_steering_rate = checkValidSteeringRate(resampled);
+  s.is_valid_latency = checkValidLatency(trajectory);
 
   s.invalid_count = isAllValid(s) ? 0 : s.invalid_count + 1;
 }
@@ -544,14 +549,20 @@ bool PlanningValidator::checkValidForwardTrajectoryLength(const Trajectory & tra
   return forward_length > forward_length_required;
 }
 
+bool PlanningValidator::checkValidLatency(const Trajectory & trajectory)
+{
+  validation_status_.latency = (this->now() - trajectory.header.stamp).seconds();
+  return validation_status_.latency < validation_params_.nominal_latency_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_forward_trajectory_length && s.is_valid_latency;
 }
 
 void PlanningValidator::displayStatus()
@@ -582,6 +593,7 @@ void PlanningValidator::displayStatus()
     s.is_valid_longitudinal_distance_deviation,
     "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!!");
 }
 
 }  // namespace autoware::planning_validator