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);
+}