diff --git a/controllers/nugus_controller/nugus_controller.cpp b/controllers/nugus_controller/nugus_controller.cpp index b7f1d633c..7cb259a16 100644 --- a/controllers/nugus_controller/nugus_controller.cpp +++ b/controllers/nugus_controller/nugus_controller.cpp @@ -17,12 +17,10 @@ * Copyright 2021 NUbots */ -// You may need to add webots include files such as -// , , etc. -// and/or add some other includes #include #include #include +#include // definition of poll and pollfd #include #include #include @@ -31,17 +29,17 @@ #include "utility/tcp.hpp" +using utility::tcp::check_for_connection; using utility::tcp::close_socket; using utility::tcp::create_socket_server; class NUgus : public webots::Robot { public: NUgus(const int& time_step, const int& server_port) - : time_step(time_step), server_port(server_port), tcp_fd(create_socket_server(server_port)) { - send(tcp_fd, "Welcome", 8, 0); - } - ~NUgus() override { - close_socket(tcp_fd); + : time_step(time_step), server_port(server_port), server_fd(create_socket_server(server_port)), client_fd(-1) {} + ~NUgus() { + close_socket(client_fd); + close_socket(server_fd); } // We want to prevent multiple NUguses connecting with the same port NUgus(NUgus& other) = delete; @@ -55,72 +53,96 @@ class NUgus : public webots::Robot { uint32_t current_num = 1; while (step(time_step) != -1) { - // Don't bother doing anything unless we have an active TCP connection - if (tcp_fd == -1) { - std::cerr << "Error: Failed to start TCP server, retrying ..." << std::endl; - tcp_fd = create_socket_server(server_port); - send(tcp_fd, "Welcome", 8, 0); - continue; - } - - // Setup arguments for select call - fd_set rfds; - FD_ZERO(&rfds); - FD_SET(tcp_fd, &rfds); - timeval timeout = {0, 0}; - - // Watch TCP file descriptor to see when it has input. - // No wait - polling as fast as possible - int num_ready = select(tcp_fd + 1, &rfds, nullptr, nullptr, &timeout); - if (num_ready < 0) { - std::cerr << "Error: Polling of TCP connection failed: " << strerror(errno) << std::endl; - continue; + // Make sure we have a server + if (server_fd == -1) { + std::cerr << "Error: Lost TCP server, retrying ..." << std::endl; + server_fd = create_socket_server(server_port); + + // If we had to recreate the server then the client is no longer valid + close_socket(client_fd); + client_fd = -1; } - if (num_ready > 0) { - // Wire format - // unit32_t Nn message size in bytes. The bytes are in network byte order (big endian) - // uint8_t * Nn the message - uint32_t Nn = 0; - if (recv(tcp_fd, &Nn, sizeof(Nn), 0) != sizeof(Nn)) { - std::cerr << "Error: Failed to read message size from TCP connection: " << strerror(errno) - << std::endl; - continue; - } - // Covert to host endianness, which might be different to network endianness - uint32_t Nh = ntohl(Nn); + // Make sure we have an active TCP connection + if (server_fd != -1 && client_fd == -1) { + std::cerr << "Warning: No active TCP connection, retrying ..." << std::endl; + client_fd = check_for_connection(server_fd, server_port); - std::vector data(Nh, 0); - if (recv(tcp_fd, data.data(), Nh, 0) != Nh) { - std::cerr << "Error: Failed to read message from TCP connection: " << strerror(errno) << std::endl; - continue; + // There was an error accepting the new connection, our server is no longer valid + if (client_fd < 0) { + server_fd = -1; } - - // Parse message data - controller::nugus::RobotControl msg; - if (!msg.ParseFromArray(data.data(), Nh)) { - std::cerr << "Error: Failed to parse serialised message" << std::endl; - continue; + // No new connection came in, we are still waiting + else if (client_fd == 0) { + client_fd = -1; } + // We just accepted a new connection, send our welcome message + else { + send(client_fd, "Welcome", 8, 0); + } + } - // Read out the current message counter from the message - current_num = msg.num(); - - // Send a message to the client - msg.set_num(current_num); - - Nh = msg.ByteSizeLong(); - data.resize(Nh); - msg.SerializeToArray(data.data(), Nh); - - Nn = htonl(Nh); - - if (send(tcp_fd, &Nn, sizeof(Nn), 0) < 0) { - std::cerr << "Error: Failed to send message size over TCP connection: " << strerror(errno) - << std::endl; + // Server is good and client is good + if (server_fd != -1 && client_fd != -1) { + // Setup arguments for poll call + pollfd fds; + fds.fd = client_fd; + fds.events = POLLIN | POLLPRI; // Check for data to read and urgent data to read + fds.revents = 0; + + // Watch TCP file descriptor to see when it has input. + // No wait - polling as fast as possible + int num_ready = poll(&fds, 1, 0); + if (num_ready < 0) { + std::cerr << "Error: Polling of TCP connection failed: " << strerror(errno) << std::endl; + continue; } - else if (send(tcp_fd, data.data(), data.size(), 0) < 0) { - std::cerr << "Error: Failed to send data over TCP connection: " << strerror(errno) << std::endl; + else if (num_ready > 0) { + // Wire format + // unit32_t Nn message size in bytes. The bytes are in network byte order (big endian) + // uint8_t * Nn the message + uint32_t Nn; + if (recv(client_fd, &Nn, sizeof(Nn), 0) != sizeof(Nn)) { + std::cerr << "Error: Failed to read message size from TCP connection: " << strerror(errno) + << std::endl; + continue; + } + + // Covert to host endianness, which might be different to network endianness + uint32_t Nh = ntohl(Nn); + + std::vector data(Nh, 0); + if (recv(client_fd, data.data(), Nh, 0) != Nh) { + std::cerr << "Error: Failed to read message from TCP connection: " << strerror(errno) + << std::endl; + continue; + } + + // Parse message data + controller::nugus::RobotControl msg; + if (!msg.ParseFromArray(data.data(), Nh)) { + std::cerr << "Error: Failed to parse serialised message" << std::endl; + continue; + } + + // Read out the current message counter from the message + current_num = msg.num(); + + // Send a message to the client + msg.set_num(current_num); + + Nh = msg.ByteSizeLong(); + data.resize(Nh); + msg.SerializeToArray(data.data(), Nh); + + Nn = htonl(Nh); + + if (send(client_fd, &Nn, sizeof(Nn), 0) < 0) { + std::cerr << "Error: Failed to send data over TCP connection: " << strerror(errno) << std::endl; + } + else if (send(client_fd, data.data(), data.size(), 0) < 0) { + std::cerr << "Error: Failed to send data over TCP connection: " << strerror(errno) << std::endl; + } } } } @@ -131,8 +153,10 @@ class NUgus : public webots::Robot { const int time_step; /// TCP server port const int server_port; + /// File descriptor to use for the TCP server + int server_fd; /// File descriptor to use for the TCP connection - int tcp_fd; + int client_fd; }; diff --git a/scripts/controller.py b/scripts/controller.py index 52f1b874e..f1386d50f 100755 --- a/scripts/controller.py +++ b/scripts/controller.py @@ -87,11 +87,14 @@ def run(controller, **kwargs): // and/or add some other includes #include #include + #include // definition of poll and pollfd #include #include "utility/tcp.hpp" - using namespace utility::tcp; + using utility::tcp::check_for_connection; + using utility::tcp::close_socket; + using utility::tcp::create_socket_server; // This is the main program of your controller. // It creates an instance of your Robot instance, launches its @@ -128,25 +131,67 @@ def run(controller, **kwargs): } // Start the TCP server - int tcp_fd = create_socket_server(server_port); + int server_fd = create_socket_server(server_port); + int client_fd = -1; // Create the Robot instance std::unique_ptr robot = std::make_unique(); // Run the robot controller while (robot->step(time_step) != -1) { - // Don't bother doing anything unless we have an active TCP connection - if (tcp_fd == -1) { - std::cerr << "Error: Failed to start TCP server, retrying ..." << std::endl; - tcp_fd = create_socket_server(server_port); - continue; + // Make sure we have a server + if (server_fd == -1) { + std::cerr << "Error: Lost TCP server, retrying ..." << std::endl; + server_fd = create_socket_server(server_port); + + // If we had to recreate the server then the client is no longer valid + close_socket(client_fd); + client_fd = -1; } - // TODO: Do things .... + // Make sure we have an active TCP connection + if (server_fd != -1 && client_fd == -1) { + std::cerr << "Warning: No active TCP connection, retrying ..." << std::endl; + client_fd = check_for_connection(server_fd, server_port); + + // There was an error accepting the new connection, our server is no longer valid + if (client_fd < 0) { + server_fd = -1; + } + // No new connection came in, we are still waiting + else if (client_fd == 0) { + client_fd = -1; + } + // We just accepted a new connection, send our welcome message + else { + send(client_fd, "Welcome", 8, 0); + } + } + + // Server is good and client is good + if (server_fd != -1 && client_fd != -1) { + // Setup arguments for poll call + pollfd fds; + fds.fd = client_fd; + fds.events = POLLIN | POLLPRI; // Check for data to read and urgent data to read + fds.revents = 0; + + // Watch TCP file descriptor to see when it has input. + // No wait - polling as fast as possible + int num_ready = poll(&fds, 1, 0); + if (num_ready < 0) { + std::cerr << "Error: Polling of TCP connection failed: " << strerror(errno) << std::endl; + continue; + } + else if (num_ready > 0) { + // TODO: Do things .... + } + } } // Stop the TCP server - close_socket(tcp_fd); + close_socket(client_fd); + close_socket(server_fd); return EXIT_SUCCESS; } diff --git a/shared/utility/tcp.hpp b/shared/utility/tcp.hpp index 1be110517..b8aa3bf2b 100644 --- a/shared/utility/tcp.hpp +++ b/shared/utility/tcp.hpp @@ -26,12 +26,12 @@ #ifdef _WIN32 #include #else - #include /* definition of inet_ntoa */ - #include /* definition of gethostbyname */ - #include /* definition of struct sockaddr_in */ - #include - #include - #include /* definition of close */ + #include // definition of inet_ntoa + #include // definition of gethostbyname + #include // definition of struct sockaddr_in + #include // definition of poll and pollfd + #include // definition of socket, accept, listen, and bind + #include // definition of close #endif namespace utility::tcp { @@ -53,15 +53,32 @@ namespace utility::tcp { WSADATA info; // Winsock 1.1 - if (WSAStartup(MAKEWORD(1, 1), &info) != 0) { - std::cerr << "Cannot initialize Winsock" << std::endl; - return -1; + int err = WSAStartup(MAKEWORD(1, 1), &info); + switch (err) { + case 0: break; + case WSASYSNOTREADY: + std::cerr << "Error: Cannot initialize Winsock: Network subsystem is not ready for communication (" + << err << ")" << std::endl; + case WSAVERNOTSUPPORTED: + std::cerr << "Error: Cannot initialize Winsock: Winsock version 1.1 is not supported (" << err << ")" + << std::endl; + case WSAEINPROGRESS: + std::cerr << "Error: Cannot initialize Winsock: A blocking operation is currently in progress (" << err + << ")" << std::endl; + case WSAEPROCLIM: + std::cerr << "Error: Cannot initialize Winsock: Process limit exceeded (" << err << ")" << std::endl; + case WSAEFAULT: + std::cerr << "Error: Cannot initialize Winsock: Invalid data pointer (" << err << ")" << std::endl; + default: + std::cerr << "Error: Cannot initialize Winsock: Unknown error (" << err << ")" << std::endl; + return -1; } + #endif // create the socket - const int sfd = socket(AF_INET, SOCK_STREAM, 0); - if (sfd == -1) { - std::cerr << "Cannot create socket" << std::endl; + const int server_fd = socket(AF_INET, SOCK_STREAM, 0); + if (server_fd == -1) { + std::cerr << "Error: Cannot create socket: " << strerror(errno) << std::endl; return -1; } @@ -73,39 +90,74 @@ namespace utility::tcp { address.sin_addr.s_addr = INADDR_ANY; // bind to port - if (bind(sfd, reinterpret_cast(&address), sizeof(sockaddr)) == -1) { - std::cerr << "Cannot bind port " << port << std::endl; - close_socket(sfd); + if (bind(server_fd, reinterpret_cast(&address), sizeof(sockaddr)) == -1) { + std::cerr << "Error: Cannot bind port " << port << ": " << strerror(errno) << std::endl; + close_socket(server_fd); return -1; } // listen for connections - if (listen(sfd, 1) == -1) { - std::cerr << "Cannot listen for connections" << std::endl; - close_socket(sfd); + if (listen(server_fd, 1) == -1) { + std::cerr << "Error: Cannot listen for connections: " << strerror(errno) << std::endl; + close_socket(server_fd); return -1; } - std::cerr << "Waiting for a connection on port " << port << " ..." << std::endl; + // Server is now set up and listening for connections + std::cout << "Waiting for a connection on port " << port << " ..." << std::endl; + + return server_fd; + } + + inline int check_for_connection(const int& server_fd, const int& port) { + + // Setup the polling data + pollfd fds; + fds.fd = server_fd; + fds.events = POLLIN | POLLPRI; // Check for data to read and urgent data to read + fds.revents = 0; + + // Poll the server fd to see if there is any data to read + const int num_ready = poll(&fds, 1, 0); + + // Polling failed + if (num_ready < 0) { + std::cerr << "Error: Polling of TCP connection failed: " << strerror(errno) << std::endl; + return -1; + } + + // We have an incoming connection + else if (num_ready > 0) { #ifdef _WIN32 - int asize = sizeof(sockaddr_in); + int asize = sizeof(sockaddr_in); #else - socklen_t asize = sizeof(sockaddr_in); + socklen_t asize = sizeof(sockaddr_in); #endif - sockaddr_in client{}; - const int cfd = accept(sfd, reinterpret_cast(&client), &asize); + // Accept the connection + sockaddr_in client; + const int client_fd = accept(server_fd, reinterpret_cast(&client), &asize); - if (cfd == -1) { - std::cerr << "Cannot accept client" << std::endl; - close_socket(sfd); - } - else { + // Failed to accept the connection + if (client_fd == -1) { + std::cerr << "Error: Cannot accept client connection on port " << port << ": " << strerror(errno) + << std::endl; + close_socket(server_fd); + return -1; + } + + // Get client information const hostent* client_info = gethostbyname(inet_ntoa(client.sin_addr)); - std::cerr << "Accepted connection from: " << client_info->h_name << std::endl; + std::cout << "Accepted connection on port " << port << " from: " << client_info->h_name << std::endl; + + // Return the client fd + return client_fd; } - return cfd; + std::cout << "Waiting for a connection on port " << port << " ..." << std::endl; + + // Nothing yet + return 0; } } // namespace utility::tcp