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 2528088..a61d684 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 @@ -37,5 +37,6 @@ namespace Modelec Point home_point_; rclcpp::Publisher::SharedPtr score_pub_; int mission_score_ = 0; + rclcpp::Time go_timeout_; }; } diff --git a/src/modelec_strat/src/missions/go_home_mission.cpp b/src/modelec_strat/src/missions/go_home_mission.cpp index ea52d70..899d106 100644 --- a/src/modelec_strat/src/missions/go_home_mission.cpp +++ b/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;