@@ -80,6 +80,7 @@ void PlanningValidator::setupParameters()
80
80
p.distance_deviation_threshold = declare_parameter<double >(t + " distance_deviation" );
81
81
p.longitudinal_distance_deviation_threshold =
82
82
declare_parameter<double >(t + " longitudinal_distance_deviation" );
83
+ p.nominal_latency_threshold = declare_parameter<double >(t + " nominal_latency" );
83
84
84
85
const std::string ps = " parameters." ;
85
86
p.forward_trajectory_length_acceleration =
@@ -168,6 +169,9 @@ void PlanningValidator::setupDiag()
168
169
stat, validation_status_.is_valid_forward_trajectory_length ,
169
170
" trajectory length is too short" );
170
171
});
172
+ d->add (ns + " latency" , [&](auto & stat) {
173
+ setStatus (stat, validation_status_.is_valid_latency , " latency is larger than expected value." );
174
+ });
171
175
}
172
176
173
177
bool PlanningValidator::isDataReady ()
@@ -319,6 +323,7 @@ void PlanningValidator::validate(const Trajectory & trajectory)
319
323
s.is_valid_lateral_acc = checkValidLateralAcceleration (resampled);
320
324
s.is_valid_steering = checkValidSteering (resampled);
321
325
s.is_valid_steering_rate = checkValidSteeringRate (resampled);
326
+ s.is_valid_latency = checkValidLatency (trajectory);
322
327
323
328
s.invalid_count = isAllValid (s) ? 0 : s.invalid_count + 1 ;
324
329
}
@@ -544,14 +549,20 @@ bool PlanningValidator::checkValidForwardTrajectoryLength(const Trajectory & tra
544
549
return forward_length > forward_length_required;
545
550
}
546
551
552
+ bool PlanningValidator::checkValidLatency (const Trajectory & trajectory)
553
+ {
554
+ validation_status_.latency = (this ->now () - trajectory.header .stamp ).seconds ();
555
+ return validation_status_.latency < validation_params_.nominal_latency_threshold ;
556
+ }
557
+
547
558
bool PlanningValidator::isAllValid (const PlanningValidatorStatus & s) const
548
559
{
549
560
return s.is_valid_size && s.is_valid_finite_value && s.is_valid_interval &&
550
561
s.is_valid_relative_angle && s.is_valid_curvature && s.is_valid_lateral_acc &&
551
562
s.is_valid_longitudinal_max_acc && s.is_valid_longitudinal_min_acc &&
552
563
s.is_valid_steering && s.is_valid_steering_rate && s.is_valid_velocity_deviation &&
553
564
s.is_valid_distance_deviation && s.is_valid_longitudinal_distance_deviation &&
554
- s.is_valid_forward_trajectory_length ;
565
+ s.is_valid_forward_trajectory_length && s. is_valid_latency ;
555
566
}
556
567
557
568
void PlanningValidator::displayStatus ()
@@ -582,6 +593,7 @@ void PlanningValidator::displayStatus()
582
593
s.is_valid_longitudinal_distance_deviation ,
583
594
" planning trajectory is too far from ego in longitudinal direction!!" );
584
595
warn (s.is_valid_forward_trajectory_length , " planning trajectory forward length is not enough!!" );
596
+ warn (s.is_valid_latency , " planning component latency is larger than threshold!!" );
585
597
}
586
598
587
599
} // namespace autoware::planning_validator
0 commit comments