Skip to content

Use a poll-based method for accepting TCP connections #43

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Bidski wants to merge 9 commits into
base: main
Choose a base branch
from
Open

Use a poll-based method for accepting TCP connections #43

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
2e9689e
Split up tcp connection into server creation and client connection fu…
May 18, 2021
1bbc5b8
Tidy up includes
May 18, 2021
400f5c5
Use poll-based connection accepting and switch to poll instead of select
May 18, 2021
a621f7a
Clean up error handling
May 19, 2021
8b83d39
Merge branch 'main' into biddulph/async-socket
May 19, 2021
96439ad
Add using statement
May 19, 2021
ac9a725
Update controller creation script
May 19, 2021
9d39c6c
Fix formatting
May 19, 2021
7e5b22d
Merge branch 'main' into biddulph/async-socket
May 20, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
160 changes: 92 additions & 68 deletions controllers/nugus_controller/nugus_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,12 +17,10 @@
* Copyright 2021 NUbots <nubots@newcastle.edu.au>
*/

// You may need to add webots include files such as
// <webots/DistanceSensor.hpp>, <webots/Motor.hpp>, etc.
// and/or add some other includes
#include <cstdlib>
#include <iostream>
#include <memory>
#include <poll.h> // definition of poll and pollfd
#include <string>
#include <vector>
#include <webots/Robot.hpp>
Expand All @@ -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;
Expand All @@ -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<uint8_t> 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<uint8_t> 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;
}
}
}
}
Expand All @@ -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;
};


Expand Down
63 changes: 54 additions & 9 deletions scripts/controller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,11 +87,14 @@ def run(controller, **kwargs):
// and/or add some other includes
#include <cstdlib>
#include <memory>
#include <poll.h> // definition of poll and pollfd
#include <webots/Robot.hpp>

#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
Expand Down Expand Up @@ -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<webots::Robot> robot = std::make_unique<webots::Robot>();

// 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;
}
Expand Down
Loading