test go home

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();

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

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_;
 

src/modelec_strat/src/missions/test.go_home_mission.cpp

@@ -0,0 +1,69 @@
+#include <modelec_strat/missions/go_home_mission.hpp>
+
+#include <modelec_utils/config.hpp>
+
+namespace Modelec
+{
+    GoHomeMission::GoHomeMission(const std::shared_ptr<NavigationHelper>& 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<int>("config.mission_score.go_home", 0);
+
+        score_pub_ = node_->create_publisher<std_msgs::msg::Int64>("/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_;
+    }
+}

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)
                 {