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

Develop strat 2K26 v1 (#15)

Browse Source
This commit is contained in:
Ackimixs
2025-12-16 23:13:21 +01:00
committed by GitHub
parent d4e3c322de
commit 5f1c39d8eb
23 changed files with 951 additions and 965 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
src/modelec_com/src/pcb_action_interface.cpp
View File

@@ -265,10 +265,9 @@ namespace Modelec
servo_value_ = {
{0, 0},
{1, 3},
{2, 3},
{3, 0},
{4, 1.57},
{5, 1.4},
{2, 1.45},
{3, 1.6},
{4, 0},
};
std::string data = "MOV;SERVO;" + std::to_string(servo_value_.size()) + ";";

2
src/modelec_core/launch/modelec.launch.py
View File

@@ -150,3 +150,5 @@ def generate_launch_description():
OpaqueFunction(function=launch_com),
OpaqueFunction(function=launch_strat),
])
# to run in debug : , prefix=['xterm -e gdb -ex run --args']

6
src/modelec_gui/CMakeLists.txt
View File

@@ -37,8 +37,7 @@ add_executable(modelec_gui
src/pages/home_page.cpp
src/pages/test_map_page.cpp
src/pages/map_page.cpp
# src/pages/action_page.cpp
src/pages/action_page.new.cpp
src/pages/action_page.cpp
src/pages/alim_page.cpp
src/widget/action_widget.cpp
@@ -47,8 +46,7 @@ add_executable(modelec_gui
include/modelec_gui/pages/home_page.hpp
include/modelec_gui/pages/test_map_page.hpp
include/modelec_gui/pages/map_page.hpp
# include/modelec_gui/pages/action_page.hpp
include/modelec_gui/pages/action_page.new.hpp
include/modelec_gui/pages/action_page.hpp
include/modelec_gui/pages/alim_page.hpp
include/modelec_gui/widget/action_widget.hpp
)

2
src/modelec_gui/include/modelec_gui/modelec_gui.hpp
View File

@@ -8,7 +8,7 @@
#include <modelec_gui/pages/test_map_page.hpp>
#include <modelec_gui/pages/odo_page.hpp>
#include <modelec_gui/pages/map_page.hpp>
#include <modelec_gui/pages/action_page.new.hpp>
#include <modelec_gui/pages/action_page.hpp>
#include <modelec_gui/pages/alim_page.hpp>

81
src/modelec_gui/include/modelec_gui/pages/action_page.hpp
View File

@@ -1,10 +1,12 @@
#pragma once
#include <QLabel>
#include <QWidget>
#include <QPushButton>
#include <QLineEdit>
#include <QVBoxLayout>
#include <QSpinBox>
#include <QLabel>
#include <modelec_gui/widget/action_widget.hpp>
@@ -12,6 +14,7 @@
#include <modelec_interfaces/msg/action_asc_pos.hpp>
#include <modelec_interfaces/msg/action_servo_pos_array.hpp>
#include <modelec_interfaces/msg/action_servo_timed_array.hpp>
#include <modelec_interfaces/msg/action_relay_state_array.hpp>
#include <modelec_interfaces/msg/action_exec.hpp>
@@ -26,6 +29,7 @@ namespace ModelecGUI
using ActionAscPos = modelec_interfaces::msg::ActionAscPos;
using ActionServoPosArray = modelec_interfaces::msg::ActionServoPosArray;
using ActionRelayStateArray = modelec_interfaces::msg::ActionRelayStateArray;
using ActionServoTimedArray = modelec_interfaces::msg::ActionServoTimedArray;
using ActionExec = modelec_interfaces::msg::ActionExec;
ActionPage(rclcpp::Node::SharedPtr node, QWidget* parent = nullptr);
@@ -34,56 +38,65 @@ namespace ModelecGUI
rclcpp::Node::SharedPtr get_node() const { return node_; }
private:
QPushButton* max_size_button_;
QVBoxLayout* layout_;
QHBoxLayout* asc_layout_;
QVBoxLayout* main_layout_;
QPushButton* deploy_max_button_;
ActionWidget* asc_action_;
bool waiting_for_move_asc_;
// ---- Action1 front ----
QHBoxLayout* deploy_action1_layout_;
QVBoxLayout* deploy_action1_front_layout_;
QLabel* deploy_action1_front_label_;
QPushButton* deploy_action1_front_up_button_;
QGridLayout* servo_layout_;
QHBoxLayout* deploy_action1_front_servos_layout_;
ActionWidget* servo0_action_;
ActionWidget* servo1_action_;
ActionWidget* servo2_action_;
ActionWidget* servo3_action_;
ActionWidget* servo4_action_;
ActionWidget* servo5_action_;
ActionWidget* servo6_action_;
ActionWidget* servo7_action_;
ActionWidget* servo8_action_;
std::vector<ActionWidget*> servo_actions_;
QPushButton* deploy_action1_front_servo1_button_,
*deploy_action1_front_servo2_button_,
*deploy_action1_front_servo3_button_,
*deploy_action1_front_servo4_button_;
QPushButton* test_button_;
bool test_ = false;
bool deploy_action1_front_servo1_state_,
deploy_action1_front_servo2_state_,
deploy_action1_front_servo3_state_,
deploy_action1_front_servo4_state_;
QHBoxLayout* relay_layout_;
QPushButton* deploy_action1_front_down_button_;
QPushButton* relay_top_button_;
QPushButton* relay_bottom_button_;
QPushButton* relay_third_button_;
std::vector<QPushButton*> relay_buttons_;
std::vector<bool> relay_values_;
// ---- Action1 back ----
QVBoxLayout* deploy_action1_back_layout_;
QLabel* deploy_action1_back_label_;
QPushButton* deploy_action1_back_up_button_;
QPushButton* deploy_banner_button_;
QHBoxLayout* deploy_action1_back_servos_layout_;
QPushButton* deploy_action1_back_servo1_button_,
*deploy_action1_back_servo2_button_,
*deploy_action1_back_servo3_button_,
*deploy_action1_back_servo4_button_;
bool deploy_action1_back_servo1_state_,
deploy_action1_back_servo2_state_,
deploy_action1_back_servo3_state_,
deploy_action1_back_servo4_state_;
QPushButton* deploy_action1_back_down_button_;
// ---- Action2 ----
QVBoxLayout* deploy_action2_layout_;
QLabel* deploy_action2_label_;
QPushButton* deploy_action2_button_;
QPushButton* undeploy_action2_button_;
// ---- Ros ----
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<ActionExec>::SharedPtr action_exec_pub_;
rclcpp::Subscription<ActionAscPos>::SharedPtr asc_get_sub_;
rclcpp::Publisher<ActionAscPos>::SharedPtr asc_set_pub_;
rclcpp::Subscription<ActionAscPos>::SharedPtr asc_set_res_sub_;
rclcpp::Publisher<ActionAscPos>::SharedPtr asc_move_pub_;
rclcpp::Subscription<ActionAscPos>::SharedPtr asc_move_res_sub_;
rclcpp::Subscription<ActionServoPosArray>::SharedPtr servo_get_sub_;
rclcpp::Publisher<ActionServoPosArray>::SharedPtr servo_move_pub_;
rclcpp::Subscription<ActionServoPosArray>::SharedPtr servo_move_res_sub_;
rclcpp::Subscription<ActionRelayStateArray>::SharedPtr relay_get_sub_;
rclcpp::Publisher<ActionRelayStateArray>::SharedPtr relay_move_pub_;
rclcpp::Subscription<ActionRelayStateArray>::SharedPtr relay_move_res_sub_;
rclcpp::Publisher<ActionServoTimedArray>::SharedPtr servo_timed_move_pub_;
rclcpp::Subscription<ActionServoTimedArray>::SharedPtr servo_timed_move_res_sub_;
};
} // namespace Modelec

102
src/modelec_gui/include/modelec_gui/pages/action_page.new.hpp
View File

@@ -1,102 +0,0 @@
#pragma once
#include <QLabel>
#include <QWidget>
#include <QPushButton>
#include <QLineEdit>
#include <QVBoxLayout>
#include <QSpinBox>
#include <QLabel>
#include <modelec_gui/widget/action_widget.hpp>
#include <rclcpp/rclcpp.hpp>
#include <modelec_interfaces/msg/action_asc_pos.hpp>
#include <modelec_interfaces/msg/action_servo_pos_array.hpp>
#include <modelec_interfaces/msg/action_servo_timed_array.hpp>
#include <modelec_interfaces/msg/action_relay_state_array.hpp>
#include <modelec_interfaces/msg/action_exec.hpp>
namespace ModelecGUI
{
class ActionPage : public QWidget
{
Q_OBJECT
public:
using ActionAscPos = modelec_interfaces::msg::ActionAscPos;
using ActionServoPosArray = modelec_interfaces::msg::ActionServoPosArray;
using ActionRelayStateArray = modelec_interfaces::msg::ActionRelayStateArray;
using ActionServoTimedArray = modelec_interfaces::msg::ActionServoTimedArray;
using ActionExec = modelec_interfaces::msg::ActionExec;
ActionPage(rclcpp::Node::SharedPtr node, QWidget* parent = nullptr);
~ActionPage() override;
rclcpp::Node::SharedPtr get_node() const { return node_; }
private:
QVBoxLayout* main_layout_;
QPushButton* deploy_max_button_;
// ---- Action1 front ----
QHBoxLayout* deploy_action1_layout_;
QVBoxLayout* deploy_action1_front_layout_;
QLabel* deploy_action1_front_label_;
QPushButton* deploy_action1_front_up_button_;
QHBoxLayout* deploy_action1_front_servos_layout_;
QPushButton* deploy_action1_front_servo1_button_,
*deploy_action1_front_servo2_button_,
*deploy_action1_front_servo3_button_,
*deploy_action1_front_servo4_button_;
bool deploy_action1_front_servo1_state_,
deploy_action1_front_servo2_state_,
deploy_action1_front_servo3_state_,
deploy_action1_front_servo4_state_;
QPushButton* deploy_action1_front_down_button_;
// ---- Action1 back ----
QVBoxLayout* deploy_action1_back_layout_;
QLabel* deploy_action1_back_label_;
QPushButton* deploy_action1_back_up_button_;
QHBoxLayout* deploy_action1_back_servos_layout_;
QPushButton* deploy_action1_back_servo1_button_,
*deploy_action1_back_servo2_button_,
*deploy_action1_back_servo3_button_,
*deploy_action1_back_servo4_button_;
bool deploy_action1_back_servo1_state_,
deploy_action1_back_servo2_state_,
deploy_action1_back_servo3_state_,
deploy_action1_back_servo4_state_;
QPushButton* deploy_action1_back_down_button_;
// ---- Action2 ----
QVBoxLayout* deploy_action2_layout_;
QLabel* deploy_action2_label_;
QPushButton* deploy_action2_button_;
QPushButton* undeploy_action2_button_;
// ---- Ros ----
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<ActionExec>::SharedPtr action_exec_pub_;
rclcpp::Subscription<ActionServoPosArray>::SharedPtr servo_get_sub_;
rclcpp::Publisher<ActionServoPosArray>::SharedPtr servo_move_pub_;
rclcpp::Subscription<ActionServoPosArray>::SharedPtr servo_move_res_sub_;
rclcpp::Publisher<ActionServoTimedArray>::SharedPtr servo_timed_move_pub_;
rclcpp::Subscription<ActionServoTimedArray>::SharedPtr servo_timed_move_res_sub_;
};
} // namespace Modelec

2
src/modelec_gui/src/modelec_gui.cpp
View File

@@ -14,7 +14,7 @@ namespace ModelecGUI {
this->showFullScreen();
home_page_ = new HomePage(get_node(), this);
//odo_page_ = new OdoPage(get_node(), this);
// odo_page_ = new OdoPage(get_node(), this);
test_map_page_ = new TestMapPage(get_node(), this);
map_page_ = new MapPage(get_node(), this);
action_page_ = new ActionPage(get_node(), this);

499
src/modelec_gui/src/pages/action_page.cpp
View File

@@ -6,294 +6,257 @@ namespace ModelecGUI
QWidget(parent),
node_(node)
{
layout_ = new QVBoxLayout(this);
setLayout(layout_);
setWindowTitle("Action Page");
max_size_button_ = new QPushButton("Deploy Max Size");
connect(max_size_button_, &QPushButton::clicked, this,
[this]()
{
ActionExec action_exec;
// action_exec.action = ActionExec::DEPLOY_MAX;
// action_exec.mission = {ActionExec::DEPLOY_MAX_STEP};
action_exec_pub_->publish(action_exec);
});
asc_layout_ = new QHBoxLayout;
asc_action_ = new ActionWidget(this);
asc_action_->SetSpinBoxRange(0, 1000);
asc_action_->SetSpinBoxStep(1);
asc_action_->SetSpinBoxValue(0);
asc_action_->SetButtonText("Send ASC");
waiting_for_move_asc_ = false;
connect(asc_action_, &ActionWidget::ButtonClicked, this,
[this](double value)
{
ActionAscPos asc_move;
asc_move.value = value;
asc_move.pos = 128;
asc_set_pub_->publish(asc_move);
asc_action_->setDisabled(true);
waiting_for_move_asc_ = true;
});
asc_layout_->addWidget(asc_action_);
servo_layout_ = new QGridLayout;
servo0_action_ = new ActionWidget(this);
servo0_action_->SetSpinBoxRange(0, 180);
servo0_action_->SetSpinBoxStep(.1);
servo0_action_->SetSpinBoxValue(90);
servo0_action_->SetButtonText("Servo 0");
servo1_action_ = new ActionWidget(this);
servo1_action_->SetSpinBoxRange(0, 180);
servo1_action_->SetSpinBoxStep(.1);
servo1_action_->SetSpinBoxValue(90);
servo1_action_->SetButtonText("Servo 1");
servo2_action_ = new ActionWidget(this);
servo2_action_->SetSpinBoxRange(0, 180);
servo2_action_->SetSpinBoxStep(.1);
servo2_action_->SetSpinBoxValue(90);
servo2_action_->SetButtonText("Servo 2");
servo3_action_ = new ActionWidget(this);
servo3_action_->SetSpinBoxRange(0, 180);
servo3_action_->SetSpinBoxStep(.1);
servo3_action_->SetSpinBoxValue(90);
servo3_action_->SetButtonText("Servo 3");
servo4_action_ = new ActionWidget(this);
servo4_action_->SetSpinBoxRange(0, 180);
servo4_action_->SetSpinBoxStep(.1);
servo4_action_->SetSpinBoxValue(90);
servo4_action_->SetButtonText("Servo 4");
servo5_action_ = new ActionWidget(this);
servo5_action_->SetSpinBoxRange(0, 180);
servo5_action_->SetSpinBoxStep(.1);
servo5_action_->SetSpinBoxValue(90);
servo5_action_->SetButtonText("Servo 5");
servo6_action_ = new ActionWidget(this);
servo6_action_->SetSpinBoxRange(0, 180);
servo6_action_->SetSpinBoxStep(.1);
servo6_action_->SetSpinBoxValue(90);
servo6_action_->SetButtonText("Servo 6");
servo7_action_ = new ActionWidget(this);
servo7_action_->SetSpinBoxRange(0, 180);
servo7_action_->SetSpinBoxStep(.1);
servo7_action_->SetSpinBoxValue(90);
servo7_action_->SetButtonText("Servo 7");
servo8_action_ = new ActionWidget(this);
servo8_action_->SetSpinBoxRange(0, 180);
servo8_action_->SetSpinBoxStep(.1);
servo8_action_->SetSpinBoxValue(90);
servo8_action_->SetButtonText("Servo 8");
servo_layout_->addWidget(servo0_action_, 0, 0);
servo_layout_->addWidget(servo1_action_, 0, 1);
servo_layout_->addWidget(servo2_action_, 0, 2);
servo_layout_->addWidget(servo3_action_, 1, 0);
servo_layout_->addWidget(servo4_action_, 1, 1);
servo_layout_->addWidget(servo5_action_, 1, 2);
servo_layout_->addWidget(servo6_action_, 2, 0);
servo_layout_->addWidget(servo7_action_, 2, 1);
servo_layout_->addWidget(servo8_action_, 2, 2);
servo_actions_.push_back(servo0_action_);
servo_actions_.push_back(servo1_action_);
servo_actions_.push_back(servo2_action_);
servo_actions_.push_back(servo3_action_);
servo_actions_.push_back(servo4_action_);
servo_actions_.push_back(servo5_action_);
servo_actions_.push_back(servo6_action_);
servo_actions_.push_back(servo7_action_);
servo_actions_.push_back(servo8_action_);
for (size_t i = 0; i < servo_actions_.size(); ++i)
{
connect(servo_actions_[i], &ActionWidget::ButtonClicked, this,
[this, i](double value)
{
ActionServoPosArray servo_move;
servo_move.items.resize(1);
servo_move.items[0].id = i;
servo_move.items[0].angle = value * M_PI / 180.0;
servo_move_pub_->publish(servo_move);
servo_actions_[i]->setDisabled(true);
});
}
test_button_ = new QPushButton("Test POC Servos");
connect(test_button_, &QPushButton::clicked, this,
[this]()
{
test_ = !test_;
ActionServoPosArray servo_move;
servo_move.items.resize(2);
servo_move.items[0].id = 1;
servo_move.items[0].angle = test_ ? M_PI_2 : 0;
servo_actions_[1]->setDisabled(true);
servo_move.items[1].id = 2;
servo_move.items[1].angle = test_ ? 0 : M_PI_2;
servo_actions_[2]->setDisabled(true);
servo_move_pub_->publish(servo_move);
});
relay_layout_ = new QHBoxLayout;
relay_top_button_ = new QPushButton(this);
relay_top_button_->setText("Toggle Relay Top");
relay_bottom_button_ = new QPushButton(this);
relay_bottom_button_->setText("Toggle Relay Bottom");
relay_third_button_ = new QPushButton(this);
relay_third_button_->setText("Toggle Third Relay");
relay_layout_->addWidget(relay_top_button_);
relay_layout_->addWidget(relay_bottom_button_);
relay_layout_->addWidget(relay_third_button_);
relay_buttons_.push_back(relay_top_button_);
relay_buttons_.push_back(relay_bottom_button_);
relay_buttons_.push_back(relay_third_button_);
relay_values_ = {false, false, false};
for (size_t i = 0; i < relay_buttons_.size(); ++i)
{
connect(relay_buttons_[i], &QPushButton::clicked, this,
[this, i]()
{
ActionRelayStateArray relay_state;
relay_state.items.resize(1);
relay_state.items[0].id = i;
relay_state.items[0].state = !relay_values_[i];
relay_move_pub_->publish(relay_state);
relay_buttons_[i]->setDisabled(true);
});
}
deploy_banner_button_ = new QPushButton("Deploy Banner");
connect(deploy_banner_button_, &QPushButton::clicked, this,
[this]()
{
ActionExec action_exec;
action_exec.action = ActionExec::DOWN;
action_exec.mission = {ActionExec::DOWN_STEP};
action_exec_pub_->publish(action_exec);
});
layout_->addWidget(max_size_button_);
layout_->addLayout(asc_layout_);
layout_->addLayout(servo_layout_);
layout_->addWidget(test_button_);
layout_->addLayout(relay_layout_);
layout_->addWidget(deploy_banner_button_);
action_exec_pub_ = node_->create_publisher<ActionExec>(
"action/exec", 10);
asc_get_sub_ = node_->create_subscription<ActionAscPos>(
"action/get/asc", 10, [this](const ActionAscPos::SharedPtr msg)
{
asc_action_->SetSpinBoxValue(msg->value);
});
asc_set_pub_ = node_->create_publisher<ActionAscPos>(
"action/set/asc", 10);
asc_set_res_sub_ = node_->create_subscription<ActionAscPos>(
"action/set/asc/res", 10, [this](const ActionAscPos::SharedPtr msg)
{
if (waiting_for_move_asc_)
{
ActionAscPos asc_move;
asc_move.pos = msg->pos;
asc_move_pub_->publish(asc_move);
waiting_for_move_asc_ = false;
}
});
asc_move_pub_ = node_->create_publisher<ActionAscPos>(
"action/move/asc", 10);
asc_move_res_sub_ = node_->create_subscription<ActionAscPos>(
"action/move/asc/res", 10, [this](const ActionAscPos::SharedPtr)
{
asc_action_->setDisabled(false);
});
servo_get_sub_ = node_->create_subscription<ActionServoPosArray>(
"action/get/servo", 10, [this](const ActionServoPosArray::SharedPtr msg)
"action/get/servo", 10, [this](const ActionServoPosArray::SharedPtr)
{
for (const auto& item : msg->items)
{
if (static_cast<int>(servo_actions_.size()) > item.id && servo_actions_[item.id] != nullptr)
{
servo_actions_[item.id]->SetSpinBoxValue(item.angle * 180.0 / M_PI);
}
}
});
servo_move_pub_ = node_->create_publisher<ActionServoPosArray>(
"action/move/servo", 10);
servo_move_res_sub_ = node_->create_subscription<ActionServoPosArray>(
"action/move/servo/res", 10, [this](const ActionServoPosArray::SharedPtr msg)
"action/move/servo/res", 10, [this](const ActionServoPosArray::SharedPtr)
{
for (const auto& item : msg->items)
{
if (static_cast<int>(servo_actions_.size()) > item.id && servo_actions_[item.id] != nullptr)
{
servo_actions_[item.id]->setDisabled(false);
}
}
});
relay_get_sub_ = node_->create_subscription<ActionRelayStateArray>(
"action/get/relay", 10, [this](const ActionRelayStateArray::SharedPtr msg)
servo_timed_move_pub_ = node_->create_publisher<ActionServoTimedArray>(
"action/move/servo/timed", 10);
servo_timed_move_res_sub_ = node_->create_subscription<ActionServoTimedArray>(
"action/move/servo/timed/res", 10, [this](const ActionServoTimedArray::SharedPtr)
{
for (const auto& item : msg->items)
{
if (static_cast<int>(relay_buttons_.size()) > item.id && relay_buttons_[item.id] != nullptr)
{
relay_values_[item.id] = item.state;
}
}
});
relay_move_pub_ = node_->create_publisher<ActionRelayStateArray>(
"action/move/relay", 10);
main_layout_ = new QVBoxLayout(this);
setLayout(main_layout_);
setWindowTitle("Action Page");
relay_move_res_sub_ = node_->create_subscription<ActionRelayStateArray>(
"action/move/relay/res", 10, [this](const ActionRelayStateArray::SharedPtr msg)
deploy_max_button_ = new QPushButton("Deploy Max Size");
connect(deploy_max_button_, &QPushButton::clicked, this,
[this]()
{
for (const auto& item : msg->items)
{
if (static_cast<int>(relay_buttons_.size()) > item.id && relay_buttons_[item.id] != nullptr)
{
relay_buttons_[item.id]->setDisabled(false);
relay_values_[item.id] = item.state;
}
}
ActionExec action_exec;
action_exec.action = ActionExec::MAX_DEPLOY;
action_exec_pub_->publish(action_exec);
});
deploy_action1_layout_ = new QHBoxLayout;
deploy_action1_front_layout_ = new QVBoxLayout;
deploy_action1_front_label_ = new QLabel("Front Action 1");
deploy_action1_front_up_button_ = new QPushButton("Up");
deploy_action1_front_down_button_ = new QPushButton("Down");
connect(deploy_action1_front_up_button_, &QPushButton::clicked, this,
[this]()
{
ActionExec action_exec;
action_exec.action = ActionExec::UP + ActionExec::DELIMITER + "1";
action_exec_pub_->publish(action_exec);
});
connect(deploy_action1_front_down_button_, &QPushButton::clicked, this,
[this]()
{
ActionExec action_exec;
action_exec.action = ActionExec::DOWN + ActionExec::DELIMITER + "1";
action_exec_pub_->publish(action_exec);
});
deploy_action1_front_servos_layout_ = new QHBoxLayout;
deploy_action1_front_servo1_button_ = new QPushButton("Servo 1");
deploy_action1_front_servo2_button_ = new QPushButton("Servo 2");
deploy_action1_front_servo3_button_ = new QPushButton("Servo 3");
deploy_action1_front_servo4_button_ = new QPushButton("Servo 4");
connect(deploy_action1_front_servo1_button_, &QPushButton::clicked, this,
[this]()
{
deploy_action1_front_servo1_state_ = !deploy_action1_front_servo1_state_;
ActionExec action_exec;
action_exec.action = (deploy_action1_front_servo1_state_
? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "1";
action_exec_pub_->publish(action_exec);
});
connect(deploy_action1_front_servo2_button_, &QPushButton::clicked, this,
[this]()
{
deploy_action1_front_servo2_state_ = !deploy_action1_front_servo2_state_;
ActionExec action_exec;
action_exec.action = (deploy_action1_front_servo2_state_
? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "2";
action_exec_pub_->publish(action_exec);
});
connect(deploy_action1_front_servo3_button_, &QPushButton::clicked, this,
[this]()
{
deploy_action1_front_servo3_state_ = !deploy_action1_front_servo3_state_;
ActionExec action_exec;
action_exec.action = (deploy_action1_front_servo3_state_
? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "3";
action_exec_pub_->publish(action_exec);
});
connect(deploy_action1_front_servo4_button_, &QPushButton::clicked, this,
[this]()
{
deploy_action1_front_servo4_state_ = !deploy_action1_front_servo4_state_;
ActionExec action_exec;
action_exec.action = (deploy_action1_front_servo4_state_
? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "4";
action_exec_pub_->publish(action_exec);
});
deploy_action1_front_servos_layout_->addWidget(deploy_action1_front_servo1_button_);
deploy_action1_front_servos_layout_->addWidget(deploy_action1_front_servo2_button_);
deploy_action1_front_servos_layout_->addWidget(deploy_action1_front_servo3_button_);
deploy_action1_front_servos_layout_->addWidget(deploy_action1_front_servo4_button_);
deploy_action1_front_layout_->addWidget(deploy_action1_front_label_);
deploy_action1_front_layout_->addWidget(deploy_action1_front_up_button_);
deploy_action1_front_layout_->addLayout(deploy_action1_front_servos_layout_);
deploy_action1_front_layout_->addWidget(deploy_action1_front_down_button_);
deploy_action1_back_layout_ = new QVBoxLayout;
deploy_action1_back_label_ = new QLabel("Back Action 1");
deploy_action1_back_up_button_ = new QPushButton("Up");
deploy_action1_back_down_button_ = new QPushButton("Down");
connect(deploy_action1_back_up_button_, &QPushButton::clicked, this,
[this]()
{
ActionExec action_exec;
action_exec.action = ActionExec::UP + ActionExec::DELIMITER + "0";
action_exec_pub_->publish(action_exec);
});
connect(deploy_action1_back_down_button_, &QPushButton::clicked, this,
[this]()
{
ActionExec action_exec;
action_exec.action = ActionExec::DOWN + ActionExec::DELIMITER + "0";
action_exec_pub_->publish(action_exec);
});
deploy_action1_back_servos_layout_ = new QHBoxLayout;
deploy_action1_back_servo1_button_ = new QPushButton("Servo 1");
deploy_action1_back_servo2_button_ = new QPushButton("Servo 2");
deploy_action1_back_servo3_button_ = new QPushButton("Servo 3");
deploy_action1_back_servo4_button_ = new QPushButton("Servo 4");
connect(deploy_action1_back_servo1_button_, &QPushButton::clicked, this,
[this]()
{
deploy_action1_back_servo1_state_ = !deploy_action1_back_servo1_state_;
ActionExec action_exec;
action_exec.action = (deploy_action1_back_servo1_state_
? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "1";
action_exec_pub_->publish(action_exec);
});
connect(deploy_action1_back_servo2_button_, &QPushButton::clicked, this,
[this]()
{
deploy_action1_back_servo2_state_ = !deploy_action1_back_servo2_state_;
ActionExec action_exec;
action_exec.action = (deploy_action1_back_servo2_state_
? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "2";
action_exec_pub_->publish(action_exec);
});
connect(deploy_action1_back_servo3_button_, &QPushButton::clicked, this,
[this]()
{
deploy_action1_back_servo3_state_ = !deploy_action1_back_servo3_state_;
ActionExec action_exec;
action_exec.action = (deploy_action1_back_servo3_state_
? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "3";
action_exec_pub_->publish(action_exec);
});
connect(deploy_action1_back_servo4_button_, &QPushButton::clicked, this,
[this]()
{
deploy_action1_back_servo4_state_ = !deploy_action1_back_servo4_state_;
ActionExec action_exec;
action_exec.action = (deploy_action1_back_servo4_state_
? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "4";
action_exec_pub_->publish(action_exec);
});
deploy_action1_back_servos_layout_->addWidget(deploy_action1_back_servo1_button_);
deploy_action1_back_servos_layout_->addWidget(deploy_action1_back_servo2_button_);
deploy_action1_back_servos_layout_->addWidget(deploy_action1_back_servo3_button_);
deploy_action1_back_servos_layout_->addWidget(deploy_action1_back_servo4_button_);
deploy_action1_back_layout_->addWidget(deploy_action1_back_label_);
deploy_action1_back_layout_->addWidget(deploy_action1_back_up_button_);
deploy_action1_back_layout_->addLayout(deploy_action1_back_servos_layout_);
deploy_action1_back_layout_->addWidget(deploy_action1_back_down_button_);
deploy_action1_layout_->addLayout(deploy_action1_front_layout_);
deploy_action1_layout_->addLayout(deploy_action1_back_layout_);
deploy_action2_layout_ = new QVBoxLayout;
deploy_action2_label_ = new QLabel("Thermometrer");
deploy_action2_button_ = new QPushButton("Deploy");
undeploy_action2_button_ = new QPushButton("Undeploy");
connect(deploy_action2_button_, &QPushButton::clicked, this,
[this]()
{
ActionExec action_exec;
action_exec.action = ActionExec::THERMO_DEPLOY;
action_exec_pub_->publish(action_exec);
});
connect(undeploy_action2_button_, &QPushButton::clicked, this,
[this]()
{
ActionExec action_exec;
action_exec.action = ActionExec::THERMO_UNDEPLOY;
action_exec_pub_->publish(action_exec);
});
deploy_action2_layout_->addWidget(deploy_action2_label_);
deploy_action2_layout_->addWidget(deploy_action2_button_);
deploy_action2_layout_->addWidget(undeploy_action2_button_);
main_layout_->addWidget(deploy_max_button_);
main_layout_->addLayout(deploy_action1_layout_);
main_layout_->addLayout(deploy_action2_layout_);
}
ActionPage::~ActionPage()

295
src/modelec_gui/src/pages/action_page.new.cpp
View File

@@ -1,295 +0,0 @@
#include <modelec_gui/pages/action_page.new.hpp>
namespace ModelecGUI
{
ActionPage::ActionPage(rclcpp::Node::SharedPtr node, QWidget* parent) :
QWidget(parent),
node_(node)
{
action_exec_pub_ = node_->create_publisher<ActionExec>(
"action/exec", 10);
servo_get_sub_ = node_->create_subscription<ActionServoPosArray>(
"action/get/servo", 10, [this](const ActionServoPosArray::SharedPtr)
{
});
servo_move_pub_ = node_->create_publisher<ActionServoPosArray>(
"action/move/servo", 10);
servo_move_res_sub_ = node_->create_subscription<ActionServoPosArray>(
"action/move/servo/res", 10, [this](const ActionServoPosArray::SharedPtr)
{
});
servo_timed_move_pub_ = node_->create_publisher<ActionServoTimedArray>(
"action/move/servo/timed", 10);
servo_timed_move_res_sub_ = node_->create_subscription<ActionServoTimedArray>(
"action/move/servo/timed/res", 10, [this](const ActionServoTimedArray::SharedPtr)
{
});
main_layout_ = new QVBoxLayout(this);
setLayout(main_layout_);
setWindowTitle("Action Page");
deploy_max_button_ = new QPushButton("Deploy Max Size");
connect(deploy_max_button_, &QPushButton::clicked, this,
[this]()
{
ActionExec action_exec;
action_exec.action = ActionExec::MAX_DEPLOY;
action_exec.mission = {ActionExec::MAX_DEPLOY_STEP};
action_exec_pub_->publish(action_exec);
});
deploy_action1_layout_ = new QHBoxLayout;
deploy_action1_front_layout_ = new QVBoxLayout;
deploy_action1_front_label_ = new QLabel("Front Action 1");
deploy_action1_front_up_button_ = new QPushButton("Up");
deploy_action1_front_down_button_ = new QPushButton("Down");
connect(deploy_action1_front_up_button_, &QPushButton::clicked, this,
[this]()
{
ActionExec action_exec;
action_exec.action = ActionExec::FRONT_UP;
action_exec.mission = {ActionExec::FRONT_UP_STEP};
action_exec_pub_->publish(action_exec);
});
connect(deploy_action1_front_down_button_, &QPushButton::clicked, this,
[this]()
{
ActionExec action_exec;
action_exec.action = ActionExec::FRONT_DOWN;
action_exec.mission = {ActionExec::FRONT_DOWN_STEP};
action_exec_pub_->publish(action_exec);
});
deploy_action1_front_servos_layout_ = new QHBoxLayout;
deploy_action1_front_servo1_button_ = new QPushButton("Servo 1");
deploy_action1_front_servo2_button_ = new QPushButton("Servo 2");
deploy_action1_front_servo3_button_ = new QPushButton("Servo 3");
deploy_action1_front_servo4_button_ = new QPushButton("Servo 4");
connect(deploy_action1_front_servo1_button_, &QPushButton::clicked, this,
[this]()
{
deploy_action1_front_servo1_state_ = !deploy_action1_front_servo1_state_;
ActionExec action_exec;
action_exec.action = deploy_action1_front_servo1_state_
? ActionExec::FRONT_TAKE_1 : ActionExec::FRONT_FREE_1;
action_exec.mission = {deploy_action1_front_servo1_state_
? ActionExec::FRONT_TAKE_1_STEP : ActionExec::FRONT_FREE_1_STEP};
action_exec_pub_->publish(action_exec);
});
connect(deploy_action1_front_servo2_button_, &QPushButton::clicked, this,
[this]()
{
deploy_action1_front_servo2_state_ = !deploy_action1_front_servo2_state_;
ActionExec action_exec;
action_exec.action = deploy_action1_front_servo2_state_
? ActionExec::FRONT_TAKE_2 : ActionExec::FRONT_FREE_2;
action_exec.mission = {deploy_action1_front_servo2_state_
? ActionExec::FRONT_TAKE_2_STEP : ActionExec::FRONT_FREE_2_STEP};
action_exec_pub_->publish(action_exec);
});
connect(deploy_action1_front_servo3_button_, &QPushButton::clicked, this,
[this]()
{
deploy_action1_front_servo3_state_ = !deploy_action1_front_servo3_state_;
ActionExec action_exec;
action_exec.action = deploy_action1_front_servo3_state_
? ActionExec::FRONT_TAKE_3 : ActionExec::FRONT_FREE_3;
action_exec.mission = {deploy_action1_front_servo3_state_
? ActionExec::FRONT_TAKE_3_STEP : ActionExec::FRONT_FREE_3_STEP};
action_exec_pub_->publish(action_exec);
});
connect(deploy_action1_front_servo4_button_, &QPushButton::clicked, this,
[this]()
{
deploy_action1_front_servo4_state_ = !deploy_action1_front_servo4_state_;
ActionExec action_exec;
action_exec.action = deploy_action1_front_servo4_state_
? ActionExec::FRONT_TAKE_4 : ActionExec::FRONT_FREE_4;
action_exec.mission = {deploy_action1_front_servo4_state_
? ActionExec::FRONT_TAKE_4_STEP : ActionExec::FRONT_FREE_4_STEP};
action_exec_pub_->publish(action_exec);
});
deploy_action1_front_servos_layout_->addWidget(deploy_action1_front_servo1_button_);
deploy_action1_front_servos_layout_->addWidget(deploy_action1_front_servo2_button_);
deploy_action1_front_servos_layout_->addWidget(deploy_action1_front_servo3_button_);
deploy_action1_front_servos_layout_->addWidget(deploy_action1_front_servo4_button_);
deploy_action1_front_layout_->addWidget(deploy_action1_front_label_);
deploy_action1_front_layout_->addWidget(deploy_action1_front_up_button_);
deploy_action1_front_layout_->addLayout(deploy_action1_front_servos_layout_);
deploy_action1_front_layout_->addWidget(deploy_action1_front_down_button_);
deploy_action1_back_layout_ = new QVBoxLayout;
deploy_action1_back_label_ = new QLabel("Back Action 1");
deploy_action1_back_up_button_ = new QPushButton("Up");
deploy_action1_back_down_button_ = new QPushButton("Down");
connect(deploy_action1_back_up_button_, &QPushButton::clicked, this,
[this]()
{
ActionExec action_exec;
action_exec.action = ActionExec::BACK_UP;
action_exec.mission = {ActionExec::BACK_UP_STEP};
action_exec_pub_->publish(action_exec);
});
connect(deploy_action1_back_down_button_, &QPushButton::clicked, this,
[this]()
{
ActionExec action_exec;
action_exec.action = ActionExec::BACK_DOWN;
action_exec.mission = {ActionExec::BACK_DOWN_STEP};
action_exec_pub_->publish(action_exec);
});
deploy_action1_back_servos_layout_ = new QHBoxLayout;
deploy_action1_back_servo1_button_ = new QPushButton("Servo 1");
deploy_action1_back_servo2_button_ = new QPushButton("Servo 2");
deploy_action1_back_servo3_button_ = new QPushButton("Servo 3");
deploy_action1_back_servo4_button_ = new QPushButton("Servo 4");
connect(deploy_action1_back_servo1_button_, &QPushButton::clicked, this,
[this]()
{
deploy_action1_back_servo1_state_ = !deploy_action1_back_servo1_state_;
ActionExec action_exec;
action_exec.action = deploy_action1_back_servo1_state_
? ActionExec::BACK_TAKE_1 : ActionExec::BACK_FREE_1;
action_exec.mission = {deploy_action1_back_servo1_state_
? ActionExec::BACK_TAKE_1_STEP : ActionExec::BACK_FREE_1_STEP};
action_exec_pub_->publish(action_exec);
});
connect(deploy_action1_back_servo2_button_, &QPushButton::clicked, this,
[this]()
{
deploy_action1_back_servo2_state_ = !deploy_action1_back_servo2_state_;
ActionExec action_exec;
action_exec.action = deploy_action1_back_servo2_state_
? ActionExec::BACK_TAKE_2 : ActionExec::BACK_FREE_2;
action_exec.mission = {deploy_action1_back_servo2_state_
? ActionExec::BACK_TAKE_2_STEP : ActionExec::BACK_FREE_2_STEP};
action_exec_pub_->publish(action_exec);
});
connect(deploy_action1_back_servo3_button_, &QPushButton::clicked, this,
[this]()
{
deploy_action1_back_servo3_state_ = !deploy_action1_back_servo3_state_;
ActionExec action_exec;
action_exec.action = deploy_action1_back_servo3_state_
? ActionExec::BACK_TAKE_3 : ActionExec::BACK_FREE_3;
action_exec.mission = {deploy_action1_back_servo3_state_
? ActionExec::BACK_TAKE_3_STEP : ActionExec::BACK_FREE_3_STEP};
action_exec_pub_->publish(action_exec);
});
connect(deploy_action1_back_servo4_button_, &QPushButton::clicked, this,
[this]()
{
deploy_action1_back_servo4_state_ = !deploy_action1_back_servo4_state_;
ActionExec action_exec;
action_exec.action = deploy_action1_back_servo4_state_
? ActionExec::BACK_TAKE_4 : ActionExec::BACK_FREE_4;
action_exec.mission = {deploy_action1_back_servo4_state_
? ActionExec::BACK_TAKE_4_STEP : ActionExec::BACK_FREE_4_STEP};
action_exec_pub_->publish(action_exec);
});
deploy_action1_back_servos_layout_->addWidget(deploy_action1_back_servo1_button_);
deploy_action1_back_servos_layout_->addWidget(deploy_action1_back_servo2_button_);
deploy_action1_back_servos_layout_->addWidget(deploy_action1_back_servo3_button_);
deploy_action1_back_servos_layout_->addWidget(deploy_action1_back_servo4_button_);
deploy_action1_back_layout_->addWidget(deploy_action1_back_label_);
deploy_action1_back_layout_->addWidget(deploy_action1_back_up_button_);
deploy_action1_back_layout_->addLayout(deploy_action1_back_servos_layout_);
deploy_action1_back_layout_->addWidget(deploy_action1_back_down_button_);
deploy_action1_layout_->addLayout(deploy_action1_front_layout_);
deploy_action1_layout_->addLayout(deploy_action1_back_layout_);
deploy_action2_layout_ = new QVBoxLayout;
deploy_action2_label_ = new QLabel("Thermometrer");
deploy_action2_button_ = new QPushButton("Deploy");
undeploy_action2_button_ = new QPushButton("Undeploy");
connect(deploy_action2_button_, &QPushButton::clicked, this,
[this]()
{
ActionExec action_exec;
action_exec.action = ActionExec::THERMO_DEPLOY;
action_exec.mission = {ActionExec::THERMO_DEPLOY_STEP};
action_exec_pub_->publish(action_exec);
});
connect(undeploy_action2_button_, &QPushButton::clicked, this,
[this]()
{
ActionExec action_exec;
action_exec.action = ActionExec::THERMO_UNDEPLOY;
action_exec.mission = {ActionExec::THERMO_UNDEPLOY_STEP};
action_exec_pub_->publish(action_exec);
});
deploy_action2_layout_->addWidget(deploy_action2_label_);
deploy_action2_layout_->addWidget(deploy_action2_button_);
deploy_action2_layout_->addWidget(undeploy_action2_button_);
main_layout_->addWidget(deploy_max_button_);
main_layout_->addLayout(deploy_action1_layout_);
main_layout_->addLayout(deploy_action2_layout_);
}
ActionPage::~ActionPage()
= default;
}

77
src/modelec_interfaces/msg/Action/ActionExec.msg
View File

@@ -1,64 +1,23 @@
string DELIMITER = ";"
# Action
int32 NONE = 0
int32 FRONT_UP = 1
int32 FRONT_DOWN = 2
int32 FRONT_TAKE_1 = 10
int32 FRONT_FREE_1 = 11
int32 FRONT_TAKE_2 = 20
int32 FRONT_FREE_2 = 21
int32 FRONT_TAKE_3 = 30
int32 FRONT_FREE_3 = 31
int32 FRONT_TAKE_4 = 40
int32 FRONT_FREE_4 = 41
int32 BACK_UP = 101
int32 BACK_DOWN = 102
int32 BACK_TAKE_1 = 110
int32 BACK_FREE_1 = 111
int32 BACK_TAKE_2 = 120
int32 BACK_FREE_2 = 121
int32 BACK_TAKE_3 = 130
int32 BACK_FREE_3 = 131
int32 BACK_TAKE_4 = 140
int32 BACK_FREE_4 = 141
int32 THERMO_DEPLOY = 200
int32 THERMO_UNDEPLOY = 201
int32 MAX_DEPLOY = 99
string UP = "UP"
string DOWN = "DOWN"
string TAKE = "TAKE"
string FREE = "FREE"
string THERMO_DEPLOY = "THERMO_DEPLOY"
string THERMO_UNDEPLOY = "THERMO_UNDEPLOY"
string MAX_DEPLOY = "MAX_DEPLOY"
# Step
int32 FRONT_UP_STEP = 1
int32 FRONT_DOWN_STEP = 2
int32 FRONT_TAKE_1_STEP = 10
int32 FRONT_FREE_1_STEP = 11
int32 FRONT_TAKE_2_STEP = 20
int32 FRONT_FREE_2_STEP = 21
int32 FRONT_TAKE_3_STEP = 30
int32 FRONT_FREE_3_STEP = 31
int32 FRONT_TAKE_4_STEP = 40
int32 FRONT_FREE_4_STEP = 41
int32 BACK_UP_STEP = 101
int32 BACK_DOWN_STEP = 102
int32 BACK_TAKE_1_STEP = 110
int32 BACK_FREE_1_STEP = 111
int32 BACK_TAKE_2_STEP = 120
int32 BACK_FREE_2_STEP = 121
int32 BACK_TAKE_3_STEP = 130
int32 BACK_FREE_3_STEP = 131
int32 BACK_TAKE_4_STEP = 140
int32 BACK_FREE_4_STEP = 141
int32 THERMO_DEPLOY_STEP = 200
int32 THERMO_UNDEPLOY_STEP = 201
int32 DONE_STEP = 0
int32 UP_STEP = 1
int32 DOWN_STEP = 2
int32 TAKE_STEP = 3
int32 FREE_STEP = 4
int32 THERMO_DEPLOY_STEP = 5
int32 THERMO_UNDEPLOY_STEP = 6
int32 MAX_DEPLOY_STEP = 99
int32[] mission
int32 action
int32[] steps
string action

23
src/modelec_strat/CMakeLists.txt
View File

@@ -15,7 +15,28 @@ 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/missions/go_home_mission.cpp src/missions/take_mission.cpp src/missions/free_mission.cpp src/obstacle/obstacle.cpp src/obstacle/box.cpp src/deposite_zone.cpp)
set(strat_fsm_sources
src/strat_fms.cpp
src/navigation_helper.cpp
src/pathfinding.cpp
src/action_executor.cpp
src/action/up_action.cpp
src/action/down_action.cpp
src/action/free_action.cpp
src/action/take_action.cpp
src/missions/go_home_mission.cpp
src/missions/take_mission.cpp
src/missions/free_mission.cpp
src/obstacle/obstacle.cpp
src/obstacle/box.cpp
src/deposite_zone.cpp
)
add_executable(strat_fsm ${strat_fsm_sources})
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

74
src/modelec_strat/include/modelec_strat/action/base_action.hpp Normal file
View File

@@ -0,0 +1,74 @@
#pragma once
#include <memory>
#include <string>
#include <unordered_map>
#include <functional>
#include <vector>
#include <modelec_interfaces/msg/action_exec.hpp>
#include <modelec_interfaces/msg/action_servo_timed_array.hpp>
#include <modelec_interfaces/msg/action_servo_timed.hpp>
#include <modelec_interfaces/msg/action_servo_pos_array.hpp>
#include <modelec_interfaces/msg/action_servo_pos.hpp>
namespace Modelec
{
class ActionExecutor;
using ActionExec = modelec_interfaces::msg::ActionExec;
using ActionServoTimedArray = modelec_interfaces::msg::ActionServoTimedArray;
using ActionServoTimed = modelec_interfaces::msg::ActionServoTimed;
using ActionServoPosArray = modelec_interfaces::msg::ActionServoPosArray;
using ActionServoPos = modelec_interfaces::msg::ActionServoPos;
class BaseAction
{
public:
using Ptr = std::shared_ptr<BaseAction>;
using FactoryFn =
std::function<Ptr(const std::shared_ptr<ActionExecutor>&)>;
static std::unordered_map<std::string, FactoryFn>& Registry()
{
static std::unordered_map<std::string, FactoryFn> registry;
return registry;
};
BaseAction(const std::shared_ptr<ActionExecutor>& action_executor)
: action_executor_(action_executor)
{
}
virtual ~BaseAction() = default;
virtual void Execute() = 0;
virtual void Next() = 0;
virtual bool IsDone() const { return done_; }
virtual void Init(const std::vector<std::string>& params) = 0;
// static constexpr std::string_view Name = "BaseAction";
static Ptr CreateAction(
const std::string& action_name,
const std::shared_ptr<ActionExecutor>& action_executor);
protected:
std::shared_ptr<ActionExecutor> action_executor_;
bool done_ = false;
};
inline BaseAction::Ptr BaseAction::CreateAction(const std::string& action_name,
const std::shared_ptr<ActionExecutor>& action_executor)
{
auto& registry = Registry();
auto it = registry.find(action_name);
if (it != registry.end())
{
return it->second(action_executor);
}
return nullptr;
}
}

37
src/modelec_strat/include/modelec_strat/action/down_action.hpp Normal file
View File

@@ -0,0 +1,37 @@
#pragma once
#include <queue>
#include <modelec_strat/action/base_action.hpp>
namespace Modelec
{
class DownAction : public BaseAction
{
public:
DownAction(const std::shared_ptr<ActionExecutor>& action_executor);
DownAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front);
void Execute() override;
void Next() override;
void Init(const std::vector<std::string>& params) override;
void SetFront(bool front);
inline static const std::string Name = ActionExec::DOWN;
private:
bool front_;
std::queue<int> steps_;
inline static const bool registered_ =
[]()
{
BaseAction::Registry()[Name] =
[](const std::shared_ptr<ActionExecutor>& exec)
{
return std::make_shared<DownAction>(exec);
};
return true;
}();
};
}

39
src/modelec_strat/include/modelec_strat/action/free_action.hpp Normal file
View File

@@ -0,0 +1,39 @@
#pragma once
#include <queue>
#include <modelec_strat/action/base_action.hpp>
namespace Modelec
{
class FreeAction : public BaseAction
{
public:
FreeAction(const std::shared_ptr<ActionExecutor>& action_executor);
FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front, int n);
void Execute() override;
void Next() override;
void Init(const std::vector<std::string>& params) override;
void SetFront(bool front);
void SetN(int n);
inline static const std::string Name = ActionExec::FREE;
private:
bool front_;
int n_;
std::queue<int> steps_;
inline static const bool registered_ =
[]()
{
BaseAction::Registry()[Name] =
[](const std::shared_ptr<ActionExecutor>& exec)
{
return std::make_shared<FreeAction>(exec);
};
return true;
}();
};
}

39
src/modelec_strat/include/modelec_strat/action/take_action.hpp Normal file
View File

@@ -0,0 +1,39 @@
#pragma once
#include <queue>
#include <modelec_strat/action/base_action.hpp>
namespace Modelec
{
class TakeAction : public BaseAction
{
public:
TakeAction(const std::shared_ptr<ActionExecutor>& action_executor);
TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front, int n);
void Execute() override;
void Next() override;
void Init(const std::vector<std::string>& params) override;
void SetFront(bool front);
void SetN(int n);
inline static const std::string Name = ActionExec::TAKE;
private:
bool front_;
int n_;
std::queue<int> steps_;
inline static const bool registered_ =
[]()
{
BaseAction::Registry()[Name] =
[](const std::shared_ptr<ActionExecutor>& exec)
{
return std::make_shared<TakeAction>(exec);
};
return true;
}();
};
}

37
src/modelec_strat/include/modelec_strat/action/up_action.hpp Normal file
View File

@@ -0,0 +1,37 @@
#pragma once
#include <queue>
#include <modelec_strat/action/base_action.hpp>
namespace Modelec
{
class UPAction : public BaseAction
{
public:
UPAction(const std::shared_ptr<ActionExecutor>& action_executor);
UPAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front);
void Execute() override;
void Next() override;
void Init(const std::vector<std::string>& params) override;
void SetFront(bool front);
inline static const std::string Name = ActionExec::UP;
private:
bool front_;
std::queue<int> steps_;
inline static const bool registered_ =
[]()
{
BaseAction::Registry()[Name] =
[](const std::shared_ptr<ActionExecutor>& exec)
{
return std::make_shared<UPAction>(exec);
};
return true;
}();
};
}

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

@@ -9,73 +9,19 @@
namespace Modelec
{
class ActionExecutor
class BaseAction;
class ActionExecutor : public std::enable_shared_from_this<ActionExecutor>
{
public:
enum Action
{
NONE = 0,
NONE,
UP,
DOWN,
FRONT_UP = 1,
FRONT_DOWN = 2,
FRONT_TAKE_1 = 10,
FRONT_FREE_1 = 11,
FRONT_TAKE_2 = 20,
FRONT_FREE_2 = 21,
FRONT_TAKE_3 = 30,
FRONT_FREE_3 = 31,
FRONT_TAKE_4 = 40,
FRONT_FREE_4 = 41,
BACK_UP = 101,
BACK_DOWN = 102,
BACK_TAKE_1 = 110,
BACK_FREE_1 = 111,
BACK_TAKE_2 = 120,
BACK_FREE_2 = 121,
BACK_TAKE_3 = 130,
BACK_FREE_3 = 131,
BACK_TAKE_4 = 140,
BACK_FREE_4 = 141,
THERMO_DEPLOY = 200,
THERMO_UNDEPLOY = 201,
MAX_DEPLOY = 99
};
enum Step
{
FRONT_UP_STEP = 1,
FRONT_DOWN_STEP = 2,
FRONT_TAKE_1_STEP = 10,
FRONT_FREE_1_STEP = 11,
FRONT_TAKE_2_STEP = 20,
FRONT_FREE_2_STEP = 21,
FRONT_TAKE_3_STEP = 30,
FRONT_FREE_3_STEP = 31,
FRONT_TAKE_4_STEP = 40,
FRONT_FREE_4_STEP = 41,
BACK_UP_STEP = 101,
BACK_DOWN_STEP = 102,
BACK_TAKE_1_STEP = 110,
BACK_FREE_1_STEP = 111,
BACK_TAKE_2_STEP = 120,
BACK_FREE_2_STEP = 121,
BACK_TAKE_3_STEP = 130,
BACK_FREE_3_STEP = 131,
BACK_TAKE_4_STEP = 140,
BACK_FREE_4_STEP = 141,
THERMO_DEPLOY_STEP = 200,
THERMO_UNDEPLOY_STEP = 201,
MAX_DEPLOY_STEP = 99,
TAKE,
FREE
};
ActionExecutor();
@@ -94,10 +40,14 @@ namespace Modelec
void Up(bool front = true);
void Take(bool front = true, int n = 1);
void Take(bool front = true, int n = 0);
void Free(bool front = true, int n = 1);
void Free(bool front = true, int n = 0);
void MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg);
void MoveServo(const modelec_interfaces::msg::ActionServoPosArray& msg);
protected:
rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_move_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionServoPosArray>::SharedPtr servo_move_pub_;
@@ -111,9 +61,7 @@ namespace Modelec
rclcpp::Subscription<modelec_interfaces::msg::ActionExec>::SharedPtr action_exec_sub_;
Action action_ = NONE;
std::queue<Step> step_;
std::shared_ptr<BaseAction> action_;
bool action_done_ = true;
int step_running_ = 0;

82
src/modelec_strat/src/action/down_action.cpp Normal file
View File

@@ -0,0 +1,82 @@
#include <modelec_strat/action/down_action.hpp>
#include "modelec_strat/action_executor.hpp"
Modelec::DownAction::DownAction(const std::shared_ptr<ActionExecutor>& action_executor) : BaseAction(action_executor)
{
steps_.push(ActionExec::DOWN_STEP);
steps_.push(ActionExec::DONE_STEP);
}
Modelec::DownAction::DownAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front) : DownAction(action_executor)
{
front_ = front;
}
void Modelec::DownAction::Execute()
{
}
void Modelec::DownAction::Next()
{
if (steps_.empty())
{
done_ = true;
return;
}
auto step = steps_.front();
steps_.pop();
switch (step)
{
case ActionExec::DOWN_STEP:
{
ActionServoTimedArray msg;
msg.items.resize(4);
msg.items[0].id = front_ ? 0 : 8;
msg.items[0].start_angle = front_ ? 1.49 : 0;
msg.items[0].end_angle = front_ ? 0 : 0;
msg.items[0].duration_s = 1;
msg.items[1].id = front_ ? 1 : 9;
msg.items[1].start_angle = front_ ? 1.5 : 0;
msg.items[1].end_angle = front_ ? 3 : 0;
msg.items[1].duration_s = 1;
msg.items[2].id = front_ ? 2 : 10;
msg.items[2].start_angle = front_ ? 3 : 0;
msg.items[2].end_angle = front_ ? 1.45 : 0;
msg.items[2].duration_s = 1;
msg.items[3].id = front_ ? 3 : 11;
msg.items[3].start_angle = front_ ? 0 : 0;
msg.items[3].end_angle = front_ ? 1.6 : 0;
msg.items[3].duration_s = 1;
action_executor_->MoveServoTimed(msg);
}
break;
case ActionExec::DONE_STEP:
{
done_ = true;
}
break;
default:
break;
}
}
void Modelec::DownAction::Init(const std::vector<std::string>& params)
{
if (!params.empty())
{
SetFront(params[1] == "1" || params[1] == "true" || params[1] == "front");
}
}
void Modelec::DownAction::SetFront(bool front)
{
front_ = front;
}

74
src/modelec_strat/src/action/free_action.cpp Normal file
View File

@@ -0,0 +1,74 @@
#include <modelec_strat/action/free_action.hpp>
#include "modelec_strat/action_executor.hpp"
Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor) : BaseAction(action_executor)
{
steps_.push(ActionExec::FREE_STEP);
steps_.push(ActionExec::DONE_STEP);
}
Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front, int n) : FreeAction(action_executor)
{
front_ = front;
n_ = n;
}
void Modelec::FreeAction::Execute()
{
}
void Modelec::FreeAction::Next()
{
if (steps_.empty())
{
done_ = true;
return;
}
auto step = steps_.front();
steps_.pop();
switch (step)
{
case ActionExec::FREE_STEP:
{
modelec_interfaces::msg::ActionServoTimedArray msg;
msg.items.resize(1);
msg.items[0].id = n_ + (front_ ? 3 : 11);
msg.items[0].start_angle = front_ ? 3 : 0;
msg.items[0].end_angle = front_ ? 0 : 0;
msg.items[0].duration_s = 0.5;
action_executor_->MoveServoTimed(msg);
}
break;
case ActionExec::DONE_STEP:
{
done_ = true;
}
break;
default:
break;
}
}
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]));
}
}
void Modelec::FreeAction::SetFront(bool front)
{
front_ = front;
}
void Modelec::FreeAction::SetN(int n)
{
n_ = n;
}

74
src/modelec_strat/src/action/take_action.cpp Normal file
View File

@@ -0,0 +1,74 @@
#include <modelec_strat/action/take_action.hpp>
#include "modelec_strat/action_executor.hpp"
Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor) : BaseAction(action_executor)
{
steps_.push(ActionExec::TAKE_STEP);
steps_.push(ActionExec::DONE_STEP);
}
Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front, int n) : TakeAction(action_executor)
{
front_ = front;
n_ = n;
}
void Modelec::TakeAction::Execute()
{
}
void Modelec::TakeAction::Next()
{
if (steps_.empty())
{
done_ = true;
return;
}
auto step = steps_.front();
steps_.pop();
switch (step)
{
case ActionExec::TAKE_STEP:
{
modelec_interfaces::msg::ActionServoTimedArray msg;
msg.items.resize(1);
msg.items[0].id = n_ + (front_ ? 3 : 11);
msg.items[0].start_angle = front_ ? 0 : 0;
msg.items[0].end_angle = front_ ? 3 : 0;
msg.items[0].duration_s = 0.5;
action_executor_->MoveServoTimed(msg);
}
break;
case ActionExec::DONE_STEP:
{
done_ = true;
}
break;
default:
break;
}
}
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]));
}
}
void Modelec::TakeAction::SetFront(bool front)
{
front_ = front;
}
void Modelec::TakeAction::SetN(int n)
{
n_ = n;
}

82
src/modelec_strat/src/action/up_action.cpp Normal file
View File

@@ -0,0 +1,82 @@
#include <modelec_strat/action/up_action.hpp>
#include "modelec_strat/action_executor.hpp"
Modelec::UPAction::UPAction(const std::shared_ptr<ActionExecutor>& action_executor) : BaseAction(action_executor)
{
steps_.push(ActionExec::UP_STEP);
steps_.push(ActionExec::DONE_STEP);
}
Modelec::UPAction::UPAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front) : UPAction(action_executor)
{
front_ = front;
}
void Modelec::UPAction::Execute()
{
}
void Modelec::UPAction::Next()
{
if (steps_.empty())
{
done_ = true;
return;
}
auto step = steps_.front();
steps_.pop();
switch (step)
{
case ActionExec::UP_STEP:
{
ActionServoTimedArray msg;
msg.items.resize(4);
msg.items[0].id = front_ ? 0 : 8;
msg.items[0].start_angle = front_ ? 0 : 0;
msg.items[0].end_angle = front_ ? 1.49 : 0;
msg.items[0].duration_s = 1;
msg.items[1].id = front_ ? 1 : 9;
msg.items[1].start_angle = front_ ? 3 : 0;
msg.items[1].end_angle = front_ ? 1.5 : 0;
msg.items[1].duration_s = 1;
msg.items[2].id = front_ ? 2 : 10;
msg.items[2].start_angle = front_ ? 1.45 : 0;
msg.items[2].end_angle = front_ ? 3 : 0;
msg.items[2].duration_s = 1;
msg.items[3].id = front_ ? 3 : 11;
msg.items[3].start_angle = front_ ? 1.6 : 0;
msg.items[3].end_angle = front_ ? 0 : 0;
msg.items[3].duration_s = 1;
action_executor_->MoveServoTimed(msg);
}
break;
case ActionExec::DONE_STEP:
{
done_ = true;
}
break;
default:
break;
}
}
void Modelec::UPAction::Init(const std::vector<std::string>& params)
{
if (!params.empty())
{
SetFront(params[1] == "1" || params[1] == "true" || params[1] == "front");
}
}
void Modelec::UPAction::SetFront(bool front)
{
front_ = front;
}

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

@@ -1,5 +1,11 @@
#include <modelec_strat/action_executor.hpp>
#include "modelec_strat/action/down_action.hpp"
#include "modelec_strat/action/up_action.hpp"
#include "modelec_strat/action/free_action.hpp"
#include "modelec_strat/action/take_action.hpp"
#include "modelec_utils/utils.hpp"
namespace Modelec
{
ActionExecutor::ActionExecutor()
@@ -20,10 +26,12 @@ namespace Modelec
});
servo_move_res_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionServoPosArray>(
"/action/move/servo/res", 10, [this](const modelec_interfaces::msg::ActionServoPosArray::SharedPtr msg)
"/action/move/servo/res", 10, [this](const modelec_interfaces::msg::ActionServoPosArray::SharedPtr)
{
step_running_ -= msg->items.size();
Update();
// BUG
// if ServoTimed is called this one will trigger so step_running_ will be decremented at the beginning of the Timed one
// step_running_ -= msg->items.size();
// Update();
});
relay_move_res_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionRelayStateArray>(
@@ -36,11 +44,28 @@ namespace Modelec
action_exec_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionExec>(
"/action/exec", 10, [this](const modelec_interfaces::msg::ActionExec::SharedPtr msg)
{
action_ = static_cast<Action>(msg->action);
for (const auto& step : msg->mission)
auto token = split(msg->action, ActionExec::DELIMITER[0]);
if (token.size() == 0)
{
step_.push(static_cast<Step>(step));
RCLCPP_WARN(
node_->get_logger(),
"Empty action received");
return;
}
action_ = BaseAction::CreateAction(
token[0],
shared_from_this());
if (action_ == nullptr)
{
RCLCPP_WARN(
node_->get_logger(),
"Unknown action: %s",
msg->action.c_str());
return;
}
action_->Init(token);
action_done_ = false;
step_running_ = 0;
Update();
@@ -68,116 +93,27 @@ namespace Modelec
void ActionExecutor::Update()
{
if (step_.empty())
if (action_ != nullptr && !action_->IsDone() && step_running_ <= 0)
{
action_ = NONE;
action_done_ = true;
return;
}
RCLCPP_DEBUG(
node_->get_logger(),
"ActionExecutor Update: Executing next step of action, step_running_=%d", step_running_);
if (step_running_ <= 0)
{
switch (step_.front())
action_->Next();
if (action_->IsDone())
{
case FRONT_DOWN_STEP:
{
modelec_interfaces::msg::ActionServoTimedArray msg;
msg.items.resize(4);
msg.items[0].id = 0;
msg.items[0].start_angle = 1.49;
msg.items[0].end_angle = 0;
msg.items[0].duration_s = 1;
msg.items[1].id = 1;
msg.items[1].start_angle = 1.5;
msg.items[1].end_angle = 3;
msg.items[1].duration_s = 1;
msg.items[2].id = 4;
msg.items[2].start_angle = 3;
msg.items[2].end_angle = 1.57;
msg.items[2].duration_s = 1;
msg.items[3].id = 5;
msg.items[3].start_angle = 0;
msg.items[3].end_angle = 1.4;
msg.items[3].duration_s = 1;
servo_timed_move_pub_->publish(msg);
step_running_ = msg.items.size();
}
break;
case FRONT_UP_STEP:
{
modelec_interfaces::msg::ActionServoTimedArray msg;
msg.items.resize(4);
msg.items[0].id = 0;
msg.items[0].start_angle = 0;
msg.items[0].end_angle = 1.49;
msg.items[0].duration_s = 1;
msg.items[1].id = 1;
msg.items[1].start_angle = 3;
msg.items[1].end_angle = 1.5;
msg.items[1].duration_s = 1;
msg.items[2].id = 4;
msg.items[2].start_angle = 1.57;
msg.items[2].end_angle = 3;
msg.items[2].duration_s = 1;
msg.items[3].id = 5;
msg.items[3].start_angle = 1.4;
msg.items[3].end_angle = 0;
msg.items[3].duration_s = 1;
servo_timed_move_pub_->publish(msg);
step_running_ = msg.items.size();
}
break;
case FRONT_TAKE_1_STEP:
{
modelec_interfaces::msg::ActionServoPosArray msg;
msg.items.resize(1);
msg.items[0].id = 2;
msg.items[0].angle = 0;
servo_move_pub_->publish(msg);
step_running_ = msg.items.size();
}
break;
case FRONT_FREE_1_STEP:
{
modelec_interfaces::msg::ActionServoPosArray msg;
msg.items.resize(1);
msg.items[0].id = 2;
msg.items[0].angle = 3;
servo_move_pub_->publish(msg);
step_running_ = msg.items.size();
}
break;
case MAX_DEPLOY_STEP:
{
}
break;
default:
return;
action_done_ = true;
action_ = nullptr;
}
}
else if (action_ != nullptr && action_->IsDone())
{
RCLCPP_DEBUG(
node_->get_logger(),
"ActionExecutor Update: Action is done, step_running_=%d", step_running_);
step_.pop();
action_done_ = true;
action_ = nullptr;
}
}
@@ -185,22 +121,16 @@ namespace Modelec
{
action_done_ = true;
step_running_ = 0;
while (!step_.empty())
{
step_.pop();
}
action_ = NONE;
action_ = nullptr;
}
void ActionExecutor::Down(bool front) {
if (action_done_)
{
action_ = front ? FRONT_DOWN : BACK_DOWN;
action_ = std::make_shared<DownAction>(shared_from_this(), front);
action_done_ = false;
step_running_ = 0;
step_.push(front ? FRONT_DOWN_STEP : BACK_DOWN_STEP);
Update();
}
}
@@ -208,12 +138,10 @@ namespace Modelec
void ActionExecutor::Up(bool front) {
if (action_done_)
{
action_ = front ? FRONT_UP : BACK_UP;
action_ = std::make_shared<UPAction>(shared_from_this(), front);
action_done_ = false;
step_running_ = 0;
step_.push(front ? FRONT_UP_STEP : BACK_UP_STEP);
Update();
}
}
@@ -221,12 +149,10 @@ namespace Modelec
void ActionExecutor::Take(bool front, int n) {
if (action_done_)
{
action_ = static_cast<Action>(n * 10 + (front ? 0 : 100));
action_ = std::make_shared<TakeAction>(shared_from_this(), front, n);
action_done_ = false;
step_running_ = 0;
step_.push(static_cast<Step>(n * 10 + (front ? 0 : 100)));
Update();
}
}
@@ -234,13 +160,29 @@ namespace Modelec
void ActionExecutor::Free(bool front, int n) {
if (action_done_)
{
action_ = static_cast<Action>(1 + n * 10 + (front ? 0 : 100));
action_ = std::make_shared<FreeAction>(shared_from_this(), front, n);
action_done_ = false;
step_running_ = 0;
step_.push(static_cast<Step>(1 + n * 10 + (front ? 0 : 100)));
Update();
}
}
void ActionExecutor::MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg)
{
servo_timed_move_pub_->publish(msg);
step_running_ += msg.items.size();
RCLCPP_DEBUG(
node_->get_logger(),
"ActionExecutor MoveServoTimed called with %d items, step_running_=%d",
static_cast<int>(msg.items.size()),
step_running_);
}
void ActionExecutor::MoveServo(const modelec_interfaces::msg::ActionServoPosArray& msg)
{
servo_move_pub_->publish(msg);
step_running_ += msg.items.size();
}
}

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

@@ -24,7 +24,7 @@ namespace Modelec {
if (!nav_->HasArrived())
{
if ((node_->now() - go_timeout_).seconds() < 10)
if ((node_->now() - go_timeout_).seconds() < 20)
{
return;
}