diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp index c383418c8eac8..1ca450dfb1a08 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp @@ -357,8 +357,13 @@ bool DetectionAreaModule::isOverLine( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const { - return tier4_autoware_utils::calcSignedArcLength( - path.points, self_pose.position, line_pose.position) < 0; + const PointWithSearchRangeIndex src_point_with_search_range_index = + planning_utils::findFirstNearSearchRangeIndex(path.points, self_pose.position); + const PointWithSearchRangeIndex dst_point_with_search_range_index = { + line_pose.position, planning_utils::getPathIndexRangeIncludeLaneId(path, lane_id_)}; + + return planning_utils::calcSignedArcLengthWithSearchIndex( + path.points, src_point_with_search_range_index, dst_point_with_search_range_index) < 0; } bool DetectionAreaModule::hasEnoughBrakingDistance(