From eea5746adbffa85e7ceff0f39cef1e133dae27a1 Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 19 Dec 2025 22:26:10 +0100 Subject: [PATCH] rewrite action i'm not 100% ok with it but gonna test it --- .../modelec_strat/action/free_action.hpp | 10 ++-- .../modelec_strat/action/take_action.hpp | 11 +++-- .../include/modelec_strat/action_executor.hpp | 11 +++-- src/modelec_strat/src/action/free_action.cpp | 48 ++++++++++++++----- src/modelec_strat/src/action/take_action.cpp | 46 ++++++++++++------ src/modelec_strat/src/action/up_action.cpp | 6 +-- src/modelec_strat/src/action_executor.cpp | 48 ++++++++++++------- .../src/missions/free_mission.cpp | 2 +- .../src/missions/take_mission.cpp | 2 +- src/modelec_strat/src/strat_fms.cpp | 6 ++- 10 files changed, 125 insertions(+), 65 deletions(-) diff --git a/src/modelec_strat/include/modelec_strat/action/free_action.hpp b/src/modelec_strat/include/modelec_strat/action/free_action.hpp index 2167139..acb123a 100644 --- a/src/modelec_strat/include/modelec_strat/action/free_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/free_action.hpp @@ -10,17 +10,19 @@ namespace Modelec public: FreeAction(const std::shared_ptr& action_executor); FreeAction(const std::shared_ptr& action_executor, bool front, int n); + FreeAction(const std::shared_ptr& action_executor, std::pair servo); + FreeAction(const std::shared_ptr& action_executor, std::vector> servos); void Next() override; void Init(const std::vector& params) override; - void SetFront(bool front); - void SetN(int n); + void AddServo(int id, bool front); + void AddServo(std::pair servo); + void AddServos(const std::vector>& servos); inline static const std::string Name = ActionExec::FREE; private: - bool front_; - int n_; + std::vector> servos_; std::queue steps_; diff --git a/src/modelec_strat/include/modelec_strat/action/take_action.hpp b/src/modelec_strat/include/modelec_strat/action/take_action.hpp index 4ae0008..fd9f226 100644 --- a/src/modelec_strat/include/modelec_strat/action/take_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/take_action.hpp @@ -10,18 +10,19 @@ namespace Modelec public: TakeAction(const std::shared_ptr& action_executor); TakeAction(const std::shared_ptr& action_executor, bool front, int n); + TakeAction(const std::shared_ptr& action_executor, std::pair servo); + TakeAction(const std::shared_ptr& action_executor, std::vector> servos); void Next() override; void Init(const std::vector& params) override; - void Init(bool front, int n); - void SetFront(bool front); - void SetN(int n); + void AddServo(int id, bool front); + void AddServo(std::pair servo); + void AddServos(const std::vector>& servos); inline static const std::string Name = ActionExec::TAKE; private: - bool front_; - int n_; + std::vector> servos_; std::queue steps_; diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index b9cbc15..46c7e84 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -9,6 +9,8 @@ #include +#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 Free(bool front = true, int n = 0); + void Take(std::vector> servos, bool force = false); + void Free(std::vector> servos, bool force = false); void MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg); diff --git a/src/modelec_strat/src/action/free_action.cpp b/src/modelec_strat/src/action/free_action.cpp index 7ab3b24..fcc18a1 100644 --- a/src/modelec_strat/src/action/free_action.cpp +++ b/src/modelec_strat/src/action/free_action.cpp @@ -10,8 +10,17 @@ Modelec::FreeAction::FreeAction(const std::shared_ptr& action_ex Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, bool front, int n) : FreeAction(action_executor) { - front_ = front; - n_ = n; + AddServo(n, front); +} + +Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, std::pair servo) : FreeAction(action_executor) +{ + AddServo(servo.first, servo.second); +} + +Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, std::vector> 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& params) { if (params.size() >= 2) { - SetFront(params[1] == "1" || params[1] == "true" || params[1] == "front"); - SetN(std::stoi(params[2])); + for (size_t i = 1; i < params.size(); i += 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 servo) { - n_ = n; + servos_.emplace_back(servo); +} + +void Modelec::FreeAction::AddServos(const std::vector>& servos) +{ + servos_.insert(servos_.end(), servos.begin(), servos.end()); } diff --git a/src/modelec_strat/src/action/take_action.cpp b/src/modelec_strat/src/action/take_action.cpp index be11108..01a1469 100644 --- a/src/modelec_strat/src/action/take_action.cpp +++ b/src/modelec_strat/src/action/take_action.cpp @@ -8,8 +8,17 @@ Modelec::TakeAction::TakeAction(const std::shared_ptr& action_ex Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, bool front, int n) : TakeAction(action_executor) { - front_ = front; - n_ = n; + AddServo(n, front); +} + +Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, std::pair servo) : TakeAction(action_executor) +{ + AddServo(servo); +} + +Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, std::vector> 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& params) { if (params.size() >= 2) { - SetFront(params[1] == "1" || params[1] == "true" || params[1] == "front"); - SetN(std::stoi(params[2])); + for (size_t i = 1; i < params.size(); i += 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 servo) { - front_ = front; + servos_.emplace_back(servo); } -void Modelec::TakeAction::SetN(int n) +void Modelec::TakeAction::AddServos(const std::vector>& servos) { - n_ = n; + servos_.insert(servos_.end(), servos.begin(), servos.end()); } diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index 4774027..1cbeb21 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -8,7 +8,7 @@ Modelec::UPAction::UPAction(const std::shared_ptr& action_execut steps_.push(ActionExec::DONE_STEP); } -Modelec::UPAction::UPAction(const std::shared_ptr& action_executor, front front) : UPAction(action_executor) +Modelec::UPAction::UPAction(const std::shared_ptr& action_executor, Front front) : UPAction(action_executor) { front_ = front; } @@ -95,11 +95,11 @@ void Modelec::UPAction::Init(const std::vector& params) { if (!params.empty()) { - SetFront(static_cast(std::stoi(params[1]))); + SetFront(static_cast(std::stoi(params[1]))); } } -void Modelec::UPAction::SetFront(front front) +void Modelec::UPAction::SetFront(Front front) { front_ = front; } diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 13380bb..0d36e83 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/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) { - if (action_done_) + void ActionExecutor::Down(BaseAction::Front front, bool force) { + if (action_done_ || force) { action_ = std::make_shared(shared_from_this(), front); + if (action_done_) + { + step_running_ = 0; + } action_done_ = false; - step_running_ = 0; Update(); } } - void ActionExecutor::Up(BaseAction::front front) { - if (action_done_) + void ActionExecutor::Up(BaseAction::Front front, bool force) { + if (action_done_ || force) { action_ = std::make_shared(shared_from_this(), front); + if (action_done_) + { + step_running_ = 0; + } action_done_ = false; - step_running_ = 0; Update(); } } - void ActionExecutor::Take(bool front, int n) { - if (action_done_) + void ActionExecutor::Take(std::vector> servos, bool force) { + if (action_done_ || force) { - action_ = std::make_shared(shared_from_this(), front, n); + action_ = std::make_shared(shared_from_this(), servos); + if (action_done_) + { + step_running_ = 0; + } action_done_ = false; - step_running_ = 0; Update(); } } - void ActionExecutor::Free(bool front, int n) { - if (action_done_) + void ActionExecutor::Free(std::vector> servos, bool force) { + if (action_done_ || force) { - action_ = std::make_shared(shared_from_this(), front, n); + action_ = std::make_shared(shared_from_this(), servos); + if (action_done_) + { + step_running_ = 0; + } action_done_ = false; - step_running_ = 0; Update(); } diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index dd6ad37..31fcb5b 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/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(); } diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp index 9d2f54a..3f9892c 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/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(); } diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index e5282d1..414218c 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/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"); }