Fix logic

at-wat · Jul 5, 2023 · ccf7857 · ccf7857
1 parent 964200f
commit ccf7857
Showing 1 changed file with 8 additions and 5 deletions.
diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp
@@ -551,7 +551,6 @@ class Planner3dNode
       default:
         break;
     }
-    cnt_stuck_ = 0;
     const auto ts = boost::chrono::high_resolution_clock::now();
     cost_estim_cache_.create(s, e);
     const auto tnow = boost::chrono::high_resolution_clock::now();
@@ -1437,10 +1436,6 @@ class Planner3dNode
 
             continue;
           }
-          else if (cnt_stuck_ > 0)
-          {
-            status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND;
-          }
           else
           {
             status_.error = planner_cspace_msgs::PlannerStatus::GOING_WELL;
@@ -1529,6 +1524,14 @@ class Planner3dNode
     if (!cm_.validate(s, range_))
     {
       ROS_ERROR("You are on the edge of the world.");
+      status_.error = planner_cspace_msgs::PlannerStatus::IN_ROCK;
+      return false;
+    }
+
+    if (cm_[e] == 100)
+    {
+      ROS_DEBUG("Planning failed as the goal is occupied.");
+      status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND;
       return false;
     }