fix(lane_change): remove overlapping preceding lanes (autowarefoundat…

…ion#9526) * fix(lane_change): remove overlapping preceding lanes Signed-off-by: Zulfaqar Azmi <[email protected]> * fix cpp check Signed-off-by: Zulfaqar Azmi <[email protected]> * start searching disconnected lanes directly Signed-off-by: Zulfaqar Azmi <[email protected]> * just remove starting from overlapped found Signed-off-by: Zulfaqar Azmi <[email protected]> * return non reversed lanes Signed-off-by: Zulfaqar Azmi <[email protected]> * fix precommit Signed-off-by: Zulfaqar Azmi <[email protected]> --------- Signed-off-by: Zulfaqar Azmi <[email protected]>
kyoichi-sugahara · Dec 13, 2024 · 9c255f7 · 9c255f7
1 parent 6537905
commit 9c255f7
Show file tree

Hide file tree

Showing 3 changed files with 60 additions and 4 deletions.
diff --git a/...path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/...path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp
@@ -383,6 +383,22 @@ bool filter_target_lane_objects(
   const bool before_terminal, TargetLaneLeadingObjects & leading_objects,
   ExtendedPredictedObjects & trailing_objects);
 
+/**
+ * @brief Retrieves the preceding lanes for the target lanes while removing overlapping and
+ * disconnected lanes.
+ *
+ * This function identifies all lanes that precede the target lanes based on the ego vehicle's
+ * current position and a specified backward search length. The resulting preceding lanes are
+ * filtered to remove lanes that overlap with the current lanes or are not connected to the route.
+ *
+ * @param common_data_ptr Shared pointer to commonly used data in lane change module, which contains
+ * route handler information, lane details, ego vehicle pose, and behavior parameters.
+ *
+ * @return A vector of preceding lanelet groups, with each group containing only the connected and
+ * non-overlapping preceding lanes.
+ */
+std::vector<lanelet::ConstLanelets> get_preceding_lanes(const CommonDataPtr & common_data_ptr);
+
 /**
  * @brief Determines if the object's predicted path overlaps with the given lane polygon.
  *

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
@@ -112,9 +112,8 @@ void NormalLaneChange::update_lanes(const bool is_approved)
   common_data_ptr_->lanes_ptr->target_lane_in_goal_section =
     route_handler_ptr->isInGoalRouteSection(target_lanes.back());
 
-  common_data_ptr_->lanes_ptr->preceding_target = utils::getPrecedingLanelets(
-    *route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(),
-    common_data_ptr_->lc_param_ptr->backward_lane_length);
+  common_data_ptr_->lanes_ptr->preceding_target =
+    utils::lane_change::get_preceding_lanes(common_data_ptr_);
 
   lane_change_debug_.current_lanes = common_data_ptr_->lanes_ptr->current;
   lane_change_debug_.target_lanes = common_data_ptr_->lanes_ptr->target;

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp
@@ -34,9 +34,12 @@
 #include <autoware_lanelet2_extension/utility/query.hpp>
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info.hpp>
+#include <range/v3/action/insert.hpp>
 #include <range/v3/algorithm/any_of.hpp>
+#include <range/v3/algorithm/find_if.hpp>
+#include <range/v3/algorithm/for_each.hpp>
 #include <range/v3/range/conversion.hpp>
-#include <range/v3/view/transform.hpp>
+#include <range/v3/view.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <geometry_msgs/msg/detail/pose__struct.hpp>
@@ -56,6 +59,7 @@
 #include <memory>
 #include <optional>
 #include <string>
+#include <unordered_set>
 #include <vector>
 
 namespace autoware::behavior_path_planner::utils::lane_change
@@ -1246,6 +1250,43 @@ bool filter_target_lane_objects(
   return false;
 }
 
+std::vector<lanelet::ConstLanelets> get_preceding_lanes(const CommonDataPtr & common_data_ptr)
+{
+  const auto & route_handler_ptr = common_data_ptr->route_handler_ptr;
+  const auto & target_lanes = common_data_ptr->lanes_ptr->target;
+  const auto & ego_pose = common_data_ptr->get_ego_pose();
+  const auto backward_lane_length = common_data_ptr->lc_param_ptr->backward_lane_length;
+
+  const auto preceding_lanes_list =
+    utils::getPrecedingLanelets(*route_handler_ptr, target_lanes, ego_pose, backward_lane_length);
+
+  const auto & current_lanes = common_data_ptr->lanes_ptr->current;
+  std::unordered_set<lanelet::Id> current_lanes_id;
+  for (const auto & lane : current_lanes) {
+    current_lanes_id.insert(lane.id());
+  }
+  const auto is_overlapping = [&](const lanelet::ConstLanelet & lane) {
+    return current_lanes_id.find(lane.id()) != current_lanes_id.end();
+  };
+
+  std::vector<lanelet::ConstLanelets> non_overlapping_lanes_vec;
+  for (const auto & lanes : preceding_lanes_list) {
+    auto lanes_reversed = lanes | ranges::views::reverse;
+    auto overlapped_itr = ranges::find_if(lanes_reversed, is_overlapping);
+
+    if (overlapped_itr == lanes_reversed.begin()) {
+      continue;
+    }
+
+    // Lanes are not reversed by default. Avoid returning reversed lanes to prevent undefined
+    // behavior.
+    lanelet::ConstLanelets non_overlapping_lanes(overlapped_itr.base(), lanes.end());
+    non_overlapping_lanes_vec.push_back(non_overlapping_lanes);
+  }
+
+  return non_overlapping_lanes_vec;
+}
+
 bool object_path_overlaps_lanes(
   const ExtendedPredictedObject & object, const lanelet::BasicPolygon2d & lanes_polygon)
 {