From 38d0ca1cb91d5b3f9a3152c587447fed0d50702d Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Mon, 17 Jun 2024 14:44:42 +0900 Subject: [PATCH 1/2] feat(control_validator): add min velocity for max distance deviation checking 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 | 6 ++++++ 3 files changed, 8 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..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 ( From 8436fcc37dd1c500373ad793e2b500be4ed2110a Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Mon, 17 Jun 2024 15:11:15 +0900 Subject: [PATCH 2/2] chore(control_validator): remove debug logs Signed-off-by: Makoto Kurihara --- control/control_validator/src/control_validator.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/control/control_validator/src/control_validator.cpp b/control/control_validator/src/control_validator.cpp index 512be1dae5c82..f6d49de4c1386 100644 --- a/control/control_validator/src/control_validator.cpp +++ b/control/control_validator/src/control_validator.cpp @@ -177,7 +177,6 @@ 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; }