modelec/TCPSocketServer
1
0
Fork 0
mirror of https://github.com/modelec/TCPSocketServer.git synced 2026-01-19 00:47:36 +01:00
Code Issues Packages Projects Releases Wiki Activity

lidar

Browse Source
This commit is contained in:
ackimixs
2024-05-07 16:00:32 +02:00
parent 135f9d45d2
commit f66120d5c5
1 changed files with 10 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
TCPServer.cpp
View File

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