Skip to content

Commit 15fdd87

Browse files
authored
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>
1 parent 658c112 commit 15fdd87

File tree

6 files changed

+24
-1
lines changed

6 files changed

+24
-1
lines changed

planning/autoware_planning_validator/config/planning_validator.param.yaml

+1
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@
2727
velocity_deviation: 100.0
2828
distance_deviation: 100.0
2929
longitudinal_distance_deviation: 1.0
30+
nominal_latency: 1.0
3031

3132
parameters:
3233
# The required trajectory length is calculated as the distance needed

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

+2
Original file line numberDiff line numberDiff line change
@@ -59,6 +59,7 @@ struct ValidationParams
5959
double velocity_deviation_threshold;
6060
double distance_deviation_threshold;
6161
double longitudinal_distance_deviation_threshold;
62+
double nominal_latency_threshold;
6263

6364
// parameters
6465
double forward_trajectory_length_acceleration;
@@ -86,6 +87,7 @@ class PlanningValidator : public rclcpp::Node
8687
bool checkValidDistanceDeviation(const Trajectory & trajectory);
8788
bool checkValidLongitudinalDistanceDeviation(const Trajectory & trajectory);
8889
bool checkValidForwardTrajectoryLength(const Trajectory & trajectory);
90+
bool checkValidLatency(const Trajectory & trajectory);
8991

9092
private:
9193
void setupDiag();

planning/autoware_planning_validator/msg/PlanningValidatorStatus.msg

+2
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@ bool is_valid_velocity_deviation
1515
bool is_valid_distance_deviation
1616
bool is_valid_longitudinal_distance_deviation
1717
bool is_valid_forward_trajectory_length
18+
bool is_valid_latency
1819

1920
# values
2021
int64 trajectory_size
@@ -31,5 +32,6 @@ float64 distance_deviation
3132
float64 longitudinal_distance_deviation
3233
float64 forward_trajectory_length_required
3334
float64 forward_trajectory_length_measured
35+
float64 latency
3436

3537
int64 invalid_count

planning/autoware_planning_validator/src/planning_validator.cpp

+13-1
Original file line numberDiff line numberDiff line change
@@ -80,6 +80,7 @@ void PlanningValidator::setupParameters()
8080
p.distance_deviation_threshold = declare_parameter<double>(t + "distance_deviation");
8181
p.longitudinal_distance_deviation_threshold =
8282
declare_parameter<double>(t + "longitudinal_distance_deviation");
83+
p.nominal_latency_threshold = declare_parameter<double>(t + "nominal_latency");
8384

8485
const std::string ps = "parameters.";
8586
p.forward_trajectory_length_acceleration =
@@ -168,6 +169,9 @@ void PlanningValidator::setupDiag()
168169
stat, validation_status_.is_valid_forward_trajectory_length,
169170
"trajectory length is too short");
170171
});
172+
d->add(ns + "latency", [&](auto & stat) {
173+
setStatus(stat, validation_status_.is_valid_latency, "latency is larger than expected value.");
174+
});
171175
}
172176

173177
bool PlanningValidator::isDataReady()
@@ -319,6 +323,7 @@ void PlanningValidator::validate(const Trajectory & trajectory)
319323
s.is_valid_lateral_acc = checkValidLateralAcceleration(resampled);
320324
s.is_valid_steering = checkValidSteering(resampled);
321325
s.is_valid_steering_rate = checkValidSteeringRate(resampled);
326+
s.is_valid_latency = checkValidLatency(trajectory);
322327

323328
s.invalid_count = isAllValid(s) ? 0 : s.invalid_count + 1;
324329
}
@@ -544,14 +549,20 @@ bool PlanningValidator::checkValidForwardTrajectoryLength(const Trajectory & tra
544549
return forward_length > forward_length_required;
545550
}
546551

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+
547558
bool PlanningValidator::isAllValid(const PlanningValidatorStatus & s) const
548559
{
549560
return s.is_valid_size && s.is_valid_finite_value && s.is_valid_interval &&
550561
s.is_valid_relative_angle && s.is_valid_curvature && s.is_valid_lateral_acc &&
551562
s.is_valid_longitudinal_max_acc && s.is_valid_longitudinal_min_acc &&
552563
s.is_valid_steering && s.is_valid_steering_rate && s.is_valid_velocity_deviation &&
553564
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;
555566
}
556567

557568
void PlanningValidator::displayStatus()
@@ -582,6 +593,7 @@ void PlanningValidator::displayStatus()
582593
s.is_valid_longitudinal_distance_deviation,
583594
"planning trajectory is too far from ego in longitudinal direction!!");
584595
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!!");
585597
}
586598

587599
} // namespace autoware::planning_validator

planning/autoware_planning_validator/test/src/test_parameter.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@ constexpr double THRESHOLD_STEERING_RATE = 7.0 * deg2rad;
3232
constexpr double THRESHOLD_VELOCITY_DEVIATION = 15.0 * kmph2mps;
3333
constexpr double THRESHOLD_DISTANCE_DEVIATION = 3.0;
3434
constexpr double THRESHOLD_LONGITUDINAL_DISTANCE_DEVIATION = 2.0;
35+
constexpr double THRESHOLD_NOMINAL_LATENCY = 1.0;
3536
constexpr double PARAMETER_FORWARD_TRAJECTORY_LENGTH_ACCELERATION = -5.0;
3637
constexpr double PARAMETER_FORWARD_TRAJECTORY_LENGTH_MARGIN = 2.0;
3738

planning/autoware_planning_validator/test/src/test_planning_validator_helper.cpp

+5
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@ Trajectory generateTrajectoryWithConstantAcceleration(
2828
const double acceleration)
2929
{
3030
Trajectory trajectory;
31+
trajectory.header.stamp = rclcpp::Clock{RCL_ROS_TIME}.now();
3132
double s = 0.0, v = speed, a = acceleration;
3233
constexpr auto MAX_DT = 10.0;
3334
for (size_t i = 0; i < size; ++i) {
@@ -71,6 +72,7 @@ Trajectory generateTrajectoryWithConstantCurvature(
7172
const auto radius = 1.0 / curvature;
7273

7374
Trajectory trajectory;
75+
trajectory.header.stamp = rclcpp::Clock{RCL_ROS_TIME}.now();
7476
double x = 0.0, y = 0.0, yaw = 0.0;
7577

7678
for (size_t i = 0; i <= size; ++i) {
@@ -106,6 +108,7 @@ Trajectory generateTrajectoryWithConstantSteeringRate(
106108
const double wheelbase)
107109
{
108110
Trajectory trajectory;
111+
trajectory.header.stamp = rclcpp::Clock{RCL_ROS_TIME}.now();
109112
double x = 0.0, y = 0.0, yaw = 0.0, steering_angle_rad = 0.0;
110113

111114
constexpr double MAX_STEERING_ANGLE_RAD = M_PI / 3.0;
@@ -158,6 +161,7 @@ Trajectory generateInfTrajectory()
158161
Trajectory generateBadCurvatureTrajectory()
159162
{
160163
Trajectory trajectory;
164+
trajectory.header.stamp = rclcpp::Clock{RCL_ROS_TIME}.now();
161165

162166
double y = 1.5;
163167
for (double s = 0.0; s <= 10.0; s += 1.0) {
@@ -206,6 +210,7 @@ rclcpp::NodeOptions getNodeOptionsWithDefaultParams()
206210
"thresholds.distance_deviation", THRESHOLD_DISTANCE_DEVIATION);
207211
node_options.append_parameter_override(
208212
"thresholds.longitudinal_distance_deviation", THRESHOLD_LONGITUDINAL_DISTANCE_DEVIATION);
213+
node_options.append_parameter_override("thresholds.nominal_latency", THRESHOLD_NOMINAL_LATENCY);
209214
node_options.append_parameter_override(
210215
"parameters.forward_trajectory_length_acceleration",
211216
PARAMETER_FORWARD_TRAJECTORY_LENGTH_ACCELERATION);

0 commit comments

Comments
 (0)