From f4efd38059b74a8ba35b3e4fa50f3dd74b0387da Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 19 Apr 2024 10:55:26 +0200 Subject: [PATCH] handle the SIGTERM, now stop the the thread of the game --- TCPServer.cpp | 217 +++----------------------------------------------- TCPServer.h | 5 +- main.cpp | 15 +++- 3 files changed, 26 insertions(+), 211 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 3c7d4c9..cc4fe0d 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -461,10 +461,8 @@ void TCPServer::startGameBlueTeam() { this->broadcastMessage("strat;arduino;go;1000,1700\n"); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;speed;150\n"); this->broadcastMessage("strat;arduino;angle;157\n"); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;speed;200\n"); usleep(1'000'000); @@ -507,10 +505,8 @@ void TCPServer::startGameBlueTeam() { this->broadcastMessage("strat;arduino;go;1000,1700\n"); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;speed;150\n"); this->broadcastMessage("strat;arduino;angle;157\n"); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;speed;200\n"); usleep(1'000'000); @@ -553,10 +549,8 @@ void TCPServer::startGameBlueTeam() { this->broadcastMessage("strat;arduino;go;1000,1700\n"); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;speed;150\n"); this->broadcastMessage("strat;arduino;angle;314\n"); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;speed;200\n"); usleep(1'000'000); @@ -681,10 +675,8 @@ void TCPServer::startGameBlueTeam() { this->broadcastMessage("strat;arduino;go;1000,250\n"); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;speed;150\n"); this->broadcastMessage("strat;arduino;angle;-157\n"); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;speed;200\n"); arucoTags.clear(); this->broadcastMessage("strat;aruco;get aruco;1\n"); @@ -809,12 +801,10 @@ void TCPServer::startGameBlueTeam() { this->broadcastMessage("strat;arduino;angle;157\n"); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;speed;150\n"); this->broadcastMessage("strat;arduino;go;762,0\n"); usleep(1'000'000); - this->broadcastMessage("strat;arduino;speed;200\n"); for (int i = 0; i < 3; i++) { if (pinceState[i] == WHITE_FLOWER) { @@ -835,207 +825,14 @@ void TCPServer::startGameBlueTeam() { } } - /*toSend = "strat;arduino;go;" + std::to_string(static_cast(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y + 200)) + "\n"; - this->broadcastMessage(toSend); - awaitRobotIdle(); - - this->broadcastMessage("strat;servo_moteur;baisser bras;1\n"); - - this->broadcastMessage("strat;arduino;angle;-157\n"); - awaitRobotIdle(); - - timeout = 0; - found = false; - while (!found) { - for (const auto & arucoTag : this->arucoTags) { - if (TCPUtils::endWith(arucoTag.name(), "flower")) { - if (arucoTag.pos().first[0] < 800 && arucoTag.pos().first[0] > 300 && arucoTag.pos().first[1] < 300 && arucoTag.pos().first[1] > -300) { - tag = arucoTag; - found = true; - break; - } - } - } - - if (!found) { - this->broadcastMessage("start;aruco;get aruco;1\n"); - usleep(500'000); - timeout++; - if (timeout > 10) { - return; - } - } - } - - if (pinceState[1] == NONE) { - goToAruco(tag, 1); - } else if (pinceState[2] == NONE) { - goToAruco(tag, 2); - } else if (pinceState[0] == NONE) { - goToAruco(tag, 0); - } else { - return; - } - - toSend = "strat;arduino;go;" + std::to_string(static_cast(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y - 350)) + "\n"; - this->broadcastMessage(toSend); - awaitRobotIdle(); - - this->broadcastMessage("strat;arduino;angle;157\n"); - awaitRobotIdle(); - - timeout = 0; - found = false; - while (!found) { - for (const auto & arucoTag : this->arucoTags) { - if (TCPUtils::endWith(arucoTag.name(), "flower")) { - if (arucoTag.pos().first[0] < 800 && arucoTag.pos().first[0] > 300 && arucoTag.pos().first[1] < 300 && arucoTag.pos().first[1] > -300) { - tag = arucoTag; - found = true; - break; - } - } - } - - if (!found) { - this->broadcastMessage("start;aruco;get aruco;1\n"); - usleep(500'000); - timeout++; - if (timeout > 10) { - return; - } - } - } - - if (pinceState[1] == NONE) { - goToAruco(tag, 1); - } else if (pinceState[2] == NONE) { - goToAruco(tag, 2); - } else if (pinceState[0] == NONE) { - goToAruco(tag, 0); - } else { - return; - } - - toSend = "strat;arduino;go;" + std::to_string(static_cast(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y - 350)) + "\n"; - this->broadcastMessage(toSend); - awaitRobotIdle(); - - this->broadcastMessage("strat;arduino;angle;157\n"); - awaitRobotIdle(); - - timeout = 0; - found = false; - while (!found) { - for (const auto & arucoTag : this->arucoTags) { - if (TCPUtils::endWith(arucoTag.name(), "flower")) { - if (arucoTag.pos().first[0] < 800 && arucoTag.pos().first[0] > 300 && arucoTag.pos().first[1] < 300 && arucoTag.pos().first[1] > -300) { - tag = arucoTag; - found = true; - break; - } - } - } - - if (!found) { - this->broadcastMessage("start;aruco;get aruco;1\n"); - usleep(500'000); - timeout++; - if (timeout > 10) { - return; - } - } - } - - if (pinceState[1] == NONE) { - goToAruco(tag, 1); - } else if (pinceState[2] == NONE) { - goToAruco(tag, 2); - } else if (pinceState[0] == NONE) { - goToAruco(tag, 0); - } else { - return; - } - - toSend = "strat;arduino;go;" + std::to_string(static_cast(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y - 200)) + "\n"; - this->broadcastMessage(toSend); - awaitRobotIdle(); - - this->broadcastMessage("strat;arduino;angle;314\n"); - awaitRobotIdle(); - - pinceHavePurpleFlower.clear(); - for (int i = 0; i < 3; i++) { - if (pinceState[i] == PURPLE_FLOWER) { - pinceHavePurpleFlower.push_back(i); - } - } - - if (!pinceHavePurpleFlower.empty()) { - this->broadcastMessage("strat;servo_moteur;lever bras;1\n"); - - this->broadcastMessage("strat;aduino;go;225,225\n"); - awaitRobotIdle(); - this->broadcastMessage("strat;arduino;angle;314\n"); - awaitRobotIdle(); - - for (auto & toDrop : pinceHavePurpleFlower) { - toSend = "strat;servo_moteur;ouvrir pince;" + std::to_string(toDrop) + "\n"; - this->broadcastMessage(toSend); - usleep(200'000); - pinceState[toDrop] = NONE; - toSend = "strat;servo_moteur;fermer pince;" + std::to_string(toDrop) + "\n"; - this->broadcastMessage(toSend); - usleep(100'000); - } - - toSend = "strat;arduino;go;" + std::to_string(static_cast(this->robotPose.pos.x + 150)) + "," + std::to_string(static_cast(this->robotPose.pos.y)) + "\n"; - this->broadcastMessage(toSend); - awaitRobotIdle(); - } - - - if (pinceHavePurpleFlower.size() < 3) { - this->broadcastMessage("strat;arduino;go;762,300\n"); - awaitRobotIdle(); - - this->broadcastMessage("strat;arduino;angle;157\n"); - awaitRobotIdle(); - - this->broadcastMessage("strat;arduino;speed;150\n"); - this->broadcastMessage("strat;arduino;go;762,0\n"); - usleep(1'000'000); - - this->broadcastMessage("strat;arduino;speed;200\n"); - - for (int i = 0; i < 3; i++) { - if (pinceState[i] == WHITE_FLOWER) { - toSend = "strat;servo_moteur;ouvrir pince;" + std::to_string(i) + "\n"; - this->broadcastMessage(toSend); - usleep(200'000); - pinceState[i] = NONE; - toSend = "strat;servo_moteur;fermer pince;" + std::to_string(i) + "\n"; - this->broadcastMessage(toSend); - usleep(100'000); - } - - this->broadcastMessage("strat;arduino;speed;200\n"); - - toSend = "strat;arduino;go;" + std::to_string(static_cast(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y + 350)) + "\n"; - this->broadcastMessage(toSend); - awaitRobotIdle(); - } - }*/ toSend = "strat;arduino;go;" + std::to_string(static_cast(this->endRobotPose.pos.x)) + "," + std::to_string(static_cast(this->endRobotPose.pos.y)) + "\n"; this->broadcastMessage(toSend); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;speed;150\n"); toSend = "strat;arduino;angle;" + std::to_string(static_cast(this->endRobotPose.theta * 100)); this->broadcastMessage(toSend); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;speed;200\n"); this->broadcastMessage("strat;servo_moteur;clear;1"); } @@ -1276,10 +1073,8 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { double thetaPrime = std::atan2(yPrime, xPrime); toSend = "strat;arduino;angle;" + std::to_string(static_cast((this->robotPose.theta + rotate - thetaPrime) * 100)) + "\n"; - this->broadcastMessage("strat;arduino;speed;150\n"); this->broadcastMessage(toSend); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;speed;200\n"); double robotPosForPotX = (xPrime * std::cos(theta) + yPrime * std::sin(theta)) + robotPosX; double robotPosForPotY = (-xPrime * std::sin(theta) + yPrime * std::cos(theta)) + robotPosY; @@ -1291,11 +1086,8 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { toSend = "strat;servo_moteur;fermer pince;" + std::to_string(pince) + "\n"; this->broadcastMessage(toSend); usleep(500'000); - //this->broadcastMessage("strat;servo_moteur;lever bras;1\n"); this->broadcastMessage("strat;arduino;speed;200\n"); pinceState[pince] = TCPUtils::startWith(arucoTag.name(), "Purple_flower") ? PURPLE_FLOWER : WHITE_FLOWER; - // toSend = "strat;client;have aruco;" + std::to_string(pince) + "," + arucoTag.name() + "," + (pinceState[pince] == PURPLE_FLOWER ? "purple" : "white") + "\n"; - // this->broadcastMessage(toSend); } void TCPServer::askArduinoPos() { @@ -1319,11 +1111,20 @@ void TCPServer::askArduinoPos() { void TCPServer::awaitRobotIdle() { isRobotIdle = 0; + int timeout = 0; // ReSharper disable once CppDFAConstantConditions // ReSharper disable once CppDFAEndlessLoop usleep(200'000); while (isRobotIdle < 3) { usleep(200'000); this->broadcastMessage("strat;arduino;get state;1\n"); + timeout++; + if (timeout > 20) { + this->broadcastMessage("strat;arduino;clear;1"); + } } } + +void TCPServer::sleepServer(int ms) { + +} diff --git a/TCPServer.h b/TCPServer.h index 9e13b7e..8c1c636 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -8,6 +8,7 @@ #include #include #include +#include #include #include "utils.h" @@ -48,7 +49,7 @@ private: std::vector clientThreads; std::vector clientSockets; // Store connected client sockets int connectedClients = 0; // Track the number of connected clients - bool _shouldStop = false; // Flag to indicate if the server should stop + std::atomic _shouldStop = false; // Flag to indicate if the server should stop std::vector clients; // Store connected clients PinceState pinceState[3] = {NONE, NONE, NONE}; @@ -110,5 +111,7 @@ public: void awaitRobotIdle(); + void sleepServer(int ms); + ~TCPServer(); }; diff --git a/main.cpp b/main.cpp index 1166f46..8d02ba5 100644 --- a/main.cpp +++ b/main.cpp @@ -1,6 +1,17 @@ #include "TCPServer.h" +#include + +std::atomic shouldStop = false; + +void signalHandler( int signum ) { + + shouldStop = true; + + exit(signum); +} int main(int argc, char* argv[]) { + signal(SIGINT, signalHandler); int port = 8080; if (argc >= 2) { @@ -12,8 +23,8 @@ int main(int argc, char* argv[]) { try { server.start(); - while (!server.shouldStop()) { - usleep(1'000'000); + while (!server.shouldStop() && !shouldStop) { + usleep(500'000); } server.stop();