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

new Front Back system

Browse Source
This commit is contained in:
acki
2025-12-19 16:56:01 +01:00
parent 980beaa8bf
commit ec7fca430d
12 changed files with 167 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
src/modelec_strat/include/modelec_strat/action/base_action.hpp
View File

@@ -25,6 +25,13 @@ namespace Modelec
class BaseAction
{
public:
enum Front
{
FRONT = 1,
BACK = 0,
BOTH = -1,
};
using Ptr = std::shared_ptr<BaseAction>;
using FactoryFn =
std::function<Ptr(const std::shared_ptr<ActionExecutor>&)>;

6
src/modelec_strat/include/modelec_strat/action/down_action.hpp
View File

@@ -9,16 +9,16 @@ namespace Modelec
{
public:
DownAction(const std::shared_ptr<ActionExecutor>& action_executor);
DownAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front);
DownAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front);
void Next() override;
void Init(const std::vector<std::string>& params) override;
void SetFront(bool front);
void SetFront(Front front);
inline static const std::string Name = ActionExec::DOWN;
private:
bool front_;
Front front_;
std::queue<int> steps_;

6
src/modelec_strat/include/modelec_strat/action/up_action.hpp
View File

@@ -9,16 +9,16 @@ namespace Modelec
{
public:
UPAction(const std::shared_ptr<ActionExecutor>& action_executor);
UPAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front);
UPAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front);
void Next() override;
void Init(const std::vector<std::string>& params) override;
void SetFront(bool front);
void SetFront(Front front);
inline static const std::string Name = ActionExec::UP;
private:
bool front_;
Front front_;
std::queue<int> steps_;

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

@@ -16,16 +16,6 @@ namespace Modelec
class ActionExecutor : public std::enable_shared_from_this<ActionExecutor>
{
public:
enum Action
{
NONE,
UP,
DOWN,
TAKE,
FREE
};
ActionExecutor();
ActionExecutor(const rclcpp::Node::SharedPtr& node);
@@ -38,9 +28,9 @@ namespace Modelec
void ReInit();
void Down(bool front = true);
void Down(BaseAction::Front front);
void Up(bool front = true);
void Up(BaseAction::Front front);
void Take(bool front = true, int n = 0);

2
src/modelec_strat/include/modelec_strat/missions/free_mission.hpp
View File

@@ -20,7 +20,9 @@ namespace Modelec {
enum Step
{
GO_TO_FREE,
DOWN,
FREE,
UP,
DONE,
} step_;

2
src/modelec_strat/include/modelec_strat/missions/take_mission.hpp
View File

@@ -20,7 +20,9 @@ namespace Modelec {
enum Step
{
GO_TO_TAKE,
DOWN,
TAKE,
UP,
DONE,
} step_;

67
src/modelec_strat/src/action/down_action.cpp
View File

@@ -8,7 +8,7 @@ Modelec::DownAction::DownAction(const std::shared_ptr<ActionExecutor>& action_ex
steps_.push(ActionExec::DONE_STEP);
}
Modelec::DownAction::DownAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front) : DownAction(action_executor)
Modelec::DownAction::DownAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front) : DownAction(action_executor)
{
front_ = front;
}
@@ -29,27 +29,54 @@ void Modelec::DownAction::Next()
case ActionExec::DOWN_STEP:
{
ActionServoTimedArray msg;
msg.items.resize(4);
msg.items.resize(front_ == BOTH ? 8 : 4);
msg.items[0].id = front_ ? 0 : 8;
msg.items[0].start_angle = front_ ? 1.95 : 0;
msg.items[0].end_angle = front_ ? 2.95 : 0;
msg.items[0].duration_s = 1;
if (front_ == FRONT || front_ == BOTH)
{
msg.items[0].id = 0;
msg.items[0].start_angle = 2.95;
msg.items[0].end_angle = 1.95;
msg.items[0].duration_s = 1;
msg.items[1].id = front_ ? 1 : 9;
msg.items[1].start_angle = front_ ? 1.9 : 0;
msg.items[1].end_angle = front_ ? 0.9 : 0;
msg.items[1].duration_s = 1;
msg.items[1].id = 1;
msg.items[1].start_angle = 0.9;
msg.items[1].end_angle = 1.9;
msg.items[1].duration_s = 1;
msg.items[2].id = front_ ? 2 : 10;
msg.items[2].start_angle = front_ ? 0.3 : 0;
msg.items[2].end_angle = front_ ? 0 : 0;
msg.items[2].duration_s = 1;
msg.items[2].id = 2;
msg.items[2].start_angle = 0;
msg.items[2].end_angle = 0.3;
msg.items[2].duration_s = 1;
msg.items[3].id = front_ ? 3 : 11;
msg.items[3].start_angle = front_ ? 2.7 : 0;
msg.items[3].end_angle = front_ ? 3 : 0;
msg.items[3].duration_s = 1;
msg.items[3].id = 3;
msg.items[3].start_angle = 3;
msg.items[3].end_angle = 2.7;
msg.items[3].duration_s = 1;
}
if (front_ == BACK || front_ == BOTH)
{
int i = front_ == BOTH ? 4 : 0;
msg.items[i].id = 8;
msg.items[i].start_angle = 0;
msg.items[i].end_angle = 0;
msg.items[i].duration_s = 1;
msg.items[i+1].id = 9;
msg.items[i+1].start_angle = 0;
msg.items[i+1].end_angle = 0;
msg.items[i+1].duration_s = 1;
msg.items[i+2].id = 10;
msg.items[i+2].start_angle = 0;
msg.items[i+2].end_angle = 0;
msg.items[i+2].duration_s = 1;
msg.items[i+3].id = 11;
msg.items[i+3].start_angle = 0;
msg.items[i+3].end_angle = 0;
msg.items[i+3].duration_s = 1;
}
action_executor_->MoveServoTimed(msg);
}
@@ -68,11 +95,11 @@ void Modelec::DownAction::Init(const std::vector<std::string>& params)
{
if (!params.empty())
{
SetFront(params[1] == "1" || params[1] == "true" || params[1] == "front");
SetFront(static_cast<Front>(std::stoi(params[1])));
}
}
void Modelec::DownAction::SetFront(bool front)
void Modelec::DownAction::SetFront(Front front)
{
front_ = front;
}

67
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, bool front) : UPAction(action_executor)
Modelec::UPAction::UPAction(const std::shared_ptr<ActionExecutor>& action_executor, front front) : UPAction(action_executor)
{
front_ = front;
}
@@ -29,27 +29,54 @@ void Modelec::UPAction::Next()
case ActionExec::UP_STEP:
{
ActionServoTimedArray msg;
msg.items.resize(4);
msg.items.resize(front_ == BOTH ? 8 : 4);
msg.items[0].id = front_ ? 0 : 8;
msg.items[0].start_angle = front_ ? 2.95 : 0;
msg.items[0].end_angle = front_ ? 1.95 : 0;
msg.items[0].duration_s = 1;
if (front_ == FRONT || front_ == BOTH)
{
msg.items[0].id = 0;
msg.items[0].start_angle = 2.95;
msg.items[0].end_angle = 1.95;
msg.items[0].duration_s = 1;
msg.items[1].id = front_ ? 1 : 9;
msg.items[1].start_angle = front_ ? 0.9 : 0;
msg.items[1].end_angle = front_ ? 1.9 : 0;
msg.items[1].duration_s = 1;
msg.items[1].id = 1;
msg.items[1].start_angle = 0.9;
msg.items[1].end_angle = 1.9;
msg.items[1].duration_s = 1;
msg.items[2].id = front_ ? 2 : 10;
msg.items[2].start_angle = front_ ? 0 : 0;
msg.items[2].end_angle = front_ ? 0.3 : 0;
msg.items[2].duration_s = 1;
msg.items[2].id = 2;
msg.items[2].start_angle = 0;
msg.items[2].end_angle = 0.3;
msg.items[2].duration_s = 1;
msg.items[3].id = front_ ? 3 : 11;
msg.items[3].start_angle = front_ ? 3 : 0;
msg.items[3].end_angle = front_ ? 2.5 : 0;
msg.items[3].duration_s = 1;
msg.items[3].id = 3;
msg.items[3].start_angle = 3;
msg.items[3].end_angle = 2.5;
msg.items[3].duration_s = 1;
}
if (front_ == BACK || front_ == BOTH) {
int i = front_ == BOTH ? 4 : 0;
msg.items[i].id = 8;
msg.items[i].start_angle = 0;
msg.items[i].end_angle = 0;
msg.items[i].duration_s = 1;
msg.items[i+1].id = 9;
msg.items[i+1].start_angle = 0;
msg.items[i+1].end_angle = 0;
msg.items[i+1].duration_s = 1;
msg.items[i+2].id = 10;
msg.items[i+2].start_angle = 0;
msg.items[i+2].end_angle = 0;
msg.items[i+2].duration_s = 1;
msg.items[i+3].id = 11;
msg.items[i+3].start_angle = 0;
msg.items[i+3].end_angle = 0;
msg.items[i+3].duration_s = 1;
}
action_executor_->MoveServoTimed(msg);
}
@@ -68,11 +95,11 @@ void Modelec::UPAction::Init(const std::vector<std::string>& params)
{
if (!params.empty())
{
SetFront(params[1] == "1" || params[1] == "true" || params[1] == "front");
SetFront(static_cast<front>(std::stoi(params[1])));
}
}
void Modelec::UPAction::SetFront(bool front)
void Modelec::UPAction::SetFront(front front)
{
front_ = front;
}

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

@@ -149,7 +149,7 @@ namespace Modelec
action_ = nullptr;
}
void ActionExecutor::Down(bool front) {
void ActionExecutor::Down(BaseAction::front front) {
if (action_done_)
{
action_ = std::make_shared<DownAction>(shared_from_this(), front);
@@ -160,7 +160,7 @@ namespace Modelec
}
}
void ActionExecutor::Up(bool front) {
void ActionExecutor::Up(BaseAction::front front) {
if (action_done_)
{
action_ = std::make_shared<UPAction>(shared_from_this(), front);

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

@@ -1,5 +1,7 @@
#include <modelec_strat/missions/free_mission.hpp>
#include "modelec_strat/action/base_action.hpp"
namespace Modelec {
FreeMission::FreeMission(const std::shared_ptr<NavigationHelper>& nav, const std::shared_ptr<ActionExecutor>& action_executor)
: step_(GO_TO_FREE), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) {
@@ -24,9 +26,9 @@ namespace Modelec {
if (!nav_->HasArrived())
{
if ((node_->now() - go_timeout_).seconds() < 2)
if ((node_->now() - go_timeout_).seconds() < 5)
{
// nav_->AskWaypoint();
nav_->AskWaypoint();
return;
}
if ((node_->now() - go_timeout_).seconds() < 10)
@@ -55,17 +57,33 @@ namespace Modelec {
angle + M_PI), true, Pathfinding::FREE);
go_timeout_ = node_->now();
step_ = FREE;
}
step_ = FREE;
break;
case DOWN:
{
action_executor_->Down(BaseAction::FRONT);
deploy_time_ = node_->now();
}
step_ = FREE;
break;
case FREE:
{
action_executor_->Down();
action_executor_->Free();
deploy_time_ = node_->now();
step_ = DONE;
}
step_ = UP;
break;
case UP:
{
action_executor_->Up(BaseAction::FRONT);
deploy_time_ = node_->now();
}
step_ = DONE;
break;
case DONE:
{
@@ -88,4 +106,4 @@ namespace Modelec {
return status_;
}
}
}

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

@@ -1,5 +1,7 @@
#include <modelec_strat/missions/take_mission.hpp>
#include "modelec_strat/action/base_action.hpp"
namespace Modelec {
TakeMission::TakeMission(const std::shared_ptr<NavigationHelper>& nav, const std::shared_ptr<ActionExecutor>& action_executor)
: step_(GO_TO_TAKE), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) {
@@ -24,9 +26,9 @@ namespace Modelec {
if (!nav_->HasArrived())
{
if ((node_->now() - go_timeout_).seconds() < 2)
if ((node_->now() - go_timeout_).seconds() < 5)
{
// nav_->AskWaypoint();
nav_->AskWaypoint();
return;
}
if ((node_->now() - go_timeout_).seconds() < 10)
@@ -51,11 +53,27 @@ namespace Modelec {
go_timeout_ = node_->now();
}
step_ = DOWN;
break;
case DOWN:
{
action_executor_->Up(BaseAction::FRONT);
deploy_time_ = node_->now();
}
step_ = TAKE;
break;
case TAKE:
{
action_executor_->Up();
action_executor_->Take();
deploy_time_ = node_->now();
}
step_ = UP;
break;
case UP:
{
action_executor_->Up(BaseAction::FRONT);
deploy_time_ = node_->now();
}
@@ -78,4 +96,4 @@ namespace Modelec {
return status_;
}
}
}

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

@@ -7,6 +7,8 @@
#include <modelec_strat/missions/take_mission.hpp>
#include <modelec_strat/missions/free_mission.hpp>
#include "modelec_strat/action/base_action.hpp"
namespace Modelec
{
@@ -157,6 +159,8 @@ namespace Modelec
.count();
start_time_pub_->publish(msg);
action_executor_->Up(BaseAction::Front::BOTH);
Transition(State::SELECT_MISSION, "Match started");
}
break;