go home

src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp

@@ -37,5 +37,6 @@ namespace Modelec
         Point home_point_;
         rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr score_pub_;
         int mission_score_ = 0;
+        rclcpp::Time go_timeout_;
     };
 }

src/modelec_strat/src/missions/go_home_mission.cpp

@@ -29,6 +29,8 @@ namespace Modelec
         }
         nav_->RotateTo(home_point_);
 
+        go_timeout_ = node_->now();
+
         status_ = MissionStatus::RUNNING;
     }
 
@@ -36,7 +38,10 @@ namespace Modelec
     {
         if (!nav_->HasArrived())
         {
-            return;
+            if ((node_->now() - go_timeout_).seconds() < 10)
+            {
+                return;
+            }
         }
 
         switch (step_)
@@ -51,6 +56,8 @@ namespace Modelec
                         return;
                     }
                 }
+
+                go_timeout_ = node_->now();
             }
 
             step_ = GO_HOME;
@@ -61,6 +68,8 @@ namespace Modelec
                 break;
             }
 
+            go_timeout_ = node_->now();
+
             nav_->GoTo(home_point_.GetTakePosition(0), true);
 
             step_ = GO_CLOSE;