From 6e6277464514b33f18a8c280e16671d4b2c70003 Mon Sep 17 00:00:00 2001
From: acki <ackimixs@protonmail.com>
Date: Thu, 29 May 2025 16:48:32 -0400
Subject: [PATCH] test go home

---
 .../modelec_strat/missions/go_home_mission.hpp    |  2 ++
 .../src/missions/test.go_home_mission.cpp         | 15 ++++++++++++++-
 2 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp
index 3459d7c..3f9e202 100644
--- a/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp
+++ b/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp
@@ -21,12 +21,14 @@ namespace Modelec
         enum Step
         {
             GO_FRONT,
+            AWAIT_10S,
             GO_HOME,
             DONE
         } step_;
 
         MissionStatus status_;
         std::shared_ptr<NavigationHelper> nav_;
+        rclcpp::Time go_home_time_;
         rclcpp::Node::SharedPtr node_;
         rclcpp::Time start_time_;
         Point home_point_;
diff --git a/src/modelec_strat/src/missions/test.go_home_mission.cpp b/src/modelec_strat/src/missions/test.go_home_mission.cpp
index fef5827..d7a1731 100644
--- a/src/modelec_strat/src/missions/test.go_home_mission.cpp
+++ b/src/modelec_strat/src/missions/test.go_home_mission.cpp
@@ -21,6 +21,8 @@ namespace Modelec
 
         nav_->GoTo(t->x, 1200, -M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
 
+        go_home_time_ = node_->now();
+
         status_ = MissionStatus::RUNNING;
     }
 
@@ -34,13 +36,24 @@ namespace Modelec
         switch (step_)
         {
         case GO_FRONT:
+            {
+                if ((node_->now() - go_home_time_).seconds() >= 10)
+                {
+                    step_ = AWAIT_10S;
+                }
+            }
+
+            break;
+
+        case AWAIT_10S:
             {
                 auto t = nav_->GetCurrentPos();
 
                 nav_->GoTo(t->x, 1700, -M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
+
+                step_ = GO_HOME;
             }
 
-            step_ = GO_HOME;
             break;
         case GO_HOME:
             {