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

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

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

* fix: relax threshold

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

* fix: lacking param

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

* fix: relax threshold

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

* fix: relax threshold

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

* fix: add time stamp

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

---------

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

Author: satoshi-ota

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: 1.0
 
     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

planning/autoware_planning_validator/test/src/test_parameter.hpp

@@ -32,6 +32,7 @@ constexpr double THRESHOLD_STEERING_RATE = 7.0 * deg2rad;
 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 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

@@ -28,6 +28,7 @@ Trajectory generateTrajectoryWithConstantAcceleration(
   const double acceleration)
 {
   Trajectory trajectory;
+  trajectory.header.stamp = rclcpp::Clock{RCL_ROS_TIME}.now();
   double s = 0.0, v = speed, a = acceleration;
   constexpr auto MAX_DT = 10.0;
   for (size_t i = 0; i < size; ++i) {
@@ -71,6 +72,7 @@ Trajectory generateTrajectoryWithConstantCurvature(
   const auto radius = 1.0 / curvature;
 
   Trajectory trajectory;
+  trajectory.header.stamp = rclcpp::Clock{RCL_ROS_TIME}.now();
   double x = 0.0, y = 0.0, yaw = 0.0;
 
   for (size_t i = 0; i <= size; ++i) {
@@ -106,6 +108,7 @@ Trajectory generateTrajectoryWithConstantSteeringRate(
   const double wheelbase)
 {
   Trajectory trajectory;
+  trajectory.header.stamp = rclcpp::Clock{RCL_ROS_TIME}.now();
   double x = 0.0, y = 0.0, yaw = 0.0, steering_angle_rad = 0.0;
 
   constexpr double MAX_STEERING_ANGLE_RAD = M_PI / 3.0;
@@ -158,6 +161,7 @@ Trajectory generateInfTrajectory()
 Trajectory generateBadCurvatureTrajectory()
 {
   Trajectory trajectory;
+  trajectory.header.stamp = rclcpp::Clock{RCL_ROS_TIME}.now();
 
   double y = 1.5;
   for (double s = 0.0; s <= 10.0; s += 1.0) {
@@ -206,6 +210,7 @@ rclcpp::NodeOptions getNodeOptionsWithDefaultParams()
     "thresholds.distance_deviation", THRESHOLD_DISTANCE_DEVIATION);
   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(
     "parameters.forward_trajectory_length_acceleration",
     PARAMETER_FORWARD_TRAJECTORY_LENGTH_ACCELERATION);