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 <[email protected]> * chore(control_validator): remove debug logs Signed-off-by: Makoto Kurihara <[email protected]> --------- Signed-off-by: Makoto Kurihara <[email protected]>
tier4 · Jun 17, 2024 · cf4a195 · cf4a195
1 parent 08bf607
commit cf4a195
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 0 deletions.
diff --git 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
@@ -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
@@ -61,6 +61,7 @@ void ControlValidator::setupParameters()
     auto & p = validation_params_;
     const std::string t = "thresholds.";
     p.max_distance_deviation_threshold = declare_parameter<double>(t + "max_distance_deviation");
+    p.min_velocity_for_checking = declare_parameter<double>(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 (
Original file line number	Diff line number	Diff line change
Expand Up		@@ -12,3 +12,4 @@

		thresholds:
		max_distance_deviation: 1.0
		min_velocity_for_checking: 1.0 # m/s