From 4154d912ce72fdcd0852e1746a836a1c2cc47621 Mon Sep 17 00:00:00 2001
From: Xuan <tanqazx@gmail.com>
Date: Thu, 2 Nov 2023 22:15:31 -0400
Subject: [PATCH] Add reachability / fail-fast check to planners.

---
 src/planners/backward.jl | 8 +++++++-
 src/planners/forward.jl  | 8 +++++++-
 2 files changed, 14 insertions(+), 2 deletions(-)

diff --git a/src/planners/backward.jl b/src/planners/backward.jl
index 6596ea4..511220d 100644
--- a/src/planners/backward.jl
+++ b/src/planners/backward.jl
@@ -9,6 +9,7 @@ export ProbBackwardPlanner, ProbBackwardAStarPlanner
         h_mult::Float32 = 1.0f0,
         max_nodes::Int = typemax(Int),
         max_time::Float64 = Inf,
+        fail_fast::Bool = false,
         save_search::Bool = false,
         save_search_order::Bool = save_search,
         verbose::Bool = false,
@@ -60,6 +61,8 @@ $(FIELDS)
     max_nodes::Int = typemax(Int)
     "Maximum time in seconds before planner times out."
     max_time::Float64 = Inf
+    "Flag to terminate search if the heuristic estimates an infinite cost."
+    fail_fast::Bool = false
     "Flag to save the search tree and frontier in the returned solution."
     save_search::Bool = false
     "Flag to save the node expansion order in the returned solution."
@@ -120,7 +123,7 @@ ProbBackwardAStarPlanner(heuristic::Heuristic; search_noise=1.0, kwargs...) =
 function Base.copy(p::BackwardPlanner)
     return BackwardPlanner(p.heuristic, p.search_noise,
                            p.g_mult, p.h_mult, p.max_nodes, p.max_time,
-                           p.save_search, p.save_search_order,
+                           p.fail_fast, p.save_search, p.save_search_order,
                            p.verbose, p.callback)
 end
 
@@ -173,6 +176,9 @@ function search!(sol::PathSearchSolution, planner::BackwardPlanner,
             sol.status = :max_nodes # Node budget reached
         elseif time() - start_time >= planner.max_time
             sol.status = :max_time # Time budget reached
+        elseif planner.fail_fast && priority[1] == Inf
+            sol.status = :failure # Search space exhausted
+            break
         end
         if sol.status == :in_progress
             # Dequeue current node
diff --git a/src/planners/forward.jl b/src/planners/forward.jl
index 72625ee..82a869d 100644
--- a/src/planners/forward.jl
+++ b/src/planners/forward.jl
@@ -10,6 +10,7 @@ export ProbForwardPlanner, ProbAStarPlanner
         h_mult::Float32 = 1.0f0,
         max_nodes::Int = typemax(Int),
         max_time::Float64 = Inf,
+        fail_fast::Bool = false,
         save_search::Bool = false,
         save_search_order::Bool = save_search,
         verbose::Bool = false,
@@ -54,6 +55,8 @@ $(FIELDS)
     max_nodes::Int = typemax(Int)
     "Maximum time in seconds before planner times out."
     max_time::Float64 = Inf
+    "Flag to terminate search if the heuristic estimates an infinite cost."
+    fail_fast::Bool = false
     "Flag to save the search tree and frontier in the returned solution."
     save_search::Bool = false
     "Flag to save the node expansion order in the returned solution."
@@ -154,7 +157,7 @@ ProbAStarPlanner(heuristic::Heuristic; search_noise=1.0, kwargs...) =
 function Base.copy(p::ForwardPlanner)
     return ForwardPlanner(p.heuristic, p.search_noise, 
                           p.g_mult, p.h_mult, p.max_nodes, p.max_time,
-                          p.save_search, p.save_search_order,
+                          p.fail_fast, p.save_search, p.save_search_order,
                           p.verbose, p.callback)
 end
 
@@ -204,6 +207,9 @@ function search!(sol::PathSearchSolution, planner::ForwardPlanner,
             sol.status = :max_nodes # Node budget reached
         elseif time() - start_time >= planner.max_time
             sol.status = :max_time # Time budget reached
+        elseif planner.fail_fast && priority[1] == Inf
+            sol.status = :failure # Search space exhausted
+            break
         end
         if sol.status == :in_progress
             # Dequeue current node