Revize 9360266c

Re #8735 - Server data retrieval structure

deltarobot/curvedataserver.cpp

@@ -6 +6 @@
 #include <QVector4D>
 #include <chrono>
 
+#include "../element/trajectory.hpp"
 
 // Utility functions ///////////////////////////////////////////////////////////////////////////////////////////////////
 
@@ -101 +102 @@
     return result;
 }
 
-inline long currentTimeMillis() {
+inline uint64_t currentTimeMillis() {
     return std::chrono::duration_cast<std::chrono::milliseconds>(
                 std::chrono::steady_clock::now().time_since_epoch())
             .count();
@@ -121 +122 @@
 
 CurveDataServer::CurveDataServer() :
     _server(this),
+    _pingThread([this](){threadFunction();}),
     _sockets()
 {
     _server.listen(QHostAddress::Any, 4242);
@@ -132 +134 @@
     std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
     for (auto& client : _sockets)
     {
-        sendEot(client.socket, "Server is stopping");
-        client.socket->close();
+        terminateConnection(client, "Server is stopping");
     }
     _sockets.clear();
+
+    _threadIsRunning = false;
 }
 
 void CurveDataServer::onNewConnection()
@@ -154 +157 @@
     {
         QTcpSocket* sender = static_cast<QTcpSocket*>(QObject::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;
-            }
-        }
+        removeSocket(sender);
     }
 }
 
@@ -206 +203 @@
             break;
         case 0xF000:
             // TODO - implement support for variable length messages
-            return;
+            contentSize = 0;
+            break;
         default:
             // unsupported message size
             return;
@@ -228 +226 @@
     switch (messageId) {
     case 0x3001:
         qDebug() << "Magic arrived!";
+        handleProtocolMagic(client, (char *) content);
         break;
     case 0x2002:
         qDebug() << "Version arrived!";
+        handleVersion(client, *((uint8_t*) content));
         break;
     case 0x3005:
         qDebug() << "Pong " << *((uint64_t*) content) << " arrived!";
+        handlePong(client, content);
+        break;
+    case 0xF006:
+        qDebug() << "EOT!";
+        handleEOT(client, content);
         break;
     }
 }
 
+void CurveDataServer::handleProtocolMagic(Client& client, char *content)
+{
+    char *correct = "DeltaRVr";
+
+    if (strncmp(content, correct, 8) == 0) {
+        client.magic = true;
+    }
+}
+
+void CurveDataServer::handleVersion(Client& client, uint8_t content)
+{
+    if (((uint8_t*) &content)[0] == 1)
+    {
+        client.version = true;
+    }
+}
+
+void CurveDataServer::handlePong(Client& client, void* content)
+{
+    long time = currentTimeMillis();
+
+    long number = *((long*) content);
+
+    long timeSent = client.ping.at(number);
+
+    client.ping.erase(number);
+
+    client.latency = time - timeSent;
+    client.lastPong = time;
+}
+
+void CurveDataServer::handleEOT(Client& client, void* content)
+{
+    qDebug() << "Client" << client.socket->peerName() << "disconnected:" << content;
+    terminateConnection(client, "Recieved EOT from client");
+}
+
+
+void CurveDataServer::terminateConnection(Client& client, std::string&& reason)
+{
+    removeSocket(client.socket);
+
+    sendEot(client.socket, std::forward<std::string&&>(reason));
+
+    client.socket->close();
+}
+
 void CurveDataServer::sendActuatorPosition(float x, float y, float z)
 {
     QByteArray message = message128(0x4003,
@@ -245 +297 @@
                                     *((uint32_t*) &y),
                                     *((uint32_t*) &z));
 
-    {
-        std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
-        for (auto& client : _sockets) {
-                client.socket->write(message);
-        }
-    }
+    sendMessageToAllConnected(message);
 }
 
 void CurveDataServer::sendActualDirection(float x, float y, float z)
@@ -260 +307 @@
                                     *((uint32_t*) &y),
                                     *((uint32_t*) &z));
 
-    {
-        std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
-        for (auto& client : _sockets) {
-                client.socket->write(message);
-        }
-    }
+    sendMessageToAllConnected(message);
 }
 
 void CurveDataServer::sendTargetDirection(float x, float y, float z)
@@ -275 +317 @@
                                     *((uint32_t*) &y),
                                     *((uint32_t*) &z));
 
-    {
-        std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
-        for (auto& client : _sockets) {
-                client.socket->write(message);
-        }
-    }
+    sendMessageToAllConnected(message);
 }
 
 void CurveDataServer::sendNewCurve(QList<QVector3D> &points)
@@ -315 +352 @@
         message[2 + 4 + i*12 + 11] = z[3];
     }
 
-    {
-        std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
-        for (auto& client : _sockets) {
-                client.socket->write(message);
-        }
-    }
+    sendMessageToAllConnected(message);
+
+
+    std::lock_guard<decltype (_currentCurveLock)> l(_currentCurveLock);
+    _currentCurve.clear();
+    _currentCurve.append(points);
 }
 
 void CurveDataServer::sendNewCurve(QList<QVector4D> &points)
@@ -363 +400 @@
     socket->write(message);
 
     qDebug() << "Sent Protocol Magic to" << socket->peerAddress();
-    //qDebug() << message.toHex();
 }
 
 void CurveDataServer::sendVersion(QTcpSocket *socket)
@@ -373 +409 @@
     socket->write(message);
 
     qDebug() << "Sent Protocol Version to" << socket->peerAddress();
-    //qDebug() << message.toHex();
 }
 
 void CurveDataServer::sendEot(QTcpSocket *socket, std::string&& reason)
@@ -381 +416 @@
     QByteArray message = messageVarLength(0xF006, ((uint32_t) reason.length()), reason.c_str());
     socket->write(message);
 }
+
+
+
+void CurveDataServer::sendMessageToAllConnected(QByteArray &message)
+{
+    std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
+    for (auto& client : _sockets) {
+        if (client.magic && client.version)
+        {
+            client.socket->write(message);
+        }
+    }
+}
+
+void CurveDataServer::threadFunction()
+{
+    while (_threadIsRunning)
+    {
+        // odeslat ping, popr krivku vsem, take kontrola odpojeni
+        {
+            std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
+            for (auto& client : _sockets) {
+                if (client.magic && client.version)
+                {
+                    if (client.lastPong  < currentTimeMillis() - 10000)
+                    {
+                        terminateConnection(client, "Timeout");
+                    }
+
+                    sendPing(client);
+
+                    std::lock_guard<decltype (_currentCurveLock)> l(_currentCurveLock);
+                    if (!client.initialized && !_currentCurve.isEmpty())
+                    {
+                        sendCurve(client);
+                        client.initialized = true;
+                    }
+                }
+                else if (client.lastPong  < currentTimeMillis() - 10000)
+                {
+                    terminateConnection(client, "Preamble timeout");
+                }
+            }
+        }
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+}
+
+void CurveDataServer::removeSocket(QTcpSocket *socket)
+{
+    std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
+    for (auto it = _sockets.begin(); it != _sockets.end(); it++) {
+        if (it->socket == socket) {
+            _sockets.erase(it);
+            break;
+        }
+    }
+}
+
+void CurveDataServer::sendPing(Client& client)
+{
+    uint64_t number = (((uint64_t) rand()) << 32) + (uint64_t) rand();
+
+    client.ping.emplace(std::make_pair(number, currentTimeMillis()));
+    //client.ping.insert({number, currentTimeMillis()});
+
+    client.socket->write(message64(0x3004, (uint64_t) number));
+}
+
+void CurveDataServer::sendCurve(Client &client)
+{
+    int size = _currentCurve.size();
+
+    QByteArray message = messageVarLength(0xF009, (uint32_t) size * 12);
+
+    for (int i = 0; i < size; i++)
+    {
+        float xf = _currentCurve[i].x();
+        float yf = _currentCurve[i].y();
+        float zf = _currentCurve[i].z();
+
+        uint8_t *x = ((uint8_t*) &xf);
+        uint8_t *y = ((uint8_t*) &yf);
+        uint8_t *z = ((uint8_t*) &zf);
+
+        message[2 + 4 + i*12 + 0] = x[0];
+        message[2 + 4 + i*12 + 1] = x[1];
+        message[2 + 4 + i*12 + 2] = x[2];
+        message[2 + 4 + i*12 + 3] = x[3];
+
+        message[2 + 4 + i*12 + 4] = y[0];
+        message[2 + 4 + i*12 + 5] = y[1];
+        message[2 + 4 + i*12 + 6] = y[2];
+        message[2 + 4 + i*12 + 7] = y[3];
+
+        message[2 + 4 + i*12 + 8] = z[0];
+        message[2 + 4 + i*12 + 9] = z[1];
+        message[2 + 4 + i*12 + 10] = z[2];
+        message[2 + 4 + i*12 + 11] = z[3];
+    }
+
+    client.socket->write(message);
+}

deltarobot/curvedataserver.h

@@ -6 +6 @@
 #include <QTcpSocket>
 #include <map>
 #include <mutex>
-
-
-#include "../element/trajectory.hpp" //obsahuje MAX_CURVE_SIZE kterou se ridi sendNewCurve 4D
+#include <thread>
 
 class CurveDataServer : QObject
 {
@@ -16 +14 @@
 
 public:
     struct Client {
-        static const size_t BUF_SIZE = 32;
+        static const size_t BUF_SIZE = 4096;
         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
+        std::map<uint64_t, uint64_t> ping;  ///< key: ping ID;  value: ping timestamp (msec);
+        uint64_t latency;               ///< client's latency
+        uint64_t lastPong;              ///< timestamp of last pong
 
         char buf[BUF_SIZE];
         size_t bufUsed;
 
+        bool magic = false;
+        bool version = false;
+        bool initialized = false;
+
         Client(QTcpSocket* socket);
     };
 
@@ -38 +40 @@
     void sendTargetDirection(float x, float y, float z);
     void sendNewCurve(QList<QVector4D> &points);
     void sendNewCurve(QList<QVector3D> &points);
+    void sendCurve(Client &client);
 
 public slots:
     void onNewConnection();
@@ -48 +51 @@
     QList<Client>::iterator clientOf(QTcpSocket *socket);
 
     void handleMessage(Client& client, uint16_t messageId, void* content);
+    void handleProtocolMagic(Client& client, char* content);
+    void handleVersion(Client& client, uint8_t content);
+    void handlePong(Client& client, void* content);
+    void handleEOT(Client& client, void* content);
+
+    void validPreamble(Client& client);
+    void removeSocket(QTcpSocket *socket);
+    void terminateConnection(Client& client, std::string&& reason);
+    void sendPing(Client& client);
 
     void sendPreamble(QTcpSocket *socket);
     void sendProtocolMagic(QTcpSocket *socket);
     void sendVersion(QTcpSocket *socket);
     void sendEot(QTcpSocket *socket, std::string&& message);
 
+    void sendMessageToAllConnected(QByteArray &message);
+
+    void threadFunction();
+
     QTcpServer _server;
+    std::thread _pingThread;
+
+    volatile bool _threadIsRunning = true;
 
     std::recursive_mutex _socketsLock;
     QList<Client> _sockets;
+
+    std::recursive_mutex _currentCurveLock;
+    QList<QVector3D> _currentCurve;
 };
 
 #endif // CURVEDATASERVER_H