diff --git a/src/modelec_strat/src/missions/test.go_home_mission.cpp b/src/modelec_strat/src/missions/test.go_home_mission.cpp deleted file mode 100644 index 37291d1..0000000 --- a/src/modelec_strat/src/missions/test.go_home_mission.cpp +++ /dev/null @@ -1,83 +0,0 @@ -#include - -#include - -namespace Modelec -{ - GoHomeMission::GoHomeMission(const std::shared_ptr& nav, const rclcpp::Time& start_time) : - step_(GO_FRONT), status_(MissionStatus::READY), nav_(nav), start_time_(start_time) - { - } - - void GoHomeMission::Start(rclcpp::Node::SharedPtr node) - { - node_ = node; - - mission_score_ = Config::get("config.mission_score.go_home", 0); - - score_pub_ = node_->create_publisher("/strat/score", 10); - - auto curr = nav_->GetCurrentPos(); - RCLCPP_INFO(node_->get_logger(), "GoHomeMission: Starting at position (%f, %f)", curr->x, curr->y); - - auto spawn_pos = nav_->GetSpawn(); - nav_->GoTo(spawn_pos.x, 1300, -M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); - - go_home_time_ = node_->now(); - - status_ = MissionStatus::RUNNING; - } - - void GoHomeMission::Update() - { - /*if (!nav_->HasArrived()) - { - return; - }*/ - - switch (step_) - { - case GO_FRONT: - { - if ((node_->now() - go_home_time_).seconds() >= 95) - { - step_ = AWAIT_95S; - } - } - - break; - - case AWAIT_95S: - { - auto spawn_pos = nav_->GetSpawn(); - nav_->GoTo(spawn_pos.x, 1500, -M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); - - step_ = GO_HOME; - } - - break; - case GO_HOME: - { - std_msgs::msg::Int64 msg; - msg.data = mission_score_; - score_pub_->publish(msg); - - step_ = DONE; - status_ = MissionStatus::DONE; - } - break; - - default: - break; - } - } - - void GoHomeMission::Clear() - { - } - - MissionStatus GoHomeMission::GetStatus() const - { - return status_; - } -}