From 49aa2d539c62dd741fb83544c2c8f4b578a27753 Mon Sep 17 00:00:00 2001
From: acki <ackimixs@protonmail.com>
Date: Wed, 28 May 2025 18:12:25 -0400
Subject: [PATCH] ping lidar

---
 src/modelec_gui/src/modelec_gui.cpp           | 20 +++++++++++++++++++
 .../include/modelec_strat/enemy_manager.hpp   |  4 ++++
 src/modelec_strat/src/enemy_manager.cpp       |  7 +++++++
 3 files changed, 31 insertions(+)

diff --git a/src/modelec_gui/src/modelec_gui.cpp b/src/modelec_gui/src/modelec_gui.cpp
index 8bb770c..67b96e6 100644
--- a/src/modelec_gui/src/modelec_gui.cpp
+++ b/src/modelec_gui/src/modelec_gui.cpp
@@ -9,6 +9,26 @@ namespace ModelecGUI {
     ROS2QtGUI::ROS2QtGUI(rclcpp::Node::SharedPtr node, QWidget *parent)
         : QMainWindow(parent), node_(std::move(node)), stackedWidget(new QStackedWidget(this)) {
 
+        auto request = std::make_shared<std_srvs::srv::Empty::Request>();
+        auto client = node_->create_client<std_srvs::srv::Empty>("enemy_manager/ping");
+        int timeout = 3;
+        while (!client->wait_for_service(std::chrono::seconds(1)) && timeout-- > 0)
+        {
+            if (!rclcpp::ok())
+            {
+                RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
+                return;
+            }
+            RCLCPP_INFO(node_->get_logger(), "service not available, waiting again...");
+        }
+
+        if (timeout <= 0)
+        {
+            // Quit app
+            this->close();
+            return;
+        }
+
         // Add pages to stack
         resize(1200, 800);
 
diff --git a/src/modelec_strat/include/modelec_strat/enemy_manager.hpp b/src/modelec_strat/include/modelec_strat/enemy_manager.hpp
index 5e3a5ce..487e0f9 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 <modelec_interfaces/msg/odometry_pos.hpp>
 #include <modelec_interfaces/msg/strat_state.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
+#include <std_srvs/srv/empty.hpp>
 
 namespace Modelec
 {
@@ -27,6 +28,9 @@ namespace Modelec
         rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_pub_;
         rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr close_enemy_pos_pub_;
         rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_long_time_pub_;
+
+        rclcpp::Service<std_srvs::srv::Empty>::SharedPtr ping_service_;
+
         rclcpp::TimerBase::SharedPtr timer_;
 
         rclcpp::Time last_movement_time_;
diff --git a/src/modelec_strat/src/enemy_manager.cpp b/src/modelec_strat/src/enemy_manager.cpp
index e0ffd57..e7b1395 100644
--- a/src/modelec_strat/src/enemy_manager.cpp
+++ b/src/modelec_strat/src/enemy_manager.cpp
@@ -75,6 +75,13 @@ namespace Modelec
         );
 
         last_publish_time_ = this->now();
+
+        ping_service_ = this->create_service<std_srvs::srv::Empty>(
+            "enemy_manager/ping",
+            [this](const std::shared_ptr<std_srvs::srv::Empty::Request>,
+                   std::shared_ptr<std_srvs::srv::Empty::Response>)
+            {
+            });
     }
 
     void EnemyManager::OnCurrentPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)