diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index c79d9bf..f7104c8 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -274,6 +274,10 @@ namespace Modelec bool success = true; ResolveSetPIDRequest(success); } + else if (tokens[1] == "POS") + { + // position set + } else { RCLCPP_INFO(this->get_logger(), "PCB response: %s", msg->data.c_str()); 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 bb61bdb..1336c33 100644 --- a/src/modelec_gui/include/modelec_gui/pages/home_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/home_page.hpp @@ -8,6 +8,7 @@ #include #include +#include namespace ModelecGUI { @@ -18,6 +19,8 @@ namespace ModelecGUI public: HomePage(rclcpp::Node::SharedPtr node, QWidget* parent = nullptr); + void Init(); + public slots: void onYellowButtonClicked(); @@ -31,7 +34,8 @@ namespace ModelecGUI rclcpp::Node::SharedPtr node_; - rclcpp::Publisher::SharedPtr team_publisher_; + rclcpp::Publisher::SharedPtr team_pub_; + rclcpp::Publisher::SharedPtr reset_strat_pub_; QVBoxLayout* v_layout_; QHBoxLayout* h_layout_; 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 bd690f0..27c5082 100644 --- a/src/modelec_gui/include/modelec_gui/pages/map_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/map_page.hpp @@ -33,6 +33,10 @@ namespace ModelecGUI { rclcpp::Node::SharedPtr get_node() const { return node_; } + void AskMap(); + + void Reset(); + protected: void paintEvent(QPaintEvent*) override; @@ -42,6 +46,8 @@ namespace ModelecGUI { void resizeEvent(QResizeEvent* event) override; + rclcpp::TimerBase::SharedPtr reset_timer_; + QSvgRenderer* renderer; QVBoxLayout* v_layout; 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 index 834b4cc..ffb6ce3 100644 --- a/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp @@ -39,6 +39,8 @@ namespace ModelecGUI { void toggleShowObstacle(); + void AskMap(); + protected: void paintEvent(QPaintEvent*) override; diff --git a/src/modelec_gui/src/modelec_gui.cpp b/src/modelec_gui/src/modelec_gui.cpp index 8a7e8eb..4936e19 100644 --- a/src/modelec_gui/src/modelec_gui.cpp +++ b/src/modelec_gui/src/modelec_gui.cpp @@ -61,6 +61,8 @@ namespace ModelecGUI { connect(home_action_, &QAction::triggered, this, [this]() { stackedWidget->setCurrentIndex(0); + home_page_->Init(); + map_page_->Reset(); }); connect(odo_action_, &QAction::triggered, this, [this]() { diff --git a/src/modelec_gui/src/pages/home_page.cpp b/src/modelec_gui/src/pages/home_page.cpp index 0b015b2..e8a2c84 100644 --- a/src/modelec_gui/src/pages/home_page.cpp +++ b/src/modelec_gui/src/pages/home_page.cpp @@ -33,17 +33,24 @@ namespace ModelecGUI setLayout(v_layout_); - team_publisher_ = node_->create_publisher("/strat/team", 10); + team_pub_ = node_->create_publisher("/strat/team", 10); + + reset_strat_pub_ = node_->create_publisher("/strat/reset", 10); connect(yellow_button_, &QPushButton::clicked, this, &HomePage::onYellowButtonClicked); connect(blue_button_, &QPushButton::clicked, this, &HomePage::onBlueButtonClicked); } + void HomePage::Init() + { + reset_strat_pub_->publish(std_msgs::msg::Empty()); + } + void HomePage::onYellowButtonClicked() { std_msgs::msg::Int8 msg; msg.data = 0; - team_publisher_->publish(msg); + team_pub_->publish(msg); emit TeamChoose(); } @@ -52,7 +59,7 @@ namespace ModelecGUI { std_msgs::msg::Int8 msg; msg.data = 1; - team_publisher_->publish(msg); + team_pub_->publish(msg); emit TeamChoose(); } diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp index 162a656..0f74aa6 100644 --- a/src/modelec_gui/src/pages/map_page.cpp +++ b/src/modelec_gui/src/pages/map_page.cpp @@ -146,6 +146,37 @@ namespace ModelecGUI rclcpp::spin_until_future_complete(node_->get_node_base_interface(), result2); } + void MapPage::AskMap() + { + reset_timer_ = node_->create_wall_timer( + std::chrono::seconds(1), + [this]() { + + 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..."); + } + + ask_map_obstacle_client_->async_send_request(std::make_shared()); + + reset_timer_->cancel(); + }); + } + + void MapPage::Reset() + { + isGameStarted_ = false; + lastWapointWasEnd = true; + + qpoints.clear(); + + AskMap(); + } + void MapPage::paintEvent(QPaintEvent* paint_event) { QWidget::paintEvent(paint_event); @@ -226,7 +257,7 @@ namespace ModelecGUI void MapPage::OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg) { - obstacle_.emplace(msg->id, *msg); + obstacle_[msg->id] = *msg; } void MapPage::resizeEvent(QResizeEvent* event) diff --git a/src/modelec_gui/src/pages/test_map_page.cpp b/src/modelec_gui/src/pages/test_map_page.cpp index 0b5fb91..008c307 100644 --- a/src/modelec_gui/src/pages/test_map_page.cpp +++ b/src/modelec_gui/src/pages/test_map_page.cpp @@ -170,6 +170,11 @@ namespace ModelecGUI show_obstacle_ = !show_obstacle_; } + void TestMapPage::AskMap() + { + ask_map_obstacle_client_->async_send_request(std::make_shared()); + } + void TestMapPage::paintEvent(QPaintEvent* paint_event) { QWidget::paintEvent(paint_event); diff --git a/src/modelec_strat/include/modelec_strat/pami_manager.hpp b/src/modelec_strat/include/modelec_strat/pami_manager.hpp index 4751a76..7a69109 100644 --- a/src/modelec_strat/include/modelec_strat/pami_manager.hpp +++ b/src/modelec_strat/include/modelec_strat/pami_manager.hpp @@ -10,6 +10,7 @@ #include #include +#include namespace Modelec { @@ -40,6 +41,7 @@ namespace Modelec { rclcpp::Publisher::SharedPtr add_obs_pub_; rclcpp::Publisher::SharedPtr remove_obs_pub_; rclcpp::Publisher::SharedPtr score_pub_; + rclcpp::Subscription::SharedPtr reset_strat_sub_; }; } \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/strat_fms.hpp b/src/modelec_strat/include/modelec_strat/strat_fms.hpp index bc4ba53..21f54ed 100644 --- a/src/modelec_strat/include/modelec_strat/strat_fms.hpp +++ b/src/modelec_strat/include/modelec_strat/strat_fms.hpp @@ -17,6 +17,7 @@ #include #include +#include namespace Modelec @@ -41,6 +42,8 @@ namespace Modelec void Init(); + void Reset(); + protected: void transition(State next, const std::string& reason); @@ -51,6 +54,7 @@ namespace Modelec rclcpp::TimerBase::SharedPtr timer_; rclcpp::Time match_start_time_; bool started_ = false; + bool setup_ = false; std::unique_ptr current_mission_; int team_id_ = 0; @@ -62,6 +66,7 @@ namespace Modelec rclcpp::Publisher::SharedPtr state_pub_; rclcpp::Publisher::SharedPtr start_time_pub_; rclcpp::Subscription::SharedPtr team_id_sub_; + rclcpp::Subscription::SharedPtr reset_strat_sub_; rclcpp::Client::SharedPtr client_start_; }; diff --git a/src/modelec_strat/src/pami_manager.cpp b/src/modelec_strat/src/pami_manager.cpp index 82be537..f593f26 100644 --- a/src/modelec_strat/src/pami_manager.cpp +++ b/src/modelec_strat/src/pami_manager.cpp @@ -73,6 +73,13 @@ namespace Modelec "obstacle/remove", 10); score_pub_ = create_publisher("/strat/score", 10); + + reset_strat_sub_ = create_subscription( + "/strat/reset", 10, [this](const std_msgs::msg::Empty::SharedPtr) + { + timer_add_->cancel(); + timer_remove_->cancel(); + }); } bool PamiManger::ReadFromXML(const std::string& filename) diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 148a991..cf6f259 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -5,12 +5,12 @@ namespace Modelec { - StratFMS::StratFMS() : Node("start_fms"), state_(State::INIT) + StratFMS::StratFMS() : Node("start_fms") { tirette_sub_ = create_subscription( "/tirette", 10, [this](const std_msgs::msg::Bool::SharedPtr msg) { - if (!started_ && msg->data) + if (setup_ && !started_ && msg->data) { started_ = true; } @@ -23,11 +23,19 @@ namespace Modelec team_id_sub_ = create_subscription( "/strat/team", 10, [this](const std_msgs::msg::Int8::SharedPtr msg) { + setup_ = true; team_id_ = static_cast(msg->data); nav_->SetTeamId(team_id_); nav_->SetSpawn(); }); + reset_strat_sub_ = create_subscription( + "/strat/reset", 10, [this](const std_msgs::msg::Empty::SharedPtr) + { + RCLCPP_INFO(get_logger(), "Resetting strat"); + Reset(); + }); + client_start_ = create_client("/odometry/start"); while (!client_start_->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { @@ -61,12 +69,25 @@ namespace Modelec RCLCPP_INFO(this->get_logger(), "StratFMS fully initialized"); + state_ = State::INIT; + started_ = false; + setup_ = false; + current_mission_.reset(); + match_start_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + timer_ = create_wall_timer(std::chrono::milliseconds(100), [this] { update(); }); } + void StratFMS::Reset() + { + timer_->cancel(); + + Init(); + } + void StratFMS::transition(State next, const std::string& reason) { RCLCPP_INFO(get_logger(), "Transition %d -> %d: %s", static_cast(state_), static_cast(next), @@ -88,7 +109,6 @@ namespace Modelec RCLCPP_INFO_ONCE(get_logger(), "State: INIT"); transition(State::WAIT_START, "System ready"); break; - case State::WAIT_START: if (started_) {