[OrientationVisualizer] Added new module to visualize the rocket orientation

.vscode/c_cpp_properties.json

@@ -4,6 +4,7 @@
             "name": "Linux",
             "includePath": [
                 "${workspaceFolder}/src/shared",
+                "${workspaceFolder}/libs/mavlink-skyward-lib",
                 "~/Qt/5.15.2/**"
             ],
             "defines": [],

.vscode/settings.json

 {
     "files.associations": {
+        ".clang-format": "bibtex",
         "ratio": "cpp",
         "sstream": "cpp",
         "filesystem": "cpp",
@@ -111,7 +112,9 @@
         "qthread": "cpp",
         "qtopengl": "cpp",
         "forward_list": "cpp",
-        "cfenv": "cpp"
+        "cfenv": "cpp",
+        "qaspectengine": "cpp",
+        "qorbitcameracontroller": "cpp"
     },
     "editor.defaultFormatter": "ms-vscode.cpptools",
     "[xml]": {

CMakeLists.txt

@@ -31,9 +31,12 @@ set(CMAKE_AUTOMOC ON)
 set(CMAKE_AUTORCC ON)
 set(CMAKE_AUTOUIC ON)
 
-find_package(Qt5 COMPONENTS Widgets REQUIRED)
-find_package(Qt5 COMPONENTS SerialPort REQUIRED)
-find_package(Qt5 COMPONENTS PrintSupport REQUIRED)
+find_package(Qt5 REQUIRED COMPONENTS 
+    Widgets
+    SerialPort
+    PrintSupport
+    3DCore 3DExtras 3DRender 3DInput
+)
 
 add_executable(groundstation
     src/shared/Components/ContextMenuSeparator/contextmenuseparator.cpp
@@ -81,6 +84,7 @@ add_executable(groundstation
     src/shared/Modules/SkywardHub/prefabviewelement.cpp
     src/shared/Modules/SkywardHub/skywardhubmodule.cpp
     src/shared/Modules/Splitter/Splitter.cpp
+    src/shared/Modules/OrientationVisualizer/OrientationVisualizer.cpp
     src/shared/Modules/StateViewer/StateViewer.cpp
     src/shared/Modules/Tabs/tabsmodule.cpp
     src/shared/Modules/Test/testmodule.cpp
@@ -98,6 +102,10 @@ target_link_libraries(groundstation PUBLIC
     Qt5::Widgets
     Qt5::SerialPort
     Qt5::PrintSupport
+    Qt5::3DCore
+    Qt5::3DExtras
+    Qt5::3DRender
+    Qt5::3DInput
     Mavlink::Mavlink
 )
 if(APPLE)

SkywardHub.pro

-QT       += core gui
+QT       += core gui 3dcore 3drender 3dinput 3dlogic 3dextras 3danimation
 
 greaterThan(QT_MAJOR_VERSION, 4): QT += widgets printsupport serialport
 

sbs

@@ -322,7 +322,7 @@ EOF
 }
 
 CMAKE_FILENAME="CMakeCache.txt"
-CMAKE_PREFIX_PATH="/Users/alberton/Qt/5.15.2/clang_64/lib/cmake/"
+CMAKE_PREFIX_PATH="~/Qt/5.15.2/gcc_64/lib/cmake/"
 DEBUG_FILENAME=".sbsdebug"
 VERBOSE_FILENAME=".sbsverbose"
 BUILD_DIRNAME="build"

src/shared/Modules/OrientationVisualizer/OrientationVisualizer.cpp

+#include "OrientationVisualizer.h"
+
+#include <Qt3DRender/qpointlight.h>
+
+#include <QSizePolicy>
+#include <Qt3DCore/QAspectEngine>
+#include <Qt3DCore/QEntity>
+#include <Qt3DExtras/QConeMesh>
+#include <Qt3DExtras/QFirstPersonCameraController>
+#include <Qt3DExtras/QForwardRenderer>
+#include <Qt3DExtras/QOrbitCameraController>
+#include <Qt3DExtras/QPhongMaterial>
+#include <Qt3DExtras/QSphereMesh>
+#include <Qt3DExtras/QTorusMesh>
+#include <Qt3DExtras/Qt3DWindow>
+#include <Qt3DInput/QInputAspect>
+#include <Qt3DRender/QCamera>
+#include <Qt3DRender/QCameraLens>
+#include <Qt3DRender/QGeometryRenderer>
+#include <Qt3DRender/QRenderAspect>
+
+OrientationVisualizer::OrientationVisualizer(QWidget *parent)
+    : DefaultModule(parent)
+{
+
+    setupUi();
+    defaultContextMenuSetup();
+
+    updateOrientation(0, 0, 0, 1);
+
+    getCore()->getModuleMessagesBroker()->subscribe(
+        {"Mav/PAYLOAD_FLIGHT_TM"}, this,
+        [this](const ModuleMessage &msg)
+        {
+            updateOrientation(msg.getField("nas_qx").getDouble(0.0),
+                              msg.getField("nas_qy").getDouble(0.0),
+                              msg.getField("nas_qz").getDouble(0.0),
+                              msg.getField("nas_qw").getDouble(0.0));
+        });
+}
+
+QWidget *OrientationVisualizer::toWidget() { return this; }
+
+XmlObject OrientationVisualizer::toXmlObject()
+{
+    XmlObject obj(getName(ModuleId::ORIENTATION_VISUALIZER));
+    return obj;
+}
+
+void OrientationVisualizer::fromXmlObject(const XmlObject &xmlObject) {}
+
+void OrientationVisualizer::setupUi()
+{
+    Qt3DExtras::Qt3DWindow *view = new Qt3DExtras::Qt3DWindow;
+    QWidget *windowContainer     = QWidget::createWindowContainer(view);
+
+    Qt3DCore::QEntity *rootEntity   = new Qt3DCore::QEntity;
+    Qt3DCore::QEntity *rocketEntity = new Qt3DCore::QEntity(rootEntity);
+
+    Qt3DExtras::QConeMesh *rocketMesh = new Qt3DExtras::QConeMesh();
+    rocketMesh->setBottomRadius(10);
+    rocketMesh->setBottomRadius(3);
+    rocketMesh->setLength(20);
+    rocketMesh->setRings(100);
+    rocketMesh->setSlices(20);
+
+    Qt3DExtras::QPhongMaterial *rocketMaterial =
+        new Qt3DExtras::QPhongMaterial();
+    rocketMaterial->setDiffuse(QColor(QRgb(0x15628c)));
+
+    rocketTransform = new Qt3DCore::QTransform();
+
+    rocketEntity->addComponent(rocketMesh);
+    rocketEntity->addComponent(rocketMaterial);
+    rocketEntity->addComponent(rocketTransform);
+
+    // Spheres
+
+    Qt3DExtras::QSphereMesh *sphereMesh = new Qt3DExtras::QSphereMesh;
+    sphereMesh->setRadius(3);
+    sphereMesh->setGenerateTangents(true);
+
+    // Qt3DCore::QEntity *sphereEntity1 = new Qt3DCore::QEntity(rootEntity);
+    // Qt3DCore::QTransform *sphereTransform1 = new Qt3DCore::QTransform;
+    // sphereTransform1->setTranslation(QVector3D(0.0f, 0.0f, 5.0f));
+    // Qt3DExtras::QPhongMaterial *sphereMaterial1 = new
+    // Qt3DExtras::QPhongMaterial();
+    // sphereMaterial1->setDiffuse(QColor(QRgb(0xff0000)));
+    // sphereEntity1->addComponent(sphereMesh);
+    // sphereEntity1->addComponent(sphereTransform1);
+    // sphereEntity1->addComponent(sphereMaterial1);
+
+    // Qt3DCore::QEntity *sphereEntity2 = new Qt3DCore::QEntity(rootEntity);
+    // Qt3DCore::QTransform *sphereTransform2 = new Qt3DCore::QTransform;
+    // sphereTransform2->setTranslation(QVector3D(0.0f, 5.0f, 0.0f));
+    // Qt3DExtras::QPhongMaterial *sphereMaterial2 = new
+    // Qt3DExtras::QPhongMaterial();
+    // sphereMaterial2->setDiffuse(QColor(QRgb(0x00ff00)));
+    // sphereEntity2->addComponent(sphereMesh);
+    // sphereEntity2->addComponent(sphereTransform2);
+    // sphereEntity2->addComponent(sphereMaterial2);
+
+    // Qt3DCore::QEntity *sphereEntity3 = new Qt3DCore::QEntity(rootEntity);
+    // Qt3DCore::QTransform *sphereTransform3 = new Qt3DCore::QTransform;
+    // sphereTransform3->setTranslation(QVector3D(5.0f, 0.0f, 0.0f));
+    // Qt3DExtras::QPhongMaterial *sphereMaterial3 = new
+    // Qt3DExtras::QPhongMaterial();
+    // sphereMaterial3->setDiffuse(QColor(QRgb(0x0000ff)));
+    // sphereEntity3->addComponent(sphereMesh);
+    // sphereEntity3->addComponent(sphereTransform3);
+    // sphereEntity3->addComponent(sphereMaterial3);
+
+    // Camera
+    Qt3DRender::QCamera *camera = view->camera();
+    camera->lens()->setPerspectiveProjection(45.0f, 16.0f / 9.0f, 0.1f,
+                                             1000.0f);
+    camera->setPosition(QVector3D(30.0f, 30.0f, 30.0f));
+    camera->setViewCenter(QVector3D(0, 0, 0));
+    camera->setUpVector(QVector3D(-1, -1, 1));
+
+    // Light
+    Qt3DCore::QEntity *lightEntity = new Qt3DCore::QEntity(rootEntity);
+    Qt3DRender::QPointLight *light = new Qt3DRender::QPointLight(lightEntity);
+    light->setColor("white");
+    light->setIntensity(1);
+    lightEntity->addComponent(light);
+    Qt3DCore::QTransform *lightTransform =
+        new Qt3DCore::QTransform(lightEntity);
+    lightTransform->setTranslation(camera->position());
+    lightEntity->addComponent(lightTransform);
+
+    // For camera controls
+    // Qt3DExtras::QFirstPersonCameraController *camController = new
+    // Qt3DExtras::QFirstPersonCameraController(rootEntity);
+    // camController->setLinearSpeed( 50.0f );
+    // camController->setLookSpeed( 180.0f );
+    // camController->setCamera(camera);
+
+    view->setRootEntity(rootEntity);
+
+    // Visualizer of roll/pitch/yaw
+    QHBoxLayout *lower = new QHBoxLayout();
+
+    yawLabel = new QLabel(this);
+    yawLabel->setText("Y: 0.00");
+    yawLabel->setAlignment(Qt::AlignCenter);
+    pitchLabel = new QLabel(this);
+    pitchLabel->setText("P: 0.00");
+    pitchLabel->setAlignment(Qt::AlignCenter);
+    rollLabel = new QLabel(this);
+    rollLabel->setText("R: 0.00");
+    rollLabel->setAlignment(Qt::AlignCenter);
+    this->setStyleSheet("QLabel {font-weight: bold;}");
+
+    lower->addWidget(yawLabel);
+    lower->addWidget(pitchLabel);
+    lower->addWidget(rollLabel);
+
+    QVBoxLayout *container = new QVBoxLayout(this);
+    container->addWidget(windowContainer);
+    container->addLayout(lower);
+
+    container->setStretch(0, 1);
+    container->setStretch(1, 0);
+}
+
+void OrientationVisualizer::updateOrientation(double qx, double qy, double qz,
+                                              double qw)
+{
+    float yaw, pitch, roll;
+
+    QQuaternion quaternion(QVector4D(qx, qy, qz, qw));
+    quaternion.getEulerAngles(&pitch, &yaw, &roll);
+
+    yawLabel->setText(QString::asprintf("Y: %.2lf", yaw));
+    pitchLabel->setText(QString::asprintf("P: %.2lf", pitch));
+    rollLabel->setText(QString::asprintf("R: %.2lf", roll));
+
+    QQuaternion displayQuaternion = QQuaternion::fromRotationMatrix(
+        quaternion.toRotationMatrix() *
+        QQuaternion::fromEulerAngles(90, 0, 0).toRotationMatrix());
+
+    rocketTransform->setRotation(displayQuaternion);
+}

src/shared/Modules/OrientationVisualizer/OrientationVisualizer.h

+#pragma once
+
+#include <Modules/DefaultModule/defaultmodule.h>
+
+#include <Qt3DCore/QTransform>
+
+class OrientationVisualizer : public DefaultModule
+{
+    Q_OBJECT
+
+public:
+    explicit OrientationVisualizer(QWidget* parent = nullptr);
+
+    QWidget* toWidget() override;
+
+    XmlObject toXmlObject() override;
+    void fromXmlObject(const XmlObject& xmlObject) override;
+
+private:
+    QLabel *rollLabel, *pitchLabel, *yawLabel;
+
+    Qt3DCore::QTransform* rocketTransform;
+
+    void updateOrientation(double qx, double qy, double qz, double qw);
+    void setupUi();
+};

src/shared/Modules/ValuesConverterViewer/valuesconverterviewermodule.h

@@ -17,7 +17,6 @@ class ValuesConverterViewerModule;
 
 class ValuesConverterViewerModule : public DefaultModule
 {
-
     Q_OBJECT
 
 public:

src/shared/Modules/moduleinfo.h

@@ -18,6 +18,7 @@ enum ModuleId
     INCOMINGMESSAGESVIEWER,
     MAVLINK,
     FILESTREAM,
+    ORIENTATION_VISUALIZER,
     STATEVIEWER,
     VALUESCONVERTERVIEWER,
     MAVLINK_RCK_TESTING,

src/shared/Modules/moduleslist.cpp

@@ -9,6 +9,7 @@
 #include <Modules/IncomingMessagesViewer/incomingmessagesviewermodule.h>
 #include <Modules/Mavlink/mavlinkmodule.h>
 #include <Modules/Mavlink/mavlinkrocketmsgtestingmodule.h>
+#include <Modules/OrientationVisualizer/OrientationVisualizer.h>
 #include <Modules/OutgoingMessagesViewer/outgoingmessagesviewermodule.h>
 #include <Modules/SkywardHub/skywardhubmodule.h>
 #include <Modules/Splitter/Splitter.h>
@@ -113,14 +114,23 @@ void ModulesList::createModuleList()
     stateViewer.addModuleSourceFiles("Modules/StateViewer/");
     addModuleInfo(stateViewer);
 
-    ModuleInfo valuesconverterviewer(ModuleId::VALUESCONVERTERVIEWER,
+    ModuleInfo valuesConverterViewer(ModuleId::VALUESCONVERTERVIEWER,
                                      "ValuesConverterViewer",
                                      ModuleCategory::UTILITY);
-    valuesconverterviewer.setFactory(
+    valuesConverterViewer.setFactory(
         []() { return new ValuesConverterViewerModule(); });
-    valuesconverterviewer.addModuleSourceFiles(
+    valuesConverterViewer.addModuleSourceFiles(
         "Modules/ValuesConverterViewer/");
-    addModuleInfo(valuesconverterviewer);
+    addModuleInfo(valuesConverterViewer);
+
+    ModuleInfo orientationVisualizer(ModuleId::ORIENTATION_VISUALIZER,
+                                     "OrientationVisualizer",
+                                     ModuleCategory::UTILITY);
+    orientationVisualizer.setFactory([]()
+                                     { return new OrientationVisualizer(); });
+    orientationVisualizer.addModuleSourceFiles(
+        "Modules/OrientationVisualizer/");
+    addModuleInfo(orientationVisualizer);
 
     ModuleInfo timerController(ModuleId::TIMER_CONTROLLER, "TimerController",
                                ModuleCategory::UTILITY);