feat(lane_change): expand target lanes for object filtering (#5167)

* feat(lane_change): expand target lanes for object filtering

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* use expanded target lanes in collision check

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

---------

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

planning/behavior_path_planner/config/lane_change/lane_change.param.yaml

@@ -38,6 +38,11 @@
         longitudinal_distance_min_threshold: 3.0
         longitudinal_velocity_delta_time: 0.8
 
+        # lane expansion for object filtering
+        lane_expansion:
+          left_offset: 0.0 # [m]
+          right_offset: 0.0 # [m]
+
       # lateral acceleration map
       lateral_acceleration:
         velocity: [0.0, 4.0, 10.0]

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp

@@ -67,6 +67,8 @@ struct LaneChangeParameters
   bool check_objects_on_current_lanes{true};
   bool check_objects_on_other_lanes{true};
   bool use_all_predicted_path{false};
+  double lane_expansion_left_offset{0.0};
+  double lane_expansion_right_offset{0.0};
 
   // regulatory elements
   bool regulate_on_crosswalk{false};

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp

@@ -186,5 +186,24 @@ ExtendedPredictedObject transform(
 
 bool isCollidedPolygonsInLanelet(
   const std::vector<Polygon2d> & collided_polygons, const lanelet::ConstLanelets & lanes);
+
+/**
+ * @brief Generates expanded lanelets based on the given direction and offsets.
+ *
+ * Expands the provided lanelets in either the left or right direction based on
+ * the specified direction. If the direction is 'LEFT', the lanelets are expanded
+ * using the left_offset; if 'RIGHT', they are expanded using the right_offset.
+ * Otherwise, no expansion occurs.
+ *
+ * @param lanes The lanelets to be expanded.
+ * @param direction The direction of expansion: either LEFT or RIGHT.
+ * @param left_offset The offset value for left expansion.
+ * @param right_offset The offset value for right expansion.
+ * @return lanelet::ConstLanelets A collection of expanded lanelets.
+ */
+lanelet::ConstLanelets generateExpandedLanelets(
+  const lanelet::ConstLanelets & lanes, const Direction direction, const double left_offset,
+  const double right_offset);
+
 }  // namespace behavior_path_planner::utils::lane_change
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_

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

@@ -77,7 +77,10 @@ LaneChangeModuleManager::LaneChangeModuleManager(
     getOrDeclareParameter<bool>(*node, parameter("check_objects_on_other_lanes"));
   p.use_all_predicted_path =
     getOrDeclareParameter<bool>(*node, parameter("use_all_predicted_path"));
-
+  p.lane_expansion_left_offset =
+    getOrDeclareParameter<double>(*node, parameter("safety_check.lane_expansion.left_offset"));
+  p.lane_expansion_right_offset =
+    getOrDeclareParameter<double>(*node, parameter("safety_check.lane_expansion.right_offset"));
   // lane change regulations
   p.regulate_on_crosswalk = getOrDeclareParameter<bool>(*node, parameter("regulation.crosswalk"));
   p.regulate_on_intersection =

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -674,8 +674,11 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
 
   const auto current_polygon =
     utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits<double>::max());
-  const auto target_polygon =
-    utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits<double>::max());
+  const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets(
+    target_lanes, direction_, lane_change_parameters_->lane_expansion_left_offset,
+    lane_change_parameters_->lane_expansion_right_offset);
+  const auto target_polygon = utils::lane_change::createPolygon(
+    expanded_target_lanes, 0.0, std::numeric_limits<double>::max());
   const auto dist_ego_to_current_lanes_center =
     lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose);
   std::vector<std::optional<lanelet::BasicPolygon2d>> target_backward_polygons;
@@ -1449,6 +1452,11 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
       target_objects.other_lane.end());
   }
 
+  const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets(
+    lane_change_path.info.target_lanes, direction_,
+    lane_change_parameters_->lane_expansion_left_offset,
+    lane_change_parameters_->lane_expansion_right_offset);
+
   for (const auto & obj : collision_check_objects) {
     auto current_debug_data = marker_utils::createObjectDebug(obj);
     const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
@@ -1464,8 +1472,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
 
       const auto collision_in_current_lanes = utils::lane_change::isCollidedPolygonsInLanelet(
         collided_polygons, lane_change_path.info.current_lanes);
-      const auto collision_in_target_lanes = utils::lane_change::isCollidedPolygonsInLanelet(
-        collided_polygons, lane_change_path.info.target_lanes);
+      const auto collision_in_target_lanes =
+        utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_lanes);
 
       if (!collision_in_current_lanes && !collision_in_target_lanes) {
         continue;

planning/behavior_path_planner/src/utils/lane_change/utils.cpp

@@ -1104,4 +1104,13 @@ bool isCollidedPolygonsInLanelet(
 
   return std::any_of(collided_polygons.begin(), collided_polygons.end(), is_in_lanes);
 }
+
+lanelet::ConstLanelets generateExpandedLanelets(
+  const lanelet::ConstLanelets & lanes, const Direction direction, const double left_offset,
+  const double right_offset)
+{
+  const auto left_extend_offset = (direction == Direction::LEFT) ? left_offset : 0.0;
+  const auto right_extend_offset = (direction == Direction::RIGHT) ? -right_offset : 0.0;
+  return lanelet::utils::getExpandedLanelets(lanes, left_extend_offset, right_extend_offset);
+}
 }  // namespace behavior_path_planner::utils::lane_change