modelec/TCPSocketServer
1
0
Fork 0
mirror of https://github.com/modelec/TCPSocketServer.git synced 2026-03-24 16:40:30 +01:00
Code Issues Packages Projects Releases Wiki Activity

don't move if no aruco (seg fault)

Browse Source
This commit is contained in:
ackimixs
2024-04-11 10:50:22 +02:00
parent fb1c46efc3
commit d483b5be71
1 changed files with 12 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
TCPServer.cpp
View File

@@ -180,10 +180,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket)
}
if (tokens[0] == "aruco" && tokens[2] == "get aruco") {
std::string arucoResponse = tokens[3];
if (arucoResponse == "404") {
usleep(50'000);
this->broadcastMessage("strat;aruco;get aruco;1\n");
} else {
if (arucoResponse != "404") {
this->arucoTags.clear();
std::vector<std::string> aruco = split(arucoResponse, ",");
for (int i = 0; i < aruco.size() - 1; i += 7) {
@@ -198,8 +195,8 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket)
this->arucoTags.push_back(tag);
}
this->waitForAruco = false;
}
this->waitForAruco = false;
}
std::cout << "Received: " << message << std::endl;
}
@@ -283,20 +280,29 @@ void TCPServer::startGame() {
waitForAruco = true;
this->broadcastMessage("strat;aruco;get aruco;1\n");
while (this->waitForAruco) {
usleep(100'000);
}
// ReSharper disable once CppDFAUnreachableCode
if (this->arucoTags.empty()) {
return;
// TODO maybe rotate the robot but handle that here
}
goToAruco(this->arucoTags[0], 1);
// pi/4
this->broadcastMessage("strat;arduino;angle;0");
this->robotPose.theta = 0;
usleep(1'000'000);
this->broadcastMessage("strat;aruco;get aruco;1\n");
while (this->waitForAruco) {
usleep(100'000);
}
if (this->arucoTags.empty()) {
return;
}
goToAruco(this->arucoTags[0], 0);
this->broadcastMessage("strat;arduino;go;500,500\n");