From 135f9d45d214ef5589e5dfb313ab14cbffdc7cbc Mon Sep 17 00:00:00 2001
From: ackimixs <ackimixs@protonmail.com>
Date: Tue, 7 May 2024 15:59:15 +0200
Subject: [PATCH] lidar

---
 TCPServer.cpp | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/TCPServer.cpp b/TCPServer.cpp
index 6bb8e01..707074e 100644
--- a/TCPServer.cpp
+++ b/TCPServer.cpp
@@ -295,7 +295,13 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket)
             std::vector<std::string> pos = TCPUtils::split(tokens[3], ",");
             this->robotPose = {std::stof(pos[0]), std::stof(pos[1]), std::stof(pos[2]) / 100};
             if (!awaitForLidar) {
-                std::string toSend = "strat;lidar;set pos;" + std::to_string(static_cast<int>(this->robotPose.pos.x)) + "," + std::to_string(static_cast<int>(this->robotPose.pos.y)) + "," + std::to_string(static_cast<int>(this->robotPose.theta * 100)) + "\n";
+                std::string toSend;
+
+                if (this->robotPose.theta < 0 ) {
+                    toSend = "strat;lidar;set pos;" + std::to_string(static_cast<int>(this->robotPose.pos.x)) + "," + std::to_string(static_cast<int>(this->robotPose.pos.y)) + "," + std::to_string(static_cast<int>((this->robotPose.theta + 2 * PI) * 100)) + "\n";
+                } else {
+                    toSend = "strat;lidar;set pos;" + std::to_string(static_cast<int>(this->robotPose.pos.x)) + "," + std::to_string(static_cast<int>(this->robotPose.pos.y)) + "," + std::to_string(static_cast<int>(this->robotPose.theta * 100)) + "\n";
+                }
                 this->broadcastMessage(toSend, clientSocket);
             }
         }