planner_cspace: fix condition of open queue (#576)

at-wat · Jan 28, 2021 · eae9699 · eae9699
1 parent 26d54c7
commit eae9699
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp
@@ -1028,7 +1028,7 @@ class Planner3dNode
         for (p[1] = 0; p[1] < static_cast<int>(map_info_.height); p[1]++)
         {
           const auto gp = cost_estim_cache_[p];
-          if (gp > rough_cost_max_)
+          if ((gp > rough_cost_max_) && (gp != std::numeric_limits<float>::max()))
           {
             pq_open_.emplace(gp, gp, p);
           }