fix(avoidance): limit drivable lane only when the ego in on original lane

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

Author: satoshi-ota

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp

@@ -36,6 +36,9 @@ bool isOnRight(const ObjectData & obj);
 double calcShiftLength(
   const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin);
 
+bool isWithinLanes(
+  const lanelet::ConstLanelets & lanelets, std::shared_ptr<const PlannerData> & planner_data);
+
 bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length);
 
 bool isSameDirectionShift(const bool & is_object_on_right, const double & shift_length);

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -32,8 +32,6 @@
 #include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp"
 #include <tier4_planning_msgs/msg/detail/avoidance_debug_factor__struct.hpp>
 
-#include <boost/geometry/algorithms/centroid.hpp>
-
 #include <algorithm>
 #include <limits>
 #include <memory>
@@ -223,12 +221,12 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
     utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_);
 
   // expand drivable lanes
-  const auto has_shift_point = !path_shifter_.getShiftLines().empty();
-  const auto in_avoidance_maneuver = has_shift_point || helper_->isShifted();
+  const auto is_within_current_lane =
+    utils::avoidance::isWithinLanes(data.current_lanelets, planner_data_);
   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_, in_avoidance_maneuver));
+        lanelet, planner_data_, parameters_, !is_within_current_lane));
     });
 
   // calc drivable bound

planning/behavior_path_avoidance_module/src/utils.cpp

@@ -943,6 +943,44 @@ double calcShiftLength(
   return std::fabs(shift_length) > 1e-3 ? shift_length : 0.0;
 }
 
+bool isWithinLanes(
+  const lanelet::ConstLanelets & lanelets, std::shared_ptr<const PlannerData> & planner_data)
+{
+  const auto & rh = planner_data->route_handler;
+  const auto & ego_pose = planner_data->self_odometry->pose.pose;
+  const auto transform = tier4_autoware_utils::pose2transform(ego_pose);
+  const auto footprint = tier4_autoware_utils::transformVector(
+    planner_data->parameters.vehicle_info.createFootprint(), transform);
+
+  lanelet::ConstLanelet closest_lanelet{};
+  if (!lanelet::utils::query::getClosestLanelet(lanelets, ego_pose, &closest_lanelet)) {
+    return true;
+  }
+
+  lanelet::ConstLanelets concat_lanelets{};
+
+  // push previous lanelet
+  lanelet::ConstLanelets prev_lanelet;
+  if (rh->getPreviousLaneletsWithinRoute(closest_lanelet, &prev_lanelet)) {
+    concat_lanelets.push_back(prev_lanelet.front());
+  }
+
+  // push nearest lanelet
+  {
+    concat_lanelets.push_back(closest_lanelet);
+  }
+
+  // push next lanelet
+  lanelet::ConstLanelet next_lanelet;
+  if (rh->getNextLaneletWithinRoute(closest_lanelet, &next_lanelet)) {
+    concat_lanelets.push_back(next_lanelet);
+  }
+
+  const auto combine_lanelet = lanelet::utils::combineLaneletsShape(concat_lanelets);
+
+  return boost::geometry::within(footprint, combine_lanelet.polygon2d().basicPolygon());
+}
+
 bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length)
 {
   /**