modelec/modelec-marcel-ROS
1
0
Fork 0
mirror of https://github.com/modelec/modelec-marcel-ROS.git synced 2026-01-18 16:47:43 +01:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
acki
2025-12-19 22:26:10 +01:00
parent ec7fca430d
commit eea5746adb
10 changed files with 125 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/modelec_strat/include/modelec_strat/action/free_action.hpp
View File

@@ -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 SetN(int n);
void AddServo(int id, bool front);
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_;
int n_;
std::vector<std::pair<int, bool>> servos_;
std::queue<int> steps_;

11
src/modelec_strat/include/modelec_strat/action/take_action.hpp
View File

@@ -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 SetFront(bool front);
void SetN(int n);
void AddServo(int id, bool front);
void AddServo(std::pair<int, bool> servo);
void AddServos(const std::vector<std::pair<int, bool>>& servos);
inline static const std::string Name = ActionExec::TAKE;
private:
bool front_;
int n_;
std::vector<std::pair<int, bool>> servos_;
std::queue<int> steps_;

11
src/modelec_strat/include/modelec_strat/action_executor.hpp
View File

@@ -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 Free(bool front = true, int n = 0);
void Take(std::vector<std::pair<int, bool>> servos, bool force = false);
void Free(std::vector<std::pair<int, bool>> servos, bool force = false);
void MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg);

48
src/modelec_strat/src/action/free_action.cpp
View File

@@ -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;
n_ = n;
AddServo(n, front);
}
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");
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<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());
}

46
src/modelec_strat/src/action/take_action.cpp
View File

@@ -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;
n_ = n;
AddServo(n, front);
}
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");
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<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());
}

6
src/modelec_strat/src/action/up_action.cpp
View File

@@ -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;
}

48
src/modelec_strat/src/action_executor.cpp
View File

@@ -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<DownAction>(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<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) {
if (action_done_)
void ActionExecutor::Take(std::vector<std::pair<int, bool>> servos, bool force) {
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) {
if (action_done_)
void ActionExecutor::Free(std::vector<std::pair<int, bool>> servos, bool force) {
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();
}

2
src/modelec_strat/src/missions/free_mission.cpp
View File

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

2
src/modelec_strat/src/missions/take_mission.cpp
View File

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

6
src/modelec_strat/src/strat_fms.cpp
View File

@@ -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");
}