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

real map page + score + some bug fix

Browse Source
This commit is contained in:
acki
2025-05-07 04:48:21 -04:00
parent 698b420e6f
commit c7e4dc18e2
26 changed files with 820 additions and 224 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/modelec_com/include/modelec_com/pcb_odo_interface.hpp
View File

@@ -142,7 +142,7 @@ public:
void GetToF(const int &tof) const;
void SetRobotPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) const;
void SetRobotPos(long x, long y, long theta) const;
void SetRobotPos(long x, long y, double theta) const;
void AddWaypoint(modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) const;
void AddWaypoint(int index, bool IsStopPoint, long x, long y, double theta) const;

2
src/modelec_com/src/pcb_odo_interface.cpp
View File

@@ -578,7 +578,7 @@ namespace Modelec
SetRobotPos(msg->x, msg->y, msg->theta);
}
void PCBOdoInterface::SetRobotPos(const long x, const long y, const long theta) const
void PCBOdoInterface::SetRobotPos(const long x, const long y, const double theta) const
{
std::vector<std::string> data = {
std::to_string(x),

7
src/modelec_gui/CMakeLists.txt
View File

@@ -33,15 +33,18 @@ add_executable(modelec_gui
src/main.cpp
src/modelec_gui.cpp
src/pages/test_page.cpp
src/pages/odo_page.cpp
src/pages/home_page.cpp
src/pages/test_map_page.cpp
src/pages/map_page.cpp
include/modelec_gui/modelec_gui.hpp
include/modelec_gui/pages/test_page.hpp
include/modelec_gui/pages/odo_page.hpp
include/modelec_gui/pages/home_page.hpp
include/modelec_gui/pages/test_map_page.hpp
include/modelec_gui/pages/map_page.hpp
)
ament_target_dependencies(modelec_gui
rclcpp
std_msgs

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

@@ -4,7 +4,10 @@
#include <QMainWindow>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <modelec_gui/pages/home_page.hpp>
#include <modelec_gui/pages/test_map_page.hpp>
#include <modelec_gui/pages/odo_page.hpp>
#include <modelec_gui/pages/map_page.hpp>
namespace ModelecGUI
{
@@ -24,8 +27,13 @@ namespace ModelecGUI
QStackedWidget *stackedWidget;
void setupMenu();
HomePage* home_page_;
TestMapPage* test_map_page_;
OdoPage* odo_page_;
MapPage* map_page_;
QAction* home_action_;
QAction* test_action_;
QAction* odo_action_;
QAction* map_action_;
QAction* exit_action_;

49
src/modelec_gui/include/modelec_gui/pages/home_page.hpp
View File

@@ -1,16 +1,45 @@
#pragma once
#include <QWidget>
#include <QLabel>
#include <QHBoxLayout>
#include <QPushButton>
#include <QSvgRenderer>
namespace ModelecGUI {
class HomePage : public QWidget
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/int8.hpp>
namespace ModelecGUI
{
Q_OBJECT
public:
HomePage(QWidget *parent = nullptr);
class HomePage : public QWidget
{
Q_OBJECT
protected:
QLayout* m_layout;
};
}
public:
HomePage(rclcpp::Node::SharedPtr node, QWidget* parent = nullptr);
public slots:
void onYellowButtonClicked();
void onBlueButtonClicked();
signals:
void TeamChoose();
protected:
void paintEvent(QPaintEvent*) override;
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<std_msgs::msg::Int8>::SharedPtr team_publisher_;
QVBoxLayout* v_layout_;
QHBoxLayout* h_layout_;
QPushButton* blue_button_;
QPushButton* yellow_button_;
QSvgRenderer* renderer_;
};
}

26
src/modelec_gui/include/modelec_gui/pages/map_page.hpp
View File

@@ -33,23 +33,9 @@ namespace ModelecGUI {
rclcpp::Node::SharedPtr get_node() const { return node_; }
void setPlaymatGrid();
void setPlaymatMap();
void toggleShowObstacle();
protected:
void paintEvent(QPaintEvent*) override;
void mousePressEvent(QMouseEvent* event) override;
void mouseReleaseEvent(QMouseEvent* event) override;
void mouseDoubleClickEvent(QMouseEvent* event) override;
void mouseMoveEvent(QMouseEvent* event) override;
void onOdometryReceived(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
void OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg);
@@ -61,15 +47,11 @@ namespace ModelecGUI {
QVBoxLayout* v_layout;
QHBoxLayout* h_layout;
QPushButton* tirette_button_;
QLabel* timer_label_;
QLabel* score_label_;
modelec_interfaces::msg::OdometryPos robotPos;
std::vector<QPoint> qpoints;
//std::vector<modelec_interfaces::msg::OdometryAddWaypoint> points;
modelec_interfaces::msg::OdometryPos go_to_point;
bool lastWapointWasEnd = true;
@@ -83,25 +65,25 @@ namespace ModelecGUI {
int robot_width_ = 0;
int enemy_length_ = 0;
int enemy_width_ = 0;
int score_ = 0;
float ratioBetweenMapAndWidgetX_;
float ratioBetweenMapAndWidgetY_;
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr tirette_pub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryAddWaypoint>::SharedPtr add_waypoint_sub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr odometry_sub_;
rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr go_to_pub_;
rclcpp::Subscription<std_msgs::msg::Int64>::SharedPtr score_sub_;
rclcpp::Client<modelec_interfaces::srv::MapSize>::SharedPtr map_client_;
rclcpp::Subscription<modelec_interfaces::msg::Obstacle>::SharedPtr obstacle_added_sub_;
rclcpp::Subscription<modelec_interfaces::msg::Obstacle>::SharedPtr obstacle_removed_sub_;
rclcpp::Client<std_srvs::srv::Empty>::SharedPtr ask_map_obstacle_client_;
modelec_interfaces::msg::OdometryPos enemy_pos_;
rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_pub_;
bool hasEnemy = false;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_sub_;
rclcpp::Subscription<std_msgs::msg::Int64>::SharedPtr strat_start_sub_;
bool isGameStarted_ = false;

6
src/modelec_gui/include/modelec_gui/pages/test_page.hpp → src/modelec_gui/include/modelec_gui/pages/odo_page.hpp
View File

@@ -17,12 +17,12 @@
namespace ModelecGUI {
class TestPage : public QWidget
class OdoPage : public QWidget
{
Q_OBJECT
public:
TestPage(rclcpp::Node::SharedPtr node, QWidget *parent = nullptr);
~TestPage() override;
OdoPage(rclcpp::Node::SharedPtr node, QWidget *parent = nullptr);
~OdoPage() override;
rclcpp::Node::SharedPtr get_node() const { return node_; }

108
src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp Normal file
View File

@@ -0,0 +1,108 @@
#pragma once
#include <QWidget>
#include <QLabel>
#include <QPainter>
#include <QPushButton>
#include <QSvgRenderer>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <rclcpp/rclcpp.hpp>
#include <std_srvs/srv/empty.hpp>
#include <modelec_interfaces/msg/odometry_add_waypoint.hpp>
#include <modelec_interfaces/msg/odometry_pos.hpp>
#include <modelec_interfaces/srv/map.hpp>
#include <modelec_interfaces/srv/map_size.hpp>
#include <modelec_interfaces/msg/obstacle.hpp>
#include <modelec_interfaces/msg/odometry_waypoint_reach.hpp>
#include <modelec_interfaces/msg/strat_state.hpp>
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/int64.hpp>
namespace ModelecGUI {
class TestMapPage : public QWidget
{
Q_OBJECT
public:
TestMapPage(rclcpp::Node::SharedPtr node, QWidget *parent = nullptr);
rclcpp::Node::SharedPtr get_node() const { return node_; }
void setPlaymatGrid();
void setPlaymatMap();
void toggleShowObstacle();
protected:
void paintEvent(QPaintEvent*) override;
void mousePressEvent(QMouseEvent* event) override;
void onOdometryReceived(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
void OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg);
void resizeEvent(QResizeEvent* event) override;
QSvgRenderer* renderer;
QVBoxLayout* v_layout;
QHBoxLayout* h_layout;
QPushButton* tirette_button_;
QLabel* timer_label_;
QLabel* score_label_;
modelec_interfaces::msg::OdometryPos robotPos;
std::vector<QPoint> qpoints;
//std::vector<modelec_interfaces::msg::OdometryAddWaypoint> points;
modelec_interfaces::msg::OdometryPos go_to_point;
bool lastWapointWasEnd = true;
std::map<int, modelec_interfaces::msg::Obstacle> obstacle_;
bool show_obstacle_ = true;
int map_width_ = 0;
int map_height_ = 0;
int robot_length_ = 0;
int robot_width_ = 0;
int enemy_length_ = 0;
int enemy_width_ = 0;
float ratioBetweenMapAndWidgetX_;
float ratioBetweenMapAndWidgetY_;
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr tirette_pub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryAddWaypoint>::SharedPtr add_waypoint_sub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr odometry_sub_;
rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr go_to_pub_;
rclcpp::Client<modelec_interfaces::srv::MapSize>::SharedPtr map_client_;
rclcpp::Subscription<modelec_interfaces::msg::Obstacle>::SharedPtr obstacle_added_sub_;
rclcpp::Subscription<modelec_interfaces::msg::Obstacle>::SharedPtr obstacle_removed_sub_;
rclcpp::Client<std_srvs::srv::Empty>::SharedPtr ask_map_obstacle_client_;
modelec_interfaces::msg::OdometryPos enemy_pos_;
bool hasEnemy = false;
rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_pub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_sub_;
rclcpp::Subscription<std_msgs::msg::Int64>::SharedPtr strat_start_sub_;
bool isGameStarted_ = false;
long int start_time_ = 0;
rclcpp::Subscription<modelec_interfaces::msg::StratState>::SharedPtr strat_state_sub_;
};
}

33
src/modelec_gui/src/modelec_gui.cpp
View File

@@ -2,9 +2,7 @@
#include <rclcpp/rclcpp.hpp>
#include <QMenuBar>
#include <utility>
#include <modelec_gui/pages/home_page.hpp>
#include <modelec_gui/pages/map_page.hpp>
#include <modelec_gui/pages/test_page.hpp>
namespace ModelecGUI {
@@ -12,14 +10,25 @@ namespace ModelecGUI {
: QMainWindow(parent), node_(std::move(node)), stackedWidget(new QStackedWidget(this)) {
// Add pages to stack
stackedWidget->addWidget(new HomePage(this));
stackedWidget->addWidget(new TestPage(get_node(), this));
stackedWidget->addWidget(new MapPage(get_node(), this));
home_page_ = new HomePage(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);
stackedWidget->addWidget(home_page_);
stackedWidget->addWidget(odo_page_);
stackedWidget->addWidget(test_map_page_);
stackedWidget->addWidget(map_page_);
setCentralWidget(stackedWidget);
setupMenu();
resize(1200, 800);
connect(home_page_, &HomePage::TeamChoose, this, [this]()
{
stackedWidget->setCurrentIndex(3);
});
}
void ROS2QtGUI::setupMenu() {
@@ -28,7 +37,7 @@ namespace ModelecGUI {
QMenu *optionsMenu = menuBar->addMenu("View");
home_action_ = new QAction("Home", this);
test_action_ = new QAction("Test", this);
odo_action_ = new QAction("Odometrie", this);
map_action_ = new QAction("Map", this);
playmat_map_menu_ = new QMenu("playmat");
@@ -40,7 +49,7 @@ namespace ModelecGUI {
exit_action_ = new QAction("Exit", this);
optionsMenu->addAction(home_action_);
optionsMenu->addAction(test_action_);
optionsMenu->addAction(odo_action_);
optionsMenu->addSeparator();
optionsMenu->addAction(map_action_);
optionsMenu->addMenu(playmat_map_menu_);
@@ -54,7 +63,7 @@ namespace ModelecGUI {
stackedWidget->setCurrentIndex(0);
});
connect(test_action_, &QAction::triggered, this, [this]() {
connect(odo_action_, &QAction::triggered, this, [this]() {
stackedWidget->setCurrentIndex(1);
});
@@ -63,21 +72,21 @@ namespace ModelecGUI {
});
connect(playmat_map_, &QAction::triggered, this, [this]() {
auto map_page = dynamic_cast<MapPage *>(stackedWidget->currentWidget());
auto map_page = dynamic_cast<TestMapPage *>(stackedWidget->currentWidget());
if (map_page) {
map_page->setPlaymatMap();
}
});
connect(playmat_grid_, &QAction::triggered, this, [this]() {
auto map_page = dynamic_cast<MapPage *>(stackedWidget->currentWidget());
auto map_page = dynamic_cast<TestMapPage *>(stackedWidget->currentWidget());
if (map_page) {
map_page->setPlaymatGrid();
}
});
connect(toggle_show_obstacle_action_, &QAction::triggered, this, [this]() {
auto map_page = dynamic_cast<MapPage *>(stackedWidget->currentWidget());
auto map_page = dynamic_cast<TestMapPage *>(stackedWidget->currentWidget());
if (map_page)
{
map_page->toggleShowObstacle();

64
src/modelec_gui/src/pages/home_page.cpp
View File

@@ -1,17 +1,67 @@
#include <QPainter>
#include <modelec_gui/pages/home_page.hpp>
#include <QVBoxLayout>
namespace ModelecGUI
{
HomePage::HomePage(QWidget* parent) : QWidget(parent)
HomePage::HomePage(rclcpp::Node::SharedPtr node, QWidget* parent)
: QWidget(parent), node_(node),
renderer_(new QSvgRenderer(QString(":/img/playmat/2025_FINAL.svg"), this))
{
// label with default text
m_layout = new QVBoxLayout(this);
m_layout->addWidget(new QLabel("Welcome to Modelec GUI!"));
yellow_button_ = new QPushButton("Yellow", this);
blue_button_ = new QPushButton("Blue", this);
this->setLayout(m_layout);
yellow_button_->setStyleSheet("background-color: rgba(255, 255, 0, 128); border: none; color: black; font-size: 24px;");
blue_button_->setStyleSheet("background-color: rgba(0, 0, 255, 128); border: none; color: white; font-size: 24px;");
yellow_button_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
blue_button_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
h_layout_ = new QHBoxLayout();
h_layout_->setContentsMargins(0, 0, 0, 0);
h_layout_->setSpacing(0);
h_layout_->addWidget(yellow_button_);
h_layout_->addWidget(blue_button_);
h_layout_->setStretch(0, 1);
h_layout_->setStretch(1, 1);
v_layout_ = new QVBoxLayout();
v_layout_->setContentsMargins(0, 0, 0, 0);
v_layout_->setSpacing(0);
v_layout_->addLayout(h_layout_, 1); // Stretch to fill vertical
setLayout(v_layout_);
team_publisher_ = node_->create_publisher<std_msgs::msg::Int8>("/strat/team", 10);
connect(yellow_button_, &QPushButton::clicked, this, &HomePage::onYellowButtonClicked);
connect(blue_button_, &QPushButton::clicked, this, &HomePage::onBlueButtonClicked);
}
}
void HomePage::onYellowButtonClicked()
{
std_msgs::msg::Int8 msg;
msg.data = 0;
team_publisher_->publish(msg);
emit TeamChoose();
}
void HomePage::onBlueButtonClicked()
{
std_msgs::msg::Int8 msg;
msg.data = 1;
team_publisher_->publish(msg);
emit TeamChoose();
}
void HomePage::paintEvent(QPaintEvent* paint_event)
{
QWidget::paintEvent(paint_event);
QPainter painter(this);
renderer_->render(&painter, rect()); // Scales SVG to widget size
}
}

98
src/modelec_gui/src/pages/map_page.cpp
View File

@@ -15,8 +15,6 @@ namespace ModelecGUI
v_layout = new QVBoxLayout(this);
tirette_button_ = new QPushButton("Tirette", this);
timer_label_ = new QLabel("00", this);
timer_label_->setAlignment(Qt::AlignCenter);
timer_label_->setFont(QFont("Arial", 24));
@@ -28,10 +26,12 @@ namespace ModelecGUI
h_layout = new QHBoxLayout(this);
h_layout->addStretch();
h_layout->addStretch();
h_layout->addWidget(score_label_);
h_layout->addWidget(tirette_button_);
h_layout->addStretch();
h_layout->addWidget(timer_label_);
h_layout->addStretch();
h_layout->addStretch();
v_layout->addLayout(h_layout);
v_layout->addStretch();
@@ -40,18 +40,12 @@ namespace ModelecGUI
qpoints = {};
enemy_pos_.x = 1775;
enemy_pos_.y = 200;
enemy_pos_.theta = 3.1415/2;
robot_length_ = Modelec::Config::get<int>("config.robot.size.length_mm", 200);
robot_width_ = Modelec::Config::get<int>("config.robot.size.width_mm", 300);
enemy_length_ = Modelec::Config::get<int>("config.enemy.size.length_mm", 300);
enemy_width_ = Modelec::Config::get<int>("config.enemy.size.width_mm", 300);
add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryAddWaypoint>("odometry/add_waypoint", 100,
[this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) {
if (lastWapointWasEnd)
@@ -71,13 +65,18 @@ namespace ModelecGUI
update();
});
// lambda
odometry_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>("odometry/position", 10,
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) {
robotPos = *msg;
update();
});
score_sub_ = node_->create_subscription<std_msgs::msg::Int64>("strat/score", 10,
[this](const std_msgs::msg::Int64::SharedPtr msg) {
score_+= msg->data;
score_label_->setText(QString("Score: %1").arg(score_));
});
obstacle_added_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>("nav/obstacle/added", 40,
[this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) {
OnObstacleReceived(msg);
@@ -88,11 +87,14 @@ namespace ModelecGUI
obstacle_.erase(msg->id);
});
go_to_pub_ = node_->create_publisher<modelec_interfaces::msg::OdometryPos>("nav/go_to", 10);
enemy_pos_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>("enemy/position", 10,
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
if (!hasEnemy) hasEnemy = true;
enemy_pos_pub_ = node_->create_publisher<modelec_interfaces::msg::OdometryPos>("enemy/position", 10);
tirette_pub_ = node_->create_publisher<std_msgs::msg::Bool>("tirette", 10);
enemy_pos_ = *msg;
update();
});
strat_start_sub_ = node_->create_subscription<std_msgs::msg::Int64>("/strat/start_time", 10,
[this](const std_msgs::msg::Int64::SharedPtr msg){
@@ -109,12 +111,6 @@ namespace ModelecGUI
}
});
connect(tirette_button_, &QPushButton::clicked, this, [this]() {
std_msgs::msg::Bool msg;
msg.data = true;
tirette_pub_->publish(msg);
});
// client to nav/map
map_client_ = node_->create_client<modelec_interfaces::srv::MapSize>("nav/map_size");
while (!map_client_->wait_for_service(std::chrono::seconds(1))) {
@@ -150,23 +146,6 @@ namespace ModelecGUI
rclcpp::spin_until_future_complete(node_->get_node_base_interface(), result2);
}
void MapPage::setPlaymatGrid()
{
renderer->load(QString(":/img/playmat/grid_v1.svg"));
update();
}
void MapPage::setPlaymatMap()
{
renderer->load(QString(":/img/playmat/2025_FINAL.svg"));
update();
}
void MapPage::toggleShowObstacle()
{
show_obstacle_ = !show_obstacle_;
}
void MapPage::paintEvent(QPaintEvent* paint_event)
{
QWidget::paintEvent(paint_event);
@@ -228,8 +207,11 @@ namespace ModelecGUI
}
// -- Draw enemy position --
painter.setBrush(Qt::red);
painter.drawRect((enemy_pos_.x - (enemy_width_ / 2)) * ratioBetweenMapAndWidgetX_, height() - (enemy_pos_.y + (enemy_length_ / 2)) * ratioBetweenMapAndWidgetY_, enemy_width_*ratioBetweenMapAndWidgetX_, enemy_length_*ratioBetweenMapAndWidgetY_);
if (hasEnemy)
{
painter.setBrush(Qt::red);
painter.drawRect((enemy_pos_.x - (enemy_width_ / 2)) * ratioBetweenMapAndWidgetX_, height() - (enemy_pos_.y + (enemy_length_ / 2)) * ratioBetweenMapAndWidgetY_, enemy_width_*ratioBetweenMapAndWidgetX_, enemy_length_*ratioBetweenMapAndWidgetY_);
}
}
// -- Draw robot position --
@@ -242,42 +224,6 @@ namespace ModelecGUI
painter.drawRect(rect);
}
void MapPage::mousePressEvent(QMouseEvent* event)
{
QWidget::mousePressEvent(event);
if (event->button() == Qt::LeftButton)
{
modelec_interfaces::msg::OdometryPos msg;
msg.x = Modelec::mapValue(event->pos().x(), 0, width(), 0, 3000);
msg.y = 2000 - Modelec::mapValue(event->pos().y(), 0, height(), 0, 2000);
msg.theta = 0;
go_to_pub_->publish(msg);
}
}
void MapPage::mouseReleaseEvent(QMouseEvent* event)
{
QWidget::mouseReleaseEvent(event);
}
void MapPage::mouseDoubleClickEvent(QMouseEvent* event)
{
QWidget::mouseDoubleClickEvent(event);
}
void MapPage::mouseMoveEvent(QMouseEvent* event)
{
QWidget::mouseMoveEvent(event);
enemy_pos_.x = Modelec::mapValue(event->pos().x(), 0, width(), 0, 3000);
enemy_pos_.y = 2000 - Modelec::mapValue(event->pos().y(), 0, height(), 0, 2000);
enemy_pos_.theta = 0;
enemy_pos_pub_->publish(enemy_pos_);
}
void MapPage::OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg)
{
obstacle_.emplace(msg->id, *msg);
@@ -290,4 +236,4 @@ namespace ModelecGUI
ratioBetweenMapAndWidgetX_ = width() / 3000.0f;
ratioBetweenMapAndWidgetY_ = height() / 2000.0f;
}
}
}

10
src/modelec_gui/src/pages/test_page.cpp → src/modelec_gui/src/pages/odo_page.cpp
View File

@@ -1,11 +1,11 @@
#include <modelec_gui/pages/test_page.hpp>
#include <modelec_gui/pages/odo_page.hpp>
#include <utility>
#include <boost/asio/connect.hpp>
namespace ModelecGUI
{
TestPage::TestPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), node_(node)
OdoPage::OdoPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), node_(node)
{
startButton_ = new QPushButton("Start");
connect(startButton_, &QPushButton::clicked, this, [this]() {
@@ -156,7 +156,7 @@ namespace ModelecGUI
// Set up subscription
sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
"/odometry/position", 10,
std::bind(&TestPage::PositionCallback, this, std::placeholders::_1));
std::bind(&OdoPage::PositionCallback, this, std::placeholders::_1));
pub_add_waypoint_ = node_->create_publisher<modelec_interfaces::msg::OdometryAddWaypoint>(
"/odometry/add_waypoint", 10);
@@ -199,9 +199,9 @@ namespace ModelecGUI
}
}
TestPage::~TestPage() = default;
OdoPage::~OdoPage() = default;
void TestPage::PositionCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
void OdoPage::PositionCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
QMetaObject::invokeMethod(this, [this, msg]() {
xBox_->setText(QString("x: %1").arg(msg->x));

287
src/modelec_gui/src/pages/test_map_page.cpp Normal file
View File

@@ -0,0 +1,287 @@
#include <modelec_gui/pages/test_map_page.hpp>
#include <modelec_utils/utils.hpp>
#include <QMouseEvent>
#include <utility>
#include <modelec_utils/config.hpp>
#include <cmath>
namespace ModelecGUI
{
TestMapPage::TestMapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), renderer(new QSvgRenderer(QString(":/img/playmat/2025_FINAL.svg"), this)), node_(node)
{
ratioBetweenMapAndWidgetX_ = width() / 3000.0f;
ratioBetweenMapAndWidgetY_ = height() / 2000.0f;
v_layout = new QVBoxLayout(this);
tirette_button_ = new QPushButton("Tirette", this);
timer_label_ = new QLabel("00", this);
timer_label_->setAlignment(Qt::AlignCenter);
timer_label_->setFont(QFont("Arial", 24));
timer_label_->setStyleSheet("QLabel { color: white; }");
score_label_ = new QLabel("Score: 0", this);
score_label_->setAlignment(Qt::AlignCenter);
score_label_->setFont(QFont("Arial", 24));
score_label_->setStyleSheet("QLabel { color: white; }");
h_layout = new QHBoxLayout(this);
h_layout->addStretch();
h_layout->addWidget(score_label_);
h_layout->addWidget(tirette_button_);
h_layout->addWidget(timer_label_);
h_layout->addStretch();
v_layout->addLayout(h_layout);
v_layout->addStretch();
this->setLayout(v_layout);
qpoints = {};
robot_length_ = Modelec::Config::get<int>("config.robot.size.length_mm", 200);
robot_width_ = Modelec::Config::get<int>("config.robot.size.width_mm", 300);
enemy_length_ = Modelec::Config::get<int>("config.enemy.size.length_mm", 300);
enemy_width_ = Modelec::Config::get<int>("config.enemy.size.width_mm", 300);
add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryAddWaypoint>("odometry/add_waypoint", 100,
[this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) {
if (lastWapointWasEnd)
{
qpoints.clear();
lastWapointWasEnd = false;
qpoints.push_back(QPoint(robotPos.x * ratioBetweenMapAndWidgetX_, height() - robotPos.y * ratioBetweenMapAndWidgetY_));
}
if (msg->is_end)
{
lastWapointWasEnd = true;
}
qpoints.push_back(QPoint(msg->x * ratioBetweenMapAndWidgetX_, height() - msg->y * ratioBetweenMapAndWidgetY_));
update();
});
odometry_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>("odometry/position", 10,
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) {
robotPos = *msg;
update();
});
obstacle_added_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>("nav/obstacle/added", 40,
[this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) {
OnObstacleReceived(msg);
});
obstacle_removed_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>("nav/obstacle/removed", 40,
[this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) {
obstacle_.erase(msg->id);
});
go_to_pub_ = node_->create_publisher<modelec_interfaces::msg::OdometryPos>("nav/go_to", 10);
enemy_pos_pub_ = node_->create_publisher<modelec_interfaces::msg::OdometryPos>("enemy/position", 10);
enemy_pos_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>("enemy/position", 10,
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
if (!hasEnemy) hasEnemy = true;
enemy_pos_ = *msg;
update();
});
tirette_pub_ = node_->create_publisher<std_msgs::msg::Bool>("tirette", 10);
strat_start_sub_ = node_->create_subscription<std_msgs::msg::Int64>("/strat/start_time", 10,
[this](const std_msgs::msg::Int64::SharedPtr msg){
isGameStarted_ = true;
start_time_ = msg->data;
});
strat_state_sub_ = node_->create_subscription<modelec_interfaces::msg::StratState>("/strat/state", 10,
[this](const modelec_interfaces::msg::StratState::SharedPtr msg){
if (msg->state == modelec_interfaces::msg::StratState::STOP)
{
RCLCPP_INFO(node_->get_logger(), "Game stop");
isGameStarted_ = false;
}
});
connect(tirette_button_, &QPushButton::clicked, this, [this]() {
std_msgs::msg::Bool msg;
msg.data = true;
tirette_pub_->publish(msg);
});
// client to nav/map
map_client_ = node_->create_client<modelec_interfaces::srv::MapSize>("nav/map_size");
while (!map_client_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(node_->get_logger(), "Waiting for the service...");
}
auto result = map_client_->async_send_request(std::make_shared<modelec_interfaces::srv::MapSize::Request>());
if (rclcpp::spin_until_future_complete(node_->get_node_base_interface(), result) ==
rclcpp::FutureReturnCode::SUCCESS)
{
if (auto res = result.get())
{
RCLCPP_INFO(node_->get_logger(), "Map received: %d x %d", res->width, res->height);
map_width_ = res->width;
map_height_ = res->height;
}
}
ask_map_obstacle_client_ = node_->create_client<std_srvs::srv::Empty>("nav/ask_map_obstacle");
while (!ask_map_obstacle_client_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(node_->get_logger(), "Waiting for the service...");
}
auto result2 = ask_map_obstacle_client_->async_send_request(std::make_shared<std_srvs::srv::Empty::Request>());
rclcpp::spin_until_future_complete(node_->get_node_base_interface(), result2);
}
void TestMapPage::setPlaymatGrid()
{
renderer->load(QString(":/img/playmat/grid_v1.svg"));
update();
}
void TestMapPage::setPlaymatMap()
{
renderer->load(QString(":/img/playmat/2025_FINAL.svg"));
update();
}
void TestMapPage::toggleShowObstacle()
{
show_obstacle_ = !show_obstacle_;
}
void TestMapPage::paintEvent(QPaintEvent* paint_event)
{
QWidget::paintEvent(paint_event);
if (isGameStarted_)
{
auto now = std::chrono::system_clock::now().time_since_epoch();
auto start = std::chrono::nanoseconds(start_time_);
auto elapsed = now - start;
auto elapsed_s = std::chrono::duration_cast<std::chrono::seconds>(elapsed).count();
timer_label_->setText(QString::number(elapsed_s));
}
QPainter painter(this);
renderer->render(&painter, rect()); // Scales SVG to widget size
painter.save();
painter.setRenderHint(QPainter::Antialiasing);
// --- Draw lines ---
painter.setPen(QPen(Qt::red, 2)); // Red lines, 2px wide
for (size_t i = 0; i + 1 < qpoints.size(); ++i) {
painter.drawLine(qpoints[i], qpoints[i + 1]);
}
painter.setPen(Qt::NoPen);
// --- Draw colored points ---
const int radius = 5;
for (size_t i = 0; i < qpoints.size(); ++i) {
if (i == qpoints.size() - 1)
painter.setBrush(Qt::blue); // Last = blue
else
painter.setBrush(Qt::red); // Middle = red
painter.drawEllipse(qpoints[i], radius, radius);
}
painter.restore();
if (show_obstacle_)
{
for (auto & [index, obs] : obstacle_)
{
painter.save();
QPoint obsPoint(obs.x * ratioBetweenMapAndWidgetX_, height() - obs.y * ratioBetweenMapAndWidgetY_);
painter.translate(obsPoint);
painter.rotate(90 - obs.theta * (180.0 / M_PI));
painter.setBrush(Qt::black);
QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2), -(obs.height * ratioBetweenMapAndWidgetY_ / 2),
obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_);
painter.drawRect(toDraw);
painter.restore();
}
// -- Draw enemy position --
if (hasEnemy)
{
painter.setBrush(Qt::red);
painter.drawRect((enemy_pos_.x - (enemy_width_ / 2)) * ratioBetweenMapAndWidgetX_, height() - (enemy_pos_.y + (enemy_length_ / 2)) * ratioBetweenMapAndWidgetY_, enemy_width_*ratioBetweenMapAndWidgetX_, enemy_length_*ratioBetweenMapAndWidgetY_);
}
}
// -- Draw robot position --
painter.translate(robotPos.x * ratioBetweenMapAndWidgetX_, height() - robotPos.y * ratioBetweenMapAndWidgetY_);
painter.rotate(90 - robotPos.theta * (180.0 / M_PI));
QRect rect(-(robot_width_ * ratioBetweenMapAndWidgetX_ / 2), -(robot_length_ * ratioBetweenMapAndWidgetY_ / 2),
robot_width_ * ratioBetweenMapAndWidgetX_, robot_length_ * ratioBetweenMapAndWidgetY_);
painter.setBrush(Qt::green);
painter.drawRect(rect);
}
void TestMapPage::mousePressEvent(QMouseEvent* event)
{
QWidget::mousePressEvent(event);
if (event->button() == Qt::LeftButton)
{
modelec_interfaces::msg::OdometryPos msg;
msg.x = Modelec::mapValue(event->pos().x(), 0, width(), 0, 3000);
msg.y = 2000 - Modelec::mapValue(event->pos().y(), 0, height(), 0, 2000);
msg.theta = 0;
go_to_pub_->publish(msg);
}
else if (event->button() == Qt::RightButton)
{
modelec_interfaces::msg::OdometryPos msg;
msg.x = Modelec::mapValue(event->pos().x(), 0, width(), 0, 3000);
msg.y = 2000 - Modelec::mapValue(event->pos().y(), 0, height(), 0, 2000);
msg.theta = 0;
enemy_pos_pub_->publish(msg);
}
}
void TestMapPage::OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg)
{
obstacle_.emplace(msg->id, *msg);
}
void TestMapPage::resizeEvent(QResizeEvent* event)
{
QWidget::resizeEvent(event);
ratioBetweenMapAndWidgetX_ = width() / 3000.0f;
ratioBetweenMapAndWidgetY_ = height() / 2000.0f;
}
}

2
src/modelec_strat/CMakeLists.txt
View File

@@ -15,7 +15,7 @@ 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/mission_manager.cpp src/missions/prepare_concert_mission.cpp src/missions/promotion_mission.cpp src/missions/go_home_mission.cpp src/obstacle/obstacle.cpp src/obstacle/column.cpp src/deposite_zone.cpp)
add_executable(strat_fsm src/strat_fms.cpp src/navigation_helper.cpp src/pathfinding.cpp src/action_executor.cpp src/mission_manager.cpp src/missions/prepare_concert_mission.cpp src/missions/banner_mission.cpp src/missions/go_home_mission.cpp src/obstacle/obstacle.cpp src/obstacle/column.cpp src/deposite_zone.cpp)
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

22
src/modelec_strat/data/config.xml
View File

@@ -36,21 +36,35 @@
<yellow x="350" y="1600" theta="-1.5708" />
</home>
<spawn>
<blue x="100" y="1600" theta="-1.5708" />
<yellow x="2650" y="1600" theta="-1.5708" />
<blue x="1775" y="300" theta="1.5708" />
<yellow x="1225" y="300" theta="1.5708" />
</spawn>
<usb>
<pcb>
<pcb_odo>
<name>pcb_odo</name>
<port>/dev/pts/11</port>
<port>/dev/pts/5</port>
<baud_rate>115200</baud_rate>
</pcb_odo>
<pcb_alim>
<name>pcb_alim</name>
<port>/dev/pts/12</port>
<port>/dev/pts/9</port>
<baud_rate>115200</baud_rate>
</pcb_alim>
</pcb>
</usb>
<mission_score>
<banner>20</banner>
<go_home>10</go_home>
<concert>
<niv_1>4</niv_1>
<niv_2>8</niv_2>
<niv_3>16</niv_3>
</concert>
<pami>
<goupie>5</goupie>
<superstar>5</superstar>
<all_party>10</all_party>
</pami>
</mission_score>
</config>

7
src/modelec_strat/include/modelec_strat/missions/promotion_mission.hpp → src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp
View File

@@ -2,6 +2,7 @@
#include <modelec_strat/missions/mission_base.hpp>
#include <modelec_strat/navigation_helper.hpp>
#include <std_msgs/msg/int64.hpp>
namespace Modelec
{
@@ -9,10 +10,10 @@ namespace Modelec
* Banderole mission
*
*/
class PromotionMission : public Mission
class BannerMission : public Mission
{
public:
PromotionMission(const std::shared_ptr<NavigationHelper>& nav);
BannerMission(const std::shared_ptr<NavigationHelper>& nav);
void start(rclcpp::Node::SharedPtr node) override;
void update() override;
@@ -30,5 +31,7 @@ namespace Modelec
MissionStatus status_;
std::shared_ptr<NavigationHelper> nav_;
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr score_pub_;
int mission_score_ = 0;
};
}

4
src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp
View File

@@ -2,7 +2,7 @@
#include <modelec_strat/missions/mission_base.hpp>
#include <modelec_strat/navigation_helper.hpp>
#include <std_msgs/msg/int64.hpp>
namespace Modelec
{
@@ -30,5 +30,7 @@ namespace Modelec
rclcpp::Node::SharedPtr node_;
rclcpp::Time start_time_;
Point home_point_;
rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr score_pub_;
int mission_score_ = 0;
};
}

3
src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp
View File

@@ -3,6 +3,7 @@
#include <modelec_strat/missions/mission_base.hpp>
#include <modelec_strat/navigation_helper.hpp>
#include <modelec_strat/obstacle/column.hpp>
#include <std_msgs/msg/int64.hpp>
namespace Modelec {
@@ -36,6 +37,8 @@ namespace Modelec {
std::shared_ptr<ColumnObstacle> column_;
std::shared_ptr<DepositeZone> closestDepoZone_;
Point closestDepoZonePoint_;
rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr score_pub_;
int mission_score_ = 0;
};
}

15
src/modelec_strat/include/modelec_strat/navigation_helper.hpp
View File

@@ -60,6 +60,10 @@ namespace Modelec {
std::pair<int, WaypointListMsg> FindPath(const PosMsg::SharedPtr& goal, bool isClose = false,
int collisionMask = Pathfinding::FREE);
void SetPos(const PosMsg& pos);
void SetPos(const Point& pos);
void SetPos(int x, int y, double theta);
PosMsg::SharedPtr GetCurrentPos() const;
bool LoadDepositeZoneFromXML(const std::string &filename);
@@ -79,6 +83,14 @@ namespace Modelec {
void Replan();
void SetTeamId(int id);
void SetSpawn();
Point GetSpawnYellow() const;
Point GetSpawnBlue() const;
Point GetSpawn() const;
protected:
void OnWaypointReach(const WaypointReachMsg::SharedPtr msg);
@@ -97,6 +109,8 @@ namespace Modelec {
std::shared_ptr<Pathfinding> pathfinding_;
int team_id_ = YELLOW;
Point spawn_yellow_;
Point spawn_blue_;
float factor_close_enemy_ = 0;
@@ -111,6 +125,7 @@ namespace Modelec {
rclcpp::Subscription<PosMsg>::SharedPtr go_to_sub_;
rclcpp::Subscription<PosMsg>::SharedPtr pos_sub_;
rclcpp::Publisher<PosMsg>::SharedPtr pos_pub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_sub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_long_time_sub_;

21
src/modelec_strat/include/modelec_strat/strat_fms.hpp
View File

@@ -6,14 +6,18 @@
#include "mission_manager.hpp"
#include "navigation_helper.hpp"
#include "missions/mission_base.hpp"
#include "missions/promotion_mission.hpp"
#include "missions/banner_mission.hpp"
#include "missions/prepare_concert_mission.hpp"
#include "missions/go_home_mission.hpp"
#include "std_msgs/msg/bool.hpp"
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/int64.hpp>
#include <std_msgs/msg/int8.hpp>
#include <modelec_interfaces/msg/strat_state.hpp>
#include <modelec_interfaces/srv/odometry_start.hpp>
#include "modelec_interfaces/msg/strat_state.hpp"
namespace Modelec
{
@@ -30,14 +34,6 @@ namespace Modelec
STOP
};
/*
* TODO
* - yaml strat
* - setup des missions
* - gestion des interruptions (robot adverses / obstacles)
* - scoring des missions
*/
class StratFMS : public rclcpp::Node
{
public:
@@ -65,5 +61,8 @@ namespace Modelec
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr tirette_sub_;
rclcpp::Publisher<modelec_interfaces::msg::StratState>::SharedPtr state_pub_;
rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr start_time_pub_;
rclcpp::Subscription<std_msgs::msg::Int8>::SharedPtr team_id_sub_;
rclcpp::Client<modelec_interfaces::srv::OdometryStart>::SharedPtr client_start_;
};
}

73
src/modelec_strat/src/missions/banner_mission.cpp Normal file
View File

@@ -0,0 +1,73 @@
#include <modelec_strat/missions/banner_mission.hpp>
#include <modelec_utils/config.hpp>
namespace Modelec
{
BannerMission::BannerMission(const std::shared_ptr<NavigationHelper>& nav) : step_(GO_TO_FRONT), status_(MissionStatus::READY), nav_(nav)
{
}
void BannerMission::start(rclcpp::Node::SharedPtr node)
{
node_ = node;
mission_score_ = Config::get<int>("config.mission_score.banner", 0);
score_pub_ = node_->create_publisher<std_msgs::msg::Int64>("/strat/score", 10);
auto spawn = nav_->GetSpawn();
nav_->GoTo(spawn.x, (nav_->GetPathfinding()->robot_length_mm_ / 2) + 5, M_PI_2, true);
status_ = MissionStatus::RUNNING;
}
void BannerMission::update()
{
if (status_ != MissionStatus::RUNNING) return;
if (!nav_->HasArrived()) return;
switch (step_)
{
case GO_TO_FRONT:
// TODO deploy the banner here
step_ = DEPLOY_BANNER;
break;
case DEPLOY_BANNER:
{
auto spawn = nav_->GetSpawn();
nav_->GoTo(spawn.x, (nav_->GetPathfinding()->robot_length_mm_/2) + 150, M_PI_2, true);
step_ = GO_FORWARD;
break;
}
case GO_FORWARD:
{
std_msgs::msg::Int64 msg;
msg.data = mission_score_;
score_pub_->publish(msg);
step_ = DONE;
status_ = MissionStatus::DONE;
break;
}
default:
break;
}
}
void BannerMission::clear()
{
}
MissionStatus BannerMission::getStatus() const
{
return status_;
}
}

18
src/modelec_strat/src/missions/go_home_mission.cpp
View File

@@ -1,5 +1,7 @@
#include <modelec_strat/missions/go_home_mission.hpp>
#include <modelec_utils/config.hpp>
namespace Modelec
{
GoHomeMission::GoHomeMission(const std::shared_ptr<NavigationHelper>& nav, const rclcpp::Time& start_time) : step_(ROTATE_TO_HOME), status_(MissionStatus::READY), nav_(nav), start_time_(start_time)
@@ -10,6 +12,10 @@ namespace Modelec
{
node_ = node;
mission_score_ = Config::get<int>("config.mission_score.go_home", 0);
score_pub_ = node_->create_publisher<std_msgs::msg::Int64>("/strat/score", 10);
auto pos = nav_->GetHomePosition();
home_point_ = Point(pos->x, pos->y, pos->theta);
if (nav_->CanGoTo(home_point_.GetTakeBasePosition()) != Pathfinding::FREE)
@@ -57,10 +63,16 @@ namespace Modelec
break;
case GO_CLOSE:
step_ = DONE;
status_ = MissionStatus::DONE;
{
std_msgs::msg::Int64 msg;
msg.data = mission_score_;
score_pub_->publish(msg);
break;
step_ = DONE;
status_ = MissionStatus::DONE;
break;
}
default:
break;
}

17
src/modelec_strat/src/missions/prepare_concert_mission.cpp
View File

@@ -1,5 +1,6 @@
#include <modelec_strat/missions/prepare_concert_mission.hpp>
#include <modelec_strat/obstacle/column.hpp>
#include <modelec_utils/config.hpp>
namespace Modelec
{
@@ -12,6 +13,10 @@ namespace Modelec
{
node_ = node;
mission_score_ = Config::get<int>("config.mission_score.concert.niv_1", 0);
score_pub_ = node_->create_publisher<std_msgs::msg::Int64>("/strat/score", 10);
if (!nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), nav_->GetTeamId()))
{
status_ = MissionStatus::FINISH_ALL;
@@ -188,9 +193,15 @@ namespace Modelec
break;
case GO_BACK_2:
step_ = DONE;
status_ = MissionStatus::DONE;
break;
{
std_msgs::msg::Int64 msg;
msg.data = mission_score_;
score_pub_->publish(msg);
step_ = DONE;
status_ = MissionStatus::DONE;
break;
}
default:
break;

56
src/modelec_strat/src/missions/promotion_mission.cpp
View File

@@ -1,56 +0,0 @@
#include <modelec_strat/missions/promotion_mission.hpp>
namespace Modelec
{
PromotionMission::PromotionMission(const std::shared_ptr<NavigationHelper>& nav) : step_(GO_TO_FRONT), status_(MissionStatus::READY), nav_(nav)
{
}
void PromotionMission::start(rclcpp::Node::SharedPtr node)
{
node_ = node;
nav_->GoTo(1225, 152, M_PI_2, true);
status_ = MissionStatus::RUNNING;
}
void PromotionMission::update()
{
if (status_ != MissionStatus::RUNNING) return;
if (!nav_->HasArrived()) return;
switch (step_)
{
case GO_TO_FRONT:
// TODO deploy the banner here
step_ = DEPLOY_BANNER;
break;
case DEPLOY_BANNER:
nav_->GoTo(1225, 300, M_PI_2, true);
step_ = GO_FORWARD;
break;
case GO_FORWARD:
step_ = DONE;
status_ = MissionStatus::DONE;
break;
default:
break;
}
}
void PromotionMission::clear()
{
}
MissionStatus PromotionMission::getStatus() const
{
return status_;
}
}

74
src/modelec_strat/src/navigation_helper.cpp
View File

@@ -15,6 +15,14 @@ namespace Modelec {
factor_close_enemy_ = Config::get<float>("config.enemy.factor_close_enemy", -0.5f);
spawn_yellow_.x = Config::get<int>("config.spawn.yellow@x", 0);
spawn_yellow_.y = Config::get<int>("config.spawn.yellow@y", 0);
spawn_yellow_.theta = Config::get<double>("config.spawn.yellow@theta", 0);
spawn_blue_.x = Config::get<int>("config.spawn.blue@x", 0);
spawn_blue_.y = Config::get<int>("config.spawn.blue@y", 0);
spawn_blue_.theta = Config::get<double>("config.spawn.blue@theta", 0.0f);
waypoint_reach_sub_ = node_->create_subscription<WaypointReachMsg>(
"odometry/waypoint_reach", 10,
[this](const WaypointReachMsg::SharedPtr msg) {
@@ -29,6 +37,8 @@ namespace Modelec {
OnPos(msg);
});
pos_pub_ = node_->create_publisher<PosMsg>("odometry/set_position", 10);
go_to_sub_ = node_->create_subscription<PosMsg>(
"nav/go_to", 10, [this](const PosMsg::SharedPtr msg) {
GoTo(msg, false, Pathfinding::FREE | Pathfinding::WALL);
@@ -342,6 +352,25 @@ namespace Modelec {
return pathfinding_->FindPath(current_pos_, goal, isClose, collisionMask);
}
void NavigationHelper::SetPos(const PosMsg& pos)
{
pos_pub_->publish(pos);
}
void NavigationHelper::SetPos(const Point& pos)
{
PosMsg msg;
msg.x = pos.x;
msg.y = pos.y;
msg.theta = pos.theta;
SetPos(msg);
}
void NavigationHelper::SetPos(int x, int y, double theta)
{
SetPos({x, y, theta});
}
PosMsg::SharedPtr NavigationHelper::GetCurrentPos() const
{
return current_pos_;
@@ -481,7 +510,6 @@ namespace Modelec {
{
if (GoTo(last_go_to_.goal, true, last_go_to_.collisionMask) != Pathfinding::FREE)
{
// TODO : change to reset all the waypoints
GoTo(current_pos_, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE | Pathfinding::ENEMY);
// TODO : Handle case where no path is found
}
@@ -489,6 +517,50 @@ namespace Modelec {
}
}
void NavigationHelper::SetTeamId(int id)
{
team_id_ = id;
}
void NavigationHelper::SetSpawn()
{
switch (team_id_)
{
case YELLOW:
SetPos(spawn_yellow_);
break;
case BLUE:
SetPos(spawn_blue_);
break;
default:
SetPos(spawn_yellow_);
break;
}
}
Point NavigationHelper::GetSpawnYellow() const
{
return spawn_yellow_;
}
Point NavigationHelper::GetSpawnBlue() const
{
return spawn_blue_;
}
Point NavigationHelper::GetSpawn() const
{
switch (team_id_)
{
case YELLOW:
return spawn_yellow_;
case BLUE:
return spawn_blue_;
default:
return spawn_yellow_;
}
}
void NavigationHelper::OnWaypointReach(const WaypointReachMsg::SharedPtr msg)
{
for (auto & waypoint : waypoints_)

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

@@ -20,6 +20,32 @@ namespace Modelec
start_time_pub_ = create_publisher<std_msgs::msg::Int64>("/strat/start_time", 10);
team_id_sub_ = create_subscription<std_msgs::msg::Int8>(
"/strat/team", 10, [this](const std_msgs::msg::Int8::SharedPtr msg)
{
team_id_ = static_cast<int>(msg->data);
nav_->SetTeamId(team_id_);
nav_->SetSpawn();
});
client_start_ = create_client<modelec_interfaces::srv::OdometryStart>("/odometry/start");
while (!client_start_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(get_logger(), "Service not available, waiting again...");
}
auto request = std::make_shared<modelec_interfaces::srv::OdometryStart::Request>();
request->start = true;
client_start_->async_send_request(request, [this](rclcpp::Client<modelec_interfaces::srv::OdometryStart>::SharedFuture response) {
if (!response.get()->success)
{
RCLCPP_ERROR(get_logger(), "Failed to send start command.");
}
});
std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml";
if (!Config::load(config_path))
{
@@ -125,7 +151,7 @@ namespace Modelec
case State::DO_PROMOTION:
if (!current_mission_)
{
current_mission_ = std::make_unique<PromotionMission>(nav_);
current_mission_ = std::make_unique<BannerMission>(nav_);
current_mission_->start(shared_from_this());
}
current_mission_->update();