feat(intersection): continue detection after pass judge (autowarefoun…

…dation#1719)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

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