planner_cspace: fix goal unreachable condition (#595)

at-wat · Mar 18, 2021 · 0f64e30 · 0f64e30
1 parent 2272cf0
commit 0f64e30
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp
@@ -1748,7 +1748,9 @@ class Planner3dNode
     }
     const Astar::Vec s_rough(s[0], s[1], 0);
 
-    if (cost_estim_cache_[s_rough] == std::numeric_limits<float>::max())
+    // If goal gets occupied, cost_estim_cache_ is not updated to reduce
+    // computational cost for clearing huge map. In this case, cm_[e] is 100.
+    if (cost_estim_cache_[s_rough] == std::numeric_limits<float>::max() || cm_[e] >= 100)
     {
       status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND;
       ROS_WARN("Goal unreachable.");