Merge branch 'feat/add-mrm-v0.6-launch-based-on-v3.0.0' into feat/add-leader-election-converter

Author: TetsuKawa

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

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

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 (