Revize f06d5ae1

Re #8736 - Implement Additional Messages

deltarobot/curvedataserver.cpp

@@ -3 +3 @@
 #include <QDebug>
 #include <QHostAddress>
 #include <QAbstractSocket>
+#include <QVector4D>
 #include <chrono>
 
 
@@ -252 +253 @@
     }
 }
 
+void CurveDataServer::sendActualDirection(float x, float y, float z)
+{
+    QByteArray message = message128(0x4007,
+                                    *((uint32_t*) &x),
+                                    *((uint32_t*) &y),
+                                    *((uint32_t*) &z));
+
+    {
+        std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
+        for (auto& client : _sockets) {
+                client.socket->write(message);
+        }
+    }
+}
+
+void CurveDataServer::sendTargetDirection(float x, float y, float z)
+{
+    QByteArray message = message128(0x4008,
+                                    *((uint32_t*) &x),
+                                    *((uint32_t*) &y),
+                                    *((uint32_t*) &z));
+
+    {
+        std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
+        for (auto& client : _sockets) {
+                client.socket->write(message);
+        }
+    }
+}
+
+void CurveDataServer::sendNewCurve(QList<QVector3D> points)
+{
+    int size = points.size();
+
+    QByteArray message = messageVarLength(0xF009, (uint32_t) size * 12);
+
+    for (int i = 0; i < size; i++)
+    {
+        float xf = points[i].x();
+        float yf = points[i].y();
+        float zf = points[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];
+    }
+
+    {
+        std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
+        for (auto& client : _sockets) {
+                client.socket->write(message);
+        }
+    }
+}
+
+void CurveDataServer::sendNewCurve(QList<QVector4D> points)
+{
+    QList<QVector3D> curve3d;
+    int itemCount = points.size();
+
+    curve3d.clear();
+    if (itemCount > MAX_CURVE_SIZE)
+        itemCount = MAX_CURVE_SIZE;
+    for (int i = 0; i < itemCount; i++)
+    {
+        curve3d.append(points[i].toVector3D());
+    }
+
+    sendNewCurve(curve3d);
+}
+
 QList<CurveDataServer::Client>::iterator CurveDataServer::clientOf(QTcpSocket *socket) {
     std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
     for (auto it = _sockets.begin(); it != _sockets.end(); it++) {

deltarobot/curvedataserver.h

@@ -7 +7 @@
 #include <map>
 #include <mutex>
 
+
+#include "../element/trajectory.hpp" //obsahuje MAX_CURVE_SIZE kterou se ridi sendNewCurve 4D
+
 class CurveDataServer : QObject
 {
     Q_OBJECT
@@ -31 +34 @@
     ~CurveDataServer();
 
     void sendActuatorPosition(float x, float y, float z);
+    void sendActualDirection(float x, float y, float z);
+    void sendTargetDirection(float x, float y, float z);
+    void sendNewCurve(QList<QVector4D> points);
+    void sendNewCurve(QList<QVector3D> points);
 
 public slots:
     void onNewConnection();

deltarobot/gl/scene.cpp

@@ -1 +1 @@
 #include "scene.hpp"
 
+#include "../curvedataserver.h"
+
 const int Scene::TRAJECTORY_I = 0;
 const int Scene::SPHERE_I = 1;
 const int Scene::SPHERE_II = 2;
@@ -58 +60 @@
     }
 }
 
-void Scene::setCurve(QList<QVector4D> curve)
+void Scene::setCurve(QList<QVector4D> curve, CurveDataServer * cdServer)
 {
     // Vyhledani minimalnich a maximalnich souradnic.
     QList<QVector3D> curve3d;
@@ -149 +151 @@
     
     // Vykreslit.
     update();
+
+    if (cdServer) {
+        cdServer->sendNewCurve(curve3d);
+    }
 }
 
 void Scene::setSphereData(const QVector3D &position, const QVector3D &fArrow, const QVector3D &sArrow)

deltarobot/gl/scene.hpp

@@ -18 +18 @@
 #include "../element/arrow.hpp"
 #include "../element/wall.hpp"
 
+
+#include "../curvedataserver.h"
+
+
 /// Instance třídy představuje vykreslovanou 3D scénu.
 class Scene: public QOpenGLWidget
 {
@@ -39 +43 @@
         /// hodnot pro dráhu, kouli a zdi. Změna kamery, pozice světla. Vynulování
         /// rotace.
         /// @param QList<QVector4D> curve - seznam bodů definující křivku.
-        void setCurve(QList<QVector4D> curve);
+        void setCurve(QList<QVector4D> curve, CurveDataServer * cdServer);
         
         /// Nastaveni pozice koule a vektoru pro obě šipky.
         /// @param const QVector3D &position - pozice koule.

deltarobot/mainwindow.cpp

@@ -485 +485 @@
 #endif
         ui->glwidget->setTubusColor(QVector4D(1, 0.737f, 0.231f, 0.7f));
         if (actCurveType != CURVE_NONE)
-            ui->glwidget->setCurve(calculatedCurve);
+            ui->glwidget->setCurve(calculatedCurve, cdServer);
         currState = SEND_CURVE;
     }
 }
@@ -609 +609 @@
         file.close();
         if (calculatedCurve.size() > 0) {
             ui->glwidget->setTubusColor(QVector4D(1, 0.737f, 0.231f, 0.7f));
-            ui->glwidget->setCurve(calculatedCurve);
+            ui->glwidget->setCurve(calculatedCurve, cdServer);
         }
         actCurveFileName = fileName;
         actCurveType = CURVE_EXTERN;
@@ -1072 +1072 @@
     }
 #endif
     cdServer->sendActuatorPosition(spherePos.x(), spherePos.y(), spherePos.z());
+    cdServer->sendActualDirection(actualArrow.x(), actualArrow.y(), actualArrow.z());
+    cdServer->sendTargetDirection(dirArrow.x(), dirArrow.y(), dirArrow.z());
 }
 
 //! Příjem dat ze stroje
@@ -1314 +1316 @@
                             dlg.exec();
                         }
                         if (calculatedCurve.size() > 0) {
-                            ui->glwidget->setCurve(calculatedCurve);
+                            ui->glwidget->setCurve(calculatedCurve, cdServer);
                         } else {
                             infoDialog dlg;
                             dlg.setInfoText(tr("Velikost křivky"), tr("Naučená křivka nelze použít. Neplatná data."));
@@ -1477 +1479 @@
     calculatedCurve.append(QVector4D(0,0,-4900,0.0));
     calculatedCurve.append(QVector4D(0,0,-4900,0.0));
     calculatedCurve.append(QVector4D(0,0,-4000,0.0));
-    ui->glwidget->setCurve(calculatedCurve);
+    ui->glwidget->setCurve(calculatedCurve, cdServer);
 }
 
 //! Aktualizace času při asistovaném cvičení pacienta