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 15:59:15 +02:00
parent 00767e30eb
commit 135f9d45d2
1 changed files with 7 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
TCPServer.cpp
View File

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