feat(avoidance): add feature to guard unfeasible path

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

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/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/src/scene_module/avoidance/avoidance_module.cpp

@@ -85,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(
@@ -190,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);
@@ -394,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;
@@ -431,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.
@@ -2342,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)