From ea62ea391780729f501c3283ac6a7aa49c7be365 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 13 Oct 2023 11:14:04 +0900 Subject: [PATCH] fix(detection_area): fix overline function (#930) * fix(detection_area): fix overline function * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/scene_module/detection_area/scene.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) 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(