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(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y)) + "," + std::to_string(static_cast((this->robotPose.theta + 2 * PI) * 100)) + "\n"; + } + else { + toSend = "strat;lidar;set pos;" + std::to_string(static_cast(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y)) + "," + std::to_string(static_cast(this->robotPose.theta * 100)) + "\n"; + } + + this->broadcastMessage(toSend); usleep(100'000);