diff --git a/src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp index 41d6db8..42300ea 100644 --- a/src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp @@ -37,5 +37,6 @@ namespace Modelec rclcpp::Node::SharedPtr node_; rclcpp::Publisher::SharedPtr score_pub_; int mission_score_ = 0; + Point spawn_; }; } diff --git a/src/modelec_strat/src/missions/banner_mission.cpp b/src/modelec_strat/src/missions/banner_mission.cpp index bcfc83c..aea943f 100644 --- a/src/modelec_strat/src/missions/banner_mission.cpp +++ b/src/modelec_strat/src/missions/banner_mission.cpp @@ -18,6 +18,8 @@ namespace Modelec score_pub_ = node_->create_publisher("/strat/score", 10); + spawn_ = nav_->GetSpawn(); + action_executor_->DeployBanner(); status_ = MissionStatus::RUNNING; @@ -34,23 +36,21 @@ namespace Modelec switch (step_) { case DEPLOY_BANNER: + { + nav_->GoTo(spawn_.x, (nav_->GetPathfinding()->robot_length_mm_ / 2) + 5, M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); - auto spawn = nav_->GetSpawn(); + step_ = DEPLOY_BANNER; + } - nav_->GoTo(spawn.x, (nav_->GetPathfinding()->robot_length_mm_ / 2) + 5, M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); - - step_ = DEPLOY_BANNER; break; case GO_TO_FRONT: { - auto spawn = nav_->GetSpawn(); - - nav_->GoTo(spawn.x, (nav_->GetPathfinding()->robot_length_mm_ / 2) + 150, M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); + nav_->GoTo(spawn_.x, (nav_->GetPathfinding()->robot_length_mm_ / 2) + 150, M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); step_ = GO_FORWARD; - break; } + break; case GO_FORWARD: { std_msgs::msg::Int64 msg;