⚡ improve bt exit

kas-lab · Jan 22, 2024 · 3d2757d · 3d2757d
1 parent c307f2b
commit 3d2757d
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 6 deletions.
diff --git a/src/suave_rosa.cpp b/src/suave_rosa.cpp
@@ -61,10 +61,10 @@ int main(int argc, char * argv[])
     if(node->time_limit_reached()){
       node->request_save_mission_results();
       tree.haltTree();
-      break;
+      finish = true;
+    } else{
+      finish = tree.rootNode()->executeTick() == BT::NodeStatus::SUCCESS;
     }
-
-    finish = tree.rootNode()->executeTick() == BT::NodeStatus::SUCCESS;
     rclcpp::spin_some(node);
     rate.sleep();
   }

diff --git a/src/suave_rosa/suave_mission.cpp b/src/suave_rosa/suave_mission.cpp
@@ -30,6 +30,7 @@ namespace suave_rosa
 
   bool SuaveMission::time_limit_reached(){
     if(_search_started){
+      std::cout << "Time limit reached!"<< std::endl;
       return (this->get_clock()->now() - _start_time) >= rclcpp::Duration(_time_limit, 0);
     }
     return false;

diff --git a/src/suave_rosa_extended.cpp b/src/suave_rosa_extended.cpp
@@ -61,10 +61,10 @@ int main(int argc, char * argv[])
     if(node->time_limit_reached()){
       node->request_save_mission_results();
       tree.haltTree();
-      break;
+      finish = true;
+    } else{
+      finish = tree.rootNode()->executeTick() == BT::NodeStatus::SUCCESS;
     }
-
-    finish = tree.rootNode()->executeTick() == BT::NodeStatus::SUCCESS;
     rclcpp::spin_some(node);
     rate.sleep();
   }