Revize 0cda8e8d

Re #8735 - Implement base server message retrieval

deltarobot/curvedataserver.cpp

@@ -3 +3 @@
 #include <QDebug>
 #include <QHostAddress>
 #include <QAbstractSocket>
+#include <chrono>
+
+
+// Utility functions ///////////////////////////////////////////////////////////////////////////////////////////////////
 
 static QByteArray message8(uint16_t messageId, uint8_t content = 0) {
     QByteArray result(2 + 1, 0);
@@ -79 +83 @@
     return result;
 }
 
-static QByteArray messageVarLength(uint16_t messageId, uint32_t length) {
+static QByteArray messageVarLength(uint16_t messageId, uint32_t length, const char* content = nullptr) {
     QByteArray result(2 + 4 + length, 0);
     result[0] = ((uint8_t*) &messageId)[0];
     result[1] = ((uint8_t*) &messageId)[1];
@@ -87 +91 @@
     result[3] = ((uint8_t*) &length)[1];
     result[4] = ((uint8_t*) &length)[2];
     result[5] = ((uint8_t*) &length)[3];
+
+    if (content) {
+        for (uint32_t i = 0; i < length; i++) {
+            result[6 + i] = content[i];
+        }
+    }
     return result;
 }
 
+inline long currentTimeMillis() {
+    return std::chrono::duration_cast<std::chrono::milliseconds>(
+                std::chrono::steady_clock::now().time_since_epoch())
+            .count();
+}
+
+
+// Client //////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+CurveDataServer::Client::Client(QTcpSocket* socket) :
+    socket(socket),
+    lastPong(currentTimeMillis()),
+    bufUsed(0)
+{}
+
+
+// Server //////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
 CurveDataServer::CurveDataServer() :
     _server(this),
     _sockets()
@@ -98 +126 @@
     connect(&_server, SIGNAL(newConnection()), this, SLOT(onNewConnection()));
 }
 
+CurveDataServer::~CurveDataServer()
+{
+    std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
+    for (auto& client : _sockets)
+    {
+        sendEot(client.socket, "Server is stopping");
+        client.socket->close();
+    }
+    _sockets.clear();
+}
+
 void CurveDataServer::onNewConnection()
 {
     QTcpSocket *clientSocket = _server.nextPendingConnection();
     connect(clientSocket, SIGNAL(readyRead()), this, SLOT(onReadyRead()));
     connect(clientSocket, SIGNAL(stateChanged(QAbstractSocket::SocketState)), this, SLOT(onSocketStateChanged(QAbstractSocket::SocketState)));
 
-    _sockets.push_back(clientSocket);
-
     sendPreamble(clientSocket);
+    _sockets.push_back(Client(clientSocket));
 }
 
 void CurveDataServer::onSocketStateChanged(QAbstractSocket::SocketState socketState)
@@ -114 +152 @@
     if (socketState == QAbstractSocket::UnconnectedState)
     {
         QTcpSocket* sender = static_cast<QTcpSocket*>(QObject::sender());
-        _sockets.removeOne(sender);
+
+        std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
+        for (auto it = _sockets.begin(); it != _sockets.end(); it++) {
+            if (it->socket == sender) {
+                _sockets.erase(it);
+                break;
+            }
+        }
     }
 }
 
 void CurveDataServer::onReadyRead()
 {
-    // tady actually nakonec switch zpracovani ruznych zprav dle identifikatoru
+    std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
+
     QTcpSocket* sender = static_cast<QTcpSocket*>(QObject::sender());
-    QByteArray data = sender->readAll();
-    for (QTcpSocket* socket : _sockets) {
-        if (socket != sender)
-            socket->write(QByteArray::fromStdString(sender->peerAddress().toString().toStdString() + ": " + data.toStdString()));
+    auto it = clientOf(sender);
+    if (it == _sockets.end()) {
+        return;
+    }
+    Client& client = *it;
+
+    while (sender->bytesAvailable()) {
+        if (client.bufUsed < Client::MESSAGE_ID_SIZE) {
+            qint64 toRead = qMin(qint64(Client::MESSAGE_ID_SIZE - client.bufUsed), sender->bytesAvailable());
+            client.bufUsed += sender->read(&client.buf[client.bufUsed], toRead);
+        }
+
+        if (client.bufUsed < 2) {
+            // no ID yet, wait for more data
+            return;
+        }
+
+        uint16_t messageId = *((uint16_t*) client.buf);
+        qint64 contentSize;
+        switch (messageId & 0xF000) {
+        case 0x0000:
+            contentSize = 1;
+            break;
+        case 0x1000:
+            contentSize = 2;
+            break;
+        case 0x2000:
+            contentSize = 4;
+            break;
+        case 0x3000:
+            contentSize = 8;
+            break;
+        case 0x4000:
+            contentSize = 16;
+            break;
+        case 0xF000:
+            // TODO - implement support for variable length messages
+            return;
+        default:
+            // unsupported message size
+            return;
+        }
+
+        qint64 toRead = qMin(qint64(contentSize - (client.bufUsed - Client::MESSAGE_ID_SIZE)), sender->bytesAvailable());
+        if (toRead > 0) {
+            client.bufUsed += sender->read(&client.buf[client.bufUsed], toRead);
+        }
+
+        if (client.bufUsed == (Client::MESSAGE_ID_SIZE + contentSize)) {
+            handleMessage(client, messageId, &client.buf[Client::MESSAGE_ID_SIZE]);
+            client.bufUsed = 0;
+        }
     }
 }
 
-QByteArray *CurveDataServer::putFloatIntoMessage(QByteArray *message, int index, float f)
-{
-    if (!message || message->size() < index + 4)
-        return message;
-
-    unsigned char *c = reinterpret_cast<unsigned char *>(&f);
-
-    for (int i = 0; i < 4; i++)
-    {
-        (*message)[index + i] = c[i];
+void CurveDataServer::handleMessage(Client& client, uint16_t messageId, void *content) {
+    switch (messageId) {
+    case 0x3001:
+        qDebug() << "Magic arrived!";
+        break;
+    case 0x2002:
+        qDebug() << "Version arrived!";
+        break;
+    case 0x3005:
+        qDebug() << "Pong " << *((uint64_t*) content) << " arrived!";
+        break;
     }
-
-    return message;
 }
 
-
 void CurveDataServer::sendActuatorPosition(float x, float y, float z)
 {
     QByteArray message = message128(0x4003,
@@ -152 +244 @@
                                     *((uint32_t*) &y),
                                     *((uint32_t*) &z));
 
-    for (QTcpSocket* socket : _sockets) {
-            socket->write(message);
+    {
+        std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
+        for (auto& client : _sockets) {
+                client.socket->write(message);
+        }
     }
-    //qDebug() << message.toHex();
-    //qDebug() << x << " " << y << " " << z;
 }
 
+QList<CurveDataServer::Client>::iterator CurveDataServer::clientOf(QTcpSocket *socket) {
+    std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
+    for (auto it = _sockets.begin(); it != _sockets.end(); it++) {
+        if (it->socket == socket) {
+            return it;
+        }
+    }
+    return _sockets.end();
+}
 
 void CurveDataServer::sendPreamble(QTcpSocket *socket)
 {
@@ -186 +288 @@
     qDebug() << "Sent Protocol Version to" << socket->peerAddress();
     //qDebug() << message.toHex();
 }
+
+void CurveDataServer::sendEot(QTcpSocket *socket, std::string&& reason)
+{
+    QByteArray message = messageVarLength(0xF006, ((uint32_t) reason.length()), reason.c_str());
+    socket->write(message);
+}

deltarobot/curvedataserver.h

@@ -4 +4 @@
 #include <QObject>
 #include <QTcpServer>
 #include <QTcpSocket>
+#include <map>
+#include <mutex>
 
 class CurveDataServer : QObject
 {
     Q_OBJECT
 
 public:
-    CurveDataServer();
+    struct Client {
+        static const size_t BUF_SIZE = 32;
+        static const size_t MESSAGE_ID_SIZE = 2;
+
+        QTcpSocket* socket;
+        std::map<long, long> ping;  ///< key: ping ID;  value: ping timestamp (msec);
+        long latency;               ///< client's latency
+        long lastPong;              ///< timestamp of last pong
+
+        char buf[BUF_SIZE];
+        size_t bufUsed;
+
+        Client(QTcpSocket* socket);
+    };
+
+    explicit CurveDataServer();
+    ~CurveDataServer();
+
     void sendActuatorPosition(float x, float y, float z);
 
 public slots:
@@ -19 +38 @@
     void onReadyRead();
 
 private:
-    QByteArray *putFloatIntoMessage(QByteArray *message, int index, float f);
+    QList<Client>::iterator clientOf(QTcpSocket *socket);
+
+    void handleMessage(Client& client, uint16_t messageId, void* content);
+
     void sendPreamble(QTcpSocket *socket);
     void sendProtocolMagic(QTcpSocket *socket);
     void sendVersion(QTcpSocket *socket);
+    void sendEot(QTcpSocket *socket, std::string&& message);
 
     QTcpServer _server;
-    QList<QTcpSocket*> _sockets;
+
+    std::recursive_mutex _socketsLock;
+    QList<Client> _sockets;
 };
 
 #endif // CURVEDATASERVER_H