feat(avoidance): add feature to guard unfeasible path

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

@@ -165,6 +165,7 @@
           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/scene_module/avoidance/avoidance_module.hpp

@@ -453,7 +453,7 @@ class AvoidanceModule : public SceneModuleInterface
   // TODO(murooka) freespace during turning in intersection where there is no neighbor lanes
   // NOTE: Assume that there is no situation where there is an object in the middle lane of more
   // than two lanes since which way to avoid is not obvious
-  void generateExtendedDrivableArea(BehaviorModuleOutput & output) const;
+  void generateExpandDrivableLanes(BehaviorModuleOutput & output) const;
 
   /**
    * @brief fill debug markers.

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

@@ -239,6 +239,9 @@ 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;
 
@@ -476,8 +479,16 @@ 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/include/behavior_path_planner/utils/avoidance/utils.hpp

@@ -167,6 +167,10 @@ std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
   const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
   const AvoidancePlanningData & data, const std::shared_ptr<AvoidanceParameters> & parameters,
   const bool is_running, DebugData & debug);
+
+DrivableLanes generateExpandDrivableLanes(
+  const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data,
+  const std::shared_ptr<AvoidanceParameters> & parameters);
 }  // namespace behavior_path_planner::utils::avoidance
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_

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

@@ -62,22 +62,6 @@ using tier4_planning_msgs::msg::AvoidanceDebugFactor;
 
 namespace
 {
-bool isEndPointsConnected(
-  const lanelet::ConstLanelet & left_lane, const lanelet::ConstLanelet & right_lane)
-{
-  const auto & left_back_point_2d = right_lane.leftBound2d().back().basicPoint();
-  const auto & right_back_point_2d = left_lane.rightBound2d().back().basicPoint();
-
-  constexpr double epsilon = 1e-5;
-  return (right_back_point_2d - left_back_point_2d).norm() < epsilon;
-}
-
-template <typename T>
-void pushUniqueVector(T & base_vector, const T & additional_vector)
-{
-  base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end());
-}
-
 bool isDrivingSameLane(
   const lanelet::ConstLanelets & previous_lanes, const lanelet::ConstLanelets & current_lanes)
 {
@@ -101,6 +85,14 @@ 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(
@@ -206,6 +198,23 @@ 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);
@@ -410,6 +419,7 @@ 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;
@@ -447,6 +457,17 @@ 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.
@@ -1921,148 +1942,12 @@ bool AvoidanceModule::isSafePath(
   return safe_ || safe_count_ > parameters_->hysteresis_factor_safe_count;
 }
 
-void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output) const
+void AvoidanceModule::generateExpandDrivableLanes(BehaviorModuleOutput & output) const
 {
-  const auto has_same_lane =
-    [](const lanelet::ConstLanelets lanes, const lanelet::ConstLanelet & lane) {
-      if (lanes.empty()) return false;
-      const auto has_same = [&](const auto & ll) { return ll.id() == lane.id(); };
-      return std::find_if(lanes.begin(), lanes.end(), has_same) != lanes.end();
-    };
-
-  const auto & route_handler = planner_data_->route_handler;
-  const auto & current_lanes = avoid_data_.current_lanelets;
-  const auto & enable_opposite = parameters_->use_opposite_lane;
   std::vector<DrivableLanes> drivable_lanes;
-
-  for (const auto & current_lane : current_lanes) {
-    DrivableLanes current_drivable_lanes;
-    current_drivable_lanes.left_lane = current_lane;
-    current_drivable_lanes.right_lane = current_lane;
-
-    if (!parameters_->use_adjacent_lane) {
-      drivable_lanes.push_back(current_drivable_lanes);
-      continue;
-    }
-
-    // 1. get left/right side lanes
-    const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) {
-      const auto all_left_lanelets =
-        route_handler->getAllLeftSharedLinestringLanelets(target_lane, enable_opposite, true);
-      if (!all_left_lanelets.empty()) {
-        current_drivable_lanes.left_lane = all_left_lanelets.back();  // leftmost lanelet
-        pushUniqueVector(
-          current_drivable_lanes.middle_lanes,
-          lanelet::ConstLanelets(all_left_lanelets.begin(), all_left_lanelets.end() - 1));
-      }
-    };
-    const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) {
-      const auto all_right_lanelets =
-        route_handler->getAllRightSharedLinestringLanelets(target_lane, enable_opposite, true);
-      if (!all_right_lanelets.empty()) {
-        current_drivable_lanes.right_lane = all_right_lanelets.back();  // rightmost lanelet
-        pushUniqueVector(
-          current_drivable_lanes.middle_lanes,
-          lanelet::ConstLanelets(all_right_lanelets.begin(), all_right_lanelets.end() - 1));
-      }
-    };
-
-    update_left_lanelets(current_lane);
-    update_right_lanelets(current_lane);
-
-    // 2.1 when there are multiple lanes whose previous lanelet is the same
-    const auto get_next_lanes_from_same_previous_lane =
-      [&route_handler](const lanelet::ConstLanelet & lane) {
-        // get previous lane, and return false if previous lane does not exist
-        lanelet::ConstLanelets prev_lanes;
-        if (!route_handler->getPreviousLaneletsWithinRoute(lane, &prev_lanes)) {
-          return lanelet::ConstLanelets{};
-        }
-
-        lanelet::ConstLanelets next_lanes;
-        for (const auto & prev_lane : prev_lanes) {
-          const auto next_lanes_from_prev = route_handler->getNextLanelets(prev_lane);
-          pushUniqueVector(next_lanes, next_lanes_from_prev);
-        }
-        return next_lanes;
-      };
-
-    const auto next_lanes_for_right =
-      get_next_lanes_from_same_previous_lane(current_drivable_lanes.right_lane);
-    const auto next_lanes_for_left =
-      get_next_lanes_from_same_previous_lane(current_drivable_lanes.left_lane);
-
-    // 2.2 look for neighbor lane recursively, where end line of the lane is connected to end line
-    // of the original lane
-    const auto update_drivable_lanes =
-      [&](const lanelet::ConstLanelets & next_lanes, const bool is_left) {
-        for (const auto & next_lane : next_lanes) {
-          const auto & edge_lane =
-            is_left ? current_drivable_lanes.left_lane : current_drivable_lanes.right_lane;
-          if (next_lane.id() == edge_lane.id()) {
-            continue;
-          }
-
-          const auto & left_lane = is_left ? next_lane : edge_lane;
-          const auto & right_lane = is_left ? edge_lane : next_lane;
-          if (!isEndPointsConnected(left_lane, right_lane)) {
-            continue;
-          }
-
-          if (is_left) {
-            current_drivable_lanes.left_lane = next_lane;
-          } else {
-            current_drivable_lanes.right_lane = next_lane;
-          }
-
-          if (!has_same_lane(current_drivable_lanes.middle_lanes, edge_lane)) {
-            if (is_left) {
-              if (current_drivable_lanes.right_lane.id() != edge_lane.id()) {
-                current_drivable_lanes.middle_lanes.push_back(edge_lane);
-              }
-            } else {
-              if (current_drivable_lanes.left_lane.id() != edge_lane.id()) {
-                current_drivable_lanes.middle_lanes.push_back(edge_lane);
-              }
-            }
-          }
-
-          return true;
-        }
-        return false;
-      };
-
-    const auto expand_drivable_area_recursively =
-      [&](const lanelet::ConstLanelets & next_lanes, const bool is_left) {
-        // NOTE: set max search num to avoid infinity loop for drivable area expansion
-        constexpr size_t max_recursive_search_num = 3;
-        for (size_t i = 0; i < max_recursive_search_num; ++i) {
-          const bool is_update_kept = update_drivable_lanes(next_lanes, is_left);
-          if (!is_update_kept) {
-            break;
-          }
-          if (i == max_recursive_search_num - 1) {
-            RCLCPP_ERROR(
-              rclcpp::get_logger("behavior_path_planner").get_child("avoidance"),
-              "Drivable area expansion reaches max iteration.");
-          }
-        }
-      };
-    expand_drivable_area_recursively(next_lanes_for_right, false);
-    expand_drivable_area_recursively(next_lanes_for_left, true);
-
-    // 3. update again for new left/right lanes
-    update_left_lanelets(current_drivable_lanes.left_lane);
-    update_right_lanelets(current_drivable_lanes.right_lane);
-
-    // 4. compensate that current_lane is in either of left_lane, right_lane or middle_lanes.
-    if (
-      current_drivable_lanes.left_lane.id() != current_lane.id() &&
-      current_drivable_lanes.right_lane.id() != current_lane.id()) {
-      current_drivable_lanes.middle_lanes.push_back(current_lane);
-    }
-
-    drivable_lanes.push_back(current_drivable_lanes);
+  for (const auto & lanelet : avoid_data_.current_lanelets) {
+    drivable_lanes.push_back(
+      utils::avoidance::generateExpandDrivableLanes(lanelet, planner_data_, parameters_));
   }
 
   {  // for new architecture
@@ -2215,7 +2100,7 @@ BehaviorModuleOutput AvoidanceModule::plan()
   utils::clipPathLength(*output.path, ego_idx, planner_data_->parameters);
 
   // Drivable area generation.
-  generateExtendedDrivableArea(output);
+  generateExpandDrivableLanes(output);
   setDrivableLanes(output.drivable_area_info.drivable_lanes);
 
   return output;
@@ -2494,6 +2379,32 @@ 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,6 +186,8 @@ 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)