From 4458fd814bdb24737c09f0f018e78010316c367d Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 22 Apr 2024 11:51:04 +0200 Subject: [PATCH] rework completely the aruco system --- TCPServer.cpp | 482 ++++++++++++++++++++++---------------------------- TCPServer.h | 27 ++- utils.cpp | 43 +++-- utils.h | 36 +++- 4 files changed, 296 insertions(+), 292 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index cc4fe0d..255dc6d 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1,8 +1,5 @@ #include "TCPServer.h" -#include -#include - ClientHandler::ClientHandler(int clientSocket, TCPServer* server) : clientSocket(clientSocket), server(server) {}; void ClientHandler::handle() { @@ -249,7 +246,6 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) else if (tokens[0] == "aruco" && tokens[2] == "get aruco") { std::string arucoResponse = tokens[3]; if (arucoResponse != "404") { - this->arucoTags.clear(); std::vector aruco = TCPUtils::split(arucoResponse, ","); for (int i = 0; i < aruco.size() - 1; i += 7) { ArucoTag tag; @@ -259,37 +255,38 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) tag.setPos(std::stof(aruco[i + 2]), std::stof(aruco[i + 3])); tag.setRot(std::stof(aruco[i + 4]), std::stof(aruco[i + 5]), std::stof(aruco[i + 6])); - this->arucoTags.push_back(tag); + handleArucoTag(tag); } } } - else if (tokens[0] == "arduino" && tokens[2] == "set state") { - if (TCPUtils::startWith(tokens[3], "0")) { - this->isRobotIdle++; + else if (tokens[0] == "arduino") { + if (tokens[2] == "set state") { + if (TCPUtils::startWith(tokens[3], "0")) { + this->isRobotIdle++; + } + } else if (tokens[2] == "set speed") { + this->speed = std::stoi(tokens[3]); } + } else if (tokens[2] == "get speed") { + this->sendToClient("strat;" + tokens[0] + ";set speed;" + std::to_string(this->speed) + "\n", clientSocket); } std::cout << "Received: " << message << std::endl; } void TCPServer::broadcastMessage(const char* message, int senderSocket) { - std::string temp = message; - this->broadcastMessage(temp, senderSocket); -} - -void TCPServer::broadcastMessage(std::string &message, int senderSocket) { - if (!TCPUtils::endWith(message, "\n")) { - message += "\n"; - } - for (int clientSocket : clientSockets) { if (clientSocket != senderSocket) { // Exclude the sender's socket - send(clientSocket, message.c_str(), message.size(), 0); + send(clientSocket, message, strlen(message), 0); } } } -void TCPServer::sendToClient(std::string &message, int clientSocket) { +void TCPServer::broadcastMessage(const std::string &message, int senderSocket) { + this->broadcastMessage(message.c_str(), senderSocket); +} + +void TCPServer::sendToClient(const std::string &message, int clientSocket) { this->sendToClient(message.c_str(), clientSocket); } @@ -302,7 +299,7 @@ void TCPServer::sendToClient(const char *message, int clientSocket) { } } -void TCPServer::sendToClient(std::string &message, const std::string &clientName) { +void TCPServer::sendToClient(const std::string &message, const std::string &clientName) { this->sendToClient(message.c_str(), clientName); } @@ -373,192 +370,137 @@ void TCPServer::checkIfAllClientsReady() } void TCPServer::startGameBlueTeam() { - // TODO redo that part because that give point to the other team - this->broadcastMessage("strat;arduino;speed;130\n"); + this->setSpeed(130); this->broadcastMessage("strat;servo_moteur;check panneau;7\n"); usleep(100'000); - std::string toSend = "strat;arduino;go;380," + std::to_string(static_cast(this->robotPose.pos.y)) + "\n"; - this->broadcastMessage(toSend); + std::string toSend; + this->go(380, this->robotPose.pos.y); awaitRobotIdle(); this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n"); - toSend = "strat;arduino;go;474," + std::to_string(static_cast(this->robotPose.pos.y)) + "\n"; - this->broadcastMessage(toSend); + this->go(474, this->robotPose.pos.y); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;angle;0\n"); + this->rotate(0); awaitRobotIdle(); this->broadcastMessage("strat;servo_moteur;check panneau;7\n"); usleep(100'000); - toSend = "strat;arduino;go;620," + std::to_string(static_cast(this->robotPose.pos.y)) + "\n"; - this->broadcastMessage(toSend); + this->go(620, this->robotPose.pos.y); awaitRobotIdle(); this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n"); - toSend = "strat;arduino;go;749," + std::to_string(static_cast(this->robotPose.pos.y)) + "\n"; - this->broadcastMessage(toSend); + this->go(749, this->robotPose.pos.y); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;angle;0\n"); + this->rotate(0); awaitRobotIdle(); this->broadcastMessage("strat;servo_moteur;check panneau;7\n"); usleep(100'000); - this->broadcastMessage("strat;arduino;go;1000,1700\n"); + this->go(1000, 1700); awaitRobotIdle(); this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n"); - this->broadcastMessage("strat;arduino;angle;157\n"); + this->rotate(PI / 2); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;speed;200\n"); + this->setSpeed(200); - this->broadcastMessage("strat;arduino;go;1000,1800\n"); + this->go(1000, 1800); awaitRobotIdle(); + this->rotate(PI / 2); + usleep(1'000'000); - arucoTags.clear(); + this->arucoTags.clear(); this->broadcastMessage("strat;aruco;get aruco;1\n"); + for (int i = 0; i < 5; i++) { + usleep(500'000); + this->broadcastMessage("strat;aruco;get aruco;1\n"); + } - int timeout = 0; - ArucoTag tag; - bool found = false; - while (!found) { - for (const auto & arucoTag : this->arucoTags) { - if (TCPUtils::endWith(arucoTag.name(), "flower")) { - if (arucoTag.pos().first[0] < 1000 && arucoTag.pos().first[0] > 100 && arucoTag.pos().first[1] < 300 && arucoTag.pos().first[1] > -300) { - tag = arucoTag; - found = true; - break; - } - } - } + std::optional tag = getBiggestArucoTag(300, 700, -200, 200); - if (!found) { - this->broadcastMessage("start;aruco;get aruco;1\n"); - usleep(500'000); - timeout++; - if (timeout > 10) { - return; - } + if (tag.has_value()) { + if (pinceState[1] == NONE) { + goToAruco(tag.value(), 1); + } else if (pinceState[2] == NONE) { + goToAruco(tag.value(), 2); + } else if (pinceState[0] == NONE) { + goToAruco(tag.value(), 0); } } - 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; - } - - this->broadcastMessage("strat;arduino;go;1000,1700\n"); + this->go(1000, 1700); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;angle;157\n"); + this->rotate(PI / 2); awaitRobotIdle(); usleep(1'000'000); - arucoTags.clear(); + this->arucoTags.clear(); this->broadcastMessage("strat;aruco;get aruco;1\n"); + for (int i = 0; i < 5; i++) { + usleep(500'000); + this->broadcastMessage("strat;aruco;get aruco;1\n"); + } - timeout = 0; - found = false; - while (!found) { - for (const auto & arucoTag : this->arucoTags) { - if (TCPUtils::contains(arucoTag.name(), "flower")) { - if (arucoTag.pos().first[0] < 1000 && arucoTag.pos().first[0] > 300 && arucoTag.pos().first[1] < 300 && arucoTag.pos().first[1] > -300) { - tag = arucoTag; - found = true; - break; - } - } - } + tag = getBiggestArucoTag(300, 700, -200, 200); - if (!found) { - this->broadcastMessage("start;aruco;get aruco;1\n"); - usleep(500'000); - timeout++; - if (timeout > 10) { - return; - } + if (tag.has_value()) { + if (pinceState[1] == NONE) { + goToAruco(tag.value(), 1); + } else if (pinceState[2] == NONE) { + goToAruco(tag.value(), 2); + } else if (pinceState[0] == NONE) { + goToAruco(tag.value(), 0); } } - 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; - } - - this->broadcastMessage("strat;arduino;go;1000,1700\n"); + this->go(1000, 1700); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;angle;157\n"); + this->rotate(PI / 2); awaitRobotIdle(); usleep(1'000'000); - arucoTags.clear(); + this->arucoTags.clear(); this->broadcastMessage("strat;aruco;get aruco;1\n"); + for (int i = 0; i < 5; i++) { + usleep(500'000); + this->broadcastMessage("strat;aruco;get aruco;1\n"); + } - timeout = 0; - found = false; - while (!found) { - for (const auto & arucoTag : this->arucoTags) { - if (TCPUtils::contains(arucoTag.name(), "flower")) { - if (arucoTag.pos().first[0] < 1000 && arucoTag.pos().first[0] > 300 && arucoTag.pos().first[1] < 300 && arucoTag.pos().first[1] > -300) { - tag = arucoTag; - found = true; - break; - } - } - } + tag = getBiggestArucoTag(300, 700, -200, 200); - if (!found) { - this->broadcastMessage("start;aruco;get aruco;1\n"); - usleep(500'000); - timeout++; - if (timeout > 10) { - return; - } + if (tag.has_value()) { + if (pinceState[1] == NONE) { + goToAruco(tag.value(), 1); + } else if (pinceState[2] == NONE) { + goToAruco(tag.value(), 2); + } else if (pinceState[0] == NONE) { + goToAruco(tag.value(), 0); } } - 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; - } - - this->broadcastMessage("strat;arduino;go;1000,1700\n"); + this->go(1000, 1700); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;angle;314\n"); + this->rotate(PI); awaitRobotIdle(); usleep(1'000'000); - toSend = "strat;arduino;go;" + std::to_string(static_cast(this->robotPose.pos.x - 350)) + "," + std::to_string(static_cast(this->robotPose.pos.y)) + "\n"; - this->broadcastMessage(toSend); + this->go(this->robotPose.pos.x - 350, this->robotPose.pos.y); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;go;500,500\n"); + this->go(500, 500); awaitRobotIdle(); std::vector pinceHavePurpleFlower; @@ -569,17 +511,16 @@ void TCPServer::startGameBlueTeam() { } if (!pinceHavePurpleFlower.empty()) { - this->broadcastMessage("strat;arduino;go;300,300\n"); + this->go(300, 300); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;angle;157\n"); + this->rotate(PI / 2); awaitRobotIdle(); for (auto & toDrop : pinceHavePurpleFlower) { toSend = "strat;servo_moteur;ouvrir pince;" + std::to_string(toDrop) + "\n"; this->broadcastMessage(toSend); - toSend = "strat;arduino;go;" + std::to_string(static_cast(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y + 150)) + "\n"; - this->broadcastMessage(toSend); + this->go(this->robotPose.pos.x, this->robotPose.pos.y + 150); awaitRobotIdle(); pinceState[toDrop] = NONE; @@ -588,23 +529,22 @@ void TCPServer::startGameBlueTeam() { 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); + this->go(this->robotPose.pos.x + 150, this->robotPose.pos.y); awaitRobotIdle(); } if (pinceHavePurpleFlower.size() < 3) { - this->broadcastMessage("strat;arduino;go;762,300\n"); + this->go(762, 300); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;speed;130\n"); - this->broadcastMessage("strat;arduino;angle;157\n"); + this->setSpeed(130); + this->rotate(PI / 2); awaitRobotIdle(); this->broadcastMessage("strat;servo_moteur;lever bras;1\n"); - this->broadcastMessage("strat;arduino;go;762,0\n"); + this->go(762, 0); usleep(3'000'000); for (int i = 0; i < 3; i++) { @@ -619,7 +559,7 @@ void TCPServer::startGameBlueTeam() { } } - this->broadcastMessage("strat;arduino;speed;200\n"); + this->setSpeed(200); } /* @@ -628,140 +568,88 @@ void TCPServer::startGameBlueTeam() { * Handle the second plant spot */ - this->broadcastMessage("strat;arduino;go;1000,250\n"); + this->go(1000, 250); awaitRobotIdle(); this->broadcastMessage("strat;servo_moteur;baisser bras;1\n"); - this->broadcastMessage("strat;arduino;angle;-78\n"); + this->rotate(-PI/4); awaitRobotIdle(); - arucoTags.clear(); + this->arucoTags.clear(); this->broadcastMessage("strat;aruco;get aruco;1\n"); + for (int i = 0; i < 5; i++) { + usleep(500'000); + this->broadcastMessage("strat;aruco;get aruco;1\n"); + } - 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; - } - } - } + tag = getBiggestArucoTag(300, 700, -200, 200); - if (!found) { - this->broadcastMessage("start;aruco;get aruco;1\n"); - usleep(500'000); - timeout++; - if (timeout > 10) { - return; - } + if (tag.has_value()) { + if (pinceState[1] == NONE) { + goToAruco(tag.value(), 1); + } else if (pinceState[2] == NONE) { + goToAruco(tag.value(), 2); + } else if (pinceState[0] == NONE) { + goToAruco(tag.value(), 0); } } - 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; - } - - this->broadcastMessage("strat;arduino;go;1000,250\n"); + this->go(1000, 250); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;angle;-157\n"); + this->rotate(-PI / 2); awaitRobotIdle(); - arucoTags.clear(); + this->arucoTags.clear(); this->broadcastMessage("strat;aruco;get aruco;1\n"); + for (int i = 0; i < 5; i++) { + usleep(500'000); + this->broadcastMessage("strat;aruco;get aruco;1\n"); + } - 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; - } - } - } + tag = getBiggestArucoTag(300, 700, -200, 200); - if (!found) { - this->broadcastMessage("start;aruco;get aruco;1\n"); - usleep(500'000); - timeout++; - if (timeout > 10) { - return; - } + if (tag.has_value()) { + if (pinceState[1] == NONE) { + goToAruco(tag.value(), 1); + } else if (pinceState[2] == NONE) { + goToAruco(tag.value(), 2); + } else if (pinceState[0] == NONE) { + goToAruco(tag.value(), 0); } } - 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;1000,250\n"; - this->broadcastMessage(toSend); + this->go(1000, 250); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;angle;157\n"); + this->rotate(PI / 2); awaitRobotIdle(); - arucoTags.clear(); + this->arucoTags.clear(); this->broadcastMessage("strat;aruco;get aruco;1\n"); + for (int i = 0; i < 5; i++) { + usleep(500'000); + this->broadcastMessage("strat;aruco;get aruco;1\n"); + } - 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; - } - } - } + tag = getBiggestArucoTag(300, 700, -200, 200); - if (!found) { - this->broadcastMessage("start;aruco;get aruco;1\n"); - usleep(500'000); - timeout++; - if (timeout > 10) { - return; - } + if (tag.has_value()) { + if (pinceState[1] == NONE) { + goToAruco(tag.value(), 1); + } else if (pinceState[2] == NONE) { + goToAruco(tag.value(), 2); + } else if (pinceState[0] == NONE) { + goToAruco(tag.value(), 0); } } - 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;1000,250\n"; - this->broadcastMessage(toSend); + this->go(1000, 250); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;angle;314\n"); + this->rotate(PI); awaitRobotIdle(); pinceHavePurpleFlower.clear(); @@ -774,35 +662,35 @@ void TCPServer::startGameBlueTeam() { if (!pinceHavePurpleFlower.empty()) { this->broadcastMessage("strat;servo_moteur;lever bras;1\n"); - this->broadcastMessage("strat;aduino;go;225,225\n"); + this->go(225, 225); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;angle;314\n"); + this->rotate(PI); awaitRobotIdle(); for (auto & toDrop : pinceHavePurpleFlower) { toSend = "strat;servo_moteur;ouvrir pince;" + std::to_string(toDrop) + "\n"; this->broadcastMessage(toSend); - usleep(200'000); + usleep(500'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); + this->go(this->robotPose.pos.x + 150, this->robotPose.pos.y); + awaitRobotIdle(); } if (pinceHavePurpleFlower.size() < 3) { - this->broadcastMessage("strat;arduino;go;762,300\n"); + this->go(762, 300); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;angle;157\n"); + this->rotate(PI / 2); awaitRobotIdle(); - this->broadcastMessage("strat;arduino;speed;150\n"); - this->broadcastMessage("strat;arduino;go;762,0\n"); + this->setSpeed(150); + this->go(762, 0); usleep(1'000'000); @@ -817,21 +705,18 @@ void TCPServer::startGameBlueTeam() { usleep(100'000); } - this->broadcastMessage("strat;arduino;speed;200\n"); + this->setSpeed(200); - 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); + this->go(this->robotPose.pos.x, this->robotPose.pos.y + 350); 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); + this->go(this->endRobotPose.pos.x, this->endRobotPose.pos.y); awaitRobotIdle(); - toSend = "strat;arduino;angle;" + std::to_string(static_cast(this->endRobotPose.theta * 100)); - this->broadcastMessage(toSend); + this->rotate(this->endRobotPose.theta); awaitRobotIdle(); this->broadcastMessage("strat;servo_moteur;clear;1"); @@ -1067,26 +952,28 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { this->broadcastMessage(toSend); - double xPrime = arucoTag.pos().first[0] - 50; - double yPrime = arucoTag.pos().first[1] + decalage; + double xPrime = arucoTag.pos()[0] - 50; + double yPrime = arucoTag.pos()[1] + decalage; + + // TODO : changement de base pour prendre en compte le centre de la plante avec la matrice de rotation, double thetaPrime = std::atan2(yPrime, xPrime); - toSend = "strat;arduino;angle;" + std::to_string(static_cast((this->robotPose.theta + rotate - thetaPrime) * 100)) + "\n"; - this->broadcastMessage(toSend); + this->rotate(this->robotPose.theta + rotate - thetaPrime); awaitRobotIdle(); double robotPosForPotX = (xPrime * std::cos(theta) + yPrime * std::sin(theta)) + robotPosX; double robotPosForPotY = (-xPrime * std::sin(theta) + yPrime * std::cos(theta)) + robotPosY; - toSend = "strat;arduino;transit;" + std::to_string(static_cast(robotPosForPotX)) + "," + std::to_string(static_cast(robotPosForPotY)) + ",130\n"; - this->broadcastMessage(toSend); + int previousSpeed = this->speed; + + this->transit(robotPosX, robotPosY, 130); awaitRobotIdle(); toSend = "strat;servo_moteur;fermer pince;" + std::to_string(pince) + "\n"; this->broadcastMessage(toSend); usleep(500'000); - this->broadcastMessage("strat;arduino;speed;200\n"); + this->setSpeed(previousSpeed); pinceState[pince] = TCPUtils::startWith(arucoTag.name(), "Purple_flower") ? PURPLE_FLOWER : WHITE_FLOWER; } @@ -1119,12 +1006,65 @@ void TCPServer::awaitRobotIdle() { usleep(200'000); this->broadcastMessage("strat;arduino;get state;1\n"); timeout++; - if (timeout > 20) { + if (timeout > 30) { this->broadcastMessage("strat;arduino;clear;1"); } } } -void TCPServer::sleepServer(int ms) { +void TCPServer::handleArucoTag(ArucoTag &tag) { + if (!TCPUtils::endWith(tag.name(), "flower")) { + return; + } + for (auto& t : arucoTags) { + if (tag.id() == t.id()) { + float tPosX = t.pos()[0]; + float tPosY = t.pos()[1]; + float tagPosX = tag.pos()[0]; + float tagPosY = tag.pos()[1]; + if (tagPosX > tPosX - 1 && tagPosX < tPosX + 1 && tagPosY > tPosY - 1 && tagPosY < tPosY + 1) { + t.find(); + break; + } + } + } + + this->arucoTags.push_back(tag); +} + +std::optional TCPServer::getBiggestArucoTag(float borneMinX, float borneMaxX, float borneMinY, + float borneMaxY) { + // use optional + bool found = false; + ArucoTag biggestTag = ArucoTag(); + for (const auto & tag : arucoTags) { + if (tag.getNbFind() > biggestTag.getNbFind() && tag.pos()[0] > borneMinX && tag.pos()[0] < borneMaxX && tag.pos()[1] > borneMinY && tag.pos()[1] < borneMaxY) { + biggestTag = tag; + found = true; + } + } + + return found ? std::optional(biggestTag) : std::nullopt; +} + + +template +void TCPServer::go(X x, Y y) { + this->broadcastMessage("strat;arduino;go;" + std::to_string(static_cast(x)) + "," + std::to_string(static_cast(y)) + "\n"); +} + +template +void TCPServer::rotate(X angle) { + this->broadcastMessage("strat;arduino;angle" + std::to_string(static_cast(angle * 100)) + "\n"); +} + +void TCPServer::setSpeed(int speed) { + this->broadcastMessage("strat;arduino;speed;" + std::to_string(speed) + "\n"); + this->speed = speed; +} + +template +void TCPServer::transit(X x, Y y, int endSpeed) { + this->broadcastMessage("strat;arduino;transit" + std::to_string(static_cast(x)) + "," + std::to_string(static_cast(y)) + "," + std::to_string(endSpeed) + "\n"); } diff --git a/TCPServer.h b/TCPServer.h index 8c1c636..536b341 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -10,6 +10,9 @@ #include #include #include +#include +#include +#include #include "utils.h" @@ -55,6 +58,8 @@ private: PinceState pinceState[3] = {NONE, NONE, NONE}; int isRobotIdle = 0; + int speed = 0; + struct Position { struct { float x; @@ -79,13 +84,13 @@ public: // Broadcast message to all connected clients void broadcastMessage(const char* message, int senderSocket = -1); // Modified method signature - void broadcastMessage(std::string &message, int senderSocket = -1); // Modified method signature + void broadcastMessage(const std::string &message, int senderSocket = -1); // Modified method signature void sendToClient(const char* message, int clientSocket); // New method to send message to a specific client - void sendToClient(std::string &message, int clientSocket); // New method to send message to a specific client + void sendToClient(const std::string &message, int clientSocket); // New method to send message to a specific client void sendToClient(const char* message, const std::string& clientName); // New method to send message to a specific client - void sendToClient(std::string &message, const std::string& clientName); // New method to send message to a specific client + void sendToClient(const std::string &message, const std::string& clientName); // New method to send message to a specific client void handleMessage(const std::string& message, int clientSocket = -1); @@ -111,7 +116,21 @@ public: void awaitRobotIdle(); - void sleepServer(int ms); + void handleArucoTag(ArucoTag &tag); + + std::optional getBiggestArucoTag(float borneMinX, float borneMaxX, float borneMinY, float borneMaxY); + + // Call to broadcast + void setSpeed(int speed); + + template + void go(X x, Y y); + + template + void rotate(X angle); + + template + void transit(X x,Y y, int endSpeed); ~TCPServer(); }; diff --git a/utils.cpp b/utils.cpp index 1a8fb4b..83fb50c 100644 --- a/utils.cpp +++ b/utils.cpp @@ -35,7 +35,7 @@ std::vector TCPUtils::split(const std::string& str, const std::stri } -ArucoTag::ArucoTag(int id, std::string name, std::pair pos) : _id(id), _name(std::move(name)), _pos(pos) {} +ArucoTag::ArucoTag(int id, std::string name, std::array pos, std::array rot) : _id(id), _name(std::move(name)), _pos(pos), _rot(rot) {} int ArucoTag::id() const { return _id; @@ -45,10 +45,14 @@ std::string ArucoTag::name() const { return _name; } -std::pair ArucoTag::pos() const { +std::array ArucoTag::pos() const { return _pos; } +std::array ArucoTag::rot() const { + return _rot; +} + void ArucoTag::setId(int id) { _id = id; } @@ -58,23 +62,36 @@ void ArucoTag::setName(const std::string& name) { } void ArucoTag::setPos(float x, float y) { - _pos.first[0] = x; - _pos.first[1] = y; + this->_pos[0] = x; + this->_pos[1] = y; } void ArucoTag::setRot(float x, float y, float z) { - _pos.second[0] = x; - _pos.second[1] = y; - _pos.second[2] = z; + this->_rot[0] = x; + this->_rot[1] = y; + this->_rot[2] = z; } ArucoTag& ArucoTag::operator=(const ArucoTag& tag) { _id = tag.id(); _name = tag.name(); - _pos.first[0] = tag.pos().first[0]; - _pos.first[1] = tag.pos().first[1]; - _pos.second[0] = tag.pos().second[0]; - _pos.second[1] = tag.pos().second[1]; - _pos.second[2] = tag.pos().second[2]; + _pos = tag.pos(); + _rot = tag.rot(); return *this; -} \ No newline at end of file +} + +FlowerAruco::FlowerAruco() : tag(nullptr), _realPos({0, 0}) { + +} + +FlowerAruco::FlowerAruco(ArucoTag *tag) : tag(tag), _realPos({0, 0}) { + +} + +ArucoTag *FlowerAruco::getTag() const { + return tag; +} + +std::array FlowerAruco::getPos() const { + return _realPos; +} diff --git a/utils.h b/utils.h index ba2e8bf..8c21c70 100644 --- a/utils.h +++ b/utils.h @@ -1,9 +1,12 @@ #pragma once +#include #include #include #include +#define PI 3.14159265358979323846 + enum PinceState { WHITE_FLOWER, PURPLE_FLOWER, @@ -23,7 +26,7 @@ namespace TCPUtils { class ArucoTag { public: - ArucoTag(int id, std::string name, std::pair pos); + ArucoTag(int id, std::string name, std::array pos, std::array rot); ArucoTag() = default; @@ -31,7 +34,9 @@ public: [[nodiscard]] std::string name() const; - [[nodiscard]] std::pair pos() const; + [[nodiscard]] std::array pos() const; + + [[nodiscard]] std::array rot() const; void setId(int id); @@ -43,8 +48,31 @@ public: ArucoTag& operator=(const ArucoTag& tag); + void find() { nbFind++; } + + [[nodiscard]] int getNbFind() const { return nbFind; } + private: - int _id = 0; + int _id = -1; std::string _name; - std::pair _pos; + std::array _pos; + std::array _rot; + int nbFind = 0; }; + +class FlowerAruco { + +public: + FlowerAruco(); + + explicit FlowerAruco(ArucoTag* tag); + + [[nodiscard]] ArucoTag* getTag() const; + + [[nodiscard]] std::array getPos() const; + +private: + ArucoTag* tag; + + std::array _realPos; +}; \ No newline at end of file