diff --git a/control/control_validator/config/control_validator.param.yaml b/control/control_validator/config/control_validator.param.yaml index 7bbe6a466799b..54cc0f3f9d712 100644 --- a/control/control_validator/config/control_validator.param.yaml +++ b/control/control_validator/config/control_validator.param.yaml @@ -12,3 +12,4 @@ thresholds: max_distance_deviation: 1.0 + min_velocity_for_checking: 1.0 # m/s diff --git a/control/control_validator/include/control_validator/control_validator.hpp b/control/control_validator/include/control_validator/control_validator.hpp index 48b7eba7412a2..40f636d6122c1 100644 --- a/control/control_validator/include/control_validator/control_validator.hpp +++ b/control/control_validator/include/control_validator/control_validator.hpp @@ -41,6 +41,7 @@ using nav_msgs::msg::Odometry; struct ValidationParams { double max_distance_deviation_threshold; + double min_velocity_for_checking; }; class ControlValidator : public rclcpp::Node diff --git a/control/control_validator/src/control_validator.cpp b/control/control_validator/src/control_validator.cpp index ffce38f009a41..512be1dae5c82 100644 --- a/control/control_validator/src/control_validator.cpp +++ b/control/control_validator/src/control_validator.cpp @@ -61,6 +61,7 @@ void ControlValidator::setupParameters() auto & p = validation_params_; const std::string t = "thresholds."; p.max_distance_deviation_threshold = declare_parameter(t + "max_distance_deviation"); + p.min_velocity_for_checking = declare_parameter(t + "min_velocity_for_checking"); } try { @@ -175,6 +176,11 @@ void ControlValidator::validate(const Trajectory & predicted_trajectory) bool ControlValidator::checkValidMaxDistanceDeviation(const Trajectory & predicted_trajectory) { + if (current_kinematics_->twist.twist.linear.x < validation_params_.min_velocity_for_checking) { + RCLCPP_INFO(get_logger(), "Current velocity is too low. Skip."); + return true; + } + validation_status_.max_distance_deviation = calcMaxLateralDistance(*current_reference_trajectory_, predicted_trajectory); if (