From f66120d5c515de489dc810157aaffc80aceb2fb5 Mon Sep 17 00:00:00 2001
From: ackimixs <ackimixs@protonmail.com>
Date: Tue, 7 May 2024 16:00:32 +0200
Subject: [PATCH] lidar

---
 TCPServer.cpp | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/TCPServer.cpp b/TCPServer.cpp
index 707074e..d84c995 100644
--- a/TCPServer.cpp
+++ b/TCPServer.cpp
@@ -1615,7 +1615,16 @@ void TCPServer::getLidarPos() {
 
     usleep(1'000'000);
 
-    this->setPosition(this->robotPose, this->lidarSocket);
+
+    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);
 
     usleep(100'000);