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 34acacf..6e03805 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 @@ -43,6 +43,8 @@ namespace ModelecGUI void AskMap(); + void StartLidar(); + protected: void paintEvent(QPaintEvent*) override; @@ -87,6 +89,8 @@ namespace ModelecGUI rclcpp::Subscription::SharedPtr obstacle_removed_sub_; rclcpp::Client::SharedPtr ask_map_obstacle_client_; + rclcpp::Publisher::SharedPtr start_lidar_pub_; + modelec_interfaces::msg::OdometryPos enemy_pos_; bool hasEnemy = false; rclcpp::Publisher::SharedPtr enemy_pos_pub_; diff --git a/src/modelec_gui/src/modelec_gui.cpp b/src/modelec_gui/src/modelec_gui.cpp index a4bb27b..a84aa16 100644 --- a/src/modelec_gui/src/modelec_gui.cpp +++ b/src/modelec_gui/src/modelec_gui.cpp @@ -88,6 +88,7 @@ namespace ModelecGUI { connect(map_action_, &QAction::triggered, this, [this]() { stackedWidget->setCurrentWidget(test_map_page_); + test_map_page_->StartLidar(); }); connect(playmat_map_, &QAction::triggered, this, [this]() { diff --git a/src/modelec_gui/src/pages/test_map_page.cpp b/src/modelec_gui/src/pages/test_map_page.cpp index fa14233..09514b5 100644 --- a/src/modelec_gui/src/pages/test_map_page.cpp +++ b/src/modelec_gui/src/pages/test_map_page.cpp @@ -137,6 +137,8 @@ namespace ModelecGUI auto result2 = ask_map_obstacle_client_->async_send_request(std::make_shared()); rclcpp::spin_until_future_complete(node_->get_node_base_interface(), result2); + + start_lidar_pub_ = node_->create_publisher("lidar/start", 10); } void TestMapPage::setPlaymatGrid() @@ -161,6 +163,12 @@ namespace ModelecGUI ask_map_obstacle_client_->async_send_request(std::make_shared()); } + void TestMapPage::StartLidar() { + auto msg = std_msgs::msg::Bool(); + msg.data = true; + start_lidar_pub_->publish(msg); + } + void TestMapPage::paintEvent(QPaintEvent* paint_event) { QWidget::paintEvent(paint_event); diff --git a/src/modelec_strat/include/modelec_strat/enemy_manager.hpp b/src/modelec_strat/include/modelec_strat/enemy_manager.hpp index 49844d7..2daa6f5 100644 --- a/src/modelec_strat/include/modelec_strat/enemy_manager.hpp +++ b/src/modelec_strat/include/modelec_strat/enemy_manager.hpp @@ -5,6 +5,7 @@ #include #include #include +#include namespace Modelec { @@ -27,6 +28,7 @@ namespace Modelec rclcpp::Publisher::SharedPtr enemy_pos_pub_; rclcpp::Publisher::SharedPtr close_enemy_pos_pub_; rclcpp::Publisher::SharedPtr enemy_long_time_pub_; + rclcpp::Subscription::SharedPtr start_sub_; rclcpp::TimerBase::SharedPtr timer_; diff --git a/src/modelec_strat/src/enemy_manager.cpp b/src/modelec_strat/src/enemy_manager.cpp index 5a18635..e53123f 100644 --- a/src/modelec_strat/src/enemy_manager.cpp +++ b/src/modelec_strat/src/enemy_manager.cpp @@ -71,6 +71,14 @@ namespace Modelec enemy_long_time_pub_ = this->create_publisher( "/enemy/long_time", 10); + start_sub_ = this->create_subscription( + "/lidar/start", 10, + [this](const std_msgs::msg::Bool::SharedPtr msg) + { + game_stated_ = msg->data; + RCLCPP_INFO(this->get_logger(), "Game started state changed: %s", game_stated_ ? "true" : "false"); + }); + timer_ = this->create_wall_timer( std::chrono::nanoseconds(static_cast(refresh_rate_s_ )), [this]() @@ -89,11 +97,11 @@ namespace Modelec void EnemyManager::OnLidarData(const sensor_msgs::msg::LaserScan::SharedPtr msg) { - /*if (!game_stated_) + if (!game_stated_) { RCLCPP_INFO(this->get_logger(), "Game not started, ignoring Lidar data"); return; - }*/ + } is_enemy_close_ = false;