Fix logic

at-wat · Jul 6, 2023 · c189a14 · c189a14
1 parent 0f7c602
commit c189a14
Showing 1 changed file with 23 additions and 14 deletions.
diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp
@@ -1417,6 +1417,7 @@ class Planner3dNode
         }
         else
         {
+          bool skip_path_planning = false;
           if (escaping_)
           {
             status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND;
@@ -1441,9 +1442,30 @@ class Planner3dNode
 
             continue;
           }
-          else if (cost_estim_cache_created_)
+          else if (!cost_estim_cache_created_)
+          {
+            skip_path_planning = true;
+            if (is_start_occupied_)
+            {
+              status_.error = planner_cspace_msgs::PlannerStatus::IN_ROCK;
+            }
+            else
+            {
+              status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND;
+            }
+          }
+          else
           {
             status_.error = planner_cspace_msgs::PlannerStatus::GOING_WELL;
+          }
+
+          if (skip_path_planning)
+          {
+            publishEmptyPath();
+            previous_path.poses.clear();
+          }
+          else
+          {
             nav_msgs::Path path;
             path.header = map_header_;
             path.header.stamp = now;
@@ -1467,19 +1489,6 @@ class Planner3dNode
                 sw_pos_ = path.poses[sw_index];
             }
           }
-          else
-          {
-            if (is_start_occupied_)
-            {
-              status_.error = planner_cspace_msgs::PlannerStatus::IN_ROCK;
-            }
-            else
-            {
-              status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND;
-            }
-            publishEmptyPath();
-            previous_path.poses.clear();
-          }
         }
       }
       else if (!has_goal_)