feat(lane_change): expand target lanes for object filtering

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
zulfaqar-azmi-t4 · Sep 28, 2023 · 6f3a68a · 6f3a68a
1 parent c552c3d
commit 6f3a68a
Show file tree

Hide file tree

Showing 5 changed files with 39 additions and 3 deletions.
diff --git a/..._path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/..._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};

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/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_
diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/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 =

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/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;

diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/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