From 2723ee7f1f97640f4a143d0b1c3c4f4401d15103 Mon Sep 17 00:00:00 2001
From: Giacomo Caironi <giacomo.caironi@skywarder.eu>
Date: Thu, 22 Sep 2022 23:02:06 +0200
Subject: [PATCH] [OrientationVisualizer] Added new module to visualize the
rocket orientation
---
.vscode/c_cpp_properties.json | 1 +
.vscode/settings.json | 5 +-
CMakeLists.txt | 14 +-
SkywardHub.pro | 2 +-
sbs | 2 +-
.../OrientationVisualizer.cpp | 184 ++++++++++++++++++
.../OrientationVisualizer.h | 26 +++
.../valuesconverterviewermodule.h | 1 -
src/shared/Modules/moduleinfo.h | 1 +
src/shared/Modules/moduleslist.cpp | 18 +-
10 files changed, 243 insertions(+), 11 deletions(-)
create mode 100644 src/shared/Modules/OrientationVisualizer/OrientationVisualizer.cpp
create mode 100644 src/shared/Modules/OrientationVisualizer/OrientationVisualizer.h
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index 7c8dbf1e..5180384a 100644
--- a/.vscode/c_cpp_properties.json
+++ b/.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": [],
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 614e1741..5be4d54f 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -1,5 +1,6 @@
{
"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]": {
diff --git a/CMakeLists.txt b/CMakeLists.txt
index cb0d44cc..078a34d5 100644
--- a/CMakeLists.txt
+++ b/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)
diff --git a/SkywardHub.pro b/SkywardHub.pro
index 44076ea2..55b1d3ef 100644
--- a/SkywardHub.pro
+++ b/SkywardHub.pro
@@ -1,4 +1,4 @@
-QT += core gui
+QT += core gui 3dcore 3drender 3dinput 3dlogic 3dextras 3danimation
greaterThan(QT_MAJOR_VERSION, 4): QT += widgets printsupport serialport
diff --git a/sbs b/sbs
index 28adeff9..9dd12b16 100755
--- a/sbs
+++ b/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"
diff --git a/src/shared/Modules/OrientationVisualizer/OrientationVisualizer.cpp b/src/shared/Modules/OrientationVisualizer/OrientationVisualizer.cpp
new file mode 100644
index 00000000..956209a4
--- /dev/null
+++ b/src/shared/Modules/OrientationVisualizer/OrientationVisualizer.cpp
@@ -0,0 +1,184 @@
+#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);
+}
diff --git a/src/shared/Modules/OrientationVisualizer/OrientationVisualizer.h b/src/shared/Modules/OrientationVisualizer/OrientationVisualizer.h
new file mode 100644
index 00000000..a14d7657
--- /dev/null
+++ b/src/shared/Modules/OrientationVisualizer/OrientationVisualizer.h
@@ -0,0 +1,26 @@
+#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();
+};
diff --git a/src/shared/Modules/ValuesConverterViewer/valuesconverterviewermodule.h b/src/shared/Modules/ValuesConverterViewer/valuesconverterviewermodule.h
index 873b9d6a..53a8543e 100644
--- a/src/shared/Modules/ValuesConverterViewer/valuesconverterviewermodule.h
+++ b/src/shared/Modules/ValuesConverterViewer/valuesconverterviewermodule.h
@@ -17,7 +17,6 @@ class ValuesConverterViewerModule;
class ValuesConverterViewerModule : public DefaultModule
{
-
Q_OBJECT
public:
diff --git a/src/shared/Modules/moduleinfo.h b/src/shared/Modules/moduleinfo.h
index 4ebac8d7..76b29257 100644
--- a/src/shared/Modules/moduleinfo.h
+++ b/src/shared/Modules/moduleinfo.h
@@ -18,6 +18,7 @@ enum ModuleId
INCOMINGMESSAGESVIEWER,
MAVLINK,
FILESTREAM,
+ ORIENTATION_VISUALIZER,
STATEVIEWER,
VALUESCONVERTERVIEWER,
MAVLINK_RCK_TESTING,
diff --git a/src/shared/Modules/moduleslist.cpp b/src/shared/Modules/moduleslist.cpp
index 44d46182..6076b53a 100644
--- a/src/shared/Modules/moduleslist.cpp
+++ b/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);
--
GitLab