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_by_lane_change_module/src/scene.cpp

@@ -155,8 +155,8 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData(
   // 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_, avoidance_parameters_, false));
+      data.drivable_lanes.push_back(utils::avoidance::generateExpandedDrivableLanes(
+        lanelet, planner_data_, avoidance_parameters_));
     });
 
   // calc drivable bound

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -552,6 +552,8 @@ struct AvoidancePlanningData
 
   bool found_avoidance_path{false};
 
+  bool limit_drivable_lane{false};
+
   double to_stop_line{std::numeric_limits<double>::max()};
 
   double to_start_point{std::numeric_limits<double>::lowest()};

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);
@@ -157,9 +160,11 @@ std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
   const std::shared_ptr<AvoidanceParameters> & parameters,
   const double object_check_forward_distance, DebugData & debug);
 
-DrivableLanes generateExpandDrivableLanes(
+DrivableLanes generateNotExpandedDrivableLanes(const lanelet::ConstLanelet & lanelet);
+
+DrivableLanes generateExpandedDrivableLanes(
   const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data,
-  const std::shared_ptr<AvoidanceParameters> & parameters, const bool in_avoidance_maneuver);
+  const std::shared_ptr<AvoidanceParameters> & parameters);
 
 double calcDistanceToReturnDeadLine(
   const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -21,6 +21,7 @@
 #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
 #include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp"
 #include "behavior_path_planner_common/utils/path_utils.hpp"
+#include "behavior_path_planner_common/utils/traffic_light_utils.hpp"
 #include "behavior_path_planner_common/utils/utils.hpp"
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
@@ -32,8 +33,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,25 +222,36 @@ 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));
+      const auto next_lanes = planner_data_->route_handler->getNextLanelets(lanelet);
+      const auto is_stop_signal =
+        utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data_);
+      if (is_stop_signal && is_within_current_lane) {
+        data.limit_drivable_lane = true;
+        data.drivable_lanes.push_back(utils::avoidance::generateNotExpandedDrivableLanes(lanelet));
+      } else {
+        data.drivable_lanes.push_back(
+          utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_));
+      }
     });
 
   // calc drivable bound
   auto tmp_path = getPreviousModuleOutput().path;
+  const auto use_hatched_road_markings =
+    parameters_->use_hatched_road_markings && !data.limit_drivable_lane;
+  const auto use_intersection_areas =
+    parameters_->use_intersection_areas && !data.limit_drivable_lane;
+  const auto use_freespace_areas = parameters_->use_freespace_areas && !data.limit_drivable_lane;
   const auto shorten_lanes = utils::cutOverlappedLanes(tmp_path, data.drivable_lanes);
   data.left_bound = utils::calcBound(
-    getPreviousModuleOutput().path, planner_data_, shorten_lanes,
-    parameters_->use_hatched_road_markings, parameters_->use_intersection_areas,
-    parameters_->use_freespace_areas, true);
+    getPreviousModuleOutput().path, planner_data_, shorten_lanes, use_hatched_road_markings,
+    use_intersection_areas, use_freespace_areas, true);
   data.right_bound = utils::calcBound(
-    getPreviousModuleOutput().path, planner_data_, shorten_lanes,
-    parameters_->use_hatched_road_markings, parameters_->use_intersection_areas,
-    parameters_->use_freespace_areas, false);
+    getPreviousModuleOutput().path, planner_data_, shorten_lanes, use_hatched_road_markings,
+    use_intersection_areas, use_freespace_areas, false);
 
   // reference path
   if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) {
@@ -942,12 +952,13 @@ BehaviorModuleOutput AvoidanceModule::plan()
     current_drivable_area_info.drivable_lanes = avoid_data_.drivable_lanes;
     // expand hatched road markings
     current_drivable_area_info.enable_expanding_hatched_road_markings =
-      parameters_->use_hatched_road_markings;
+      parameters_->use_hatched_road_markings && !data.limit_drivable_lane;
     // expand intersection areas
     current_drivable_area_info.enable_expanding_intersection_areas =
-      parameters_->use_intersection_areas;
+      parameters_->use_intersection_areas && !data.limit_drivable_lane;
     // expand freespace areas
-    current_drivable_area_info.enable_expanding_freespace_areas = parameters_->use_freespace_areas;
+    current_drivable_area_info.enable_expanding_freespace_areas =
+      parameters_->use_freespace_areas && !data.limit_drivable_lane;
     // generate obstacle polygons
     if (parameters_->enable_bound_clipping) {
       ObjectDataArray clip_objects;

planning/behavior_path_avoidance_module/src/utils.cpp

@@ -957,6 +957,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)
 {
   /**
@@ -2076,9 +2114,18 @@ std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
   return std::make_pair(target_objects, other_objects);
 }
 
-DrivableLanes generateExpandDrivableLanes(
+DrivableLanes generateNotExpandedDrivableLanes(const lanelet::ConstLanelet & lanelet)
+{
+  DrivableLanes current_drivable_lanes;
+  current_drivable_lanes.left_lane = lanelet;
+  current_drivable_lanes.right_lane = lanelet;
+
+  return current_drivable_lanes;
+}
+
+DrivableLanes generateExpandedDrivableLanes(
   const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data,
-  const std::shared_ptr<AvoidanceParameters> & parameters, const bool in_avoidance_maneuver)
+  const std::shared_ptr<AvoidanceParameters> & parameters)
 {
   const auto & route_handler = planner_data->route_handler;
 
@@ -2092,11 +2139,6 @@ DrivableLanes generateExpandDrivableLanes(
 
   // 1. get left/right side lanes
   const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) {
-    const auto next_lanes = route_handler->getNextLanelets(target_lane);
-    const auto is_stop_signal = utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data);
-    if (is_stop_signal && !in_avoidance_maneuver) {
-      return;
-    }
     const auto all_left_lanelets = route_handler->getAllLeftSharedLinestringLanelets(
       target_lane, parameters->use_opposite_lane, true);
     if (!all_left_lanelets.empty()) {
@@ -2107,11 +2149,6 @@ DrivableLanes generateExpandDrivableLanes(
     }
   };
   const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) {
-    const auto next_lanes = route_handler->getNextLanelets(target_lane);
-    const auto is_stop_signal = utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data);
-    if (is_stop_signal && !in_avoidance_maneuver) {
-      return;
-    }
     const auto all_right_lanelets = route_handler->getAllRightSharedLinestringLanelets(
       target_lane, parameters->use_opposite_lane, true);
     if (!all_right_lanelets.empty()) {