From e139933216db269bb0e114faf0e305e939ff05d0 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 31 Aug 2022 00:40:44 +0900 Subject: [PATCH] feat(intersection): continue detection after pass judge (#1719) Signed-off-by: Mamoru Sobue --- .../include/scene_module/intersection/util.hpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index 3ff7f4b57fbb1..24b5dd69db4eb 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -141,6 +141,21 @@ lanelet::ConstLanelet generateOffsetLanelet( geometry_msgs::msg::Pose toPose(const geometry_msgs::msg::Point & p); +/** + * @brief check if ego is over the target_idx. If the index is same, compare the exact pose + * @param path path + * @param closest_idx ego's closest index on the path + * @param current_pose ego's exact pose + * @return true if ego is over the target_idx + */ +bool isOverTargetIndex( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const geometry_msgs::msg::Pose & current_pose, const int target_idx); + +bool isBeforeTargetIndex( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const geometry_msgs::msg::Pose & current_pose, const int target_idx); + } // namespace util } // namespace behavior_velocity_planner