Merge pull request #1129 from tier4/hotfix/v0.44.0/avoidance

fix(avoidance): cherry pick
tier4 · Feb 7, 2024 · d8e1e8e · d8e1e8e
2 parents a352835 + 2c1ad39
commit d8e1e8e
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 9 deletions.
diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp
@@ -261,10 +261,11 @@ class AvoidanceHelper
 
   bool isComfortable(const AvoidLineArray & shift_lines) const
   {
+    const auto JERK_BUFFER = 0.1;  // [m/sss]
     return std::all_of(shift_lines.begin(), shift_lines.end(), [&](const auto & line) {
       return PathShifter::calcJerkFromLatLonDistance(
                line.getRelativeLength(), line.getRelativeLongitudinal(), getAvoidanceEgoSpeed()) <
-             getLateralMaxJerkLimit();
+             getLateralMaxJerkLimit() + JERK_BUFFER;
     });
   }
 

diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp
@@ -47,7 +47,7 @@ int32_t uuidToInt32(const unique_identifier_msgs::msg::UUID & uuid)
   int32_t ret = 0;
 
   for (size_t i = 0; i < sizeof(int32_t) / sizeof(int8_t); ++i) {
-    ret <<= sizeof(int8_t);
+    ret <<= sizeof(int8_t) * 8;
     ret |= uuid.uuid.at(i);
   }
 

diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp
@@ -185,29 +185,36 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
     }
 
     // prepare distance is not enough. unavoidable.
-    if (remaining_distance < 1e-3) {
+    if (avoidance_distance < 1e-3) {
       object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO;
       return std::nullopt;
     }
 
     // calculate lateral jerk.
     const auto required_jerk = PathShifter::calcJerkFromLatLonDistance(
-      avoiding_shift, remaining_distance, helper_->getAvoidanceEgoSpeed());
+      avoiding_shift, avoidance_distance, helper_->getAvoidanceEgoSpeed());
 
     // relax lateral jerk limit. avoidable.
     if (required_jerk < helper_->getLateralMaxJerkLimit()) {
       return std::make_pair(desire_shift_length, avoidance_distance);
     }
 
+    constexpr double LON_DIST_BUFFER = 1e-3;
+
     // avoidance distance is not enough. unavoidable.
     if (!isBestEffort(parameters_->policy_deceleration)) {
-      object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
-      return std::nullopt;
+      if (avoidance_distance < helper_->getMinAvoidanceDistance(avoiding_shift) + LON_DIST_BUFFER) {
+        object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO;
+        return std::nullopt;
+      } else {
+        object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
+        return std::nullopt;
+      }
     }
 
     // output avoidance path under lateral jerk constraints.
     const auto feasible_relative_shift_length = PathShifter::calcLateralDistFromJerk(
-      remaining_distance, helper_->getLateralMaxJerkLimit(), helper_->getAvoidanceEgoSpeed());
+      avoidance_distance, helper_->getLateralMaxJerkLimit(), helper_->getAvoidanceEgoSpeed());
 
     if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) {
       object.reason = "LessThanExecutionThreshold";
@@ -218,16 +225,25 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
       desire_shift_length > 0.0 ? feasible_relative_shift_length + current_ego_shift
                                 : -1.0 * feasible_relative_shift_length + current_ego_shift;
 
+    if (
+      avoidance_distance <
+      helper_->getMinAvoidanceDistance(feasible_shift_length) + LON_DIST_BUFFER) {
+      object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO;
+      return std::nullopt;
+    }
+
+    const double LAT_DIST_BUFFER = desire_shift_length > 0.0 ? 1e-3 : -1e-3;
+
     const auto infeasible =
-      std::abs(feasible_shift_length - object.overhang_dist) <
+      std::abs(feasible_shift_length - object.overhang_dist) - LAT_DIST_BUFFER <
       0.5 * data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral;
     if (infeasible) {
       RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. ");
       object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
       return std::nullopt;
     }
 
-    return std::make_pair(feasible_shift_length, avoidance_distance);
+    return std::make_pair(feasible_shift_length - LAT_DIST_BUFFER, avoidance_distance);
   };
 
   const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; };