fix(detection_area): search collision index only in lanelet (#695)

* fix(detection_area): search collision index only in lanelet

* ci(pre-commit): autofix

---------

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp

@@ -62,7 +62,8 @@ class DetectionAreaModule : public SceneModuleInterface
 
 public:
   DetectionAreaModule(
-    const int64_t module_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem,
+    const int64_t module_id, const size_t lane_id,
+    const lanelet::autoware::DetectionArea & detection_area_reg_elem,
     const PlannerParam & planner_param, const rclcpp::Logger logger,
     const rclcpp::Clock::SharedPtr clock);
 
@@ -106,6 +107,8 @@ class DetectionAreaModule : public SceneModuleInterface
 
   // Debug
   DebugData debug_data_;
+
+  int64_t lane_id_;
 };
 }  // namespace behavior_velocity_planner
 

planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp

@@ -28,22 +28,24 @@ namespace behavior_velocity_planner
 {
 namespace
 {
-std::vector<lanelet::DetectionAreaConstPtr> getDetectionAreaRegElemsOnPath(
+using DetectionAreaWithLaneId = std::pair<lanelet::DetectionAreaConstPtr, int64_t>;
+
+std::vector<DetectionAreaWithLaneId> getDetectionAreaRegElemsOnPath(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const lanelet::LaneletMapPtr lanelet_map)
 {
-  std::vector<lanelet::DetectionAreaConstPtr> detection_area_reg_elems;
+  std::vector<DetectionAreaWithLaneId> detection_areas_with_lane_id;
 
   for (const auto & p : path.points) {
     const auto lane_id = p.lane_ids.at(0);
     const auto ll = lanelet_map->laneletLayer.get(lane_id);
     const auto detection_areas = ll.regulatoryElementsAs<const lanelet::autoware::DetectionArea>();
     for (const auto & detection_area : detection_areas) {
-      detection_area_reg_elems.push_back(detection_area);
+      detection_areas_with_lane_id.push_back(std::make_pair(detection_area, lane_id));
     }
   }
 
-  return detection_area_reg_elems;
+  return detection_areas_with_lane_id;
 }
 
 std::set<int64_t> getDetectionAreaIdSetOnPath(
@@ -52,7 +54,7 @@ std::set<int64_t> getDetectionAreaIdSetOnPath(
 {
   std::set<int64_t> detection_area_id_set;
   for (const auto & detection_area : getDetectionAreaRegElemsOnPath(path, lanelet_map)) {
-    detection_area_id_set.insert(detection_area->id());
+    detection_area_id_set.insert(detection_area.first->id());
   }
   return detection_area_id_set;
 }
@@ -72,14 +74,15 @@ DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node)
 void DetectionAreaModuleManager::launchNewModules(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
-  for (const auto & detection_area :
+  for (const auto & detection_area_with_lane_id :
        getDetectionAreaRegElemsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) {
     // Use lanelet_id to unregister module when the route is changed
-    const auto module_id = detection_area->id();
+    const auto module_id = detection_area_with_lane_id.first->id();
+    const auto lane_id = detection_area_with_lane_id.second;
     if (!isModuleRegistered(module_id)) {
       registerModule(std::make_shared<DetectionAreaModule>(
-        module_id, *detection_area, planner_param_, logger_.get_child("detection_area_module"),
-        clock_));
+        module_id, lane_id, *(detection_area_with_lane_id.first), planner_param_,
+        logger_.get_child("detection_area_module"), clock_));
     }
   }
 }

planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp

@@ -69,9 +69,13 @@ boost::optional<Point2d> getNearestCollisionPoint(
 }
 
 boost::optional<PathIndexWithPoint2d> findCollisionSegment(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line)
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line,
+  const SearchRangeIndex & search_index)
 {
-  for (size_t i = 0; i < path.points.size() - 1; ++i) {
+  const auto min_search_index = std::max(static_cast<size_t>(0), search_index.min_idx);
+  const auto max_search_index = std::min(search_index.max_idx, path.points.size() - 1);
+
+  for (size_t i = min_search_index; i < max_search_index; ++i) {
     const auto & p1 = path.points.at(i).point.pose.position;      // Point before collision point
     const auto & p2 = path.points.at(i + 1).point.pose.position;  // Point after collision point
 
@@ -186,13 +190,15 @@ geometry_msgs::msg::Pose calcTargetPose(
 }  // namespace
 
 DetectionAreaModule::DetectionAreaModule(
-  const int64_t module_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem,
+  const int64_t module_id, const size_t lane_id,
+  const lanelet::autoware::DetectionArea & detection_area_reg_elem,
   const PlannerParam & planner_param, const rclcpp::Logger logger,
   const rclcpp::Clock::SharedPtr clock)
 : SceneModuleInterface(module_id, logger, clock),
   detection_area_reg_elem_(detection_area_reg_elem),
   state_(State::GO),
-  planner_param_(planner_param)
+  planner_param_(planner_param),
+  lane_id_(lane_id)
 {
 }
 
@@ -392,8 +398,10 @@ boost::optional<PathIndexWithPose> DetectionAreaModule::createTargetPoint(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line,
   const double margin) const
 {
+  const auto dst_search_range = planning_utils::getPathIndexRangeIncludeLaneId(path, lane_id_);
+
   // Find collision segment
-  const auto collision_segment = findCollisionSegment(path, stop_line);
+  const auto collision_segment = findCollisionSegment(path, stop_line, dst_search_range);
   if (!collision_segment) {
     // No collision
     return {};