feat(avoidance): add parameter to configurate avoidance return point (a…

…utowarefoundation#4467)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
Signed-off-by: Felix F Xu <felixfxu@gmail.com>

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -160,6 +160,7 @@
         # avoidance distance parameters
         longitudinal:
           prepare_time: 2.0                             # [s]
+          remain_buffer_distance: 30.0                  # [m]
           min_prepare_distance: 1.0                     # [m]
           min_slow_down_speed: 1.38                     # [m/s]
           buf_slow_down_speed: 0.56                     # [m/s]

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp

@@ -225,6 +225,9 @@ struct AvoidanceParameters
   // nominal avoidance sped
   double nominal_avoidance_speed;
 
+  // module try to return original path to keep this distance from edge point of the path.
+  double remain_buffer_distance;
+
   // The margin is configured so that the generated avoidance trajectory does not come near to the
   // road shoulder.
   double road_shoulder_safety_margin{1.0};

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -996,9 +996,10 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
       const auto offset = object_parameter.safety_buffer_longitudinal + base_link2rear + o.length;
       // The end_margin also has the purpose of preventing the return path from NOT being
       // triggered at the end point.
-      const auto end_margin = 1.0;
-      const auto return_remaining_distance =
-        std::max(data.arclength_from_ego.back() - o.longitudinal - offset - end_margin, 0.0);
+      const auto return_remaining_distance = std::max(
+        data.arclength_from_ego.back() - o.longitudinal - offset -
+          parameters_->remain_buffer_distance,
+        0.0);
 
       al_return.start_shift_length = feasible_shift_length.get();
       al_return.end_shift_length = 0.0;
@@ -1745,14 +1746,14 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates)
     return;
   }
 
-  const auto remaining_distance = arclength_from_ego.back();
+  const auto remaining_distance = arclength_from_ego.back() - parameters_->remain_buffer_distance;
 
   // If the avoidance point has already been set, the return shift must be set after the point.
   const auto last_sl_distance = avoidance_data_.arclength_from_ego.at(last_sl.end_idx);
 
   // check if there is enough distance for return.
-  if (last_sl_distance + 1.0 > remaining_distance) {  // tmp: add some small number (+1.0)
-    DEBUG_PRINT("No enough distance for return.");
+  if (last_sl_distance > remaining_distance) {  // tmp: add some small number (+1.0)
+    RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 1000, "No enough distance for return.");
     return;
   }
 

planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp

@@ -173,6 +173,7 @@ AvoidanceModuleManager::AvoidanceModuleManager(
     std::string ns = "avoidance.avoidance.longitudinal.";
     p.prepare_time = get_parameter<double>(node, ns + "prepare_time");
     p.min_prepare_distance = get_parameter<double>(node, ns + "min_prepare_distance");
+    p.remain_buffer_distance = get_parameter<double>(node, ns + "remain_buffer_distance");
     p.min_slow_down_speed = get_parameter<double>(node, ns + "min_slow_down_speed");
     p.buf_slow_down_speed = get_parameter<double>(node, ns + "buf_slow_down_speed");
     p.nominal_avoidance_speed = get_parameter<double>(node, ns + "nominal_avoidance_speed");