fix: simplify logic

autowarefoundation · May 18, 2022 · 30e73ed · 30e73ed
1 parent d8051ff
commit 30e73ed
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 31 deletions.
diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp
@@ -212,31 +212,10 @@ bool StopLineModule::modifyPathVelocity(
   SearchRangeIndex dst_search_range =
     planning_utils::getPathIndexRangeIncludeLaneId(*path, lane_id_);
 
-  std::set<int64_t> unique_lane_ids;
-  for (const auto & p : path->points) {
-    unique_lane_ids.insert(p.lane_ids.at(0));
-  }
-
-  const auto next_id_itr =
-    std::next(std::find(unique_lane_ids.begin(), unique_lane_ids.end(), lane_id_), 1);
-
-  const auto prev_id_itr =
-    std::next(std::find(unique_lane_ids.rbegin(), unique_lane_ids.rend(), lane_id_), 1);
 
   // extend following and previous search range to avoid no collision
-  if (next_itr != unique_lane_ids.end()) {
-    const auto next_lanelet_index_range =
-      planning_utils::getPathIndexRangeIncludeLaneId(*path, *next_id_itr);
-
-    dst_search_range.max_idx = next_lanelet_index_range.min_idx;
-  }
-
-  if (prev_itr != unique_lane_ids.rend()) {
-    const auto prev_lanelet_index_range =
-      planning_utils::getPathIndexRangeIncludeLaneId(*path, *prev_id_itr);
-
-    dst_search_range.min_idx = prev_lanelet_index_range.max_idx;
-  }
+  if (dst_search_range.max_idx < path->points.size() - 1) dst_search_range.max_idx--;
+  if (dst_search_range.min_idx > 0) dst_search_range.min_idx++;
 
   // Find collision
   const auto collision = findCollision(*path, stop_line, dst_search_range);

diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp
@@ -183,15 +183,14 @@ SearchRangeIndex getPathIndexRangeIncludeLaneId(
   bool found_first_idx = false;
   for (size_t i = 0; i < path.points.size(); i++) {
     const auto & p = path.points.at(i);
-    if (p.lane_ids.size() != 1)
-      continue;
-
-    if (lane_id == p.lane_ids.at(0)) {
-      if (!found_first_idx) {
-        search_range.min_idx = i;
-        found_first_idx = true;
+    for (const auto & id : p.lane_ids) {
+      if (id == lane_id) {
+        if (!found_first_idx) {
+          search_range.min_idx = i;
+          found_first_idx = true;
+        }
+        search_range.max_idx = i;
       }
-      search_range.max_idx = i;
     }
   }
   return search_range;