feat: revert "feat(avoidance): add feature to guard unfeasible path"

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

@@ -165,7 +165,6 @@
           hard_road_shoulder_margin: 0.3                # [m]
           max_right_shift_length: 5.0
           max_left_shift_length: 5.0
-          max_deviation_from_lane: 0.5                  # [m]
         # avoidance distance parameters
         longitudinal:
           prepare_time: 2.0                             # [s]

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

@@ -239,9 +239,6 @@ struct AvoidanceParameters
   // Even if the obstacle is very large, it will not avoid more than this length for left direction
   double max_left_shift_length;
 
-  // Validate vehicle departure from driving lane.
-  double max_deviation_from_lane{0.0};
-
   // To prevent large acceleration while avoidance.
   double max_lateral_acceleration;
 
@@ -479,16 +476,8 @@ struct AvoidancePlanningData
   // safe shift point
   AvoidLineArray safe_new_sl{};
 
-  std::vector<DrivableLanes> drivable_lanes{};
-
-  lanelet::BasicLineString3d right_bound{};
-
-  lanelet::BasicLineString3d left_bound{};
-
   bool safe{false};
 
-  bool valid{false};
-
   bool comfortable{false};
 
   bool avoid_required{false};

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

@@ -85,14 +85,6 @@ bool isBestEffort(const std::string & policy)
 {
   return policy == "best_effort";
 }
-
-lanelet::BasicLineString3d toLineString3d(const std::vector<Point> & bound)
-{
-  lanelet::BasicLineString3d ret{};
-  std::for_each(
-    bound.begin(), bound.end(), [&](const auto & p) { ret.emplace_back(p.x, p.y, p.z); });
-  return ret;
-}
 }  // namespace
 
 AvoidanceModule::AvoidanceModule(
@@ -198,23 +190,6 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
   data.current_lanelets = utils::avoidance::getCurrentLanesFromPath(
     *getPreviousModuleOutput().reference_path, planner_data_);
 
-  // expand drivable lanes
-  std::for_each(
-    data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) {
-      data.drivable_lanes.push_back(
-        utils::avoidance::generateExpandDrivableLanes(lanelet, planner_data_, parameters_));
-    });
-
-  // calc drivable bound
-  const auto shorten_lanes =
-    utils::cutOverlappedLanes(*getPreviousModuleOutput().path, data.drivable_lanes);
-  data.left_bound = toLineString3d(utils::calcBound(
-    planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings,
-    parameters_->use_intersection_areas, true));
-  data.right_bound = toLineString3d(utils::calcBound(
-    planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings,
-    parameters_->use_intersection_areas, false));
-
   // reference path
   if (isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) {
     data.reference_path_rough = extendBackwardLength(*getPreviousModuleOutput().path);
@@ -419,7 +394,6 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de
   const auto new_sp = findNewShiftLine(processed_raw_sp);
   if (isValidShiftLine(new_sp, path_shifter)) {
     data.unapproved_new_sl = new_sp;
-    data.valid = true;
   }
 
   const auto found_new_sl = data.unapproved_new_sl.size() > 0;
@@ -457,17 +431,6 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de
 void AvoidanceModule::fillEgoStatus(
   AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const
 {
-  /**
-   * TODO(someone): prevent meaningless stop point insertion in other way.
-   * If the candidate shift line is invalid, manage all objects as unavoidable.
-   */
-  if (!data.valid) {
-    std::for_each(data.target_objects.begin(), data.target_objects.end(), [](auto & o) {
-      o.is_avoidable = false;
-      o.reason = "InvalidShiftLine";
-    });
-  }
-
   /**
    * Find the nearest object that should be avoid. When the ego follows reference path,
    * if the both of following two conditions are satisfied, the module surely avoid the object.
@@ -2379,32 +2342,6 @@ bool AvoidanceModule::isValidShiftLine(
     }
   }
 
-  // check if the vehicle is in road. (yaw angle is not considered)
-  {
-    const auto minimum_distance = 0.5 * planner_data_->parameters.vehicle_width +
-                                  parameters_->hard_road_shoulder_margin -
-                                  parameters_->max_deviation_from_lane;
-
-    const size_t start_idx = shift_lines.front().start_idx;
-    const size_t end_idx = shift_lines.back().end_idx;
-
-    for (size_t i = start_idx; i <= end_idx; ++i) {
-      const auto p = getPoint(shifter_for_validate.getReferencePath().points.at(i));
-      lanelet::BasicPoint2d basic_point{p.x, p.y};
-
-      const auto shift_length = proposed_shift_path.shift_length.at(i);
-      const auto bound = shift_length > 0.0 ? avoid_data_.left_bound : avoid_data_.right_bound;
-      const auto THRESHOLD = minimum_distance + std::abs(shift_length);
-
-      if (boost::geometry::distance(basic_point, lanelet::utils::to2D(bound)) < THRESHOLD) {
-        RCLCPP_DEBUG_THROTTLE(
-          getLogger(), *clock_, 1000,
-          "following latest new shift line may cause deviation from drivable area.");
-        return false;
-      }
-    }
-  }
-
   debug_data_.proposed_spline_shift = proposed_shift_path.shift_length;
 
   return true;  // valid shift line.

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

@@ -186,8 +186,6 @@ AvoidanceModuleManager::AvoidanceModuleManager(
       getOrDeclareParameter<double>(*node, ns + "lateral_avoid_check_threshold");
     p.max_right_shift_length = getOrDeclareParameter<double>(*node, ns + "max_right_shift_length");
     p.max_left_shift_length = getOrDeclareParameter<double>(*node, ns + "max_left_shift_length");
-    p.max_deviation_from_lane =
-      getOrDeclareParameter<double>(*node, ns + "max_deviation_from_lane");
   }
 
   // avoidance maneuver (longitudinal)