From d483b5be71cafc4b8e8f1e51007eb260c878b4de Mon Sep 17 00:00:00 2001
From: ackimixs <ackimixs@protonmail.com>
Date: Thu, 11 Apr 2024 10:50:22 +0200
Subject: [PATCH] don't move if no aruco (seg fault)

---
 TCPServer.cpp | 18 ++++++++++++------
 1 file changed, 12 insertions(+), 6 deletions(-)

diff --git a/TCPServer.cpp b/TCPServer.cpp
index 5a19325..6db3bd7 100644
--- a/TCPServer.cpp
+++ b/TCPServer.cpp
@@ -180,10 +180,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket)
     }
     if (tokens[0] == "aruco" && tokens[2] == "get aruco") {
         std::string arucoResponse = tokens[3];
-        if (arucoResponse == "404") {
-            usleep(50'000);
-            this->broadcastMessage("strat;aruco;get aruco;1\n");
-        } else {
+        if (arucoResponse != "404") {
             this->arucoTags.clear();
             std::vector<std::string> aruco = split(arucoResponse, ",");
             for (int i = 0; i < aruco.size() - 1; i += 7) {
@@ -198,8 +195,8 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket)
 
                 this->arucoTags.push_back(tag);
             }
-            this->waitForAruco = false;
         }
+        this->waitForAruco = false;
     }
     std::cout << "Received: " << message << std::endl;
 }
@@ -283,20 +280,29 @@ void TCPServer::startGame() {
 
     waitForAruco = true;
     this->broadcastMessage("strat;aruco;get aruco;1\n");
+
     while (this->waitForAruco) {
         usleep(100'000);
     }
 
+    // ReSharper disable once CppDFAUnreachableCode
+    if (this->arucoTags.empty()) {
+        return;
+        // TODO maybe rotate the robot but handle that here
+    }
     goToAruco(this->arucoTags[0], 1);
 
     // pi/4
     this->broadcastMessage("strat;arduino;angle;0");
-    this->robotPose.theta = 0;
     usleep(1'000'000);
     this->broadcastMessage("strat;aruco;get aruco;1\n");
     while (this->waitForAruco) {
         usleep(100'000);
     }
+
+    if (this->arucoTags.empty()) {
+        return;
+    }
     goToAruco(this->arucoTags[0], 0);
 
     this->broadcastMessage("strat;arduino;go;500,500\n");