mirror of
https://github.com/modelec/TCPSocketServer.git
synced 2026-01-19 08:57:42 +01:00
345 lines
10 KiB
C++
345 lines
10 KiB
C++
#include "TCPServer.h"
|
||
|
||
ClientHandler::ClientHandler(int clientSocket, TCPServer* server) : clientSocket(clientSocket), server(server) {};
|
||
|
||
void ClientHandler::handle() {
|
||
std::string buffer;
|
||
buffer.reserve(4096); // Pre-allocate memory to avoid frequent allocations
|
||
|
||
while (true) {
|
||
char tempBuffer[4096] = {0};
|
||
ssize_t valread = recv(clientSocket, tempBuffer, sizeof(tempBuffer), 0);
|
||
|
||
if (valread > 0) {
|
||
buffer.append(tempBuffer, valread);
|
||
//std::cout << "Received: " << buffer << std::endl;
|
||
|
||
if (buffer == "quit") {
|
||
std::cout << "Client requested to quit. Closing connection." << std::endl;
|
||
break;
|
||
}
|
||
processMessage(buffer);
|
||
|
||
buffer.clear();
|
||
} else if (valread == 0) {
|
||
std::cout << "Client disconnected." << std::endl;
|
||
break; // Client disconnected
|
||
} else {
|
||
std::cerr << "Failed to receive data." << std::endl;
|
||
break; // Error in receiving data
|
||
}
|
||
}
|
||
|
||
closeConnection();
|
||
}
|
||
|
||
void ClientHandler::processMessage(const std::string& message) {
|
||
server->handleMessage(message, clientSocket);
|
||
}
|
||
|
||
void ClientHandler::closeConnection() {
|
||
close(clientSocket);
|
||
server->clientDisconnected(clientSocket); // Inform the server that the client has disconnected
|
||
}
|
||
|
||
TCPServer::TCPServer(int port)
|
||
{
|
||
this->robotPose = {500, 500, -3.1415/2};
|
||
|
||
serverSocket = socket(AF_INET, SOCK_STREAM, 0);
|
||
if (serverSocket == -1) {
|
||
std::cerr << "Socket creation failed" << std::endl;
|
||
exit(EXIT_FAILURE);
|
||
}
|
||
|
||
sockaddr_in address{};
|
||
address.sin_family = AF_INET;
|
||
address.sin_addr.s_addr = INADDR_ANY;
|
||
address.sin_port = htons(port);
|
||
|
||
if (bind(serverSocket, reinterpret_cast<struct sockaddr*>(&address), sizeof(address)) == -1) {
|
||
std::cerr << "Binding failed" << std::endl;
|
||
exit(EXIT_FAILURE);
|
||
}
|
||
|
||
if (listen(serverSocket, 5) == -1) {
|
||
std::cerr << "Listening failed" << std::endl;
|
||
exit(EXIT_FAILURE);
|
||
}
|
||
|
||
std::cout << "Server started on port " << port << std::endl;
|
||
|
||
clients.reserve(7);
|
||
ClientTCP tirette;
|
||
tirette.name = "tirette";
|
||
|
||
ClientTCP aruco;
|
||
aruco.name = "aruco";
|
||
|
||
ClientTCP ihm;
|
||
ihm.name = "ihm";
|
||
|
||
ClientTCP lidar;
|
||
lidar.name = "lidar";
|
||
|
||
ClientTCP arduino;
|
||
arduino.name = "arduino";
|
||
|
||
ClientTCP servo_moteur;
|
||
servo_moteur.name = "servo_moteur";
|
||
|
||
clients.push_back(tirette);
|
||
clients.push_back(aruco);
|
||
clients.push_back(ihm);
|
||
clients.push_back(lidar);
|
||
clients.push_back(arduino);
|
||
clients.push_back(servo_moteur);
|
||
|
||
}
|
||
|
||
void TCPServer::acceptConnections()
|
||
{
|
||
while (!shouldStop) {
|
||
sockaddr_in clientAddress{};
|
||
int addrlen = sizeof(clientAddress);
|
||
int clientSocket =
|
||
accept(serverSocket, reinterpret_cast<struct sockaddr*>(&clientAddress), reinterpret_cast<socklen_t*>(&addrlen));
|
||
if (clientSocket == -1) {
|
||
std::cerr << "Accepting connection failed" << std::endl;
|
||
continue;
|
||
}
|
||
std::cout << "Connection accepted" << std::endl;
|
||
|
||
// Add the client socket to the list
|
||
clientSockets.push_back(clientSocket);
|
||
connectedClients++;
|
||
|
||
// Handle client connection in a separate thread
|
||
clientThreads.emplace_back(&ClientHandler::handle, ClientHandler(clientSocket, this));
|
||
// Clean up finished threads
|
||
clientThreads.erase(std::remove_if(clientThreads.begin(), clientThreads.end(), [](std::thread &t) {
|
||
return !t.joinable();
|
||
}), clientThreads.end());
|
||
}
|
||
}
|
||
|
||
|
||
|
||
void TCPServer::handleMessage(const std::string& message, int clientSocket)
|
||
{
|
||
std::vector<std::string> tokens = split(message, ";");
|
||
|
||
if (tokens.size() != 4)
|
||
{
|
||
std::cerr << "Invalid message format : " << message << std::endl;
|
||
return;
|
||
}
|
||
if (tokens[1] != "strat")
|
||
{
|
||
this->broadcastMessage(message.c_str(), clientSocket);
|
||
}
|
||
if (tokens[0] == "tirette" && tokens[2] == "set state")
|
||
{
|
||
this->broadcastMessage(message.c_str(), clientSocket);
|
||
}
|
||
if (tokens[2] == "ready")
|
||
{
|
||
for (ClientTCP& client : clients)
|
||
{
|
||
if (client.name == tokens[0])
|
||
{
|
||
client.isReady = true;
|
||
client.socket = clientSocket;
|
||
break;
|
||
}
|
||
}
|
||
checkIfAllClientsReady();
|
||
}
|
||
if (tokens[2] == "set pos") {
|
||
std::vector<std::string> pos = split(tokens[3], ",");
|
||
this->robotPose = {std::stof(pos[0]), std::stof(pos[1]), std::stof(pos[2])};
|
||
this->broadcastMessage(message.c_str(), clientSocket);
|
||
}
|
||
if (tokens[2] == "get pos") {
|
||
std::string toSend = "strat;all;set pos;" + std::to_string(this->robotPose.pos.x) + "," + std::to_string(this->robotPose.pos.y) + "," + std::to_string(this->robotPose.theta);
|
||
this->broadcastMessage(toSend.c_str(), clientSocket);
|
||
}
|
||
if (tokens[2] == "spawn") {
|
||
// TODO change that to handle spawn point
|
||
this->robotPose = {500, 500, -1.57079};
|
||
const std::string toSend = "strat;all;set pos;" + std::to_string(this->robotPose.pos.x) + "," + std::to_string(this->robotPose.pos.y) + "," + std::to_string(this->robotPose.theta * 100);
|
||
this->broadcastMessage(toSend.c_str(), clientSocket);
|
||
|
||
}
|
||
if (tokens[2] == "start")
|
||
{
|
||
this->broadcastMessage(message.c_str(), clientSocket);
|
||
this->actionNb = 0;
|
||
this->startGame();
|
||
}
|
||
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 {
|
||
this->arucoTags.clear();
|
||
std::vector<std::string> aruco = split(arucoResponse, ",");
|
||
for (int i = 0; i < aruco.size(); i += 7) {
|
||
ArucoTagPos tag;
|
||
tag.tag.id = std::stoi(aruco[i]);
|
||
tag.tag.name = aruco[i + 1];
|
||
tag.pos.first[0] = std::stof(aruco[i + 2]);
|
||
tag.pos.first[1] = std::stof(aruco[i + 3]);
|
||
tag.pos.second[0] = std::stof(aruco[i + 4]);
|
||
tag.pos.second[1] = std::stof(aruco[i + 5]);
|
||
tag.pos.second[2] = std::stof(aruco[i + 6]);
|
||
|
||
this->arucoTags.push_back(tag);
|
||
}
|
||
this->waitForAruco = false;
|
||
}
|
||
}
|
||
std::cout << "Received: " << message << std::endl;
|
||
}
|
||
|
||
|
||
void TCPServer::broadcastMessage(const char* message, int senderSocket)
|
||
{
|
||
for (int clientSocket : clientSockets) {
|
||
if (clientSocket != senderSocket) { // Exclude the sender's socket
|
||
send(clientSocket, message, strlen(message), 0);
|
||
}
|
||
}
|
||
}
|
||
|
||
void TCPServer::broadcastMessage(const std::string &message, int senderSocket) {
|
||
this->broadcastMessage(message.c_str(), senderSocket);
|
||
}
|
||
|
||
void TCPServer::clientDisconnected(const int clientSocket) {
|
||
// Remove the disconnected client's socket
|
||
clientSockets.erase(std::remove(clientSockets.begin(), clientSockets.end(), clientSocket), clientSockets.end());
|
||
// Decrement the count of connected clients
|
||
connectedClients--;
|
||
}
|
||
|
||
void TCPServer::stop() {
|
||
shouldStop = true;
|
||
// Close all client sockets
|
||
for (int clientSocket : clientSockets) {
|
||
close(clientSocket);
|
||
}
|
||
// Close the server socket
|
||
close(serverSocket);
|
||
}
|
||
|
||
TCPServer::~TCPServer() {
|
||
this->stop();
|
||
// Join all threads before exiting
|
||
for (auto& thread : clientThreads) {
|
||
thread.join();
|
||
}
|
||
}
|
||
|
||
int TCPServer::nbClients() const {
|
||
return connectedClients;
|
||
}
|
||
|
||
void TCPServer::start()
|
||
{
|
||
std::thread([this]() { acceptConnections(); }).detach();
|
||
}
|
||
|
||
void TCPServer::checkIfAllClientsReady()
|
||
{
|
||
bool allReady = true;
|
||
for (auto&[name, socket, isReady] : clients)
|
||
{
|
||
if (!isReady)
|
||
{
|
||
allReady = false;
|
||
}
|
||
}
|
||
|
||
if (allReady)
|
||
{
|
||
this->broadcastMessage("strat;all;ready;1");
|
||
}
|
||
}
|
||
|
||
void TCPServer::startGame() {
|
||
this->broadcastMessage("strat;servo_moteur;baisser bras;1\n");
|
||
this->broadcastMessage("strat;servo_moteur;fermer pince;1\n");
|
||
this->broadcastMessage("strat;servo_moteur;fermer pince;2\n");
|
||
this->broadcastMessage("strat;servo_moteur;ouvrir pince;0\n");
|
||
|
||
waitForAruco = true;
|
||
this->broadcastMessage("strat;aruco;get aruco;1\n");
|
||
/*while (this->waitForAruco) {
|
||
usleep(100'000);
|
||
}*/
|
||
|
||
goToAruco(this->arucoTags[1], 1);
|
||
|
||
// pi/4
|
||
this->broadcastMessage("strat;arduino;angle;78");
|
||
this->broadcastMessage("strat;servo_moteur;baisser bras;1");
|
||
usleep(1'000'000);
|
||
this->broadcastMessage("strat;servo_moteur;ouvrir pince;" + std::to_string(1) + "\n");
|
||
this->broadcastMessage("strat;aruco;get aruco;1\n");
|
||
/*while (this->waitForAruco) {
|
||
usleep(100'000);
|
||
}*/
|
||
goToAruco(this->arucoTags[0], 0);
|
||
|
||
this->broadcastMessage("strat;arduino;go;500,500\n");
|
||
usleep(2'000'000);
|
||
// -pi/2
|
||
this->broadcastMessage("strat;arduino;angle;-157");
|
||
this->broadcastMessage("strat;servo_moteur;baisser bras;1");
|
||
this->broadcastMessage("strat;servo_moteur;clear;1");
|
||
}
|
||
|
||
void TCPServer::goToAruco(const ArucoTagPos &arucoTagPos, const int pince) {
|
||
int decalage = 0;
|
||
if (pince < 0 || pince > 2) {
|
||
return;
|
||
}
|
||
|
||
switch (pince) {
|
||
case 0:
|
||
decalage = -10;
|
||
break;
|
||
case 1:
|
||
decalage = 0;
|
||
break;
|
||
case 2:
|
||
decalage = 10;
|
||
break;
|
||
default:
|
||
decalage = 0;
|
||
break;
|
||
}
|
||
|
||
|
||
this->broadcastMessage("strat;servo_moteur;baisser bras;1\n");
|
||
this->broadcastMessage("strat;servo_moteur;ouvrir pince;" + std::to_string(pince) + "\n");
|
||
|
||
int xPrime = static_cast<int>(arucoTagPos.pos.second[0]);
|
||
int yPrime = static_cast<int>(arucoTagPos.pos.second[1]) + decalage;
|
||
|
||
/* TODO
|
||
e'1 = cos(α) e1 + sin(α) e2 ;
|
||
e'2 = –sin(α) e1 + cos(α) e2 ;
|
||
*/
|
||
int x = static_cast<int>(((xPrime - 90) / cos(this->robotPose.theta)) + this->robotPose.pos.x);
|
||
int y = static_cast<int>(((yPrime) / cos(this->robotPose.theta)) + this->robotPose.pos.y);
|
||
|
||
this->broadcastMessage("strat;arduino;go;" + std::to_string(x) + "," + std::to_string(y) + "\n");
|
||
usleep(3'000'000);
|
||
this->broadcastMessage("strat;servo_moteur;fermer pince;" + std::to_string(pince) + "\n");
|
||
this->broadcastMessage("strat;servo_moteur;lever bras;1\n");
|
||
havePot[pince] = true;
|
||
}
|