lidarTimeout

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);

TCPServer.h

@@ -175,6 +175,8 @@ private:
     int lidarSocket = -1;
     int arduinoSocket = -1;
 
+    int lidarGetPosTimeout = 0;
+
 public:
     explicit TCPServer(int port);