diff --git a/src/modelec_gui/src/pages/home_page.cpp b/src/modelec_gui/src/pages/home_page.cpp index 8b4f67c..738e4f3 100644 --- a/src/modelec_gui/src/pages/home_page.cpp +++ b/src/modelec_gui/src/pages/home_page.cpp @@ -46,7 +46,7 @@ namespace ModelecGUI { modelec_interfaces::msg::Spawn team_msg; team_msg.team_id = msg->team_id; - team_msg.name = modelec_interfaces::msg::Spawn::BOTTOM; + team_msg.name = modelec_interfaces::msg::Spawn::TOP; spawn_pub_->publish(team_msg); emit TeamChoose(); diff --git a/src/modelec_strat/CMakeLists.txt b/src/modelec_strat/CMakeLists.txt index 04ba3fb..038eb10 100644 --- a/src/modelec_strat/CMakeLists.txt +++ b/src/modelec_strat/CMakeLists.txt @@ -15,7 +15,7 @@ find_package(ament_index_cpp REQUIRED) find_package(modelec_interfaces REQUIRED) find_package(modelec_utils REQUIRED) -add_executable(strat_fsm src/strat_fms.cpp src/navigation_helper.cpp src/pathfinding.cpp src/action_executor.cpp src/missions/prepare_concert_mission.cpp src/missions/banner_mission.cpp src/missions/go_home_mission.cpp src/obstacle/obstacle.cpp src/obstacle/column.cpp src/deposite_zone.cpp) +add_executable(strat_fsm src/strat_fms.cpp src/navigation_helper.cpp src/pathfinding.cpp src/action_executor.cpp src/missions/prepare_concert_mission.cpp src/missions/banner_mission.cpp src/missions/test.go_home_mission.cpp src/obstacle/obstacle.cpp src/obstacle/column.cpp src/deposite_zone.cpp) ament_target_dependencies(strat_fsm rclcpp std_msgs std_srvs modelec_interfaces ament_index_cpp) target_link_libraries(strat_fsm modelec_utils::utils modelec_utils::config) target_include_directories(strat_fsm PUBLIC 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 797bae0..3459d7c 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 @@ -20,9 +20,8 @@ namespace Modelec private: enum Step { - ROTATE_TO_HOME, + GO_FRONT, GO_HOME, - GO_CLOSE, DONE } step_; diff --git a/src/modelec_strat/src/missions/test.go_home_mission.cpp b/src/modelec_strat/src/missions/test.go_home_mission.cpp new file mode 100644 index 0000000..fef5827 --- /dev/null +++ b/src/modelec_strat/src/missions/test.go_home_mission.cpp @@ -0,0 +1,69 @@ +#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 t = nav_->GetCurrentPos(); + + nav_->GoTo(t->x, 1200, -M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); + + status_ = MissionStatus::RUNNING; + } + + void GoHomeMission::Update() + { + if (!nav_->HasArrived()) + { + return; + } + + switch (step_) + { + case GO_FRONT: + { + auto t = nav_->GetCurrentPos(); + + nav_->GoTo(t->x, 1700, -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_; + } +} diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 970b988..f13dbe8 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -159,13 +159,11 @@ namespace Modelec auto elapsed = now - match_start_time_; // TODO : next thing to upgrade to have a good strat - if (!is_banner_done_) + /*if (!is_banner_done_) { Transition(State::DO_PROMOTION, "Start promotion"); - }else if (elapsed.seconds() >= 96) - { - Transition(State::STOP, "Finish"); - } + }*/ + Transition(State::DO_GO_HOME, "Go Home"); // TODO : check the time needed by the mission /*else if (elapsed.seconds() < 70) {