diff --git a/control/autoware_control_validator/README.md b/control/autoware_control_validator/README.md index cc3d2069c940f..471003259d5a3 100644 --- a/control/autoware_control_validator/README.md +++ b/control/autoware_control_validator/README.md @@ -57,9 +57,10 @@ The following parameters can be set for the `control_validator`: The input trajectory is detected as invalid if the index exceeds the following thresholds. -| Name | Type | Description | Default value | -| :---------------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ | -| `thresholds.max_distance_deviation` | double | invalid threshold of the max distance deviation between the predicted path and the reference trajectory [m] | 1.0 | -| `thresholds.rolling_back_velocity` | double | threshold velocity to valid the vehicle velocity [m/s] | 0.5 | -| `thresholds.over_velocity_offset` | double | threshold velocity offset to valid the vehicle velocity [m/s] | 2.0 | -| `thresholds.over_velocity_ratio` | double | threshold ratio to valid the vehicle velocity [*] | 0.2 | +| Name | Type | Description | Default value | +| :----------------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ | +| `thresholds.max_distance_deviation` | double | invalid threshold of the max distance deviation between the predicted path and the reference trajectory [m] | 1.0 | +| `thresholds.rolling_back_velocity` | double | threshold velocity to valid the vehicle velocity [m/s] | 0.5 | +| `thresholds.over_velocity_offset` | double | threshold velocity offset to valid the vehicle velocity [m/s] | 2.0 | +| `thresholds.over_velocity_ratio` | double | threshold ratio to valid the vehicle velocity [*] | 0.2 | +| `thresholds.overrun_stop_point_dist` | double | threshold distance to overrun stop point [m] | 0.8 | diff --git a/control/autoware_control_validator/config/control_validator.param.yaml b/control/autoware_control_validator/config/control_validator.param.yaml index 95f9c9be22a12..538981fdb43bd 100644 --- a/control/autoware_control_validator/config/control_validator.param.yaml +++ b/control/autoware_control_validator/config/control_validator.param.yaml @@ -12,6 +12,7 @@ rolling_back_velocity: 0.5 over_velocity_offset: 2.0 over_velocity_ratio: 0.2 + overrun_stop_point_dist: 0.8 nominal_latency: 0.01 vel_lpf_gain: 0.9 # Time constant 0.33 diff --git a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp index 61cadf3f36fe8..2cbfeade8bc01 100644 --- a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp +++ b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp @@ -53,6 +53,7 @@ struct ValidationParams double rolling_back_velocity; double over_velocity_ratio; double over_velocity_offset; + double overrun_stop_point_dist; double nominal_latency_threshold; }; @@ -95,6 +96,14 @@ class ControlValidator : public rclcpp::Node void calc_velocity_deviation_status( const Trajectory & reference_trajectory, const Odometry & kinematics); + /** + * @brief Calculate whether the vehicle has overrun a stop point in the trajectory. + * @param reference_trajectory Reference trajectory + * @param kinematics Current vehicle odometry including pose and twist + */ + void calc_stop_point_overrun_status( + const Trajectory & reference_trajectory, const Odometry & kinematics); + private: /** * @brief Setup diagnostic updater diff --git a/control/autoware_control_validator/msg/ControlValidatorStatus.msg b/control/autoware_control_validator/msg/ControlValidatorStatus.msg index c40f160510d89..934d855d0691b 100644 --- a/control/autoware_control_validator/msg/ControlValidatorStatus.msg +++ b/control/autoware_control_validator/msg/ControlValidatorStatus.msg @@ -4,12 +4,15 @@ builtin_interfaces/Time stamp bool is_valid_max_distance_deviation bool is_rolling_back bool is_over_velocity +bool has_overrun_stop_point bool is_valid_latency # values float64 max_distance_deviation float64 target_vel float64 vehicle_vel +float64 dist_to_stop +float64 nearest_trajectory_vel float64 latency int64 invalid_count diff --git a/control/autoware_control_validator/src/control_validator.cpp b/control/autoware_control_validator/src/control_validator.cpp index 528bfdfcc1df2..f176f13a18bc6 100644 --- a/control/autoware_control_validator/src/control_validator.cpp +++ b/control/autoware_control_validator/src/control_validator.cpp @@ -16,10 +16,12 @@ #include "autoware/control_validator/utils.hpp" #include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include +#include #include #include #include @@ -72,6 +74,7 @@ void ControlValidator::setup_parameters() p.rolling_back_velocity = declare_parameter(t + "rolling_back_velocity"); p.over_velocity_offset = declare_parameter(t + "over_velocity_offset"); p.over_velocity_ratio = declare_parameter(t + "over_velocity_ratio"); + p.overrun_stop_point_dist = declare_parameter(t + "overrun_stop_point_dist"); p.nominal_latency_threshold = declare_parameter(t + "nominal_latency"); } const auto lpf_gain = declare_parameter("vel_lpf_gain"); @@ -129,6 +132,11 @@ void ControlValidator::setup_diag() stat, !validation_status_.is_over_velocity, "The vehicle is over-speeding against the target."); }); + d.add(ns + "overrun_stop_point", [&](auto & stat) { + set_status( + stat, !validation_status_.has_overrun_stop_point, + "The vehicle has overrun the front stop point on the trajectory."); + }); d.add(ns + "latency", [&](auto & stat) { set_status( stat, validation_status_.is_valid_latency, "The latency is larger than expected value."); @@ -219,12 +227,14 @@ void ControlValidator::validate( } validation_status_.stamp = get_clock()->now(); + validation_status_.vehicle_vel = vehicle_vel_.filter(kinematics.twist.twist.linear.x); std::tie( validation_status_.max_distance_deviation, validation_status_.is_valid_max_distance_deviation) = calc_lateral_deviation_status(predicted_trajectory, *current_reference_trajectory_); calc_velocity_deviation_status(*current_reference_trajectory_, kinematics); + calc_stop_point_overrun_status(*current_reference_trajectory_, kinematics); validation_status_.invalid_count = is_all_valid(validation_status_) ? 0 : validation_status_.invalid_count + 1; @@ -245,7 +255,6 @@ void ControlValidator::calc_velocity_deviation_status( { auto & status = validation_status_; const auto & params = validation_params_; - status.vehicle_vel = vehicle_vel_.filter(kinematics.twist.twist.linear.x); status.target_vel = target_vel_.filter( autoware::motion_utils::calcInterpolatedPoint(reference_trajectory, kinematics.pose.pose) .longitudinal_velocity_mps); @@ -268,10 +277,41 @@ void ControlValidator::calc_velocity_deviation_status( } } +void ControlValidator::calc_stop_point_overrun_status( + const Trajectory & reference_trajectory, const Odometry & kinematics) +{ + auto & status = validation_status_; + const auto & params = validation_params_; + + status.dist_to_stop = [](const Trajectory & traj, const geometry_msgs::msg::Pose & pose) { + const auto stop_idx_opt = autoware::motion_utils::searchZeroVelocityIndex(traj.points); + + const size_t end_idx = stop_idx_opt ? *stop_idx_opt : traj.points.size() - 1; + const size_t seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(traj.points, pose); + const double signed_length_on_traj = autoware::motion_utils::calcSignedArcLength( + traj.points, pose.position, seg_idx, traj.points.at(end_idx).pose.position, + std::min(end_idx, traj.points.size() - 2)); + + if (std::isnan(signed_length_on_traj)) { + return 0.0; + } + return signed_length_on_traj; + }(reference_trajectory, kinematics.pose.pose); + + status.nearest_trajectory_vel = + autoware::motion_utils::calcInterpolatedPoint(reference_trajectory, kinematics.pose.pose) + .longitudinal_velocity_mps; + + // NOTE: the same velocity threshold as autoware::motion_utils::searchZeroVelocity + status.has_overrun_stop_point = status.dist_to_stop < -params.overrun_stop_point_dist && + status.nearest_trajectory_vel < 1e-3 && status.vehicle_vel > 1e-3; +} + bool ControlValidator::is_all_valid(const ControlValidatorStatus & s) { return s.is_valid_max_distance_deviation && !s.is_rolling_back && !s.is_over_velocity && - s.is_valid_latency; + !s.has_overrun_stop_point; } void ControlValidator::display_status()