From cf4a1951c1c1bdbae3dbf5f407ed4db1622c0264 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Mon, 17 Jun 2024 22:30:19 +0900 Subject: [PATCH] feat(control_validator): add min velocity for max distance deviation checking (#1337) * feat(control_validator): add min velocity for max distance deviation checking Signed-off-by: Makoto Kurihara * chore(control_validator): remove debug logs Signed-off-by: Makoto Kurihara --------- Signed-off-by: Makoto Kurihara --- .../control_validator/config/control_validator.param.yaml | 1 + .../include/control_validator/control_validator.hpp | 1 + control/control_validator/src/control_validator.cpp | 5 +++++ 3 files changed, 7 insertions(+) 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..f6d49de4c1386 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,10 @@ 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) { + return true; + } + validation_status_.max_distance_deviation = calcMaxLateralDistance(*current_reference_trajectory_, predicted_trajectory); if (