action executor v0.0.1

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/mission_manager.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/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/action_executor.hpp

@@ -1,6 +1,9 @@
 #pragma once
 
 #include <rclcpp/rclcpp.hpp>
+#include <modelec_interfaces/msg/action_asc_pos.hpp>
+#include <modelec_interfaces/msg/action_relay_state.hpp>
+#include <modelec_interfaces/msg/action_servo_pos.hpp>
 
 namespace Modelec
 {
@@ -8,11 +11,64 @@ namespace Modelec
     class ActionExecutor
     {
     public:
+
+        enum Action
+        {
+            NONE,
+            DEPLOY_BANNER,
+            TAKE_POT,
+            PLACE_POT,
+        };
+
+        enum Step
+        {
+        };
+
         ActionExecutor();
 
         ActionExecutor(const rclcpp::Node::SharedPtr& node);
 
-        rclcpp::Node::SharedPtr getNode() const;
+        rclcpp::Node::SharedPtr GetNode() const;
+
+        bool IsActionDone() const;
+
+        void Update();
+
+        void DeployBanner();
+
+        void TakePot();
+
+        void PlacePot();
+
+    protected:
+        rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_get_pub_;
+        rclcpp::Publisher<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_get_pub_;
+        rclcpp::Publisher<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_get_pub_;
+
+        rclcpp::Subscription<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_get_res_sub_;
+        rclcpp::Subscription<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_get_res_sub_;
+        rclcpp::Subscription<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_get_res_sub_;
+
+        rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_set_pub_;
+        rclcpp::Publisher<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_set_pub_;
+
+        rclcpp::Subscription<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_set_res_sub_;
+        rclcpp::Subscription<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_set_res_sub_;
+
+        rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_move_pub_;
+        rclcpp::Publisher<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_move_pub_;
+        rclcpp::Publisher<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_move_pub_;
+
+        rclcpp::Subscription<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_move_res_sub_;
+        rclcpp::Subscription<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_move_res_sub_;
+        rclcpp::Subscription<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_move_res_sub_;
+
+        Action action_ = NONE;
+
+        std::queue<Step> step_;
+
+        bool action_done_ = true;
+        bool step_done_ = true;
 
     private:
         rclcpp::Node::SharedPtr node_;

src/modelec_strat/include/modelec_strat/mission_manager.hpp

@@ -1,21 +0,0 @@
-#pragma once
-
-#include <rclcpp/rclcpp.hpp>
-
-namespace Modelec
-{
-
-    class MissionManager
-    {
-    public:
-        MissionManager();
-
-        MissionManager(const rclcpp::Node::SharedPtr& node);
-
-        rclcpp::Node::SharedPtr getNode() const;
-
-    private:
-        rclcpp::Node::SharedPtr node_;
-    };
-
-}

src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <modelec_strat/action_executor.hpp>
 #include <modelec_strat/missions/mission_base.hpp>
 #include <modelec_strat/navigation_helper.hpp>
 #include <std_msgs/msg/int64.hpp>
@@ -13,7 +14,8 @@ namespace Modelec
     class BannerMission : public Mission
     {
     public:
-        BannerMission(const std::shared_ptr<NavigationHelper>& nav);
+        BannerMission(const std::shared_ptr<NavigationHelper>& nav,
+                        const std::shared_ptr<ActionExecutor>& action_executor);
 
         void start(rclcpp::Node::SharedPtr node) override;
         void update() override;
@@ -30,6 +32,7 @@ namespace Modelec
 
         MissionStatus status_;
         std::shared_ptr<NavigationHelper> nav_;
+        std::shared_ptr<ActionExecutor> action_executor_;
         rclcpp::Node::SharedPtr node_;
         rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr score_pub_;
         int mission_score_ = 0;

src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <modelec_strat/action_executor.hpp>
 #include <modelec_strat/missions/mission_base.hpp>
 #include <modelec_strat/navigation_helper.hpp>
 #include <modelec_strat/obstacle/column.hpp>
@@ -9,7 +10,8 @@ namespace Modelec {
 
     class PrepareConcertMission : public Mission {
     public:
-        PrepareConcertMission(const std::shared_ptr<NavigationHelper>& nav);
+        PrepareConcertMission(const std::shared_ptr<NavigationHelper>& nav,
+                              const std::shared_ptr<ActionExecutor>& action_executor);
 
         void start(rclcpp::Node::SharedPtr node) override;
         void update() override;
@@ -32,6 +34,7 @@ namespace Modelec {
 
         MissionStatus status_;
         std::shared_ptr<NavigationHelper> nav_;
+        std::shared_ptr<ActionExecutor> action_executor_;
         rclcpp::Node::SharedPtr node_;
 
         std::shared_ptr<ColumnObstacle> column_;

src/modelec_strat/include/modelec_strat/strat_fms.hpp

@@ -3,7 +3,6 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include "action_executor.hpp"
-#include "mission_manager.hpp"
 #include "navigation_helper.hpp"
 #include "missions/mission_base.hpp"
 #include "missions/banner_mission.hpp"
@@ -59,8 +58,7 @@ namespace Modelec
         int team_id_ = 0;
 
         std::shared_ptr<NavigationHelper> nav_;
-        std::unique_ptr<MissionManager> mission_manager_;
-        std::unique_ptr<ActionExecutor> action_executor_;
+        std::shared_ptr<ActionExecutor> action_executor_;
 
         rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr tirette_sub_;
         rclcpp::Publisher<modelec_interfaces::msg::StratState>::SharedPtr state_pub_;

src/modelec_strat/src/action_executor.cpp

@@ -8,10 +8,108 @@ namespace Modelec
 
     ActionExecutor::ActionExecutor(const rclcpp::Node::SharedPtr& node) : node_(node)
     {
+        asc_get_pub_ = node_->create_publisher<modelec_interfaces::msg::ActionAscPos>("/action/get/asc", 10);
+        servo_get_pub_ = node_->create_publisher<modelec_interfaces::msg::ActionServoPos>("/action/get/servo", 10);
+        relay_get_pub_ = node_->create_publisher<modelec_interfaces::msg::ActionRelayState>("/action/get/relay", 10);
+
+        asc_get_res_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionAscPos>(
+            "/action/get/asc/res", 10, [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr msg)
+            {
+            });
+
+        servo_get_res_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionServoPos>(
+            "/action/get/servo/res", 10, [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg)
+            {
+            });
+
+        relay_get_res_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionRelayState>(
+            "/action/get/relay/res", 10, [this](const modelec_interfaces::msg::ActionRelayState::SharedPtr msg)
+            {
+            });
+
+        asc_set_pub_ = node_->create_publisher<modelec_interfaces::msg::ActionAscPos>("/action/set/asc", 10);
+        servo_set_pub_ = node_->create_publisher<modelec_interfaces::msg::ActionServoPos>("/action/set/servo", 10);
+
+        asc_set_res_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionAscPos>(
+            "/action/set/asc/res", 10, [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr msg)
+            {
+            });
+
+        servo_set_res_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionServoPos>(
+            "/action/set/servo/res", 10, [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg)
+            {
+            });
+
+        asc_move_pub_ = node_->create_publisher<modelec_interfaces::msg::ActionAscPos>("/action/move/asc", 10);
+        servo_move_pub_ = node_->create_publisher<modelec_interfaces::msg::ActionServoPos>("/action/move/servo", 10);
+        relay_move_pub_ = node_->create_publisher<modelec_interfaces::msg::ActionRelayState>("/action/move/relay", 10);
+
+        asc_move_res_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionAscPos>(
+            "/action/move/asc/res", 10, [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr msg)
+            {
+                step_done_ = true;
+                Update();
+            });
+
+        servo_move_res_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionServoPos>(
+            "/action/move/servo/res", 10, [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg)
+            {
+                step_done_ = true;
+                Update();
+            });
+
+        relay_move_res_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionRelayState>(
+            "/action/move/relay/res", 10, [this](const modelec_interfaces::msg::ActionRelayState::SharedPtr msg)
+            {
+                step_done_ = true;
+                Update();
+            });
     }
 
-    rclcpp::Node::SharedPtr ActionExecutor::getNode() const
+    rclcpp::Node::SharedPtr ActionExecutor::GetNode() const
     {
         return node_;
     }
+
+    bool ActionExecutor::IsActionDone() const
+    {
+        return action_done_;
+    }
+
+    void ActionExecutor::Update()
+    {
+        if (step_.empty())
+        {
+            action_ = NONE;
+            action_done_ = true;
+            return;
+        }
+
+        step_.pop();
+        step_done_ = false;
+    }
+
+    void ActionExecutor::DeployBanner()
+    {
+        action_ = DEPLOY_BANNER;
+        action_done_ = false;
+
+        Update();
+    }
+
+    void ActionExecutor::TakePot()
+    {
+        action_ = TAKE_POT;
+        action_done_ = false;
+
+        Update();
+    }
+
+    void ActionExecutor::PlacePot()
+    {
+        action_ = PLACE_POT;
+        action_done_ = false;
+
+        Update();
+    }
 }

src/modelec_strat/src/mission_manager.cpp

@@ -1,30 +0,0 @@
-#include <modelec_strat/mission_manager.hpp>
-
-namespace Modelec
-{
-    enum class State {
-        INIT,               // Initialisation des capteurs, timers, etc.
-        WAIT_START,         // Attente du feu vert
-        SELECT_MISSION,     // Choix intelligent de la prochaine mission
-
-        RETURN_HOME,        // Retour zone de fin
-        FAILSAFE,           // En cas d'erreur bloquante
-        STOP,                // Fin du match
-
-        GO_TO_OBJECTIVE,    // Navigation vers la cible
-        EXECUTE_MISSION,    // Exécution de l'action
-    };
-
-    MissionManager::MissionManager()
-    {
-    }
-
-    MissionManager::MissionManager(const rclcpp::Node::SharedPtr& node) : node_(node)
-    {
-    }
-
-    rclcpp::Node::SharedPtr MissionManager::getNode() const
-    {
-        return node_;
-    }
-}

src/modelec_strat/src/missions/banner_mission.cpp

@@ -4,8 +4,8 @@
 
 namespace Modelec
 {
-    BannerMission::BannerMission(const std::shared_ptr<NavigationHelper>& nav) : step_(GO_TO_FRONT),
-        status_(MissionStatus::READY), nav_(nav)
+    BannerMission::BannerMission(const std::shared_ptr<NavigationHelper>& nav, const std::shared_ptr<ActionExecutor>& action_executor) : step_(GO_TO_FRONT),
+        status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor)
     {
     }
 
@@ -30,10 +30,12 @@ namespace Modelec
 
         if (!nav_->HasArrived()) return;
 
+        if (!action_executor_->IsActionDone()) return;
+
         switch (step_)
         {
         case GO_TO_FRONT:
-            // TODO deploy the banner here
+            action_executor_->DeployBanner();
 
             step_ = DEPLOY_BANNER;
             break;

src/modelec_strat/src/missions/prepare_concert_mission.cpp

@@ -4,8 +4,8 @@
 
 namespace Modelec
 {
-    PrepareConcertMission::PrepareConcertMission(const std::shared_ptr<NavigationHelper>& nav) : step_(GO_TO_COLUMN),
-        status_(MissionStatus::READY), nav_(nav)
+    PrepareConcertMission::PrepareConcertMission(const std::shared_ptr<NavigationHelper>& nav, const std::shared_ptr<ActionExecutor>& action_executor) : step_(GO_TO_COLUMN),
+        status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor)
     {
     }
 
@@ -64,6 +64,8 @@ namespace Modelec
 
         if (!nav_->HasArrived()) return;
 
+        if (!action_executor_->IsActionDone()) return;
+
         switch (step_)
         {
         case GO_TO_COLUMN:
@@ -75,6 +77,7 @@ namespace Modelec
             step_ = GO_CLOSE_TO_COLUMN;
             break;
         case GO_CLOSE_TO_COLUMN:
+            action_executor_->TakePot();
             nav_->GetPathfinding()->RemoveObstacle(column_->id());
 
             step_ = TAKE_COLUMN;
@@ -170,6 +173,8 @@ namespace Modelec
 
         case GO_CLOSE_TO_PLATFORM:
             {
+                action_executor_->PlacePot();
+
                 column_->setX(closestDepoZonePoint_.x);
                 column_->setY(closestDepoZonePoint_.y);
                 column_->setTheta(closestDepoZonePoint_.theta);

src/modelec_strat/src/strat_fms.cpp

@@ -64,7 +64,6 @@ namespace Modelec
     void StratFMS::Init()
     {
         nav_ = std::make_shared<NavigationHelper>(shared_from_this());
-        mission_manager_ = std::make_unique<MissionManager>(shared_from_this());
         action_executor_ = std::make_unique<ActionExecutor>(shared_from_this());
 
         RCLCPP_INFO(this->get_logger(), "StratFMS fully initialized");
@@ -146,7 +145,7 @@ namespace Modelec
         case State::DO_PREPARE_CONCERT:
             if (!current_mission_)
             {
-                current_mission_ = std::make_unique<PrepareConcertMission>(nav_);
+                current_mission_ = std::make_unique<PrepareConcertMission>(nav_, action_executor_);
                 current_mission_->start(shared_from_this());
             }
             current_mission_->update();
@@ -171,7 +170,7 @@ namespace Modelec
         case State::DO_PROMOTION:
             if (!current_mission_)
             {
-                current_mission_ = std::make_unique<BannerMission>(nav_);
+                current_mission_ = std::make_unique<BannerMission>(nav_, action_executor_);
                 current_mission_->start(shared_from_this());
             }
             current_mission_->update();