refactor(avoidance): remove hard code params (#3624)

* refactor(avoidance): remove hard code params in shift line triming process Signed-off-by: satoshi-ota <[email protected]> * refactor(avoidance): update config Signed-off-by: satoshi-ota <[email protected]> * chore(avoidance): add description for params Signed-off-by: satoshi-ota <[email protected]> --------- Signed-off-by: satoshi-ota <[email protected]>
autowarefoundation · May 8, 2023 · ccdcb5c · ccdcb5c
1 parent bc41dbe
commit ccdcb5c
Show file tree

Hide file tree

Showing 6 changed files with 76 additions and 29 deletions.
diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
@@ -143,3 +143,11 @@
         matrix:
           [2.78, 13.9,                                  # velocity [m/s]
            0.50, 1.00]                                  # margin [m]
+
+      shift_line_pipeline:
+        trim:
+          quantize_filter_threshold: 0.2
+          same_grad_filter_1_threshold: 0.1
+          same_grad_filter_2_threshold: 0.2
+          same_grad_filter_3_threshold: 0.5
+          sharp_shift_filter_threshold: 0.2
diff --git a/...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
@@ -228,12 +228,12 @@ class AvoidanceModule : public SceneModuleInterface
 
   // shift point generation: trimmers
   AvoidLineArray trimShiftLine(const AvoidLineArray & shift_lines, DebugData & debug) const;
-  void quantizeShiftLine(AvoidLineArray & shift_lines, const double interval) const;
-  void trimSmallShiftLine(AvoidLineArray & shift_lines, const double shift_diff_thres) const;
+  void quantizeShiftLine(AvoidLineArray & shift_lines, const double threshold) const;
+  void trimSmallShiftLine(AvoidLineArray & shift_lines, const double threshold) const;
   void trimSimilarGradShiftLine(AvoidLineArray & shift_lines, const double threshold) const;
   void trimMomentaryReturn(AvoidLineArray & shift_lines) const;
   void trimTooSharpShift(AvoidLineArray & shift_lines) const;
-  void trimSharpReturn(AvoidLineArray & shift_lines) const;
+  void trimSharpReturn(AvoidLineArray & shift_lines, const double threshold) const;
 
   // shift point generation: return-shift generator
   void addReturnShiftLineFromEgo(

diff --git a/...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
@@ -238,6 +238,21 @@ struct AvoidanceParameters
   // avoidance path will be generated unless their lateral margin difference exceeds this value.
   double avoidance_execution_lateral_threshold;
 
+  // For shift line generation process. The continuous shift length is quantized by this value.
+  double quantize_filter_threshold;
+
+  // For shift line generation process. Merge small shift lines. (First step)
+  double same_grad_filter_1_threshold;
+
+  // For shift line generation process. Merge small shift lines. (Second step)
+  double same_grad_filter_2_threshold;
+
+  // For shift line generation process. Merge small shift lines. (Third step)
+  double same_grad_filter_3_threshold;
+
+  // For shift line generation process. Remove sharp(=jerky) shift line.
+  double sharp_shift_filter_threshold;
+
   // target velocity matrix
   std::vector<double> target_velocity_matrix;
 

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -657,6 +657,20 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
     p.target_velocity_matrix = declare_parameter<std::vector<double>>(ns + "matrix");
   }
 
+  // shift line pipeline
+  {
+    std::string ns = "avoidance.shift_line_pipeline.";
+    p.quantize_filter_threshold = declare_parameter<double>(ns + "trim.quantize_filter_threshold");
+    p.same_grad_filter_1_threshold =
+      declare_parameter<double>(ns + "trim.same_grad_filter_1_threshold");
+    p.same_grad_filter_2_threshold =
+      declare_parameter<double>(ns + "trim.same_grad_filter_2_threshold");
+    p.same_grad_filter_3_threshold =
+      declare_parameter<double>(ns + "trim.same_grad_filter_3_threshold");
+    p.sharp_shift_filter_threshold =
+      declare_parameter<double>(ns + "trim.sharp_shift_filter_threshold");
+  }
+
   return p;
 }
 

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -1288,17 +1288,17 @@ AvoidLineArray AvoidanceModule::trimShiftLine(
   // - Combine avoid points that have almost same gradient.
   // this is to remove the noise.
   {
-    const auto CHANGE_SHIFT_THRESHOLD_FOR_NOISE = 0.1;
-    trimSimilarGradShiftLine(sl_array_trimmed, CHANGE_SHIFT_THRESHOLD_FOR_NOISE);
+    const auto THRESHOLD = parameters_->same_grad_filter_1_threshold;
+    trimSimilarGradShiftLine(sl_array_trimmed, THRESHOLD);
     debug.trim_similar_grad_shift = sl_array_trimmed;
     printShiftLines(sl_array_trimmed, "after trim_similar_grad_shift");
   }
 
   // - Quantize the shift length to reduce the shift point noise
   // This is to remove the noise coming from detection accuracy, interpolation, resampling, etc.
   {
-    constexpr double QUANTIZATION_DISTANCE = 0.2;
-    quantizeShiftLine(sl_array_trimmed, QUANTIZATION_DISTANCE);
+    const auto THRESHOLD = parameters_->quantize_filter_threshold;
+    quantizeShiftLine(sl_array_trimmed, THRESHOLD);
     printShiftLines(sl_array_trimmed, "after sl_array_trimmed");
     debug.quantized = sl_array_trimmed;
   }
@@ -1313,8 +1313,8 @@ AvoidLineArray AvoidanceModule::trimShiftLine(
 
   // - Combine avoid points that have almost same gradient (again)
   {
-    const auto CHANGE_SHIFT_THRESHOLD = 0.2;
-    trimSimilarGradShiftLine(sl_array_trimmed, CHANGE_SHIFT_THRESHOLD);
+    const auto THRESHOLD = parameters_->same_grad_filter_2_threshold;
+    trimSimilarGradShiftLine(sl_array_trimmed, THRESHOLD);
     debug.trim_similar_grad_shift_second = sl_array_trimmed;
     printShiftLines(sl_array_trimmed, "after trim_similar_grad_shift_second");
   }
@@ -1323,15 +1323,16 @@ AvoidLineArray AvoidanceModule::trimShiftLine(
   // Check if it is not too sharp for the return-to-center shift point.
   // If the shift is sharp, it is combined with the next shift point until it gets non-sharp.
   {
-    trimSharpReturn(sl_array_trimmed);
+    const auto THRESHOLD = parameters_->sharp_shift_filter_threshold;
+    trimSharpReturn(sl_array_trimmed, THRESHOLD);
     debug.trim_too_sharp_shift = sl_array_trimmed;
     printShiftLines(sl_array_trimmed, "after trimSharpReturn");
   }
 
   // - Combine avoid points that have almost same gradient (again)
   {
-    const auto CHANGE_SHIFT_THRESHOLD = 0.5;
-    trimSimilarGradShiftLine(sl_array_trimmed, CHANGE_SHIFT_THRESHOLD);
+    const auto THRESHOLD = parameters_->same_grad_filter_3_threshold;
+    trimSimilarGradShiftLine(sl_array_trimmed, THRESHOLD);
     debug.trim_similar_grad_shift_third = sl_array_trimmed;
     printShiftLines(sl_array_trimmed, "after trim_similar_grad_shift_second");
   }
@@ -1362,21 +1363,20 @@ void AvoidanceModule::alignShiftLinesOrder(
   }
 }
 
-void AvoidanceModule::quantizeShiftLine(AvoidLineArray & shift_lines, const double interval) const
+void AvoidanceModule::quantizeShiftLine(AvoidLineArray & shift_lines, const double threshold) const
 {
-  if (interval < 1.0e-5) {
+  if (threshold < 1.0e-5) {
     return;  // no need to process
   }
 
   for (auto & sl : shift_lines) {
-    sl.end_shift_length = std::round(sl.end_shift_length / interval) * interval;
+    sl.end_shift_length = std::round(sl.end_shift_length / threshold) * threshold;
   }
 
   alignShiftLinesOrder(shift_lines);
 }
 
-void AvoidanceModule::trimSmallShiftLine(
-  AvoidLineArray & shift_lines, const double shift_diff_thres) const
+void AvoidanceModule::trimSmallShiftLine(AvoidLineArray & shift_lines, const double threshold) const
 {
   AvoidLineArray shift_lines_orig = shift_lines;
   shift_lines.clear();
@@ -1391,7 +1391,7 @@ void AvoidanceModule::trimSmallShiftLine(
     auto sl_modified = sl_now;
 
     // remove the shift point if the length is almost same as the previous one.
-    if (std::abs(shift_diff) < shift_diff_thres) {
+    if (std::abs(shift_diff) < threshold) {
       sl_modified.end_shift_length = sl_prev.end_shift_length;
       sl_modified.start_shift_length = sl_prev.end_shift_length;
       DEBUG_PRINT(
@@ -1409,7 +1409,7 @@ void AvoidanceModule::trimSmallShiftLine(
 }
 
 void AvoidanceModule::trimSimilarGradShiftLine(
-  AvoidLineArray & avoid_lines, const double change_shift_dist_threshold) const
+  AvoidLineArray & avoid_lines, const double threshold) const
 {
   AvoidLineArray avoid_lines_orig = avoid_lines;
   avoid_lines.clear();
@@ -1436,8 +1436,7 @@ void AvoidanceModule::trimSimilarGradShiftLine(
         const auto longitudinal = original.end_longitudinal - combined_al.start_longitudinal;
         const auto new_length =
           combined_al.getGradient() * longitudinal + combined_al.start_shift_length;
-        const bool has_large_change =
-          std::abs(new_length - original.end_shift_length) > change_shift_dist_threshold;
+        const bool has_large_change = std::abs(new_length - original.end_shift_length) > threshold;
 
         DEBUG_PRINT(
           "original.end_shift_length: %f, original.end_longitudinal: %f, "
@@ -1446,7 +1445,7 @@ void AvoidanceModule::trimSimilarGradShiftLine(
           original.end_shift_length, original.end_longitudinal, combined_al.start_longitudinal,
           combined_al.getGradient(), new_length, has_large_change);
 
-        if (std::abs(new_length - original.end_shift_length) > change_shift_dist_threshold) {
+        if (std::abs(new_length - original.end_shift_length) > threshold) {
           return true;
         }
       }
@@ -1620,7 +1619,7 @@ void AvoidanceModule::trimMomentaryReturn(AvoidLineArray & shift_lines) const
   DEBUG_PRINT("trimMomentaryReturn: size %lu -> %lu", shift_lines_orig.size(), shift_lines.size());
 }
 
-void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines) const
+void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines, const double threshold) const
 {
   AvoidLineArray shift_lines_orig = shift_lines;
   shift_lines.clear();
@@ -1651,17 +1650,14 @@ void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines) const
   };
 
   // Check if the merged shift has a conflict with the original shifts.
-  const auto hasViolation = [this](const auto & combined, const auto & combined_src) {
-    constexpr auto VIOLATION_SHIFT_THR = 0.3;
+  const auto hasViolation = [&threshold](const auto & combined, const auto & combined_src) {
     for (const auto & sl : combined_src) {
       const auto combined_shift =
         utils::avoidance::lerpShiftLengthOnArc(sl.end_longitudinal, combined);
-      if (
-        sl.end_shift_length < -0.01 && combined_shift > sl.end_shift_length + VIOLATION_SHIFT_THR) {
+      if (sl.end_shift_length < -0.01 && combined_shift > sl.end_shift_length + threshold) {
         return true;
       }
-      if (
-        sl.end_shift_length > 0.01 && combined_shift < sl.end_shift_length - VIOLATION_SHIFT_THR) {
+      if (sl.end_shift_length > 0.01 && combined_shift < sl.end_shift_length - threshold) {
         return true;
       }
     }

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp
@@ -95,6 +95,20 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
     updateParam<double>(parameters, ns + "max_lateral_jerk", p->max_lateral_jerk);
   }
 
+  {
+    const std::string ns = "avoidance.shift_line_pipeline.";
+    updateParam<double>(
+      parameters, ns + "trim.quantize_filter_threshold", p->quantize_filter_threshold);
+    updateParam<double>(
+      parameters, ns + "trim.same_grad_filter_1_threshold", p->same_grad_filter_1_threshold);
+    updateParam<double>(
+      parameters, ns + "trim.same_grad_filter_2_threshold", p->same_grad_filter_2_threshold);
+    updateParam<double>(
+      parameters, ns + "trim.same_grad_filter_3_threshold", p->same_grad_filter_3_threshold);
+    updateParam<double>(
+      parameters, ns + "trim.sharp_shift_filter_threshold", p->sharp_shift_filter_threshold);
+  }
+
   std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) {
     m->updateModuleParams(p);
   });