From c7e4dc18e2e436b3790574d71d2bd07690438d22 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 7 May 2025 04:48:21 -0400 Subject: [PATCH] real map page + score + some bug fix --- .../include/modelec_com/pcb_odo_interface.hpp | 2 +- src/modelec_com/src/pcb_odo_interface.cpp | 2 +- src/modelec_gui/CMakeLists.txt | 7 +- .../include/modelec_gui/modelec_gui.hpp | 12 +- .../include/modelec_gui/pages/home_page.hpp | 49 ++- .../include/modelec_gui/pages/map_page.hpp | 26 +- .../pages/{test_page.hpp => odo_page.hpp} | 6 +- .../modelec_gui/pages/test_map_page.hpp | 108 +++++++ src/modelec_gui/src/modelec_gui.cpp | 33 +- src/modelec_gui/src/pages/home_page.cpp | 64 +++- src/modelec_gui/src/pages/map_page.cpp | 98 ++---- .../src/pages/{test_page.cpp => odo_page.cpp} | 10 +- src/modelec_gui/src/pages/test_map_page.cpp | 287 ++++++++++++++++++ src/modelec_strat/CMakeLists.txt | 2 +- src/modelec_strat/data/config.xml | 22 +- ...omotion_mission.hpp => banner_mission.hpp} | 7 +- .../missions/go_home_mission.hpp | 4 +- .../missions/prepare_concert_mission.hpp | 3 + .../modelec_strat/navigation_helper.hpp | 15 + .../include/modelec_strat/strat_fms.hpp | 21 +- .../src/missions/banner_mission.cpp | 73 +++++ .../src/missions/go_home_mission.cpp | 18 +- .../src/missions/prepare_concert_mission.cpp | 17 +- .../src/missions/promotion_mission.cpp | 56 ---- src/modelec_strat/src/navigation_helper.cpp | 74 ++++- src/modelec_strat/src/strat_fms.cpp | 28 +- 26 files changed, 820 insertions(+), 224 deletions(-) rename src/modelec_gui/include/modelec_gui/pages/{test_page.hpp => odo_page.hpp} (92%) create mode 100644 src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp rename src/modelec_gui/src/pages/{test_page.cpp => odo_page.cpp} (96%) create mode 100644 src/modelec_gui/src/pages/test_map_page.cpp rename src/modelec_strat/include/modelec_strat/missions/{promotion_mission.hpp => banner_mission.hpp} (73%) create mode 100644 src/modelec_strat/src/missions/banner_mission.cpp delete mode 100644 src/modelec_strat/src/missions/promotion_mission.cpp diff --git a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp index 2ecd05f..038e4f7 100644 --- a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp @@ -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; diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index ea2acec..c79d9bf 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -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 data = { std::to_string(x), diff --git a/src/modelec_gui/CMakeLists.txt b/src/modelec_gui/CMakeLists.txt index 89cd8e5..0549eeb 100644 --- a/src/modelec_gui/CMakeLists.txt +++ b/src/modelec_gui/CMakeLists.txt @@ -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 diff --git a/src/modelec_gui/include/modelec_gui/modelec_gui.hpp b/src/modelec_gui/include/modelec_gui/modelec_gui.hpp index f18c96d..2e48db1 100644 --- a/src/modelec_gui/include/modelec_gui/modelec_gui.hpp +++ b/src/modelec_gui/include/modelec_gui/modelec_gui.hpp @@ -4,7 +4,10 @@ #include #include #include - +#include +#include +#include +#include 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_; diff --git a/src/modelec_gui/include/modelec_gui/pages/home_page.hpp b/src/modelec_gui/include/modelec_gui/pages/home_page.hpp index d001f40..bb61bdb 100644 --- a/src/modelec_gui/include/modelec_gui/pages/home_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/home_page.hpp @@ -1,16 +1,45 @@ #pragma once #include -#include +#include +#include +#include -namespace ModelecGUI { -class HomePage : public QWidget +#include + +#include + +namespace ModelecGUI { - Q_OBJECT -public: - HomePage(QWidget *parent = nullptr); + class HomePage : public QWidget + { + Q_OBJECT -protected: - QLayout* m_layout; -}; -} \ No newline at end of file + 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::SharedPtr team_publisher_; + + QVBoxLayout* v_layout_; + QHBoxLayout* h_layout_; + + QPushButton* blue_button_; + QPushButton* yellow_button_; + + QSvgRenderer* renderer_; + + }; +} diff --git a/src/modelec_gui/include/modelec_gui/pages/map_page.hpp b/src/modelec_gui/include/modelec_gui/pages/map_page.hpp index bfd6e19..bd690f0 100644 --- a/src/modelec_gui/include/modelec_gui/pages/map_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/map_page.hpp @@ -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 qpoints; - //std::vector 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::SharedPtr tirette_pub_; - rclcpp::Subscription::SharedPtr add_waypoint_sub_; rclcpp::Subscription::SharedPtr odometry_sub_; - rclcpp::Publisher::SharedPtr go_to_pub_; + rclcpp::Subscription::SharedPtr score_sub_; rclcpp::Client::SharedPtr map_client_; rclcpp::Subscription::SharedPtr obstacle_added_sub_; rclcpp::Subscription::SharedPtr obstacle_removed_sub_; rclcpp::Client::SharedPtr ask_map_obstacle_client_; modelec_interfaces::msg::OdometryPos enemy_pos_; - rclcpp::Publisher::SharedPtr enemy_pos_pub_; + bool hasEnemy = false; + rclcpp::Subscription::SharedPtr enemy_pos_sub_; rclcpp::Subscription::SharedPtr strat_start_sub_; bool isGameStarted_ = false; diff --git a/src/modelec_gui/include/modelec_gui/pages/test_page.hpp b/src/modelec_gui/include/modelec_gui/pages/odo_page.hpp similarity index 92% rename from src/modelec_gui/include/modelec_gui/pages/test_page.hpp rename to src/modelec_gui/include/modelec_gui/pages/odo_page.hpp index 1c15e69..7d4ae86 100644 --- a/src/modelec_gui/include/modelec_gui/pages/test_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/odo_page.hpp @@ -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_; } diff --git a/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp b/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp new file mode 100644 index 0000000..834b4cc --- /dev/null +++ b/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp @@ -0,0 +1,108 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +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 qpoints; + //std::vector points; + modelec_interfaces::msg::OdometryPos go_to_point; + + bool lastWapointWasEnd = true; + + std::map 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::SharedPtr tirette_pub_; + + rclcpp::Subscription::SharedPtr add_waypoint_sub_; + + rclcpp::Subscription::SharedPtr odometry_sub_; + rclcpp::Publisher::SharedPtr go_to_pub_; + rclcpp::Client::SharedPtr map_client_; + rclcpp::Subscription::SharedPtr obstacle_added_sub_; + rclcpp::Subscription::SharedPtr obstacle_removed_sub_; + rclcpp::Client::SharedPtr ask_map_obstacle_client_; + + modelec_interfaces::msg::OdometryPos enemy_pos_; + bool hasEnemy = false; + rclcpp::Publisher::SharedPtr enemy_pos_pub_; + rclcpp::Subscription::SharedPtr enemy_pos_sub_; + rclcpp::Subscription::SharedPtr strat_start_sub_; + + bool isGameStarted_ = false; + long int start_time_ = 0; + + rclcpp::Subscription::SharedPtr strat_state_sub_; + }; +} diff --git a/src/modelec_gui/src/modelec_gui.cpp b/src/modelec_gui/src/modelec_gui.cpp index 29c5321..8a7e8eb 100644 --- a/src/modelec_gui/src/modelec_gui.cpp +++ b/src/modelec_gui/src/modelec_gui.cpp @@ -2,9 +2,7 @@ #include #include #include -#include -#include -#include + 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(stackedWidget->currentWidget()); + auto map_page = dynamic_cast(stackedWidget->currentWidget()); if (map_page) { map_page->setPlaymatMap(); } }); connect(playmat_grid_, &QAction::triggered, this, [this]() { - auto map_page = dynamic_cast(stackedWidget->currentWidget()); + auto map_page = dynamic_cast(stackedWidget->currentWidget()); if (map_page) { map_page->setPlaymatGrid(); } }); connect(toggle_show_obstacle_action_, &QAction::triggered, this, [this]() { - auto map_page = dynamic_cast(stackedWidget->currentWidget()); + auto map_page = dynamic_cast(stackedWidget->currentWidget()); if (map_page) { map_page->toggleShowObstacle(); diff --git a/src/modelec_gui/src/pages/home_page.cpp b/src/modelec_gui/src/pages/home_page.cpp index b4dc7d2..0b015b2 100644 --- a/src/modelec_gui/src/pages/home_page.cpp +++ b/src/modelec_gui/src/pages/home_page.cpp @@ -1,17 +1,67 @@ +#include #include #include 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("/strat/team", 10); + + connect(yellow_button_, &QPushButton::clicked, this, &HomePage::onYellowButtonClicked); + connect(blue_button_, &QPushButton::clicked, this, &HomePage::onBlueButtonClicked); } -} \ No newline at end of file + 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 + } +} diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp index ceccd73..162a656 100644 --- a/src/modelec_gui/src/pages/map_page.cpp +++ b/src/modelec_gui/src/pages/map_page.cpp @@ -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("config.robot.size.length_mm", 200); robot_width_ = Modelec::Config::get("config.robot.size.width_mm", 300); enemy_length_ = Modelec::Config::get("config.enemy.size.length_mm", 300); enemy_width_ = Modelec::Config::get("config.enemy.size.width_mm", 300); - - add_waypoint_sub_ = node_->create_subscription("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("odometry/position", 10, [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { robotPos = *msg; update(); }); + score_sub_ = node_->create_subscription("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("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("nav/go_to", 10); + enemy_pos_sub_ = node_->create_subscription("enemy/position", 10, + [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) + { + if (!hasEnemy) hasEnemy = true; - enemy_pos_pub_ = node_->create_publisher("enemy/position", 10); - - tirette_pub_ = node_->create_publisher("tirette", 10); + enemy_pos_ = *msg; + update(); + }); strat_start_sub_ = node_->create_subscription("/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("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; } -} +} \ No newline at end of file diff --git a/src/modelec_gui/src/pages/test_page.cpp b/src/modelec_gui/src/pages/odo_page.cpp similarity index 96% rename from src/modelec_gui/src/pages/test_page.cpp rename to src/modelec_gui/src/pages/odo_page.cpp index b54fe0f..05917f3 100644 --- a/src/modelec_gui/src/pages/test_page.cpp +++ b/src/modelec_gui/src/pages/odo_page.cpp @@ -1,11 +1,11 @@ -#include +#include #include #include 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( "/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( "/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)); diff --git a/src/modelec_gui/src/pages/test_map_page.cpp b/src/modelec_gui/src/pages/test_map_page.cpp new file mode 100644 index 0000000..0b5fb91 --- /dev/null +++ b/src/modelec_gui/src/pages/test_map_page.cpp @@ -0,0 +1,287 @@ +#include +#include + +#include +#include +#include +#include + +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("config.robot.size.length_mm", 200); + robot_width_ = Modelec::Config::get("config.robot.size.width_mm", 300); + + enemy_length_ = Modelec::Config::get("config.enemy.size.length_mm", 300); + enemy_width_ = Modelec::Config::get("config.enemy.size.width_mm", 300); + + add_waypoint_sub_ = node_->create_subscription("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("odometry/position", 10, + [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { + robotPos = *msg; + update(); + }); + + obstacle_added_sub_ = node_->create_subscription("nav/obstacle/added", 40, + [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) { + OnObstacleReceived(msg); + }); + + obstacle_removed_sub_ = node_->create_subscription("nav/obstacle/removed", 40, + [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) { + obstacle_.erase(msg->id); + }); + + go_to_pub_ = node_->create_publisher("nav/go_to", 10); + + enemy_pos_pub_ = node_->create_publisher("enemy/position", 10); + + enemy_pos_sub_ = node_->create_subscription("enemy/position", 10, + [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) + { + if (!hasEnemy) hasEnemy = true; + + enemy_pos_ = *msg; + update(); + }); + + + tirette_pub_ = node_->create_publisher("tirette", 10); + + strat_start_sub_ = node_->create_subscription("/strat/start_time", 10, + [this](const std_msgs::msg::Int64::SharedPtr msg){ + isGameStarted_ = true; + start_time_ = msg->data; + }); + + strat_state_sub_ = node_->create_subscription("/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("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()); + 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("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()); + 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(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; + } +} diff --git a/src/modelec_strat/CMakeLists.txt b/src/modelec_strat/CMakeLists.txt index c1fb19f..28f267a 100644 --- a/src/modelec_strat/CMakeLists.txt +++ b/src/modelec_strat/CMakeLists.txt @@ -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 diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index e480e2d..36434e7 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -36,21 +36,35 @@ - - + + pcb_odo - /dev/pts/11 + /dev/pts/5 115200 pcb_alim - /dev/pts/12 + /dev/pts/9 115200 + + 20 + 10 + + 4 + 8 + 16 + + + 5 + 5 + 10 + + diff --git a/src/modelec_strat/include/modelec_strat/missions/promotion_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp similarity index 73% rename from src/modelec_strat/include/modelec_strat/missions/promotion_mission.hpp rename to src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp index d4a0c7b..473f6b6 100644 --- a/src/modelec_strat/include/modelec_strat/missions/promotion_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp @@ -2,6 +2,7 @@ #include #include +#include namespace Modelec { @@ -9,10 +10,10 @@ namespace Modelec * Banderole mission * */ - class PromotionMission : public Mission + class BannerMission : public Mission { public: - PromotionMission(const std::shared_ptr& nav); + BannerMission(const std::shared_ptr& nav); void start(rclcpp::Node::SharedPtr node) override; void update() override; @@ -30,5 +31,7 @@ namespace Modelec MissionStatus status_; std::shared_ptr nav_; rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr score_pub_; + int mission_score_ = 0; }; } diff --git a/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp index 032fe75..218fafc 100644 --- a/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp @@ -2,7 +2,7 @@ #include #include - +#include namespace Modelec { @@ -30,5 +30,7 @@ namespace Modelec rclcpp::Node::SharedPtr node_; rclcpp::Time start_time_; Point home_point_; + rclcpp::Publisher::SharedPtr score_pub_; + int mission_score_ = 0; }; } diff --git a/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp index eddbab0..eb7b59b 100644 --- a/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp @@ -3,6 +3,7 @@ #include #include #include +#include namespace Modelec { @@ -36,6 +37,8 @@ namespace Modelec { std::shared_ptr column_; std::shared_ptr closestDepoZone_; Point closestDepoZonePoint_; + rclcpp::Publisher::SharedPtr score_pub_; + int mission_score_ = 0; }; } diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index c1f7d56..6f6df25 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -60,6 +60,10 @@ namespace Modelec { std::pair 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_; int team_id_ = YELLOW; + Point spawn_yellow_; + Point spawn_blue_; float factor_close_enemy_ = 0; @@ -111,6 +125,7 @@ namespace Modelec { rclcpp::Subscription::SharedPtr go_to_sub_; rclcpp::Subscription::SharedPtr pos_sub_; + rclcpp::Publisher::SharedPtr pos_pub_; rclcpp::Subscription::SharedPtr enemy_pos_sub_; rclcpp::Subscription::SharedPtr enemy_pos_long_time_sub_; diff --git a/src/modelec_strat/include/modelec_strat/strat_fms.hpp b/src/modelec_strat/include/modelec_strat/strat_fms.hpp index a2eb56d..bc4ba53 100644 --- a/src/modelec_strat/include/modelec_strat/strat_fms.hpp +++ b/src/modelec_strat/include/modelec_strat/strat_fms.hpp @@ -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 #include +#include + +#include + +#include -#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::SharedPtr tirette_sub_; rclcpp::Publisher::SharedPtr state_pub_; rclcpp::Publisher::SharedPtr start_time_pub_; + rclcpp::Subscription::SharedPtr team_id_sub_; + + rclcpp::Client::SharedPtr client_start_; }; } diff --git a/src/modelec_strat/src/missions/banner_mission.cpp b/src/modelec_strat/src/missions/banner_mission.cpp new file mode 100644 index 0000000..bd51a57 --- /dev/null +++ b/src/modelec_strat/src/missions/banner_mission.cpp @@ -0,0 +1,73 @@ +#include + +#include + +namespace Modelec +{ + BannerMission::BannerMission(const std::shared_ptr& nav) : step_(GO_TO_FRONT), status_(MissionStatus::READY), nav_(nav) + { + } + + void BannerMission::start(rclcpp::Node::SharedPtr node) + { + node_ = node; + + mission_score_ = Config::get("config.mission_score.banner", 0); + + score_pub_ = node_->create_publisher("/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_; + } +} diff --git a/src/modelec_strat/src/missions/go_home_mission.cpp b/src/modelec_strat/src/missions/go_home_mission.cpp index 7b0a13c..f6948c1 100644 --- a/src/modelec_strat/src/missions/go_home_mission.cpp +++ b/src/modelec_strat/src/missions/go_home_mission.cpp @@ -1,5 +1,7 @@ #include +#include + namespace Modelec { GoHomeMission::GoHomeMission(const std::shared_ptr& 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("config.mission_score.go_home", 0); + + score_pub_ = node_->create_publisher("/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; } diff --git a/src/modelec_strat/src/missions/prepare_concert_mission.cpp b/src/modelec_strat/src/missions/prepare_concert_mission.cpp index d79335a..ff99799 100644 --- a/src/modelec_strat/src/missions/prepare_concert_mission.cpp +++ b/src/modelec_strat/src/missions/prepare_concert_mission.cpp @@ -1,5 +1,6 @@ #include #include +#include namespace Modelec { @@ -12,6 +13,10 @@ namespace Modelec { node_ = node; + mission_score_ = Config::get("config.mission_score.concert.niv_1", 0); + + score_pub_ = node_->create_publisher("/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; diff --git a/src/modelec_strat/src/missions/promotion_mission.cpp b/src/modelec_strat/src/missions/promotion_mission.cpp deleted file mode 100644 index ec003d7..0000000 --- a/src/modelec_strat/src/missions/promotion_mission.cpp +++ /dev/null @@ -1,56 +0,0 @@ -#include - -namespace Modelec -{ - PromotionMission::PromotionMission(const std::shared_ptr& 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_; - } -} diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 83415b2..ca5b05f 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -15,6 +15,14 @@ namespace Modelec { factor_close_enemy_ = Config::get("config.enemy.factor_close_enemy", -0.5f); + spawn_yellow_.x = Config::get("config.spawn.yellow@x", 0); + spawn_yellow_.y = Config::get("config.spawn.yellow@y", 0); + spawn_yellow_.theta = Config::get("config.spawn.yellow@theta", 0); + + spawn_blue_.x = Config::get("config.spawn.blue@x", 0); + spawn_blue_.y = Config::get("config.spawn.blue@y", 0); + spawn_blue_.theta = Config::get("config.spawn.blue@theta", 0.0f); + waypoint_reach_sub_ = node_->create_subscription( "odometry/waypoint_reach", 10, [this](const WaypointReachMsg::SharedPtr msg) { @@ -29,6 +37,8 @@ namespace Modelec { OnPos(msg); }); + pos_pub_ = node_->create_publisher("odometry/set_position", 10); + go_to_sub_ = node_->create_subscription( "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_) diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 42e6cb3..148a991 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -20,6 +20,32 @@ namespace Modelec start_time_pub_ = create_publisher("/strat/start_time", 10); + team_id_sub_ = create_subscription( + "/strat/team", 10, [this](const std_msgs::msg::Int8::SharedPtr msg) + { + team_id_ = static_cast(msg->data); + nav_->SetTeamId(team_id_); + nav_->SetSpawn(); + }); + + client_start_ = create_client("/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(); + request->start = true; + client_start_->async_send_request(request, [this](rclcpp::Client::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(nav_); + current_mission_ = std::make_unique(nav_); current_mission_->start(shared_from_this()); } current_mission_->update();