From b5d3c18eb6d02f64ce2dee1eb442f9ea1ee3e7ff Mon Sep 17 00:00:00 2001
From: ackimixs <ackimixs@protonmail.com>
Date: Wed, 8 May 2024 16:59:02 +0200
Subject: [PATCH] lidarTimeout

---
 TCPServer.cpp | 6 ++++++
 TCPServer.h   | 2 ++
 2 files changed, 8 insertions(+)

diff --git a/TCPServer.cpp b/TCPServer.cpp
index 80d200d..86ac277 100644
--- a/TCPServer.cpp
+++ b/TCPServer.cpp
@@ -163,10 +163,14 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket)
         this->sendToClient("strat;" + tokens[0] + ";set speed;" + std::to_string(this->speed) + "\n", clientSocket);
     }
     else if (tokens[0] == "lidar" && tokens[2] == "set pos") {
+        lidarGetPosTimeout++;
         std::vector<std::string> args = TCPUtils::split(tokens[3], ",");
         // TODO replace angle with the real angle calculated by the lidar when working
         this->lidarCalculatePos = {std::stof(args[0]), std::stof(args[1]), /*std::stof(args[2]) / 100*/ this->robotPose.theta};
         if (lidarCalculatePos.pos.x == -1 || lidarCalculatePos.pos.y == -1) {
+            if (lidarGetPosTimeout > 10) {
+                awaitForLidar = false;
+            }
             this->askLidarPosition();
         }
         else {
@@ -1647,6 +1651,8 @@ void TCPServer::removePot(StratPattern sp) {
 
 void TCPServer::getLidarPos() {
 
+    this->lidarGetPosTimeout = 0;
+
     this->broadcastMessage("strat;arduino;clear;1\n");
 
     usleep(1'000'000);
diff --git a/TCPServer.h b/TCPServer.h
index 8264ff6..2eb1062 100644
--- a/TCPServer.h
+++ b/TCPServer.h
@@ -175,6 +175,8 @@ private:
     int lidarSocket = -1;
     int arduinoSocket = -1;
 
+    int lidarGetPosTimeout = 0;
+
 public:
     explicit TCPServer(int port);