rewrite action i'm not 100% ok with it but gonna test it

src/modelec_strat/include/modelec_strat/action/free_action.hpp

@@ -10,17 +10,19 @@ namespace Modelec
     public:
         FreeAction(const std::shared_ptr<ActionExecutor>& action_executor);
         FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front, int n);
+        FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, bool> servo);
+        FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, bool>> servos);
 
         void Next() override;
         void Init(const std::vector<std::string>& params) override;
-        void SetFront(bool front);
+        void AddServo(int id, bool front);
-        void SetN(int n);
+        void AddServo(std::pair<int, bool> servo);
+        void AddServos(const std::vector<std::pair<int, bool>>& servos);
 
         inline static const std::string Name = ActionExec::FREE;
 
     private:
-        bool front_;
+        std::vector<std::pair<int, bool>> servos_;
-        int n_;
 
         std::queue<int> steps_;
 

src/modelec_strat/include/modelec_strat/action/take_action.hpp

@@ -10,18 +10,19 @@ namespace Modelec
     public:
         TakeAction(const std::shared_ptr<ActionExecutor>& action_executor);
         TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front, int n);
+        TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, bool> servo);
+        TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, bool>> servos);
 
         void Next() override;
         void Init(const std::vector<std::string>& params) override;
-        void Init(bool front, int n);
+        void AddServo(int id, bool front);
-        void SetFront(bool front);
+        void AddServo(std::pair<int, bool> servo);
-        void SetN(int n);
+        void AddServos(const std::vector<std::pair<int, bool>>& servos);
 
         inline static const std::string Name = ActionExec::TAKE;
 
     private:
-        bool front_;
+        std::vector<std::pair<int, bool>> servos_;
-        int n_;
 
         std::queue<int> steps_;
 

src/modelec_strat/include/modelec_strat/action_executor.hpp

@@ -9,6 +9,8 @@
 
 #include <sensor_msgs/msg/joy.hpp>
 
+#include "action/base_action.hpp"
+
 namespace Modelec
 {
     class BaseAction;
@@ -28,14 +30,13 @@ namespace Modelec
 
         void ReInit();
 
-        void Down(BaseAction::Front front);
+        void Down(BaseAction::Front front, bool force = false);
 
-        void Up(BaseAction::Front front);
+        void Up(BaseAction::Front front, bool force = false);
 
-        void Take(bool front = true, int n = 0);
+        void Take(std::vector<std::pair<int, bool>> servos, bool force = false);
-
-        void Free(bool front = true, int n = 0);
 
+        void Free(std::vector<std::pair<int, bool>> servos, bool force = false);
 
         void MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg);
 

src/modelec_strat/src/action/free_action.cpp

@@ -10,8 +10,17 @@ Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_ex
 
 Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front, int n) : FreeAction(action_executor)
 {
-    front_ = front;
+    AddServo(n, front);
-    n_ = n;
+}
+
+Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, bool> servo) : FreeAction(action_executor)
+{
+    AddServo(servo.first, servo.second);
+}
+
+Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, bool>> servos) : FreeAction(action_executor)
+{
+    AddServos(servos);
 }
 
 void Modelec::FreeAction::Next()
@@ -31,12 +40,16 @@ void Modelec::FreeAction::Next()
         {
             modelec_interfaces::msg::ActionServoTimedArray msg;
 
-            msg.items.resize(1);
+            msg.items.resize(servos_.size());
+
+            for (size_t i = 0; i < servos_.size(); i++)
+            {
+                msg.items[i].id = servos_[i].first + (servos_[i].second ? 3 : 11);
+                msg.items[i].start_angle = servos_[i].second ? 0.8 : 0;
+                msg.items[i].end_angle = servos_[i].second ? 2.5 : 0;
+                msg.items[i].duration_s = 0.5;
+            }
 
-            msg.items[0].id = n_ + (front_ ? 3 : 11);
-            msg.items[0].start_angle = front_ ? 2.5 : 0;
-            msg.items[0].end_angle = front_ ? 0.8 : 0;
-            msg.items[0].duration_s = 0.5;
             action_executor_->MoveServoTimed(msg);
         }
         break;
@@ -54,17 +67,26 @@ void Modelec::FreeAction::Init(const std::vector<std::string>& params)
 {
     if (params.size() >= 2)
     {
-        SetFront(params[1] == "1" || params[1] == "true" || params[1] == "front");
+        for (size_t i = 1; i < params.size(); i += 2)
-        SetN(std::stoi(params[2]));
+        {
+            int id = std::stoi(params[i]);
+            bool front = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "front") : true;
+            AddServo(id, front);
+        }
     }
 }
 
-void Modelec::FreeAction::SetFront(bool front)
+void Modelec::FreeAction::AddServo(int id, bool front)
 {
-    front_ = front;
+    servos_.emplace_back(id, front);
 }
 
-void Modelec::FreeAction::SetN(int n)
+void Modelec::FreeAction::AddServo(std::pair<int, bool> servo)
 {
-    n_ = n;
+    servos_.emplace_back(servo);
+}
+
+void Modelec::FreeAction::AddServos(const std::vector<std::pair<int, bool>>& servos)
+{
+    servos_.insert(servos_.end(), servos.begin(), servos.end());
 }

src/modelec_strat/src/action/take_action.cpp

@@ -8,8 +8,17 @@ Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_ex
 
 Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front, int n) : TakeAction(action_executor)
 {
-    front_ = front;
+    AddServo(n, front);
-    n_ = n;
+}
+
+Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, bool> servo) : TakeAction(action_executor)
+{
+    AddServo(servo);
+}
+
+Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, bool>> servos) : TakeAction(action_executor)
+{
+    AddServos(servos);
 }
 
 void Modelec::TakeAction::Next()
@@ -29,12 +38,16 @@ void Modelec::TakeAction::Next()
         {
             modelec_interfaces::msg::ActionServoTimedArray msg;
 
-            msg.items.resize(1);
+            msg.items.resize(servos_.size());
+
+            for (size_t i = 0; i < servos_.size(); i++)
+            {
+                msg.items[i].id = servos_[i].first + (servos_[i].second ? 3 : 11);
+                msg.items[i].start_angle = servos_[i].second ? 0.8 : 0;
+                msg.items[i].end_angle = servos_[i].second ? 2.7 : 0;
+                msg.items[i].duration_s = 0.5;
+            }
 
-            msg.items[0].id = n_ + (front_ ? 3 : 11);
-            msg.items[0].start_angle = front_ ? 0.8 : 0;
-            msg.items[0].end_angle = front_ ? 2.7 : 0;
-            msg.items[0].duration_s = 0.5;
             action_executor_->MoveServoTimed(msg);
         }
         break;
@@ -52,21 +65,26 @@ void Modelec::TakeAction::Init(const std::vector<std::string>& params)
 {
     if (params.size() >= 2)
     {
-        SetFront(params[1] == "1" || params[1] == "true" || params[1] == "front");
+        for (size_t i = 1; i < params.size(); i += 2)
-        SetN(std::stoi(params[2]));
+        {
+            int id = std::stoi(params[i]);
+            bool front = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "front") : true;
+            AddServo(id, front);
+        }
     }
 }
 
-void Modelec::TakeAction::Init(bool front, int n)
+void Modelec::TakeAction::AddServo(int id, bool front)
 {
+    servos_.emplace_back(id, front);
 }
 
-void Modelec::TakeAction::SetFront(bool front)
+void Modelec::TakeAction::AddServo(std::pair<int, bool> servo)
 {
-    front_ = front;
+    servos_.emplace_back(servo);
 }
 
-void Modelec::TakeAction::SetN(int n)
+void Modelec::TakeAction::AddServos(const std::vector<std::pair<int, bool>>& servos)
 {
-    n_ = n;
+    servos_.insert(servos_.end(), servos.begin(), servos.end());
 }

src/modelec_strat/src/action/up_action.cpp

@@ -8,7 +8,7 @@ Modelec::UPAction::UPAction(const std::shared_ptr<ActionExecutor>& action_execut
     steps_.push(ActionExec::DONE_STEP);
 }
 
-Modelec::UPAction::UPAction(const std::shared_ptr<ActionExecutor>& action_executor, front front) : UPAction(action_executor)
+Modelec::UPAction::UPAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front) : UPAction(action_executor)
 {
     front_ = front;
 }
@@ -95,11 +95,11 @@ void Modelec::UPAction::Init(const std::vector<std::string>& params)
 {
     if (!params.empty())
     {
-        SetFront(static_cast<front>(std::stoi(params[1])));
+        SetFront(static_cast<Front>(std::stoi(params[1])));
     }
 }
 
-void Modelec::UPAction::SetFront(front front)
+void Modelec::UPAction::SetFront(Front front)
 {
     front_ = front;
 }

src/modelec_strat/src/action_executor.cpp

@@ -88,19 +88,19 @@ namespace Modelec
                 {
                     if (msg->buttons[0] == 1) // A button
                     {
-                        Down(true);
+                        Down(BaseAction::BOTH);
                     }
                     else if (msg->buttons[1] == 1) // B button
                     {
-                        Up(true);
+                        Up(BaseAction::BOTH);
                     }
                     else if (msg->buttons[3] == 1) // X button
                     {
-                        Down(false);
+                        Take({{0, true}, {1, true}, {2, true}, {3, true}});
                     }
                     else if (msg->buttons[4] == 1) // Y button
                     {
-                        Up(false);
+                        Free({{0, true}, {1, true}, {2, true}, {3, true}});
                     }
                 }
             });
@@ -149,45 +149,57 @@ namespace Modelec
         action_ = nullptr;
     }
 
-    void ActionExecutor::Down(BaseAction::front front) {
+    void ActionExecutor::Down(BaseAction::Front front, bool force) {
-        if (action_done_)
+        if (action_done_ || force)
         {
             action_ = std::make_shared<DownAction>(shared_from_this(), front);
+            if (action_done_)
+            {
+                step_running_ = 0;
+            }
             action_done_ = false;
-            step_running_ = 0;
 
             Update();
         }
     }
 
-    void ActionExecutor::Up(BaseAction::front front) {
+    void ActionExecutor::Up(BaseAction::Front front, bool force) {
-        if (action_done_)
+        if (action_done_ || force)
         {
             action_ = std::make_shared<UPAction>(shared_from_this(), front);
+            if (action_done_)
+            {
+                step_running_ = 0;
+            }
             action_done_ = false;
-            step_running_ = 0;
 
             Update();
         }
     }
 
-    void ActionExecutor::Take(bool front, int n) {
+    void ActionExecutor::Take(std::vector<std::pair<int, bool>> servos, bool force) {
-        if (action_done_)
+        if (action_done_ || force)
         {
-            action_ = std::make_shared<TakeAction>(shared_from_this(), front, n);
+            action_ = std::make_shared<TakeAction>(shared_from_this(), servos);
+            if (action_done_)
+            {
+                step_running_ = 0;
+            }
             action_done_ = false;
-            step_running_ = 0;
 
             Update();
         }
     }
 
-    void ActionExecutor::Free(bool front, int n) {
+    void ActionExecutor::Free(std::vector<std::pair<int, bool>> servos, bool force) {
-        if (action_done_)
+        if (action_done_ || force)
         {
-            action_ = std::make_shared<FreeAction>(shared_from_this(), front, n);
+            action_ = std::make_shared<FreeAction>(shared_from_this(), servos);
+            if (action_done_)
+            {
+                step_running_ = 0;
+            }
             action_done_ = false;
-            step_running_ = 0;
 
             Update();
         }

src/modelec_strat/src/missions/free_mission.cpp

@@ -71,7 +71,7 @@ namespace Modelec {
             break;
         case FREE:
             {
-                action_executor_->Free();
+                action_executor_->Free({{0, true}, {1, true}, {2, true}, {3, true}});
                 deploy_time_ = node_->now();
             }
 

src/modelec_strat/src/missions/take_mission.cpp

@@ -65,7 +65,7 @@ namespace Modelec {
             break;
         case TAKE:
             {
-                action_executor_->Take();
+                action_executor_->Take({{0, true}, {1, true}, {2, true}, {3, true}});
                 deploy_time_ = node_->now();
             }
 

src/modelec_strat/src/strat_fms.cpp

@@ -159,7 +159,11 @@ namespace Modelec
                                .count();
                 start_time_pub_->publish(msg);
 
-                action_executor_->Up(BaseAction::Front::BOTH);
+                action_executor_->Up(BaseAction::Front::BOTH, true);
+                action_executor_->Free({
+                    {0, true}, {1, true}, {2, true}, {3, true},
+                    {0, false}, {1, false}, {2, false}, {3, false},
+                }, true);
 
                 Transition(State::SELECT_MISSION, "Match started");
             }