From 864e6ae8c3b3827f655b2a5211e5fe6cc38fc79a Mon Sep 17 00:00:00 2001 From: hengli Date: Wed, 15 Feb 2012 17:58:46 +0100 Subject: [PATCH] Major overhaul of local 3D view: now supports multi-MAV visualization. --- qgroundcontrol.pro | 16 +- src/ui/MainWindow.cc | 2 +- src/ui/map3D/CameraParams.cc | 58 + src/ui/map3D/CameraParams.h | 28 + src/ui/map3D/GlobalViewParams.cc | 115 ++ src/ui/map3D/GlobalViewParams.h | 48 + src/ui/map3D/ImageWindowGeode.cc | 70 +- src/ui/map3D/ImageWindowGeode.h | 11 +- src/ui/map3D/Imagery.cc | 4 +- src/ui/map3D/Imagery.h | 8 +- src/ui/map3D/Pixhawk3DWidget.cc | 1664 ++++++++++++++------------- src/ui/map3D/Pixhawk3DWidget.h | 192 ++-- src/ui/map3D/PixhawkCheetahGeode.cc | 20 +- src/ui/map3D/PixhawkCheetahGeode.h | 7 +- src/ui/map3D/Q3DWidget.cc | 516 +++++---- src/ui/map3D/Q3DWidget.h | 153 ++- src/ui/map3D/Q3DWidgetFactory.cc | 6 +- src/ui/map3D/Q3DWidgetFactory.h | 2 +- src/ui/map3D/SystemContainer.cc | 96 ++ src/ui/map3D/SystemContainer.h | 65 ++ src/ui/map3D/SystemGroupNode.cc | 101 ++ src/ui/map3D/SystemGroupNode.h | 28 + src/ui/map3D/SystemViewParams.cc | 274 +++++ src/ui/map3D/SystemViewParams.h | 81 ++ src/ui/map3D/ViewParamWidget.cc | 188 +++ src/ui/map3D/ViewParamWidget.h | 46 + src/ui/map3D/WaypointGroupNode.cc | 23 +- src/ui/map3D/WaypointGroupNode.h | 6 +- 28 files changed, 2570 insertions(+), 1258 deletions(-) create mode 100644 src/ui/map3D/CameraParams.cc create mode 100644 src/ui/map3D/CameraParams.h create mode 100644 src/ui/map3D/GlobalViewParams.cc create mode 100644 src/ui/map3D/GlobalViewParams.h create mode 100644 src/ui/map3D/SystemContainer.cc create mode 100644 src/ui/map3D/SystemContainer.h create mode 100644 src/ui/map3D/SystemGroupNode.cc create mode 100644 src/ui/map3D/SystemGroupNode.h create mode 100644 src/ui/map3D/SystemViewParams.cc create mode 100644 src/ui/map3D/SystemViewParams.h create mode 100644 src/ui/map3D/ViewParamWidget.cc create mode 100644 src/ui/map3D/ViewParamWidget.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index c48292c47..ffb7e9bfd 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -362,7 +362,13 @@ HEADERS += src/MG.h \ src/ui/firmwareupdate/QGCFirmwareUpdateWidget.h \ src/ui/QGCPluginHost.h \ src/ui/firmwareupdate/QGCPX4FirmwareUpdate.h \ - src/ui/map3D/gpl.h + src/ui/map3D/gpl.h \ + src/ui/map3D/CameraParams.h \ + src/ui/map3D/ViewParamWidget.h \ + src/ui/map3D/SystemContainer.h \ + src/ui/map3D/SystemViewParams.h \ + src/ui/map3D/GlobalViewParams.h \ + src/ui/map3D/SystemGroupNode.h # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010::HEADERS += src/ui/map3D/QGCGoogleEarthView.h @@ -496,7 +502,13 @@ SOURCES += src/main.cc \ src/ui/firmwareupdate/QGCFirmwareUpdateWidget.cc \ src/ui/QGCPluginHost.cc \ src/ui/firmwareupdate/QGCPX4FirmwareUpdate.cc \ - src/ui/map3D/gpl.cc + src/ui/map3D/gpl.cc \ + src/ui/map3D/CameraParams.cc \ + src/ui/map3D/ViewParamWidget.cc \ + src/ui/map3D/SystemContainer.cc \ + src/ui/map3D/SystemViewParams.cc \ + src/ui/map3D/GlobalViewParams.cc \ + src/ui/map3D/SystemGroupNode.cc # Enable Google Earth only on Mac OS and Windows with Visual Studio compiler macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010::SOURCES += src/ui/map3D/QGCGoogleEarthView.cc diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 35705ae9c..84e23d9bf 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -553,7 +553,7 @@ void MainWindow::buildCommonWidgets() #ifdef QGC_OSG_ENABLED if (!_3DWidget) { - _3DWidget = Q3DWidgetFactory::get("PIXHAWK"); + _3DWidget = Q3DWidgetFactory::get("PIXHAWK", this); addCentralWidget(_3DWidget, tr("Local 3D")); } #endif diff --git a/src/ui/map3D/CameraParams.cc b/src/ui/map3D/CameraParams.cc new file mode 100644 index 000000000..8a569df35 --- /dev/null +++ b/src/ui/map3D/CameraParams.cc @@ -0,0 +1,58 @@ +#include "CameraParams.h" + +CameraParams::CameraParams() + : mMinZoomRange(2.0f) + , mFov(30.0f) + , mMinClipRange(1.0f) + , mMaxClipRange(10000.0f) +{ + +} + +float& +CameraParams::minZoomRange(void) +{ + return mMinZoomRange; +} + +float +CameraParams::minZoomRange(void) const +{ + return mMinZoomRange; +} + +float& +CameraParams::fov(void) +{ + return mFov; +} + +float +CameraParams::fov(void) const +{ + return mFov; +} + +float& +CameraParams::minClipRange(void) +{ + return mMinClipRange; +} + +float +CameraParams::minClipRange(void) const +{ + return mMinClipRange; +} + +float& +CameraParams::maxClipRange(void) +{ + return mMaxClipRange; +} + +float +CameraParams::maxClipRange(void) const +{ + return mMaxClipRange; +} diff --git a/src/ui/map3D/CameraParams.h b/src/ui/map3D/CameraParams.h new file mode 100644 index 000000000..51b749da9 --- /dev/null +++ b/src/ui/map3D/CameraParams.h @@ -0,0 +1,28 @@ +#ifndef CAMERAPARAMS_H +#define CAMERAPARAMS_H + +class CameraParams +{ +public: + CameraParams(); + + float& minZoomRange(void); + float minZoomRange(void) const; + + float& fov(void); + float fov(void) const; + + float& minClipRange(void); + float minClipRange(void) const; + + float& maxClipRange(void); + float maxClipRange(void) const; + +private: + float mMinZoomRange; + float mFov; + float mMinClipRange; + float mMaxClipRange; +}; + +#endif // CAMERAPARAMS_H diff --git a/src/ui/map3D/GlobalViewParams.cc b/src/ui/map3D/GlobalViewParams.cc new file mode 100644 index 000000000..4c838b801 --- /dev/null +++ b/src/ui/map3D/GlobalViewParams.cc @@ -0,0 +1,115 @@ +#include "GlobalViewParams.h" + +#include + +GlobalViewParams::GlobalViewParams() + : mDisplayWorldGrid(true) + , mImageryType(Imagery::BLANK_MAP) + , mFollowCameraId(-1) + , mFrame(MAV_FRAME_LOCAL_NED) +{ + +} + +bool& +GlobalViewParams::displayWorldGrid(void) +{ + return mDisplayWorldGrid; +} + +bool +GlobalViewParams::displayWorldGrid(void) const +{ + return mDisplayWorldGrid; +} + +Imagery::Type& +GlobalViewParams::imageryType(void) +{ + return mImageryType; +} + +Imagery::Type +GlobalViewParams::imageryType(void) const +{ + return mImageryType; +} + +int& +GlobalViewParams::followCameraId(void) +{ + return mFollowCameraId; +} + +int +GlobalViewParams::followCameraId(void) const +{ + return mFollowCameraId; +} + +MAV_FRAME& +GlobalViewParams::frame(void) +{ + return mFrame; +} + +MAV_FRAME +GlobalViewParams::frame(void) const +{ + return mFrame; +} + +void +GlobalViewParams::followCameraChanged(const QString& text) +{ + int followCameraId = -1; + + if (text.compare("None") == 0) + { + followCameraId = -1; + } + else + { + QStringList list = text.split(" ", QString::SkipEmptyParts); + + followCameraId = list.back().toInt(); + } + + if (followCameraId != mFollowCameraId) + { + mFollowCameraId = followCameraId; + emit followCameraChanged(mFollowCameraId); + } +} + +void +GlobalViewParams::frameChanged(const QString& text) +{ + if (text.compare("Global") == 0) + { + mFrame = MAV_FRAME_GLOBAL; + } + else if (text.compare("Local") == 0) + { + mFrame = MAV_FRAME_LOCAL_NED; + } +} + +void +GlobalViewParams::imageryTypeChanged(int index) +{ + mImageryType = static_cast(index); +} + +void +GlobalViewParams::toggleWorldGrid(int state) +{ + if (state == Qt::Checked) + { + mDisplayWorldGrid = true; + } + else + { + mDisplayWorldGrid = false; + } +} diff --git a/src/ui/map3D/GlobalViewParams.h b/src/ui/map3D/GlobalViewParams.h new file mode 100644 index 000000000..db9044601 --- /dev/null +++ b/src/ui/map3D/GlobalViewParams.h @@ -0,0 +1,48 @@ +#ifndef GLOBALVIEWPARAMS_H +#define GLOBALVIEWPARAMS_H + +#include +#include +#include + +#include "QGCMAVLink.h" +#include "Imagery.h" + +class GlobalViewParams : public QObject +{ + Q_OBJECT + +public: + GlobalViewParams(); + + bool& displayWorldGrid(void); + bool displayWorldGrid(void) const; + + Imagery::Type& imageryType(void); + Imagery::Type imageryType(void) const; + + int& followCameraId(void); + int followCameraId(void) const; + + MAV_FRAME& frame(void); + MAV_FRAME frame(void) const; + +public slots: + void followCameraChanged(const QString& text); + void frameChanged(const QString &text); + void imageryTypeChanged(int index); + void toggleWorldGrid(int state); + +signals: + void followCameraChanged(int systemId); + +private: + bool mDisplayWorldGrid; + Imagery::Type mImageryType; + int mFollowCameraId; + MAV_FRAME mFrame; +}; + +typedef QSharedPointer GlobalViewParamsPtr; + +#endif // GLOBALVIEWPARAMS_H diff --git a/src/ui/map3D/ImageWindowGeode.cc b/src/ui/map3D/ImageWindowGeode.cc index 3f4d33050..66e60d375 100644 --- a/src/ui/map3D/ImageWindowGeode.cc +++ b/src/ui/map3D/ImageWindowGeode.cc @@ -32,19 +32,19 @@ #include "ImageWindowGeode.h" ImageWindowGeode::ImageWindowGeode() - : border(5) + : mBorder(5) + , mImage(new osg::Image) { } void ImageWindowGeode::init(const QString& caption, const osg::Vec4& backgroundColor, - osg::ref_ptr& image, osg::ref_ptr& font) { // image osg::ref_ptr imageGeometry = new osg::Geometry; - imageVertices = new osg::Vec3Array(4); + mImageVertices = new osg::Vec3Array(4); osg::ref_ptr textureCoords = new osg::Vec2Array; textureCoords->push_back(osg::Vec2(0.0f, 1.0f)); @@ -57,15 +57,15 @@ ImageWindowGeode::init(const QString& caption, const osg::Vec4& backgroundColor, imageGeometry->setColorArray(imageColors); imageGeometry->setColorBinding(osg::Geometry::BIND_OVERALL); - imageGeometry->setVertexArray(imageVertices); + imageGeometry->setVertexArray(mImageVertices); imageGeometry->setTexCoordArray(0, textureCoords); imageGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON, - 0, imageVertices->size())); + 0, mImageVertices->size())); osg::ref_ptr texture = new osg::Texture2D; texture->setDataVariance(osg::Object::DYNAMIC); - texture->setImage(image); + texture->setImage(mImage); texture->setResizeNonPowerOfTwoHint(false); imageGeometry->getOrCreateStateSet()-> @@ -74,10 +74,10 @@ ImageWindowGeode::init(const QString& caption, const osg::Vec4& backgroundColor, // background osg::ref_ptr backgroundGeometry = new osg::Geometry; - backgroundVertices = new osg::Vec3Array(4); - backgroundGeometry->setVertexArray(backgroundVertices); + mBackgroundVertices = new osg::Vec3Array(4); + backgroundGeometry->setVertexArray(mBackgroundVertices); backgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON, - 0, backgroundVertices->size())); + 0, mBackgroundVertices->size())); osg::ref_ptr backgroundColors(new osg::Vec4Array); backgroundColors->push_back(backgroundColor); backgroundGeometry->setColorArray(backgroundColors); @@ -85,16 +85,16 @@ ImageWindowGeode::init(const QString& caption, const osg::Vec4& backgroundColor, backgroundGeometry->setUseDisplayList(false); // caption - text = new osgText::Text; - text->setText(caption.toStdString().c_str()); - text->setCharacterSize(11); - text->setFont(font); - text->setAxisAlignment(osgText::Text::SCREEN); - text->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f)); + mText = new osgText::Text; + mText->setText(caption.toStdString().c_str()); + mText->setCharacterSize(11); + mText->setFont(font); + mText->setAxisAlignment(osgText::Text::SCREEN); + mText->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f)); addDrawable(imageGeometry); addDrawable(backgroundGeometry); - addDrawable(text); + addDrawable(mText); setAttributes(0, 0, 0, 0); } @@ -102,20 +102,26 @@ ImageWindowGeode::init(const QString& caption, const osg::Vec4& backgroundColor, void ImageWindowGeode::setAttributes(int x, int y, int width, int height) { - int imageWidth = width - border * 2; - int imageHeight = height - border * 2 - 15; - int imageXPosition = x + border; - int imageYPosition = y + border; - - imageVertices->at(0) = osg::Vec3(imageXPosition, imageYPosition, 0); - imageVertices->at(1) = osg::Vec3(imageXPosition + imageWidth, imageYPosition, 0); - imageVertices->at(2) = osg::Vec3(imageXPosition + imageWidth, imageYPosition + imageHeight, 0); - imageVertices->at(3) = osg::Vec3(imageXPosition, imageYPosition + imageHeight, 0); - - text->setPosition(osg::Vec3(imageXPosition, imageYPosition + imageHeight + 5, 0)); - - backgroundVertices->at(0) = osg::Vec3(x, y, -1); - backgroundVertices->at(1) = osg::Vec3(x + width, y, -1); - backgroundVertices->at(2) = osg::Vec3(x + width, y + height, -1); - backgroundVertices->at(3) = osg::Vec3(x, y + height, -1); + int imageWidth = width - mBorder * 2; + int imageHeight = height - mBorder * 2 - 15; + int imageXPosition = x + mBorder; + int imageYPosition = y + mBorder; + + mImageVertices->at(0) = osg::Vec3(imageXPosition, imageYPosition, 0); + mImageVertices->at(1) = osg::Vec3(imageXPosition + imageWidth, imageYPosition, 0); + mImageVertices->at(2) = osg::Vec3(imageXPosition + imageWidth, imageYPosition + imageHeight, 0); + mImageVertices->at(3) = osg::Vec3(imageXPosition, imageYPosition + imageHeight, 0); + + mText->setPosition(osg::Vec3(imageXPosition, imageYPosition + imageHeight + 5, 0)); + + mBackgroundVertices->at(0) = osg::Vec3(x, y, -1); + mBackgroundVertices->at(1) = osg::Vec3(x + width, y, -1); + mBackgroundVertices->at(2) = osg::Vec3(x + width, y + height, -1); + mBackgroundVertices->at(3) = osg::Vec3(x, y + height, -1); +} + +osg::ref_ptr& +ImageWindowGeode::image(void) +{ + return mImage; } diff --git a/src/ui/map3D/ImageWindowGeode.h b/src/ui/map3D/ImageWindowGeode.h index 5ebad10da..1d48525d7 100644 --- a/src/ui/map3D/ImageWindowGeode.h +++ b/src/ui/map3D/ImageWindowGeode.h @@ -42,17 +42,18 @@ class ImageWindowGeode : public osg::Geode public: ImageWindowGeode(); void init(const QString& caption, const osg::Vec4& backgroundColor, - osg::ref_ptr& image, osg::ref_ptr& font); void setAttributes(int x, int y, int width, int height); + osg::ref_ptr& image(void); private: - int border; + int mBorder; - osg::ref_ptr imageVertices; - osg::ref_ptr backgroundVertices; - osg::ref_ptr text; + osg::ref_ptr mImage; + osg::ref_ptr mImageVertices; + osg::ref_ptr mBackgroundVertices; + osg::ref_ptr mText; }; #endif // IMAGEWINDOWGEODE_H diff --git a/src/ui/map3D/Imagery.cc b/src/ui/map3D/Imagery.cc index fee76f6c6..2d1c50a26 100644 --- a/src/ui/map3D/Imagery.cc +++ b/src/ui/map3D/Imagery.cc @@ -46,14 +46,14 @@ Imagery::Imagery() } -Imagery::ImageryType +Imagery::Type Imagery::getImageryType(void) const { return currentImageryType; } void -Imagery::setImageryType(ImageryType type) +Imagery::setImageryType(Imagery::Type type) { currentImageryType = type; } diff --git a/src/ui/map3D/Imagery.h b/src/ui/map3D/Imagery.h index b6dc77791..15f1b00a3 100644 --- a/src/ui/map3D/Imagery.h +++ b/src/ui/map3D/Imagery.h @@ -41,7 +41,7 @@ This file is part of the QGROUNDCONTROL project class Imagery : public osg::Geode { public: - enum ImageryType { + enum Type { BLANK_MAP = 0, GOOGLE_MAP = 1, GOOGLE_SATELLITE = 2, @@ -50,8 +50,8 @@ public: Imagery(); - ImageryType getImageryType(void) const; - void setImageryType(ImageryType type); + Type getImageryType(void) const; + void setImageryType(Type type); void setOffset(double xOffset, double yOffset); void prefetch2D(double windowWidth, double windowHeight, @@ -104,7 +104,7 @@ private: QScopedPointer textureCache; - ImageryType currentImageryType; + Type currentImageryType; double xOffset; double yOffset; diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index ccc51e0f0..0e600a4b1 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -39,6 +39,7 @@ #include #include +#include "../MainWindow.h" #include "PixhawkCheetahGeode.h" #include "UASManager.h" @@ -51,189 +52,122 @@ #endif Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent) - : Q3DWidget(parent) - , uas(NULL) - , kMessageTimeout(4.0) - , mode(DEFAULT_MODE) - , selectedWpIndex(-1) - , displayLocalGrid(false) - , displayWorldGrid(true) - , displayTrails(true) - , displayImagery(true) - , displayWaypoints(true) - , displayRGBD2D(false) - , displayRGBD3D(true) - , displayObstacleList(true) - , displayPath(true) - , enableRGBDColor(false) - , enableTarget(false) - , followCamera(true) - , frame(MAV_FRAME_LOCAL_NED) - , lastRobotX(0.0f) - , lastRobotY(0.0f) - , lastRobotZ(0.0f) + : kMessageTimeout(4.0) + , mMode(DEFAULT_MODE) + , mSelectedWpIndex(-1) + , mActiveSystemId(-1) + , mActiveUAS(NULL) + , mGlobalViewParams(new GlobalViewParams) + , mFollowCameraId(-1) + , mInitCameraPos(false) + , m3DWidget(new Q3DWidget(this)) + , mViewParamWidget(new ViewParamWidget(mGlobalViewParams, mSystemViewParamMap, this, parent)) { - setCameraParams(2.0f, 30.0f, 0.01f, 10000.0f); - init(15.0f); + connect(m3DWidget, SIGNAL(sizeChanged(int,int)), this, SLOT(sizeChanged(int,int))); + connect(m3DWidget, SIGNAL(update()), this, SLOT(update())); - // generate Pixhawk Cheetah model - vehicleModel = PixhawkCheetahGeode::instance(); - egocentricMap->addChild(vehicleModel); + m3DWidget->setCameraParams(2.0f, 30.0f, 0.01f, 10000.0f); + m3DWidget->init(15.0f); + m3DWidget->handleDeviceEvents() = false; - // generate grid models - localGridNode = createLocalGrid(); - rollingMap->addChild(localGridNode); - - worldGridNode = createWorldGrid(); - allocentricMap->addChild(worldGridNode); - - // generate empty trail model - trailNode = new osg::Geode; - rollingMap->addChild(trailNode); - - orientationNode = new osg::Group; - rollingMap->addChild(orientationNode); + mWorldGridNode = createWorldGrid(); + m3DWidget->worldMap()->addChild(mWorldGridNode, false); // generate map model - mapNode = createMap(); - rollingMap->addChild(mapNode); + mImageryNode = createImagery(); + m3DWidget->worldMap()->addChild(mImageryNode, false); - // generate waypoint model - waypointGroupNode = new WaypointGroupNode; - waypointGroupNode->init(); - rollingMap->addChild(waypointGroupNode); - - // generate target model - targetNode = createTarget(); - rollingMap->addChild(targetNode); - - // generate RGBD model - rgbd3DNode = createRGBD3D(); - rollingMap->addChild(rgbd3DNode); + setupHUD(); -#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES) - obstacleGroupNode = new ObstacleGroupNode; - obstacleGroupNode->init(); - rollingMap->addChild(obstacleGroupNode); + buildLayout(); - // generate path model - pathNode = new osg::Geode; - pathNode->addDrawable(createTrail(osg::Vec4(1.0f, 0.8f, 0.0f, 1.0f))); - rollingMap->addChild(pathNode); -#endif + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), + this, SLOT(activeSystemChanged(UASInterface*))); + connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), + this, SLOT(systemCreated(UASInterface*))); + connect(mGlobalViewParams.data(), SIGNAL(followCameraChanged(int)), + this, SLOT(followCameraChanged(int))); - setupHUD(); + MainWindow* parentWindow = qobject_cast(parent); + parentWindow->addDockWidget(Qt::LeftDockWidgetArea, mViewParamWidget); - // find available vehicle models in models folder - vehicleModels = findVehicleModels(); + mViewParamWidget->hide(); - buildLayout(); + setFocusPolicy(Qt::StrongFocus); + setMouseTracking(true); +} - updateHUD(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, "32N"); +Pixhawk3DWidget::~Pixhawk3DWidget() +{ - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), - this, SLOT(setActiveUAS(UASInterface*))); } -Pixhawk3DWidget::~Pixhawk3DWidget() +void +Pixhawk3DWidget::activeSystemChanged(UASInterface* uas) { + mActiveSystemId = uas->getUASID(); + + mActiveUAS = uas; + mMode = DEFAULT_MODE; } -/** - * - * @param uas the UAS/MAV to monitor/display with the HUD - */ void -Pixhawk3DWidget::setActiveUAS(UASInterface* uas) +Pixhawk3DWidget::systemCreated(UASInterface *uas) { - if (this->uas == uas) + int systemId = uas->getUASID(); + + if (mSystemContainerMap.contains(systemId)) { return; } - if (this->uas != NULL) - { - // Disconnect any previously connected active MAV - disconnect(this->uas, SIGNAL(localPositionChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(addToTrails(UASInterface*,int,double,double,double,quint64))); - disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,int,double,double,double,quint64))); - } + mSystemViewParamMap.insert(systemId, SystemViewParamsPtr(new SystemViewParams(systemId))); + mSystemContainerMap.insert(systemId, SystemContainer()); - connect(uas, SIGNAL(localPositionChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(addToTrails(UASInterface*,int,double,double,double,quint64))); - connect(uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,int,double,double,double,quint64))); + connect(uas, SIGNAL(localPositionChanged(UASInterface*,int,double,double,double,quint64)), + this, SLOT(localPositionChanged(UASInterface*,int,double,double,double,quint64))); + connect(uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)), + this, SLOT(attitudeChanged(UASInterface*,int,double,double,double,quint64))); - trails.clear(); - trailNode->removeDrawables(0, trailNode->getNumDrawables()); - orientationNode->removeChildren(0, orientationNode->getNumChildren()); + initializeSystem(systemId, uas->getColor()); - this->uas = uas; + emit systemCreatedSignal(uas); } void -Pixhawk3DWidget::addToTrails(UASInterface* uas, int component, double x, double y, double z, quint64 time) +Pixhawk3DWidget::localPositionChanged(UASInterface* uas, int component, + double x, double y, double z, + quint64 time) { - if (this->uas->getUASID() != uas->getUASID()) + int systemId = uas->getUASID(); + + if (!mSystemContainerMap.contains(systemId)) { return; } - if (!trails.contains(component)) + SystemContainer& systemData = mSystemContainerMap[systemId]; + + // update system position + m3DWidget->systemGroup(systemId)->position()->setPosition(osg::Vec3d(y, x, -z)); + + // update trail data + if (!systemData.trailMap().contains(component)) { - trails[component] = QVarLengthArray(); - trailDrawableIdxs[component] = trails.size() - 1; + systemData.trailMap().insert(component, QVector()); + systemData.trailMap()[component].reserve(10000); + systemData.trailIndexMap().insert(component, + systemData.trailMap().size() - 1); osg::Vec4 color((float)qrand() / RAND_MAX, (float)qrand() / RAND_MAX, (float)qrand() / RAND_MAX, 0.5); - trailNode->addDrawable(createTrail(color)); - - double radius = 0.5; - - osg::ref_ptr group = new osg::Group; - - // cone indicates waypoint orientation - osg::ref_ptr sd = new osg::ShapeDrawable; - double coneRadius = radius / 2.0; - osg::ref_ptr cone = - new osg::Cone(osg::Vec3d(0.0, 0.0, 0.0), - coneRadius, radius * 2.0); - - sd->setShape(cone); - sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON); - sd->setColor(color); - - osg::ref_ptr geode = new osg::Geode; - geode->addDrawable(sd); - - osg::ref_ptr pat = - new osg::PositionAttitudeTransform; - pat->addChild(geode); - pat->setAttitude(osg::Quat(- M_PI_2, osg::Vec3d(1.0f, 0.0f, 0.0f), - M_PI_2, osg::Vec3d(0.0f, 1.0f, 0.0f), - 0.0, osg::Vec3d(0.0f, 0.0f, 1.0f))); - group->addChild(pat); - - // cylinder indicates waypoint position - sd = new osg::ShapeDrawable; - osg::ref_ptr cylinder = - new osg::Cylinder(osg::Vec3d(0.0, 0.0, 0.0), - radius, 0); - - sd->setShape(cylinder); - sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON); - sd->setColor(color); - - geode = new osg::Geode; - geode->addDrawable(sd); - group->addChild(geode); - - pat = new osg::PositionAttitudeTransform; - orientationNode->addChild(pat); - pat->addChild(group); + systemData.trailNode()->addDrawable(createTrail(color)); } - QVarLengthArray& trail = trails[component]; + QVector& trail = systemData.trailMap()[component]; bool addToTrail = false; if (trail.size() > 0) @@ -267,228 +201,199 @@ Pixhawk3DWidget::addToTrails(UASInterface* uas, int component, double x, double } void -Pixhawk3DWidget::updateAttitude(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 time) +Pixhawk3DWidget::attitudeChanged(UASInterface* uas, int component, + double roll, double pitch, double yaw, + quint64 time) { - if (this->uas->getUASID() != uas->getUASID()) - { - return; - } + int systemId = uas->getUASID(); - if (!trails.contains(component)) + if (!mSystemContainerMap.contains(systemId)) { return; } - int idx = trailDrawableIdxs[component]; - - osg::PositionAttitudeTransform* pat = - dynamic_cast(orientationNode->getChild(idx)); - - pat->setAttitude(osg::Quat(-yaw, osg::Vec3d(0.0f, 0.0f, 1.0f), - 0.0, osg::Vec3d(1.0f, 0.0f, 0.0f), - 0.0, osg::Vec3d(0.0f, 1.0f, 0.0f))); + // update system attitude + osg::Quat q(-yaw, osg::Vec3d(0.0f, 0.0f, 1.0f), + pitch, osg::Vec3d(1.0f, 0.0f, 0.0f), + roll, osg::Vec3d(0.0f, 1.0f, 0.0f)); + m3DWidget->systemGroup(systemId)->attitude()->setAttitude(q); } void -Pixhawk3DWidget::selectFrame(QString text) +Pixhawk3DWidget::showViewParamWindow(void) { - if (text.compare("Global") == 0) + if (!mViewParamWidget->isVisible()) { - frame = MAV_FRAME_GLOBAL; + mViewParamWidget->show(); } - else if (text.compare("Local") == 0) - { - frame = MAV_FRAME_LOCAL_NED; - } - - getPosition(lastRobotX, lastRobotY, lastRobotZ); - - recenter(); } void -Pixhawk3DWidget::showLocalGrid(int32_t state) +Pixhawk3DWidget::followCameraChanged(int systemId) { - if (state == Qt::Checked) + if (systemId == -1) { - displayLocalGrid = true; + mFollowCameraId = -1; } - else - { - displayLocalGrid = false; - } -} -void -Pixhawk3DWidget::showWorldGrid(int32_t state) -{ - if (state == Qt::Checked) + UASInterface* uas = UASManager::instance()->getUASForId(systemId); + if (!uas) { - displayWorldGrid = true; + return; } - else + + if (mFollowCameraId != systemId) { - displayWorldGrid = false; + double x = 0.0, y = 0.0, z = 0.0; + getPosition(uas, mGlobalViewParams->frame(), x, y, z); + + mCameraPos = QVector3D(x, y, z); + + m3DWidget->recenterCamera(y, x, -z); + + mFollowCameraId = systemId; } } void -Pixhawk3DWidget::showTrails(int32_t state) +Pixhawk3DWidget::recenterActiveCamera(void) { - if (state == Qt::Checked) + if (mFollowCameraId != -1) { - if (!displayTrails) + UASInterface* uas = UASManager::instance()->getUASForId(mFollowCameraId); + if (!uas) { - trails.clear(); + return; } - displayTrails = true; - } - else - { - displayTrails = false; - } -} + double x = 0.0, y = 0.0, z = 0.0; + getPosition(uas, mGlobalViewParams->frame(), x, y, z); -void -Pixhawk3DWidget::showWaypoints(int state) -{ - if (state == Qt::Checked) - { - displayWaypoints = true; - } - else - { - displayWaypoints = false; + mCameraPos = QVector3D(x, y, z); + + m3DWidget->recenterCamera(y, x, -z); } } void -Pixhawk3DWidget::selectMapSource(int index) +Pixhawk3DWidget::modelChanged(int systemId, int index) { - mapNode->setImageryType(static_cast(index)); - - if (mapNode->getImageryType() == Imagery::BLANK_MAP) + if (!mSystemContainerMap.contains(systemId)) { - displayImagery = false; - } - else - { - displayImagery = true; + return; } -} -void -Pixhawk3DWidget::selectVehicleModel(int index) -{ - egocentricMap->removeChild(vehicleModel); - vehicleModel = vehicleModels.at(index); - egocentricMap->addChild(vehicleModel); + SystemContainer& systemData = mSystemContainerMap[systemId]; + osg::ref_ptr& systemGroupNode = m3DWidget->systemGroup(systemId); + + systemGroupNode->egocentricMap()->removeChild(systemData.modelNode()); + systemData.modelNode() = systemData.models().at(index); + systemGroupNode->egocentricMap()->addChild(systemData.modelNode()); } void -Pixhawk3DWidget::recenter(void) +Pixhawk3DWidget::setBirdView(void) { - double robotX = 0.0f, robotY = 0.0f, robotZ = 0.0f; - getPosition(robotX, robotY, robotZ); + mViewParamWidget->setFollowCameraId(-1); - recenterCamera(robotY, robotX, -robotZ); -} + mCameraPos = QVector3D(0.0, 0.0, -100.0); -void -Pixhawk3DWidget::toggleFollowCamera(int32_t state) -{ - if (state == Qt::Checked) - { - followCamera = true; - } - else - { - followCamera = false; - } + m3DWidget->rotateCamera(0.0, 0.0, 0.0); + m3DWidget->recenterCamera(mCameraPos.y(), mCameraPos.x(), -mCameraPos.z()); } void Pixhawk3DWidget::selectTargetHeading(void) { - if (!uas) + if (!mActiveUAS) { return; } osg::Vec2d p; - if (frame == MAV_FRAME_GLOBAL) + if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL) { - double altitude = uas->getAltitude(); + double altitude = mActiveUAS->getAltitude(); - std::pair cursorWorldCoords = - getGlobalCursorPosition(getMouseX(), getMouseY(), altitude); + QPointF cursorWorldCoords = + m3DWidget->worldCursorPosition(m3DWidget->mouseCursorCoords(), altitude); - p.set(cursorWorldCoords.first, cursorWorldCoords.second); + p.set(cursorWorldCoords.x(), cursorWorldCoords.y()); } - else if (frame == MAV_FRAME_LOCAL_NED) + else if (mGlobalViewParams->frame() == MAV_FRAME_LOCAL_NED) { - double z = uas->getLocalZ(); + double z = mActiveUAS->getLocalZ(); - std::pair cursorWorldCoords = - getGlobalCursorPosition(getMouseX(), getMouseY(), -z); + QPointF cursorWorldCoords = + m3DWidget->worldCursorPosition(m3DWidget->mouseCursorCoords(), -z); - p.set(cursorWorldCoords.first, cursorWorldCoords.second); + p.set(cursorWorldCoords.x(), cursorWorldCoords.y()); } + SystemContainer& systemData = mSystemContainerMap[mActiveUAS->getUASID()]; + QVector4D& target = systemData.target(); + target.setW(atan2(p.y() - target.y(), p.x() - target.x())); } void Pixhawk3DWidget::selectTarget(void) { - if (!uas) + if (!mActiveUAS) { return; } - if (!uas->getParamManager()) + if (!mActiveUAS->getParamManager()) { return; } - if (frame == MAV_FRAME_GLOBAL) + SystemContainer& systemData = mSystemContainerMap[mActiveUAS->getUASID()]; + QVector4D& target = systemData.target(); + + if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL) { - double altitude = uas->getAltitude(); + double altitude = mActiveUAS->getAltitude(); - std::pair cursorWorldCoords = - getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(), - altitude); + QPointF cursorWorldCoords = + m3DWidget->worldCursorPosition(mCachedMousePos, altitude); QVariant zTarget; - if (!uas->getParamManager()->getParameterValue(MAV_COMP_ID_PATHPLANNER, "TARGET-ALT", zTarget)) + if (!mActiveUAS->getParamManager()->getParameterValue(MAV_COMP_ID_PATHPLANNER, "TARGET-ALT", zTarget)) { zTarget = -altitude; } - target = QVector4D(cursorWorldCoords.first, cursorWorldCoords.second, + target = QVector4D(cursorWorldCoords.x(), cursorWorldCoords.y(), zTarget.toReal(), 0.0); } - else if (frame == MAV_FRAME_LOCAL_NED) + else if (mGlobalViewParams->frame() == MAV_FRAME_LOCAL_NED) { - double z = uas->getLocalZ(); + double z = mActiveUAS->getLocalZ(); - std::pair cursorWorldCoords = - getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(), -z); + QPointF cursorWorldCoords = + m3DWidget->worldCursorPosition(mCachedMousePos, -z); QVariant zTarget; - if (!uas->getParamManager()->getParameterValue(MAV_COMP_ID_PATHPLANNER, "TARGET-ALT", zTarget)) + if (!mActiveUAS->getParamManager()->getParameterValue(MAV_COMP_ID_PATHPLANNER, "TARGET-ALT", zTarget)) { zTarget = z; } - target = QVector4D(cursorWorldCoords.first, cursorWorldCoords.second, + target = QVector4D(cursorWorldCoords.x(), cursorWorldCoords.y(), zTarget.toReal(), 0.0); } - enableTarget = true; + int systemId = mActiveUAS->getUASID(); - mode = SELECT_TARGET_HEADING_MODE; + QMap::iterator it = mSystemViewParamMap.find(systemId); + if (it != mSystemViewParamMap.end()) + { + it.value()->displayTarget() = true; + } + + mMode = SELECT_TARGET_HEADING_MODE; } void @@ -496,127 +401,129 @@ Pixhawk3DWidget::setTarget(void) { selectTargetHeading(); - uas->setTargetPosition(target.x(), target.y(), target.z(), - osg::RadiansToDegrees(target.w())); + SystemContainer& systemData = mSystemContainerMap[mActiveUAS->getUASID()]; + QVector4D& target = systemData.target(); + + mActiveUAS->setTargetPosition(target.x(), target.y(), target.z(), + osg::RadiansToDegrees(target.w())); } void Pixhawk3DWidget::insertWaypoint(void) { - if (!uas) + if (!mActiveUAS) { return; } Waypoint* wp = NULL; - if (frame == MAV_FRAME_GLOBAL) + if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL) { - double latitude = uas->getLatitude(); - double longitude = uas->getLongitude(); - double altitude = uas->getAltitude(); + double latitude = mActiveUAS->getLatitude(); + double longitude = mActiveUAS->getLongitude(); + double altitude = mActiveUAS->getAltitude(); double x, y; QString utmZone; Imagery::LLtoUTM(latitude, longitude, x, y, utmZone); - std::pair cursorWorldCoords = - getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(), - altitude); + QPointF cursorWorldCoords = + m3DWidget->worldCursorPosition(mCachedMousePos, altitude); - Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone, + Imagery::UTMtoLL(cursorWorldCoords.x(), cursorWorldCoords.y(), utmZone, latitude, longitude); wp = new Waypoint(0, longitude, latitude, altitude, 0.0, 0.25); } - else if (frame == MAV_FRAME_LOCAL_NED) + else if (mGlobalViewParams->frame() == MAV_FRAME_LOCAL_NED) { - double z = uas->getLocalZ(); + double z = mActiveUAS->getLocalZ(); - std::pair cursorWorldCoords = - getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(), -z); + QPointF cursorWorldCoords = + m3DWidget->worldCursorPosition(mCachedMousePos, -z); - wp = new Waypoint(0, cursorWorldCoords.first, - cursorWorldCoords.second, z, 0.0, 0.25); + wp = new Waypoint(0, cursorWorldCoords.x(), + cursorWorldCoords.y(), z, 0.0, 0.25); } if (wp) { - wp->setFrame(frame); - uas->getWaypointManager()->addWaypointEditable(wp); + wp->setFrame(mGlobalViewParams->frame()); + mActiveUAS->getWaypointManager()->addWaypointEditable(wp); } - selectedWpIndex = wp->getId(); - mode = MOVE_WAYPOINT_HEADING_MODE; + mSelectedWpIndex = wp->getId(); + mMode = MOVE_WAYPOINT_HEADING_MODE; } void Pixhawk3DWidget::moveWaypointPosition(void) { - if (mode != MOVE_WAYPOINT_POSITION_MODE) + if (mMode != MOVE_WAYPOINT_POSITION_MODE) { - mode = MOVE_WAYPOINT_POSITION_MODE; + mMode = MOVE_WAYPOINT_POSITION_MODE; return; } - if (!uas) + if (!mActiveUAS) { return; } const QVector waypoints = - uas->getWaypointManager()->getWaypointEditableList(); - Waypoint* waypoint = waypoints.at(selectedWpIndex); + mActiveUAS->getWaypointManager()->getWaypointEditableList(); + Waypoint* waypoint = waypoints.at(mSelectedWpIndex); - if (frame == MAV_FRAME_GLOBAL) + if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL) { - double latitude = uas->getLatitude(); - double longitude = uas->getLongitude(); - double altitude = uas->getAltitude(); + double latitude = mActiveUAS->getLatitude(); + double longitude = mActiveUAS->getLongitude(); + double altitude = mActiveUAS->getAltitude(); double x, y; QString utmZone; Imagery::LLtoUTM(latitude, longitude, x, y, utmZone); - std::pair cursorWorldCoords = - getGlobalCursorPosition(getMouseX(), getMouseY(), altitude); + QPointF cursorWorldCoords = + m3DWidget->worldCursorPosition(m3DWidget->mouseCursorCoords(), altitude); - Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, + Imagery::UTMtoLL(cursorWorldCoords.x(), cursorWorldCoords.y(), utmZone, latitude, longitude); waypoint->setX(longitude); waypoint->setY(latitude); } - else if (frame == MAV_FRAME_LOCAL_NED) + else if (mGlobalViewParams->frame() == MAV_FRAME_LOCAL_NED) { - double z = uas->getLocalZ(); + double z = mActiveUAS->getLocalZ(); - std::pair cursorWorldCoords = - getGlobalCursorPosition(getMouseX(), getMouseY(), -z); + QPointF cursorWorldCoords = + m3DWidget->worldCursorPosition(m3DWidget->mouseCursorCoords(), -z); - waypoint->setX(cursorWorldCoords.first); - waypoint->setY(cursorWorldCoords.second); + waypoint->setX(cursorWorldCoords.x()); + waypoint->setY(cursorWorldCoords.y()); } } void Pixhawk3DWidget::moveWaypointHeading(void) { - if (mode != MOVE_WAYPOINT_HEADING_MODE) + if (mMode != MOVE_WAYPOINT_HEADING_MODE) { - mode = MOVE_WAYPOINT_HEADING_MODE; + mMode = MOVE_WAYPOINT_HEADING_MODE; return; } - if (!uas) + if (!mActiveUAS) { return; } const QVector waypoints = - uas->getWaypointManager()->getWaypointEditableList(); - Waypoint* waypoint = waypoints.at(selectedWpIndex); + mActiveUAS->getWaypointManager()->getWaypointEditableList(); + Waypoint* waypoint = waypoints.at(mSelectedWpIndex); double x = 0.0, y = 0.0, z = 0.0; - if (frame == MAV_FRAME_GLOBAL) + if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL) { double latitude = waypoint->getY(); double longitude = waypoint->getX(); @@ -624,16 +531,16 @@ Pixhawk3DWidget::moveWaypointHeading(void) QString utmZone; Imagery::LLtoUTM(latitude, longitude, x, y, utmZone); } - else if (frame == MAV_FRAME_LOCAL_NED) + else if (mGlobalViewParams->frame() == MAV_FRAME_LOCAL_NED) { - z = uas->getLocalZ(); + z = mActiveUAS->getLocalZ(); } - std::pair cursorWorldCoords = - getGlobalCursorPosition(getMouseX(), getMouseY(), -z); + QPointF cursorWorldCoords = + m3DWidget->worldCursorPosition(m3DWidget->mouseCursorCoords(), -z); - double yaw = atan2(cursorWorldCoords.second - waypoint->getY(), - cursorWorldCoords.first - waypoint->getX()); + double yaw = atan2(cursorWorldCoords.y() - waypoint->getY(), + cursorWorldCoords.x() - waypoint->getX()); yaw = osg::RadiansToDegrees(yaw); waypoint->setYaw(yaw); @@ -642,41 +549,41 @@ Pixhawk3DWidget::moveWaypointHeading(void) void Pixhawk3DWidget::deleteWaypoint(void) { - if (uas) + if (mActiveUAS) { - uas->getWaypointManager()->removeWaypoint(selectedWpIndex); + mActiveUAS->getWaypointManager()->removeWaypoint(mSelectedWpIndex); } } void Pixhawk3DWidget::setWaypointAltitude(void) { - if (!uas) + if (!mActiveUAS) { return; } bool ok; const QVector waypoints = - uas->getWaypointManager()->getWaypointEditableList(); - Waypoint* waypoint = waypoints.at(selectedWpIndex); + mActiveUAS->getWaypointManager()->getWaypointEditableList(); + Waypoint* waypoint = waypoints.at(mSelectedWpIndex); double altitude = waypoint->getZ(); - if (frame == MAV_FRAME_LOCAL_NED) + if (mGlobalViewParams->frame() == MAV_FRAME_LOCAL_NED) { altitude = -altitude; } double newAltitude = - QInputDialog::getDouble(this, tr("Set altitude of waypoint %1").arg(selectedWpIndex), + QInputDialog::getDouble(this, tr("Set altitude of waypoint %1").arg(mSelectedWpIndex), tr("Altitude (m):"), waypoint->getZ(), -1000.0, 1000.0, 1, &ok); if (ok) { - if (frame == MAV_FRAME_GLOBAL) + if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL) { waypoint->setZ(newAltitude); } - else if (frame == MAV_FRAME_LOCAL_NED) + else if (mGlobalViewParams->frame() == MAV_FRAME_LOCAL_NED) { waypoint->setZ(-newAltitude); } @@ -686,36 +593,210 @@ Pixhawk3DWidget::setWaypointAltitude(void) void Pixhawk3DWidget::clearAllWaypoints(void) { - if (uas) + if (mActiveUAS) { const QVector waypoints = - uas->getWaypointManager()->getWaypointEditableList(); + mActiveUAS->getWaypointManager()->getWaypointEditableList(); for (int i = waypoints.size() - 1; i >= 0; --i) { - uas->getWaypointManager()->removeWaypoint(i); + mActiveUAS->getWaypointManager()->removeWaypoint(i); } } } -QVector< osg::ref_ptr > -Pixhawk3DWidget::findVehicleModels(void) +void +Pixhawk3DWidget::sizeChanged(int width, int height) +{ + resizeHUD(width, height); +} + +void +Pixhawk3DWidget::update(void) +{ + MAV_FRAME frame = mGlobalViewParams->frame(); + + // set node visibility + m3DWidget->worldMap()->setChildValue(mWorldGridNode, + mGlobalViewParams->displayWorldGrid()); + if (mGlobalViewParams->imageryType() == Imagery::BLANK_MAP) + { + m3DWidget->worldMap()->setChildValue(mImageryNode, false); + } + else + { + m3DWidget->worldMap()->setChildValue(mImageryNode, true); + } + + // set system-specific node visibility + QMutableMapIterator it(mSystemViewParamMap); + while (it.hasNext()) + { + it.next(); + + osg::ref_ptr& systemNode = m3DWidget->systemGroup(it.key()); + SystemContainer& systemData = mSystemContainerMap[it.key()]; + const SystemViewParamsPtr& systemViewParams = it.value(); + + osg::ref_ptr& rollingMap = systemNode->rollingMap(); + rollingMap->setChildValue(systemData.localGridNode(), + systemViewParams->displayLocalGrid()); + rollingMap->setChildValue(systemData.pointCloudNode(), + systemViewParams->displayPointCloud()); + rollingMap->setChildValue(systemData.targetNode(), + systemViewParams->displayTarget()); + rollingMap->setChildValue(systemData.trailNode(), + systemViewParams->displayTrails()); + rollingMap->setChildValue(systemData.waypointGroupNode(), + systemViewParams->displayWaypoints()); + +#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES) + rollingMap->setChildValue(systemData.obstacleGroupNode(), + systemViewParams->displayObstacleList()); + rollingMap->setChildValue(systemData.plannedPathNode(), + systemViewParams->displayPlannedPath()); + + m3DWidget->hudGroup()->setChildValue(systemData.depthImageNode(), + systemViewParams->displayRGBD()); + m3DWidget->hudGroup()->setChildValue(systemData.rgbImageNode(), + systemViewParams->displayRGBD()); +#endif + } + + mImageryNode->setImageryType(mGlobalViewParams->imageryType()); + + if (mFollowCameraId != -1) + { + UASInterface* uas = UASManager::instance()->getUASForId(mFollowCameraId); + if (uas) + { + double x = 0.0, y = 0.0, z = 0.0; + getPosition(uas, mGlobalViewParams->frame(), x, y, z); + + double dx = y - mCameraPos.y(); + double dy = x - mCameraPos.x(); + double dz = mCameraPos.z() - z; + + m3DWidget->moveCamera(dx, dy, dz); + + mCameraPos = QVector3D(x, y, z); + } + } + else + { + if (!mInitCameraPos && mActiveUAS) + { + double x = 0.0, y = 0.0, z = 0.0; + getPosition(mActiveUAS, frame, x, y, z); + + m3DWidget->recenterCamera(y, x, -z); + + mCameraPos = QVector3D(x, y, z); + + setBirdView(); + mInitCameraPos = true; + } + } + + // update system-specific data + it.toFront(); + while (it.hasNext()) + { + it.next(); + + int systemId = it.key(); + + UASInterface* uas = UASManager::instance()->getUASForId(systemId); + + osg::ref_ptr& systemNode = m3DWidget->systemGroup(systemId); + SystemContainer& systemData = mSystemContainerMap[systemId]; + SystemViewParamsPtr& systemViewParams = it.value(); + + double x = 0.0; + double y = 0.0; + double z = 0.0; + double roll = 0.0; + double pitch = 0.0; + double yaw = 0.0; + + getPose(uas, frame, x, y, z, roll, pitch, yaw); + + if (systemViewParams->displayTarget()) + { + if (systemData.target().isNull()) + { + systemViewParams->displayTarget() = false; + } + else + { + updateTarget(uas, frame, x, y, z, systemData.target(), + systemData.targetNode()); + } + } + if (systemViewParams->displayTrails()) + { + updateTrails(x, y, z, systemData.trailNode(), + systemData.trailMap(), systemData.trailIndexMap()); + } + else + { + systemData.trailMap().clear(); + } + if (systemViewParams->displayWaypoints()) + { + updateWaypoints(uas, frame, systemData.waypointGroupNode()); + } + +#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES) + if (systemViewParams->displayObstacleList()) + { + updateObstacles(uas, frame, x, y, z, systemData.obstacleGroupNode()); + } + if (systemViewParams->displayPlannedPath()) + { + updatePlannedPath(uas, frame, x, y, z, systemData.plannedPathNode()); + } + if (systemViewParams->displayPointCloud()) + { + updatePointCloud(uas, frame, x, y, z, systemData.pointCloudNode(), + systemViewParams->colorPointCloudByDistance()); + } + if (systemViewParams->displayRGBD()) + { + updateRGBD(uas, frame, systemData.rgbImageNode(), + systemData.depthImageNode()); + } +#endif + } + + if (frame == MAV_FRAME_GLOBAL && + mGlobalViewParams->imageryType() != Imagery::BLANK_MAP) + { +// updateImagery(robotX, robotY, robotZ, utmZone); + } + + updateHUD(mActiveUAS, frame); + + layout()->update(); +} + +void +Pixhawk3DWidget::addModels(QVector< osg::ref_ptr >& models, + const QColor& systemColor) { QDir directory("models"); QStringList files = directory.entryList(QStringList("*.osg"), QDir::Files); - QVector< osg::ref_ptr > nodes; - // add Pixhawk Bravo model - nodes.push_back(PixhawkCheetahGeode::instance()); + models.push_back(PixhawkCheetahGeode::create(systemColor)); // add sphere of 0.05m radius osg::ref_ptr sphere = new osg::Sphere(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.05f); osg::ref_ptr sphereDrawable = new osg::ShapeDrawable(sphere); - sphereDrawable->setColor(osg::Vec4f(0.5f, 0.0f, 0.5f, 1.0f)); + sphereDrawable->setColor(osg::Vec4f(systemColor.redF(), systemColor.greenF(), systemColor.blueF(), 1.0f)); osg::ref_ptr sphereGeode = new osg::Geode; sphereGeode->addDrawable(sphereDrawable); sphereGeode->setName("Sphere (0.1m)"); - nodes.push_back(sphereGeode); + models.push_back(sphereGeode); // add all other models in folder for (int i = 0; i < files.size(); ++i) @@ -725,280 +806,107 @@ Pixhawk3DWidget::findVehicleModels(void) if (node) { - nodes.push_back(node); + models.push_back(node); } else { printf("%s\n", QString("ERROR: Could not load file " + directory.absoluteFilePath(files[i]) + "\n").toStdString().c_str()); } } - -// QStringList dirs = directory.entryList(QDir::Dirs); -// // Add models in subfolders -// for (int i = 0; i < dirs.size(); ++i) -// { -// // Handling the current directory -// QStringList currFiles = QDir(dirs[i]).entryList(QStringList("*.ac"), QDir::Files); - -// // Load the file -// osg::ref_ptr node = -// osgDB::readNodeFile(directory.absoluteFilePath(currFiles.first()).toStdString().c_str()); - -// if (node) -// { - -// nodes.push_back(node); -// } -// else -// { -// printf(QString("ERROR: Could not load file " + directory.absoluteFilePath(files[i]) + "\n").toStdString().c_str()); -// } -// } - - return nodes; } void Pixhawk3DWidget::buildLayout(void) { - QComboBox* frameComboBox = new QComboBox(this); - frameComboBox->addItem("Local"); - frameComboBox->addItem("Global"); - frameComboBox->setFixedWidth(70); - - QCheckBox* localGridCheckBox = new QCheckBox(this); - localGridCheckBox->setText("Local Grid"); - localGridCheckBox->setChecked(displayLocalGrid); - - QCheckBox* worldGridCheckBox = new QCheckBox(this); - worldGridCheckBox->setText("World Grid"); - worldGridCheckBox->setChecked(displayWorldGrid); - - QCheckBox* trailCheckBox = new QCheckBox(this); - trailCheckBox->setText("Trails"); - trailCheckBox->setChecked(displayTrails); + QPushButton* viewParamWindowButton = new QPushButton(this); + viewParamWindowButton->setText("View Parameters"); - QCheckBox* waypointsCheckBox = new QCheckBox(this); - waypointsCheckBox->setText("Waypoints"); - waypointsCheckBox->setChecked(displayWaypoints); - - QLabel* mapLabel = new QLabel("Map", this); - QComboBox* mapComboBox = new QComboBox(this); - mapComboBox->addItem("None"); - mapComboBox->addItem("Map (Google)"); - mapComboBox->addItem("Satellite (Google)"); - - QLabel* modelLabel = new QLabel("Vehicle", this); - QComboBox* modelComboBox = new QComboBox(this); - for (int i = 0; i < vehicleModels.size(); ++i) - { - modelComboBox->addItem(vehicleModels[i]->getName().c_str()); - } + QHBoxLayout* layoutTop = new QHBoxLayout; + layoutTop->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding)); + layoutTop->addWidget(viewParamWindowButton); QPushButton* recenterButton = new QPushButton(this); recenterButton->setText("Recenter Camera"); - QCheckBox* followCameraCheckBox = new QCheckBox(this); - followCameraCheckBox->setText("Follow Camera"); - followCameraCheckBox->setChecked(followCamera); + QPushButton* birdViewButton = new QPushButton(this); + birdViewButton->setText("Bird's Eye View"); + + QHBoxLayout* layoutBottom = new QHBoxLayout; + layoutBottom->addWidget(recenterButton); + layoutBottom->addWidget(birdViewButton); + layoutBottom->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding)); QGridLayout* layout = new QGridLayout(this); layout->setMargin(0); layout->setSpacing(2); - layout->addWidget(frameComboBox, 0, 10); - layout->addWidget(localGridCheckBox, 2, 0); - layout->addWidget(worldGridCheckBox, 2, 1); - layout->addWidget(trailCheckBox, 2, 2); - layout->addWidget(waypointsCheckBox, 2, 3); - layout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 2, 4); - layout->addWidget(mapLabel, 2, 5); - layout->addWidget(mapComboBox, 2, 6); - layout->addWidget(modelLabel, 2, 7); - layout->addWidget(modelComboBox, 2, 8); - layout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 2, 9); - layout->addWidget(recenterButton, 2, 10); - layout->addWidget(followCameraCheckBox, 2, 11); + layout->addLayout(layoutTop, 0, 0); + layout->addWidget(m3DWidget, 1, 0); + layout->addLayout(layoutBottom, 2, 0); layout->setRowStretch(0, 1); layout->setRowStretch(1, 100); layout->setRowStretch(2, 1); - setLayout(layout); - - connect(frameComboBox, SIGNAL(currentIndexChanged(QString)), - this, SLOT(selectFrame(QString))); - connect(localGridCheckBox, SIGNAL(stateChanged(int)), - this, SLOT(showLocalGrid(int))); - connect(worldGridCheckBox, SIGNAL(stateChanged(int)), - this, SLOT(showWorldGrid(int))); - connect(trailCheckBox, SIGNAL(stateChanged(int)), - this, SLOT(showTrails(int))); - connect(waypointsCheckBox, SIGNAL(stateChanged(int)), - this, SLOT(showWaypoints(int))); - connect(mapComboBox, SIGNAL(currentIndexChanged(int)), - this, SLOT(selectMapSource(int))); - connect(modelComboBox, SIGNAL(currentIndexChanged(int)), - this, SLOT(selectVehicleModel(int))); - connect(recenterButton, SIGNAL(clicked()), this, SLOT(recenter())); - connect(followCameraCheckBox, SIGNAL(stateChanged(int)), - this, SLOT(toggleFollowCamera(int))); -} - -void -Pixhawk3DWidget::resizeGL(int width, int height) -{ - Q3DWidget::resizeGL(width, height); - resizeHUD(); + connect(viewParamWindowButton, SIGNAL(clicked()), + this, SLOT(showViewParamWindow())); + connect(recenterButton, SIGNAL(clicked()), + this, SLOT(recenterActiveCamera())); + connect(birdViewButton, SIGNAL(clicked()), + this, SLOT(setBirdView())); } void -Pixhawk3DWidget::display(void) +Pixhawk3DWidget::keyPressEvent(QKeyEvent* event) { - // set node visibility - allocentricMap->setChildValue(worldGridNode, displayWorldGrid); - rollingMap->setChildValue(localGridNode, displayLocalGrid); - rollingMap->setChildValue(trailNode, displayTrails); - rollingMap->setChildValue(orientationNode, displayTrails); - rollingMap->setChildValue(mapNode, displayImagery); - rollingMap->setChildValue(waypointGroupNode, displayWaypoints); - rollingMap->setChildValue(targetNode, enableTarget); -#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES) - rollingMap->setChildValue(obstacleGroupNode, displayObstacleList); - rollingMap->setChildValue(pathNode, displayPath); -#endif - rollingMap->setChildValue(rgbd3DNode, displayRGBD3D); - hudGroup->setChildValue(rgb2DGeode, displayRGBD2D); - hudGroup->setChildValue(depth2DGeode, displayRGBD2D); - - if (!uas) + QWidget::keyPressEvent(event); + if (event->isAccepted()) { return; } - double robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw; - QString utmZone; - getPose(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw, utmZone); - - if (lastRobotX == 0.0f && lastRobotY == 0.0f && lastRobotZ == 0.0f) - { - lastRobotX = robotX; - lastRobotY = robotY; - lastRobotZ = robotZ; - - recenterCamera(robotY, robotX, -robotZ); - } - - if (followCamera) - { - double dx = robotY - lastRobotY; - double dy = robotX - lastRobotX; - double dz = lastRobotZ - robotZ; - - moveCamera(dx, dy, dz); - } - - robotPosition->setPosition(osg::Vec3d(robotY, robotX, -robotZ)); - robotAttitude->setAttitude(osg::Quat(-robotYaw, osg::Vec3d(0.0f, 0.0f, 1.0f), - robotPitch, osg::Vec3d(1.0f, 0.0f, 0.0f), - robotRoll, osg::Vec3d(0.0f, 1.0f, 0.0f))); - - if (displayTrails) - { - updateTrails(robotX, robotY, robotZ); - } - - if (frame == MAV_FRAME_GLOBAL && displayImagery) - { - updateImagery(robotX, robotY, robotZ, utmZone); - } - - if (displayWaypoints) - { - updateWaypoints(); - } - - if (enableTarget) - { - updateTarget(robotX, robotY, robotZ); - } - -#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES) - if (displayRGBD2D || displayRGBD3D) - { - updateRGBD(robotX, robotY, robotZ); - } - - if (displayObstacleList) - { - updateObstacles(robotX, robotY, robotZ); - } - - if (displayPath) - { - updatePath(robotX, robotY, robotZ); - } -#endif - - updateHUD(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw, utmZone); - - lastRobotX = robotX; - lastRobotY = robotY; - lastRobotZ = robotZ; - - layout()->update(); + m3DWidget->handleKeyPressEvent(event); } void -Pixhawk3DWidget::keyPressEvent(QKeyEvent* event) +Pixhawk3DWidget::keyReleaseEvent(QKeyEvent* event) { - if (!event->text().isEmpty()) + QWidget::keyReleaseEvent(event); + if (event->isAccepted()) { - switch (*(event->text().toAscii().data())) - { - case '1': - displayRGBD2D = !displayRGBD2D; - break; - case '2': - displayRGBD3D = !displayRGBD3D; - break; - case 'c': - case 'C': - enableRGBDColor = !enableRGBDColor; - break; - case 'o': - case 'O': - displayObstacleList = !displayObstacleList; - break; - case 'p': - case 'P': - displayPath = !displayPath; - break; - } + return; } - Q3DWidget::keyPressEvent(event); + m3DWidget->handleKeyReleaseEvent(event); } void Pixhawk3DWidget::mousePressEvent(QMouseEvent* event) { + QWidget::mousePressEvent(event); + if (event->isAccepted()) + { + return; + } + if (event->button() == Qt::LeftButton) { - if (mode == SELECT_TARGET_HEADING_MODE) + if (mMode == SELECT_TARGET_HEADING_MODE) { setTarget(); + event->accept(); } - if (mode != DEFAULT_MODE) + if (mMode != DEFAULT_MODE) { - mode = DEFAULT_MODE; + mMode = DEFAULT_MODE; + event->accept(); } if (event->modifiers() == Qt::ShiftModifier) { - selectedWpIndex = findWaypoint(event->pos()); - if (selectedWpIndex == -1) + mSelectedWpIndex = findWaypoint(event->pos()); + if (mSelectedWpIndex == -1) { - cachedMousePos = event->pos(); + mCachedMousePos = event->pos(); showInsertWaypointMenu(event->globalPos()); } @@ -1006,51 +914,149 @@ Pixhawk3DWidget::mousePressEvent(QMouseEvent* event) { showEditWaypointMenu(event->globalPos()); } + event->accept(); return; } } - Q3DWidget::mousePressEvent(event); + m3DWidget->handleMousePressEvent(event); } void -Pixhawk3DWidget::showEvent(QShowEvent* event) +Pixhawk3DWidget::mouseReleaseEvent(QMouseEvent* event) { - Q3DWidget::showEvent(event); - emit visibilityChanged(true); -} + QWidget::mouseReleaseEvent(event); + if (event->isAccepted()) + { + return; + } -void -Pixhawk3DWidget::hideEvent(QHideEvent* event) -{ - Q3DWidget::hideEvent(event); - emit visibilityChanged(false); + m3DWidget->handleMouseReleaseEvent(event); } void Pixhawk3DWidget::mouseMoveEvent(QMouseEvent* event) { - if (mode == SELECT_TARGET_HEADING_MODE) + QWidget::mouseMoveEvent(event); + if (event->isAccepted()) + { + return; + } + + if (mMode == SELECT_TARGET_HEADING_MODE) { selectTargetHeading(); + event->accept(); } - if (mode == MOVE_WAYPOINT_POSITION_MODE) + if (mMode == MOVE_WAYPOINT_POSITION_MODE) { moveWaypointPosition(); + event->accept(); } - if (mode == MOVE_WAYPOINT_HEADING_MODE) + if (mMode == MOVE_WAYPOINT_HEADING_MODE) { moveWaypointHeading(); + event->accept(); + } + + m3DWidget->handleMouseMoveEvent(event); +} + +void +Pixhawk3DWidget::wheelEvent(QWheelEvent* event) +{ + QWidget::wheelEvent(event); + if (event->isAccepted()) + { + return; + } + + m3DWidget->handleWheelEvent(event); +} + +void +Pixhawk3DWidget::showEvent(QShowEvent* event) +{ + emit visibilityChanged(true); +} + +void +Pixhawk3DWidget::hideEvent(QHideEvent* event) +{ + emit visibilityChanged(false); +} + +void +Pixhawk3DWidget::initializeSystem(int systemId, const QColor& systemColor) +{ + SystemViewParamsPtr& systemViewParams = mSystemViewParamMap[systemId]; + SystemContainer& systemData = mSystemContainerMap[systemId]; + osg::ref_ptr& systemNode = m3DWidget->systemGroup(systemId); + + // generate grid model + systemData.localGridNode() = createLocalGrid(); + systemNode->rollingMap()->addChild(systemData.localGridNode(), false); + + // generate point cloud model + systemData.pointCloudNode() = createPointCloud(); + systemNode->rollingMap()->addChild(systemData.pointCloudNode(), false); + + // generate target model + systemData.targetNode() = createTarget(systemColor); + systemNode->rollingMap()->addChild(systemData.targetNode(), false); + + // generate empty trail model + systemData.trailNode() = new osg::Geode; + systemNode->rollingMap()->addChild(systemData.trailNode(), false); + + // generate waypoint model + systemData.waypointGroupNode() = new WaypointGroupNode(systemColor); + systemData.waypointGroupNode()->init(); + systemNode->rollingMap()->addChild(systemData.waypointGroupNode(), false); + +#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES) + systemData.obstacleGroupNode() = new ObstacleGroupNode; + systemData.obstacleGroupNode()->init(); + systemNode->rollingMap()->addChild(systemData.obstacleGroupNode(), false); + + // generate path model + systemData.plannedPathNode() = new osg::Geode; + systemData.plannedPathNode()->addDrawable(createTrail(osg::Vec4(1.0f, 0.8f, 0.0f, 1.0f))); + systemNode->rollingMap()->addChild(systemData.plannedPathNode(), false); +#endif + + systemData.rgbImageNode() = new ImageWindowGeode; + systemData.rgbImageNode()->init("RGB Image", osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f), + m3DWidget->font()); + m3DWidget->hudGroup()->addChild(systemData.rgbImageNode(), false); + + systemData.depthImageNode() = new ImageWindowGeode; + systemData.depthImageNode()->init("Depth Image", osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f), + m3DWidget->font()); + m3DWidget->hudGroup()->addChild(systemData.depthImageNode(), false); + + // find available models + addModels(systemData.models(), systemColor); + systemViewParams->modelNames(); + for (int i = 0; i < systemData.models().size(); ++i) + { + systemViewParams->modelNames().push_back(systemData.models()[i]->getName().c_str()); } - Q3DWidget::mouseMoveEvent(event); + systemData.modelNode() = systemData.models().front(); + systemNode->egocentricMap()->addChild(systemData.modelNode()); + + connect(systemViewParams.data(), SIGNAL(modelChangedSignal(int,int)), + this, SLOT(modelChanged(int,int))); } void -Pixhawk3DWidget::getPose(double& x, double& y, double& z, +Pixhawk3DWidget::getPose(UASInterface* uas, + MAV_FRAME frame, + double& x, double& y, double& z, double& roll, double& pitch, double& yaw, - QString& utmZone) + QString& utmZone) const { if (!uas) { @@ -1079,16 +1085,20 @@ Pixhawk3DWidget::getPose(double& x, double& y, double& z, } void -Pixhawk3DWidget::getPose(double& x, double& y, double& z, - double& roll, double& pitch, double& yaw) +Pixhawk3DWidget::getPose(UASInterface* uas, + MAV_FRAME frame, + double& x, double& y, double& z, + double& roll, double& pitch, double& yaw) const { QString utmZone; - getPose(x, y, z, roll, pitch, yaw); + getPose(uas, frame, x, y, z, roll, pitch, yaw, utmZone); } void -Pixhawk3DWidget::getPosition(double& x, double& y, double& z, - QString& utmZone) +Pixhawk3DWidget::getPosition(UASInterface* uas, + MAV_FRAME frame, + double& x, double& y, double& z, + QString& utmZone) const { if (!uas) { @@ -1113,10 +1123,12 @@ Pixhawk3DWidget::getPosition(double& x, double& y, double& z, } void -Pixhawk3DWidget::getPosition(double& x, double& y, double& z) +Pixhawk3DWidget::getPosition(UASInterface* uas, + MAV_FRAME frame, + double& x, double& y, double& z) const { QString utmZone; - getPosition(x, y, z, utmZone); + getPosition(uas, frame, x, y, z, utmZone); } osg::ref_ptr @@ -1323,13 +1335,13 @@ Pixhawk3DWidget::createTrail(const osg::Vec4& color) } osg::ref_ptr -Pixhawk3DWidget::createMap(void) +Pixhawk3DWidget::createImagery(void) { return osg::ref_ptr(new Imagery()); } osg::ref_ptr -Pixhawk3DWidget::createRGBD3D(void) +Pixhawk3DWidget::createPointCloud(void) { int frameSize = 752 * 480; @@ -1350,7 +1362,7 @@ Pixhawk3DWidget::createRGBD3D(void) } osg::ref_ptr -Pixhawk3DWidget::createTarget(void) +Pixhawk3DWidget::createTarget(const QColor& color) { osg::ref_ptr pat = new osg::PositionAttitudeTransform; @@ -1359,7 +1371,7 @@ Pixhawk3DWidget::createTarget(void) osg::ref_ptr cone = new osg::Cone(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.2f, 0.6f); osg::ref_ptr coneDrawable = new osg::ShapeDrawable(cone); - coneDrawable->setColor(osg::Vec4f(0.0f, 1.0f, 0.0f, 1.0f)); + coneDrawable->setColor(osg::Vec4f(color.redF(), color.greenF(), color.blueF(), 1.0f)); coneDrawable->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON); osg::ref_ptr coneGeode = new osg::Geode; coneGeode->addDrawable(coneDrawable); @@ -1377,98 +1389,110 @@ Pixhawk3DWidget::setupHUD(void) hudColors->push_back(osg::Vec4(0.0f, 0.0f, 0.0f, 0.5f)); hudColors->push_back(osg::Vec4(0.0f, 0.0f, 0.0f, 1.0f)); - hudBackgroundGeometry = new osg::Geometry; - hudBackgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON, - 0, 4)); - hudBackgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON, - 4, 4)); - hudBackgroundGeometry->setColorArray(hudColors); - hudBackgroundGeometry->setColorBinding(osg::Geometry::BIND_PER_PRIMITIVE_SET); - hudBackgroundGeometry->setUseDisplayList(false); - - statusText = new osgText::Text; - statusText->setCharacterSize(11); - statusText->setFont(font); - statusText->setAxisAlignment(osgText::Text::SCREEN); - statusText->setColor(osg::Vec4(255, 255, 255, 1)); + mHudBackgroundGeometry = new osg::Geometry; + mHudBackgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON, + 0, 4)); + mHudBackgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON, + 4, 4)); + mHudBackgroundGeometry->setColorArray(hudColors); + mHudBackgroundGeometry->setColorBinding(osg::Geometry::BIND_PER_PRIMITIVE_SET); + mHudBackgroundGeometry->setUseDisplayList(false); + + mStatusText = new osgText::Text; + mStatusText->setCharacterSize(11); + mStatusText->setFont(m3DWidget->font()); + mStatusText->setAxisAlignment(osgText::Text::SCREEN); + mStatusText->setColor(osg::Vec4(255, 255, 255, 1)); osg::ref_ptr statusGeode = new osg::Geode; - statusGeode->addDrawable(hudBackgroundGeometry); - statusGeode->addDrawable(statusText); - hudGroup->addChild(statusGeode); - - rgbImage = new osg::Image; - rgb2DGeode = new ImageWindowGeode; - rgb2DGeode->init("RGB Image", osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f), - rgbImage, font); - hudGroup->addChild(rgb2DGeode); - - depthImage = new osg::Image; - depth2DGeode = new ImageWindowGeode; - depth2DGeode->init("Depth Image", osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f), - depthImage, font); - hudGroup->addChild(depth2DGeode); - - scaleGeode = new HUDScaleGeode; - scaleGeode->init(font); - hudGroup->addChild(scaleGeode); + statusGeode->addDrawable(mHudBackgroundGeometry); + statusGeode->addDrawable(mStatusText); + m3DWidget->hudGroup()->addChild(statusGeode); + + mScaleGeode = new HUDScaleGeode; + mScaleGeode->init(m3DWidget->font()); + m3DWidget->hudGroup()->addChild(mScaleGeode); } void -Pixhawk3DWidget::resizeHUD(void) +Pixhawk3DWidget::resizeHUD(int width, int height) { int topHUDHeight = 25; int bottomHUDHeight = 25; - osg::Vec3Array* vertices = static_cast(hudBackgroundGeometry->getVertexArray()); + osg::Vec3Array* vertices = static_cast(mHudBackgroundGeometry->getVertexArray()); if (vertices == NULL || vertices->size() != 8) { osg::ref_ptr newVertices = new osg::Vec3Array(8); - hudBackgroundGeometry->setVertexArray(newVertices); + mHudBackgroundGeometry->setVertexArray(newVertices); - vertices = static_cast(hudBackgroundGeometry->getVertexArray()); + vertices = static_cast(mHudBackgroundGeometry->getVertexArray()); } - (*vertices)[0] = osg::Vec3(0, height(), -1); - (*vertices)[1] = osg::Vec3(width(), height(), -1); - (*vertices)[2] = osg::Vec3(width(), height() - topHUDHeight, -1); - (*vertices)[3] = osg::Vec3(0, height() - topHUDHeight, -1); + (*vertices)[0] = osg::Vec3(0, height, -1); + (*vertices)[1] = osg::Vec3(width, height, -1); + (*vertices)[2] = osg::Vec3(width, height - topHUDHeight, -1); + (*vertices)[3] = osg::Vec3(0, height - topHUDHeight, -1); (*vertices)[4] = osg::Vec3(0, 0, -1); - (*vertices)[5] = osg::Vec3(width(), 0, -1); - (*vertices)[6] = osg::Vec3(width(), bottomHUDHeight, -1); + (*vertices)[5] = osg::Vec3(width, 0, -1); + (*vertices)[6] = osg::Vec3(width, bottomHUDHeight, -1); (*vertices)[7] = osg::Vec3(0, bottomHUDHeight, -1); - statusText->setPosition(osg::Vec3(10, height() - 15, -1.5)); + mStatusText->setPosition(osg::Vec3(10, height - 15, -1.5)); - if (rgb2DGeode.valid() && depth2DGeode.valid()) + QMutableMapIterator it(mSystemContainerMap); + while (it.hasNext()) { - int windowWidth = (width() - 20) / 2; - int windowHeight = 3 * windowWidth / 4; - rgb2DGeode->setAttributes(10, (height() - windowHeight) / 2, - windowWidth, windowHeight); - depth2DGeode->setAttributes(width() / 2, (height() - windowHeight) / 2, - windowWidth, windowHeight); + it.next(); + + SystemContainer& systemData = it.value(); + + if (systemData.rgbImageNode().valid() && + systemData.depthImageNode().valid()) + { + int windowWidth = (width - 20) / 2; + int windowHeight = 3 * windowWidth / 4; + systemData.rgbImageNode()->setAttributes(10, + (height - windowHeight) / 2, + windowWidth, + windowHeight); + systemData.depthImageNode()->setAttributes(width / 2, + (height - windowHeight) / 2, + windowWidth, + windowHeight); + } } } void -Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ, - double robotRoll, double robotPitch, double robotYaw, - const QString& utmZone) +Pixhawk3DWidget::updateHUD(UASInterface* uas, MAV_FRAME frame) { - std::pair cursorPosition = - getGlobalCursorPosition(getMouseX(), getMouseY(), -robotZ); + // display pose of current system + double x = 0.0; + double y = 0.0; + double z = 0.0; + double roll = 0.0; + double pitch = 0.0; + double yaw = 0.0; + QString utmZone; + + getPose(uas, frame, x, y, z, roll, pitch, yaw, utmZone); + + QPointF cursorPosition = + m3DWidget->worldCursorPosition(m3DWidget->mouseCursorCoords(), -z); std::ostringstream oss; oss.setf(std::ios::fixed, std::ios::floatfield); oss.precision(2); + oss << "MAV " << uas->getUASID() << ": "; + if (frame == MAV_FRAME_GLOBAL) { double latitude, longitude; - Imagery::UTMtoLL(robotX, robotY, utmZone, latitude, longitude); + Imagery::UTMtoLL(x, y, utmZone, latitude, longitude); double cursorLatitude, cursorLongitude; - Imagery::UTMtoLL(cursorPosition.first, cursorPosition.second, + Imagery::UTMtoLL(cursorPosition.x(), cursorPosition.y(), utmZone, cursorLatitude, cursorLongitude); oss.precision(6); @@ -1476,10 +1500,10 @@ Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ, " Lon = " << longitude; oss.precision(2); - oss << " Altitude = " << -robotZ << - " r = " << robotRoll << - " p = " << robotPitch << - " y = " << robotYaw; + oss << " Altitude = " << -z << + " r = " << roll << + " p = " << pitch << + " y = " << yaw; oss.precision(6); oss << " Cursor [" << cursorLatitude << @@ -1487,32 +1511,36 @@ Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ, } else if (frame == MAV_FRAME_LOCAL_NED) { - oss << " x = " << robotX << - " y = " << robotY << - " z = " << robotZ << - " r = " << robotRoll << - " p = " << robotPitch << - " y = " << robotYaw << - " Cursor [" << cursorPosition.first << - " " << cursorPosition.second << "]"; + oss << " x = " << x << + " y = " << y << + " z = " << z << + " r = " << roll << + " p = " << pitch << + " y = " << yaw << + " Cursor [" << cursorPosition.x() << + " " << cursorPosition.y() << "]"; } - statusText->setText(oss.str()); + mStatusText->setText(oss.str()); bool darkBackground = true; - if (mapNode->getImageryType() == Imagery::GOOGLE_MAP) + if (mImageryNode->getImageryType() == Imagery::GOOGLE_MAP) { darkBackground = false; } - scaleGeode->update(height(), cameraParams.cameraFov, - cameraManipulator->getDistance(), darkBackground); + mScaleGeode->update(height(), m3DWidget->cameraParams().fov(), + m3DWidget->cameraManipulator()->getDistance(), + darkBackground); } void -Pixhawk3DWidget::updateTrails(double robotX, double robotY, double robotZ) +Pixhawk3DWidget::updateTrails(double robotX, double robotY, double robotZ, + osg::ref_ptr& trailNode, + QMap >& trailMap, + QMap& trailIndexMap) { - QMapIterator it(trailDrawableIdxs); + QMapIterator it(trailIndexMap); while (it.hasNext()) { @@ -1523,7 +1551,7 @@ Pixhawk3DWidget::updateTrails(double robotX, double robotY, double robotZ) osg::ref_ptr vertices(new osg::Vec3Array); - const QVarLengthArray& trail = trails.value(it.key()); + const QVector& trail = trailMap.value(it.key()); for (int i = 0; i < trail.size(); ++i) { @@ -1536,12 +1564,6 @@ Pixhawk3DWidget::updateTrails(double robotX, double robotY, double robotZ) drawArrays->setFirst(0); drawArrays->setCount(vertices->size()); geometry->dirtyBound(); - - osg::PositionAttitudeTransform* pat = - dynamic_cast(orientationNode->getChild(it.value())); - pat->setPosition(osg::Vec3(trail[trail.size() - 1].y() - robotY, - trail[trail.size() - 1].x() - robotX, - -(trail[trail.size() - 1].z() - robotZ))); } } @@ -1549,22 +1571,22 @@ void Pixhawk3DWidget::updateImagery(double originX, double originY, double originZ, const QString& zone) { - if (mapNode->getImageryType() == Imagery::BLANK_MAP) + if (mImageryNode->getImageryType() == Imagery::BLANK_MAP) { return; } - double viewingRadius = cameraManipulator->getDistance() * 10.0; + double viewingRadius = m3DWidget->cameraManipulator()->getDistance() * 10.0; if (viewingRadius < 100.0) { viewingRadius = 100.0; } double minResolution = 0.25; - double centerResolution = cameraManipulator->getDistance() / 50.0; + double centerResolution = m3DWidget->cameraManipulator()->getDistance() / 50.0; double maxResolution = 1048576.0; - Imagery::ImageryType imageryType = mapNode->getImageryType(); + Imagery::Type imageryType = mImageryNode->getImageryType(); switch (imageryType) { case Imagery::GOOGLE_MAP: @@ -1592,44 +1614,41 @@ Pixhawk3DWidget::updateImagery(double originX, double originY, double originZ, resolution = maxResolution; } - mapNode->draw3D(viewingRadius, - resolution, - cameraManipulator->getCenter().y(), - cameraManipulator->getCenter().x(), - originX, - originY, - originZ, - zone); + mImageryNode->draw3D(viewingRadius, + resolution, + m3DWidget->cameraManipulator()->getCenter().y(), + m3DWidget->cameraManipulator()->getCenter().x(), + originX, + originY, + originZ, + zone); // prefetch map tiles if (resolution / 2.0 >= minResolution) { - mapNode->prefetch3D(viewingRadius / 2.0, - resolution / 2.0, - cameraManipulator->getCenter().y(), - cameraManipulator->getCenter().x(), - zone); + mImageryNode->prefetch3D(viewingRadius / 2.0, + resolution / 2.0, + m3DWidget->cameraManipulator()->getCenter().y(), + m3DWidget->cameraManipulator()->getCenter().x(), + zone); } if (resolution * 2.0 <= maxResolution) { - mapNode->prefetch3D(viewingRadius * 2.0, - resolution * 2.0, - cameraManipulator->getCenter().y(), - cameraManipulator->getCenter().x(), - zone); + mImageryNode->prefetch3D(viewingRadius * 2.0, + resolution * 2.0, + m3DWidget->cameraManipulator()->getCenter().y(), + m3DWidget->cameraManipulator()->getCenter().x(), + zone); } - mapNode->update(); -} - -void -Pixhawk3DWidget::updateWaypoints(void) -{ - waypointGroupNode->update(frame, uas); + mImageryNode->update(); } void -Pixhawk3DWidget::updateTarget(double robotX, double robotY, double robotZ) +Pixhawk3DWidget::updateTarget(UASInterface* uas, MAV_FRAME frame, + double robotX, double robotY, double robotZ, + QVector4D& target, + osg::ref_ptr& targetNode) { osg::PositionAttitudeTransform* pat = dynamic_cast(targetNode.get()); @@ -1643,114 +1662,21 @@ Pixhawk3DWidget::updateTarget(double robotX, double robotY, double robotZ) osg::Geode* geode = dynamic_cast(pat->getChild(0)); osg::ShapeDrawable* sd = dynamic_cast(geode->getDrawable(0)); - - sd->setColor(osg::Vec4f(1.0f, 0.8f, 0.0f, 1.0f)); } -#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES) void -Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ) +Pixhawk3DWidget::updateWaypoints(UASInterface* uas, MAV_FRAME frame, + osg::ref_ptr& waypointGroupNode) { - qreal receivedRGBDImageTimestamp, receivedPointCloudTimestamp; - px::RGBDImage rgbdImage = uas->getRGBDImage(receivedRGBDImageTimestamp); - px::PointCloudXYZRGB pointCloud = uas->getPointCloud(receivedPointCloudTimestamp); - - if (rgbdImage.rows() > 0 && rgbdImage.cols() > 0 && - QGC::groundTimeSeconds() - receivedRGBDImageTimestamp < kMessageTimeout) - { - rgbImage->setImage(rgbdImage.cols(), rgbdImage.rows(), 1, - GL_LUMINANCE, GL_LUMINANCE, GL_UNSIGNED_BYTE, - reinterpret_cast(&(*(rgbdImage.mutable_imagedata1()))[0]), - osg::Image::NO_DELETE); - rgbImage->dirty(); - - QByteArray coloredDepth(rgbdImage.cols() * rgbdImage.rows() * 3, 0); - for (uint32_t r = 0; r < rgbdImage.rows(); ++r) - { - const float* depth = reinterpret_cast(rgbdImage.imagedata2().c_str() + r * rgbdImage.step2()); - uint8_t* pixel = reinterpret_cast(coloredDepth.data()) + r * rgbdImage.cols() * 3; - for (uint32_t c = 0; c < rgbdImage.cols(); ++c) - { - if (depth[c] != 0) - { - int idx = fminf(depth[c], 7.0f) / 7.0f * 127.0f; - idx = 127 - idx; - - float r, g, b; - qgc::colormap("jet", idx, r, g, b); - pixel[0] = r * 255.0f; - pixel[1] = g * 255.0f; - pixel[2] = b * 255.0f; - } - - pixel += 3; - } - } - - depthImage->setImage(rgbdImage.cols(), rgbdImage.rows(), 1, - GL_RGB, GL_RGB, GL_UNSIGNED_BYTE, - reinterpret_cast(coloredDepth.data()), - osg::Image::NO_DELETE); - depthImage->dirty(); - } - - osg::Geometry* geometry = rgbd3DNode->getDrawable(0)->asGeometry(); - osg::Vec3Array* vertices = static_cast(geometry->getVertexArray()); - osg::Vec4Array* colors = static_cast(geometry->getColorArray()); - - if (QGC::groundTimeSeconds() - receivedPointCloudTimestamp > kMessageTimeout) - { - geometry->removePrimitiveSet(0, geometry->getNumPrimitiveSets()); - return; - } - - for (int i = 0; i < pointCloud.points_size(); ++i) - { - const px::PointCloudXYZRGB_PointXYZRGB& p = pointCloud.points(i); - - double x = p.x() - robotX; - double y = p.y() - robotY; - double z = p.z() - robotZ; - - - (*vertices)[i].set(y, x, -z); - - if (enableRGBDColor) - { - float rgb = p.rgb(); - - float b = *(reinterpret_cast(&rgb)) / 255.0f; - float g = *(1 + reinterpret_cast(&rgb)) / 255.0f; - float r = *(2 + reinterpret_cast(&rgb)) / 255.0f; - - (*colors)[i].set(r, g, b, 1.0f); - } - else - { - double dist = sqrt(x * x + y * y + z * z); - int colorIndex = static_cast(fmin(dist / 7.0 * 127.0, 127.0)); - - float r, g, b; - qgc::colormap("jet", colorIndex, r, g, b); - - (*colors)[i].set(r, g, b, 1.0f); - } - } - - if (geometry->getNumPrimitiveSets() == 0) - { - geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS, - 0, pointCloud.points_size())); - } - else - { - osg::DrawArrays* drawarrays = static_cast(geometry->getPrimitiveSet(0)); - drawarrays->setCount(pointCloud.points_size()); - } + waypointGroupNode->update(uas, frame); } +#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES) + void -Pixhawk3DWidget::updateObstacles(double robotX, double robotY, double robotZ) +Pixhawk3DWidget::updateObstacles(UASInterface* uas, MAV_FRAME frame, + double robotX, double robotY, double robotZ, + osg::ref_ptr& obstacleGroupNode) { if (frame == MAV_FRAME_GLOBAL) { @@ -1772,12 +1698,14 @@ Pixhawk3DWidget::updateObstacles(double robotX, double robotY, double robotZ) } void -Pixhawk3DWidget::updatePath(double robotX, double robotY, double robotZ) +Pixhawk3DWidget::updatePlannedPath(UASInterface* uas, MAV_FRAME frame, + double robotX, double robotY, double robotZ, + osg::ref_ptr& plannedPathNode) { qreal receivedTimestamp; px::Path path = uas->getPath(receivedTimestamp); - osg::Geometry* geometry = pathNode->getDrawable(0)->asGeometry(); + osg::Geometry* geometry = plannedPathNode->getDrawable(0)->asGeometry(); osg::DrawArrays* drawArrays = reinterpret_cast(geometry->getPrimitiveSet(0)); osg::Vec4Array* colorArray = reinterpret_cast(geometry->getColorArray()); @@ -1846,25 +1774,149 @@ Pixhawk3DWidget::updatePath(double robotX, double robotY, double robotZ) geometry->dirtyBound(); } +void +Pixhawk3DWidget::updateRGBD(UASInterface* uas, MAV_FRAME frame, + osg::ref_ptr& rgbImageNode, + osg::ref_ptr& depthImageNode) +{ + qreal receivedTimestamp; + px::RGBDImage rgbdImage = uas->getRGBDImage(receivedTimestamp); + + if (rgbdImage.rows() > 0 && rgbdImage.cols() > 0 && + QGC::groundTimeSeconds() - receivedTimestamp < kMessageTimeout) + { + rgbImageNode->image()->setImage(rgbdImage.cols(), rgbdImage.rows(), 1, + GL_LUMINANCE, GL_LUMINANCE, GL_UNSIGNED_BYTE, + reinterpret_cast(&(*(rgbdImage.mutable_imagedata1()))[0]), + osg::Image::NO_DELETE); + rgbImageNode->image()->dirty(); + + QByteArray coloredDepth(rgbdImage.cols() * rgbdImage.rows() * 3, 0); + for (uint32_t r = 0; r < rgbdImage.rows(); ++r) + { + const float* depth = reinterpret_cast(rgbdImage.imagedata2().c_str() + r * rgbdImage.step2()); + uint8_t* pixel = reinterpret_cast(coloredDepth.data()) + r * rgbdImage.cols() * 3; + for (uint32_t c = 0; c < rgbdImage.cols(); ++c) + { + if (depth[c] != 0) + { + int idx = fminf(depth[c], 7.0f) / 7.0f * 127.0f; + idx = 127 - idx; + + float r, g, b; + qgc::colormap("jet", idx, r, g, b); + pixel[0] = r * 255.0f; + pixel[1] = g * 255.0f; + pixel[2] = b * 255.0f; + } + + pixel += 3; + } + } + + depthImageNode->image()->setImage(rgbdImage.cols(), rgbdImage.rows(), 1, + GL_RGB, GL_RGB, GL_UNSIGNED_BYTE, + reinterpret_cast(coloredDepth.data()), + osg::Image::NO_DELETE); + depthImageNode->image()->dirty(); + } +} + +void +Pixhawk3DWidget::updatePointCloud(UASInterface* uas, MAV_FRAME frame, + double robotX, double robotY, double robotZ, + osg::ref_ptr& pointCloudNode, + bool colorPointCloudByDistance) +{ + qreal receivedTimestamp; + px::PointCloudXYZRGB pointCloud = uas->getPointCloud(receivedTimestamp); + + osg::Geometry* geometry = pointCloudNode->getDrawable(0)->asGeometry(); + osg::Vec3Array* vertices = static_cast(geometry->getVertexArray()); + osg::Vec4Array* colors = static_cast(geometry->getColorArray()); + + if (QGC::groundTimeSeconds() - receivedTimestamp > kMessageTimeout) + { + geometry->removePrimitiveSet(0, geometry->getNumPrimitiveSets()); + return; + } + + for (int i = 0; i < pointCloud.points_size(); ++i) + { + const px::PointCloudXYZRGB_PointXYZRGB& p = pointCloud.points(i); + + double x = p.x() - robotX; + double y = p.y() - robotY; + double z = p.z() - robotZ; + + + (*vertices)[i].set(y, x, -z); + + if (!colorPointCloudByDistance) + { + float rgb = p.rgb(); + + float b = *(reinterpret_cast(&rgb)) / 255.0f; + float g = *(1 + reinterpret_cast(&rgb)) / 255.0f; + float r = *(2 + reinterpret_cast(&rgb)) / 255.0f; + + (*colors)[i].set(r, g, b, 1.0f); + } + else + { + double dist = sqrt(x * x + y * y + z * z); + int colorIndex = static_cast(fmin(dist / 7.0 * 127.0, 127.0)); + + float r, g, b; + qgc::colormap("jet", colorIndex, r, g, b); + + (*colors)[i].set(r, g, b, 1.0f); + } + } + + if (geometry->getNumPrimitiveSets() == 0) + { + geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS, + 0, pointCloud.points_size())); + } + else + { + osg::DrawArrays* drawarrays = static_cast(geometry->getPrimitiveSet(0)); + drawarrays->setCount(pointCloud.points_size()); + } +} + #endif int Pixhawk3DWidget::findWaypoint(const QPoint& mousePos) { - if (getSceneData()) + if (!m3DWidget->getSceneData() || !mActiveUAS) { - osgUtil::LineSegmentIntersector::Intersections intersections; + return -1; + } - if (computeIntersections(mousePos.x(), height() - mousePos.y(), - intersections)) + SystemContainer& systemData = mSystemContainerMap[mActiveUAS->getUASID()]; + osg::ref_ptr& waypointGroupNode = systemData.waypointGroupNode(); + + osgUtil::LineSegmentIntersector::Intersections intersections; + + QPoint widgetMousePos = m3DWidget->mapFromParent(mousePos); + + if (m3DWidget->computeIntersections(widgetMousePos.x(), + m3DWidget->height() - widgetMousePos.y(), + intersections)) + { + for (osgUtil::LineSegmentIntersector::Intersections::iterator + it = intersections.begin(); it != intersections.end(); it++) { - for (osgUtil::LineSegmentIntersector::Intersections::iterator - it = intersections.begin(); it != intersections.end(); it++) + for (uint i = 0 ; i < it->nodePath.size(); ++i) { - for (uint i = 0 ; i < it->nodePath.size(); ++i) + osg::Node* node = it->nodePath[i]; + std::string nodeName = node->getName(); + if (nodeName.substr(0, 2).compare("wp") == 0) { - std::string nodeName = it->nodePath[i]->getName(); - if (nodeName.substr(0, 2).compare("wp") == 0) + if (node->getParent(0)->getParent(0) == waypointGroupNode.get()) { return atoi(nodeName.substr(2).c_str()); } @@ -1879,11 +1931,11 @@ Pixhawk3DWidget::findWaypoint(const QPoint& mousePos) bool Pixhawk3DWidget::findTarget(int mouseX, int mouseY) { - if (getSceneData()) + if (m3DWidget->getSceneData()) { osgUtil::LineSegmentIntersector::Intersections intersections; - if (computeIntersections(mouseX, height() - mouseY, intersections)) + if (m3DWidget->computeIntersections(mouseX, height() - mouseY, intersections)) { for (osgUtil::LineSegmentIntersector::Intersections::iterator it = intersections.begin(); it != intersections.end(); it++) @@ -1919,16 +1971,16 @@ Pixhawk3DWidget::showEditWaypointMenu(const QPoint &cursorPos) QMenu menu; QString text; - text = QString("Move waypoint %1").arg(QString::number(selectedWpIndex)); + text = QString("Move waypoint %1").arg(QString::number(mSelectedWpIndex)); menu.addAction(text, this, SLOT(moveWaypointPosition())); - text = QString("Change heading of waypoint %1").arg(QString::number(selectedWpIndex)); + text = QString("Change heading of waypoint %1").arg(QString::number(mSelectedWpIndex)); menu.addAction(text, this, SLOT(moveWaypointHeading())); - text = QString("Change altitude of waypoint %1").arg(QString::number(selectedWpIndex)); + text = QString("Change altitude of waypoint %1").arg(QString::number(mSelectedWpIndex)); menu.addAction(text, this, SLOT(setWaypointAltitude())); - text = QString("Delete waypoint %1").arg(QString::number(selectedWpIndex)); + text = QString("Delete waypoint %1").arg(QString::number(mSelectedWpIndex)); menu.addAction(text, this, SLOT(deleteWaypoint())); menu.addAction("Clear all waypoints", this, SLOT(clearAllWaypoints())); diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h index 97aac4e13..534e6d615 100644 --- a/src/ui/map3D/Pixhawk3DWidget.h +++ b/src/ui/map3D/Pixhawk3DWidget.h @@ -36,20 +36,16 @@ #include "HUDScaleGeode.h" #include "Imagery.h" -#include "ImageWindowGeode.h" -#include "WaypointGroupNode.h" -#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES) - #include "ObstacleGroupNode.h" -#endif - #include "Q3DWidget.h" +#include "SystemContainer.h" +#include "ViewParamWidget.h" class UASInterface; /** * @brief A 3D View widget which displays vehicle-centric information. **/ -class Pixhawk3DWidget : public Q3DWidget +class Pixhawk3DWidget : public QWidget { Q_OBJECT @@ -58,20 +54,20 @@ public: ~Pixhawk3DWidget(); public slots: - void setActiveUAS(UASInterface* uas); - void addToTrails(UASInterface* uas, int component, double x, double y, double z, quint64 time); - void updateAttitude(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 time); + void activeSystemChanged(UASInterface* uas); + void systemCreated(UASInterface* uas); + void localPositionChanged(UASInterface* uas, int component, double x, double y, double z, quint64 time); + void attitudeChanged(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 time); + +signals: + void systemCreatedSignal(UASInterface* uas); private slots: - void selectFrame(QString text); - void showLocalGrid(int state); - void showWorldGrid(int state); - void showTrails(int state); - void showWaypoints(int state); - void selectMapSource(int index); - void selectVehicleModel(int index); - void recenter(void); - void toggleFollowCamera(int state); + void showViewParamWindow(void); + void followCameraChanged(int systemId); + void recenterActiveCamera(void); + void modelChanged(int systemId, int index); + void setBirdView(void); void selectTargetHeading(void); void selectTarget(void); @@ -83,54 +79,84 @@ private slots: void setWaypointAltitude(void); void clearAllWaypoints(void); + void sizeChanged(int width, int height); + void update(void); + protected: - QVector< osg::ref_ptr > findVehicleModels(void); + void addModels(QVector< osg::ref_ptr >& models, + const QColor& systemColor); void buildLayout(void); - virtual void resizeGL(int width, int height); - virtual void display(void); - virtual void keyPressEvent(QKeyEvent* event); - virtual void mousePressEvent(QMouseEvent* event); - virtual void showEvent(QShowEvent* event); - virtual void hideEvent(QHideEvent* event); - virtual void mouseMoveEvent(QMouseEvent* event); - UASInterface* uas; + void keyPressEvent(QKeyEvent* event); + void keyReleaseEvent(QKeyEvent* event); + void mousePressEvent(QMouseEvent* event); + void mouseReleaseEvent(QMouseEvent* event); + void mouseMoveEvent(QMouseEvent* event); + void wheelEvent(QWheelEvent* event); + + void showEvent(QShowEvent* event); + void hideEvent(QHideEvent* event); signals: void visibilityChanged(bool visible); private: - void getPose(double& x, double& y, double& z, + void initializeSystem(int systemId, const QColor& systemColor); + + void getPose(UASInterface* uas, + MAV_FRAME frame, + double& x, double& y, double& z, double& roll, double& pitch, double& yaw, - QString& utmZone); - void getPose(double& x, double& y, double& z, - double& roll, double& pitch, double& yaw); - void getPosition(double& x, double& y, double& z, - QString& utmZone); - void getPosition(double& x, double& y, double& z); + QString& utmZone) const; + void getPose(UASInterface* uas, + MAV_FRAME frame, + double& x, double& y, double& z, + double& roll, double& pitch, double& yaw) const; + void getPosition(UASInterface* uas, + MAV_FRAME frame, + double& x, double& y, double& z, + QString& utmZone) const; + void getPosition(UASInterface* uas, + MAV_FRAME frame, + double& x, double& y, double& z) const; osg::ref_ptr createLocalGrid(void); osg::ref_ptr createWorldGrid(void); osg::ref_ptr createTrail(const osg::Vec4& color); - osg::ref_ptr createMap(void); - osg::ref_ptr createRGBD3D(void); - osg::ref_ptr createTarget(void); + osg::ref_ptr createImagery(void); + osg::ref_ptr createPointCloud(void); + osg::ref_ptr createTarget(const QColor& color); void setupHUD(void); - void resizeHUD(void); + void resizeHUD(int width, int height); - void updateHUD(double robotX, double robotY, double robotZ, - double robotRoll, double robotPitch, double robotYaw, - const QString& utmZone); - void updateTrails(double robotX, double robotY, double robotZ); + void updateHUD(UASInterface* uas, MAV_FRAME frame); void updateImagery(double originX, double originY, double originZ, const QString& zone); - void updateWaypoints(void); - void updateTarget(double robotX, double robotY, double robotZ); + void updateTarget(UASInterface* uas, MAV_FRAME frame, + double robotX, double robotY, double robotZ, + QVector4D& target, + osg::ref_ptr& targetNode); + void updateTrails(double robotX, double robotY, double robotZ, + osg::ref_ptr& trailNode, + QMap >& trailMap, + QMap& trailIndexMap); + void updateWaypoints(UASInterface* uas, MAV_FRAME frame, + osg::ref_ptr& waypointGroupNode); #if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES) - void updateRGBD(double robotX, double robotY, double robotZ); - void updateObstacles(double robotX, double robotY, double robotZ); - void updatePath(double robotX, double robotY, double robotZ); + void updateRGBD(UASInterface* uas, MAV_FRAME frame, + osg::ref_ptr& rgbImageNode, + osg::ref_ptr& depthImageNode); + void updatePointCloud(UASInterface* uas, MAV_FRAME frame, + double robotX, double robotY, double robotZ, + osg::ref_ptr& pointCloudNode, + bool colorPointCloudByDistance); + void updateObstacles(UASInterface* uas, MAV_FRAME frame, + double robotX, double robotY, double robotZ, + osg::ref_ptr& obstacleGroupNode); + void updatePlannedPath(UASInterface* uas, MAV_FRAME frame, + double robotX, double robotY, double robotZ, + osg::ref_ptr& plannedPathNode); #endif int findWaypoint(const QPoint& mousePos); @@ -146,53 +172,33 @@ private: MOVE_WAYPOINT_HEADING_MODE, SELECT_TARGET_HEADING_MODE }; - Mode mode; - int selectedWpIndex; - - bool displayLocalGrid; - bool displayWorldGrid; - bool displayTrails; - bool displayImagery; - bool displayWaypoints; - bool displayRGBD2D; - bool displayRGBD3D; - bool displayObstacleList; - bool displayPath; - bool enableRGBDColor; - bool enableTarget; - - bool followCamera; - - QMap > trails; - QMap trailDrawableIdxs; - - osg::ref_ptr vehicleModel; - osg::ref_ptr hudBackgroundGeometry; - osg::ref_ptr statusText; - osg::ref_ptr scaleGeode; - osg::ref_ptr rgb2DGeode; - osg::ref_ptr depth2DGeode; - osg::ref_ptr rgbImage; - osg::ref_ptr depthImage; - osg::ref_ptr localGridNode; - osg::ref_ptr worldGridNode; - osg::ref_ptr trailNode; - osg::ref_ptr orientationNode; - osg::ref_ptr mapNode; - osg::ref_ptr waypointGroupNode; - osg::ref_ptr targetNode; - osg::ref_ptr rgbd3DNode; -#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES) - osg::ref_ptr obstacleGroupNode; - osg::ref_ptr pathNode; -#endif + Mode mMode; + int mSelectedWpIndex; + + int mActiveSystemId; + UASInterface* mActiveUAS; + + GlobalViewParamsPtr mGlobalViewParams; + + // maps system id to system-specific view parameters + QMap mSystemViewParamMap; + + // maps system id to system-specific data + QMap mSystemContainerMap; + + osg::ref_ptr mHudBackgroundGeometry; + osg::ref_ptr mImageryNode; + osg::ref_ptr mScaleGeode; + osg::ref_ptr mStatusText; + osg::ref_ptr mWorldGridNode; - QVector< osg::ref_ptr > vehicleModels; + QPoint mCachedMousePos; + int mFollowCameraId; + QVector3D mCameraPos; + bool mInitCameraPos; - MAV_FRAME frame; - QVector4D target; - QPoint cachedMousePos; - double lastRobotX, lastRobotY, lastRobotZ; + Q3DWidget* m3DWidget; + ViewParamWidget* mViewParamWidget; }; #endif // PIXHAWK3DWIDGET_H diff --git a/src/ui/map3D/PixhawkCheetahGeode.cc b/src/ui/map3D/PixhawkCheetahGeode.cc index 7853519b3..4b9a2b7f3 100644 --- a/src/ui/map3D/PixhawkCheetahGeode.cc +++ b/src/ui/map3D/PixhawkCheetahGeode.cc @@ -59335,7 +59335,7 @@ void SelectMaterial(int i) }; -osg::ref_ptr PixhawkCheetahGeode::_instance; +osg::ref_ptr PixhawkCheetahGeode::mInstance; PixhawkCheetahGeode::PixhawkCheetahGeode() { @@ -59345,15 +59345,17 @@ PixhawkCheetahGeode::PixhawkCheetahGeode() osg::ref_ptr PixhawkCheetahGeode::instance(void) { - if (!_instance.valid()) { - _instance = create(1.0f, 1.0f, 1.0f); + if (!mInstance.valid()) + { + QColor color(255, 255, 255); + mInstance = create(color); } - return _instance; + return mInstance; } osg::ref_ptr -PixhawkCheetahGeode::create(float red, float green, float blue) +PixhawkCheetahGeode::create(const QColor& color) { osg::ref_ptr geode(new osg::Geode()); geode->setName("Pixhawk Bravo"); @@ -59365,11 +59367,13 @@ PixhawkCheetahGeode::create(float red, float green, float blue) osg::ref_ptr osgTextures(new osg::Vec2Array); unsigned int numFaces = sizeof(face_indicies)/sizeof(face_indicies[0]); - for (unsigned int i = 0; i < numFaces; ++i) { + for (unsigned int i = 0; i < numFaces; ++i) + { osg::ref_ptr face( new osg::DrawElementsUInt(osg::PrimitiveSet::TRIANGLES, 0)); - for (int j = 0; j < 3; ++j) { + for (int j = 0; j < 3; ++j) + { int vi = face_indicies[i][j]; // Vertice index int ni = face_indicies[i][j + 3]; // Normal index int ti = face_indicies[i][j + 6]; // Texture index @@ -59396,7 +59400,7 @@ PixhawkCheetahGeode::create(float red, float green, float blue) geometry->setTexCoordArray(0, osgTextures.get()); osg::ref_ptr colors(new osg::Vec4Array); - colors->push_back(osg::Vec4(red, green, blue, 1.0f)); + colors->push_back(osg::Vec4(color.redF(), color.greenF(), color.blueF(), 1.0f)); geometry->setColorArray(colors.get()); geometry->setColorBinding(osg::Geometry::BIND_OVERALL); diff --git a/src/ui/map3D/PixhawkCheetahGeode.h b/src/ui/map3D/PixhawkCheetahGeode.h index d2c7a02b2..beb9c2171 100644 --- a/src/ui/map3D/PixhawkCheetahGeode.h +++ b/src/ui/map3D/PixhawkCheetahGeode.h @@ -33,6 +33,7 @@ This file is part of the QGROUNDCONTROL project #define PIXHAWKCHEETAHGEODE_H_ #include +#include /** * @brief The PixhawkCheetahGeode class. @@ -52,7 +53,6 @@ public: */ static osg::ref_ptr instance(void); -private: /** * @brief Creates an OpenSceneGraph geode which renders a Pixhawk Cheetah MAV. * @param red Red intensity of the MAV model. @@ -61,9 +61,10 @@ private: * * @return A smart pointer to the geode. **/ - static osg::ref_ptr create(float red, float green, float blue); + static osg::ref_ptr create(const QColor& color); - static osg::ref_ptr _instance; +private: + static osg::ref_ptr mInstance; }; #endif diff --git a/src/ui/map3D/Q3DWidget.cc b/src/ui/map3D/Q3DWidget.cc index 25119cfc7..ef4576803 100644 --- a/src/ui/map3D/Q3DWidget.cc +++ b/src/ui/map3D/Q3DWidget.cc @@ -44,21 +44,12 @@ This file is part of the QGROUNDCONTROL project Q3DWidget::Q3DWidget(QWidget* parent) : QGLWidget(parent) - , root(new osg::Group()) - , allocentricMap(new osg::Switch()) - , rollingMap(new osg::Switch()) - , egocentricMap(new osg::Switch()) - , robotPosition(new osg::PositionAttitudeTransform()) - , robotAttitude(new osg::PositionAttitudeTransform()) - , hudGroup(new osg::Switch()) - , hudProjectionMatrix(new osg::Projection) - , fps(30.0f) -{ - // set initial camera parameters - cameraParams.minZoomRange = 2.0f; - cameraParams.cameraFov = 30.0f; - cameraParams.minClipRange = 1.0f; - cameraParams.maxClipRange = 10000.0f; + , mHandleDeviceEvents(true) + , mRoot(new osg::Group()) + , mHudGroup(new osg::Switch()) + , mHudProjectionMatrix(new osg::Projection) + , mFps(30.0f) +{ #ifdef QGC_OSG_QT_ENABLED osg::ref_ptr fontImpl; fontImpl = new osgQt::QFontImplementation(QFont(":/general/vera.ttf")); @@ -66,13 +57,12 @@ Q3DWidget::Q3DWidget(QWidget* parent) osg::ref_ptr fontImpl; fontImpl = 0;//new osgText::Font::Font("images/Vera.ttf"); #endif - font = new osgText::Font(fontImpl); + mFont = new osgText::Font(fontImpl); - osgGW = new osgViewer::GraphicsWindowEmbedded(0, 0, width(), height()); + mOsgGW = new osgViewer::GraphicsWindowEmbedded(0, 0, width(), height()); setThreadingModel(osgViewer::Viewer::SingleThreaded); - setFocusPolicy(Qt::StrongFocus); setMouseTracking(true); } @@ -84,149 +74,63 @@ Q3DWidget::~Q3DWidget() void Q3DWidget::init(float fps) { - this->fps = fps; + mFps = fps; - getCamera()->setGraphicsContext(osgGW); + getCamera()->setGraphicsContext(mOsgGW); // manually specify near and far clip planes getCamera()->setComputeNearFarMode(osg::CullSettings::DO_NOT_COMPUTE_NEAR_FAR); setLightingMode(osg::View::SKY_LIGHT); - // set up various maps - // allocentric - world map - // rolling - map aligned to the world axes and centered on the robot - // egocentric - vehicle-centric map - root->addChild(allocentricMap); - allocentricMap->addChild(robotPosition); - robotPosition->addChild(rollingMap); - rollingMap->addChild(robotAttitude); - robotAttitude->addChild(egocentricMap); + setSceneData(mRoot); - setSceneData(root); + mWorldMap = new osg::Switch; + mRoot->addChild(mWorldMap); // set up HUD - root->addChild(createHUD()); - - // set up robot - egocentricMap->addChild(createRobot()); + mRoot->addChild(createHUD()); // set up camera control - cameraManipulator = new GCManipulator(); - setCameraManipulator(cameraManipulator); - cameraManipulator->setMinZoomRange(cameraParams.minZoomRange); - cameraManipulator->setDistance(cameraParams.minZoomRange * 2.0); + mCameraManipulator = new GCManipulator(); + setCameraManipulator(mCameraManipulator); + mCameraManipulator->setMinZoomRange(mCameraParams.minZoomRange()); + mCameraManipulator->setDistance(mCameraParams.minZoomRange() * 2.0); - connect(&timer, SIGNAL(timeout()), this, SLOT(redraw())); + connect(&mTimer, SIGNAL(timeout()), this, SLOT(redraw())); // DO NOT START TIMER IN INITIALIZATION! IT IS STARTED IN THE SHOW EVENT } -void Q3DWidget::showEvent(QShowEvent* event) -{ - // React only to internal (pre/post-display) - // events - Q_UNUSED(event) - timer.start(static_cast(floorf(1000.0f / fps))); -} - -void Q3DWidget::hideEvent(QHideEvent* event) -{ - // React only to internal (pre/post-display) - // events - Q_UNUSED(event) - timer.stop(); -} - -osg::ref_ptr -Q3DWidget::createRobot(void) -{ - // draw x,y,z-axes - osg::ref_ptr geode(new osg::Geode()); - osg::ref_ptr geometry(new osg::Geometry()); - geode->addDrawable(geometry.get()); - - osg::ref_ptr coords(new osg::Vec3Array(6)); - (*coords)[0] = (*coords)[2] = (*coords)[4] = - osg::Vec3(0.0f, 0.0f, 0.0f); - (*coords)[1] = osg::Vec3(0.0f, 0.3f, 0.0f); - (*coords)[3] = osg::Vec3(0.15f, 0.0f, 0.0f); - (*coords)[5] = osg::Vec3(0.0f, 0.0f, -0.15f); - - geometry->setVertexArray(coords); - - osg::Vec4 redColor(1.0f, 0.0f, 0.0f, 0.0f); - osg::Vec4 greenColor(0.0f, 1.0f, 0.0f, 0.0f); - osg::Vec4 blueColor(0.0f, 0.0f, 1.0f, 0.0f); - - osg::ref_ptr color(new osg::Vec4Array(6)); - (*color)[0] = redColor; - (*color)[1] = redColor; - (*color)[2] = greenColor; - (*color)[3] = greenColor; - (*color)[4] = blueColor; - (*color)[5] = blueColor; - - geometry->setColorArray(color); - geometry->setColorBinding(osg::Geometry::BIND_PER_VERTEX); - - geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, 6)); - - osg::ref_ptr stateset(new osg::StateSet); - osg::ref_ptr linewidth(new osg::LineWidth()); - linewidth->setWidth(3.0f); - stateset->setAttributeAndModes(linewidth, osg::StateAttribute::ON); - stateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF); - geometry->setStateSet(stateset); - - return geode; -} - -osg::ref_ptr -Q3DWidget::createHUD(void) -{ - hudProjectionMatrix->setMatrix(osg::Matrix::ortho(0.0, width(), - 0.0, height(), - -10.0, 10.0)); - - osg::ref_ptr hudModelViewMatrix( - new osg::MatrixTransform); - hudModelViewMatrix->setMatrix(osg::Matrix::identity()); - hudModelViewMatrix->setReferenceFrame(osg::Transform::ABSOLUTE_RF); - - hudProjectionMatrix->addChild(hudModelViewMatrix); - hudModelViewMatrix->addChild(hudGroup); - - osg::ref_ptr hudStateSet(new osg::StateSet); - hudGroup->setStateSet(hudStateSet); - hudStateSet->setMode(GL_DEPTH_TEST, osg::StateAttribute::OFF); - hudStateSet->setMode(GL_LIGHTING, osg::StateAttribute::OFF); - hudStateSet->setMode(GL_BLEND, osg::StateAttribute::ON); - hudStateSet->setRenderingHint(osg::StateSet::TRANSPARENT_BIN); - hudStateSet->setRenderBinDetails(11, "RenderBin"); - - return hudProjectionMatrix; -} - void Q3DWidget::setCameraParams(float minZoomRange, float cameraFov, float minClipRange, float maxClipRange) { - cameraParams.minZoomRange = minZoomRange; - cameraParams.cameraFov = cameraFov; - cameraParams.minClipRange = minClipRange; - cameraParams.maxClipRange = maxClipRange; + mCameraParams.minZoomRange() = minZoomRange; + mCameraParams.fov() = cameraFov; + mCameraParams.minClipRange() = minClipRange; + mCameraParams.maxClipRange() = maxClipRange; } void Q3DWidget::moveCamera(double dx, double dy, double dz) { - cameraManipulator->move(dx, dy, dz); + mCameraManipulator->move(dx, dy, dz); } void Q3DWidget::recenterCamera(double x, double y, double z) { - cameraManipulator->setCenter(osg::Vec3d(x, y, z)); + mCameraManipulator->setCenter(osg::Vec3d(x, y, z)); +} + +void +Q3DWidget::rotateCamera(double roll, double pitch, double yaw) +{ + osg::Quat q(-yaw, osg::Vec3d(0.0f, 0.0f, 1.0f), + pitch, osg::Vec3d(1.0f, 0.0f, 0.0f), + roll, osg::Vec3d(0.0f, 1.0f, 0.0f)); + + mCameraManipulator->setRotation(q); } void @@ -236,21 +140,33 @@ Q3DWidget::setDisplayMode3D(void) / static_cast(height()); getCamera()->setViewport(new osg::Viewport(0, 0, width(), height())); - getCamera()->setProjectionMatrixAsPerspective(cameraParams.cameraFov, - aspect, - cameraParams.minClipRange, - cameraParams.maxClipRange); + getCamera()->setProjectionMatrixAsPerspective(mCameraParams.fov(), + aspect, + mCameraParams.minClipRange(), + mCameraParams.maxClipRange()); +} + +osg::ref_ptr& +Q3DWidget::hudGroup(void) +{ + return mHudGroup; +} + +QPoint +Q3DWidget::mouseCursorCoords(void) +{ + return mapFromGlobal(cursor().pos()); } -std::pair -Q3DWidget::getGlobalCursorPosition(int32_t cursorX, int32_t cursorY, double z) +QPointF +Q3DWidget::worldCursorPosition(const QPoint& cursorPos, double worldZ) const { osgUtil::LineSegmentIntersector::Intersections intersections; // normalize cursor position to value between -1 and 1 - double x = -1.0f + static_cast(2 * cursorX) + double x = -1.0f + static_cast(2 * cursorPos.x()) / static_cast(width()); - double y = -1.0f + static_cast(2 * (height() - cursorY)) + double y = -1.0f + static_cast(2 * (height() - cursorPos.y())) / static_cast(height()); // compute matrix which transforms screen coordinates to world coordinates @@ -264,103 +180,109 @@ Q3DWidget::getGlobalCursorPosition(int32_t cursorX, int32_t cursorY, double z) osg::ref_ptr line = new osg::LineSegment(nearPoint, farPoint); - osg::Plane p(osg::Vec3d(0.0, 0.0, 1.0), osg::Vec3d(0.0, 0.0, z)); + osg::Plane p(osg::Vec3d(0.0, 0.0, 1.0), osg::Vec3d(0.0, 0.0, worldZ)); osg::Vec3d projectedPoint; - getPlaneLineIntersection(p.asVec4(), *line, projectedPoint); + planeLineIntersection(p.asVec4(), *line, projectedPoint); - return std::make_pair(projectedPoint.y(), projectedPoint.x()); + return QPointF(projectedPoint.y(), projectedPoint.x()); } -void -Q3DWidget::redraw(void) +CameraParams& +Q3DWidget::cameraParams(void) { -#if (QGC_EVENTLOOP_DEBUG) - qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__; -#endif - updateGL(); + return mCameraParams; } -int -Q3DWidget::getMouseX(void) +osg::ref_ptr& +Q3DWidget::cameraManipulator(void) { - return mapFromGlobal(cursor().pos()).x(); + return mCameraManipulator; } -int -Q3DWidget::getMouseY(void) +osg::ref_ptr& +Q3DWidget::font(void) { - return mapFromGlobal(cursor().pos()).y(); + return mFont; } -void -Q3DWidget::resizeGL(int width, int height) +osg::ref_ptr& +Q3DWidget::worldMap(void) { - hudProjectionMatrix->setMatrix(osg::Matrix::ortho(0.0, width, - 0.0, height, - -10.0, 10.0)); - - osgGW->getEventQueue()->windowResize(0, 0, width, height); - osgGW->resized(0 , 0, width, height); + return mWorldMap; } -void -Q3DWidget::paintGL(void) +osg::ref_ptr& +Q3DWidget::systemGroup(int systemId) { - setDisplayMode3D(); + if (!mSystemGroups.contains(systemId)) + { + osg::ref_ptr newSystem = new SystemGroupNode; + mRoot->addChild(newSystem); - getCamera()->setClearColor(osg::Vec4f(0.0f, 0.0f, 0.0f, 0.0f)); - getCamera()->setClearMask(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - - display(); + mSystemGroups.insert(systemId, newSystem); + } - frame(); + return mSystemGroups[systemId]; } -void -Q3DWidget::display(void) +bool& +Q3DWidget::handleDeviceEvents(void) { - + return mHandleDeviceEvents; } void -Q3DWidget::keyPressEvent(QKeyEvent* event) +Q3DWidget::handleKeyPressEvent(QKeyEvent* event) { - QWidget::keyPressEvent(event); - if (event->isAccepted()) { + if (event->isAccepted()) + { return; } - if (event->text().isEmpty()) { - osgGW->getEventQueue()->keyPress(convertKey(event->key())); - } else { - osgGW->getEventQueue()->keyPress( + if (event->text().isEmpty()) + { + mOsgGW->getEventQueue()->keyPress(convertKey(event->key())); + } + else + { + mOsgGW->getEventQueue()->keyPress( static_cast( *(event->text().toAscii().data()))); } } void -Q3DWidget::keyReleaseEvent(QKeyEvent* event) +Q3DWidget::handleKeyReleaseEvent(QKeyEvent* event) { - QWidget::keyReleaseEvent(event); - if (event->isAccepted()) { + if (event->isAccepted()) + { return; } - if (event->text().isEmpty()) { - osgGW->getEventQueue()->keyRelease(convertKey(event->key())); - } else { - osgGW->getEventQueue()->keyRelease( + + if (event->text().isEmpty()) + { + mOsgGW->getEventQueue()->keyRelease(convertKey(event->key())); + } + else + { + mOsgGW->getEventQueue()->keyRelease( static_cast( *(event->text().toAscii().data()))); } } void -Q3DWidget::mousePressEvent(QMouseEvent* event) +Q3DWidget::handleMousePressEvent(QMouseEvent* event) { + if (event->isAccepted()) + { + return; + } + int button = 0; - switch (event->button()) { + switch (event->button()) + { case Qt::LeftButton: button = 1; break; @@ -374,16 +296,22 @@ Q3DWidget::mousePressEvent(QMouseEvent* event) button = 0; break; default: - {} + {} } - osgGW->getEventQueue()->mouseButtonPress(event->x(), event->y(), button); + mOsgGW->getEventQueue()->mouseButtonPress(event->x(), event->y(), button); } void -Q3DWidget::mouseReleaseEvent(QMouseEvent* event) +Q3DWidget::handleMouseReleaseEvent(QMouseEvent* event) { + if (event->isAccepted()) + { + return; + } + int button = 0; - switch (event->button()) { + switch (event->button()) + { case Qt::LeftButton: button = 1; break; @@ -397,29 +325,208 @@ Q3DWidget::mouseReleaseEvent(QMouseEvent* event) button = 0; break; default: - {} + {} + } + mOsgGW->getEventQueue()->mouseButtonRelease(event->x(), event->y(), button); +} + +void +Q3DWidget::handleMouseMoveEvent(QMouseEvent* event) +{ + if (event->isAccepted()) + { + return; + } + + mOsgGW->getEventQueue()->mouseMotion(event->x(), event->y()); +} + +void +Q3DWidget::handleWheelEvent(QWheelEvent* event) +{ + if (event->isAccepted()) + { + return; + } + + mOsgGW->getEventQueue()->mouseScroll((event->delta() > 0) ? + osgGA::GUIEventAdapter::SCROLL_UP : + osgGA::GUIEventAdapter::SCROLL_DOWN); +} + +void +Q3DWidget::redraw(void) +{ + emit update(); +#if (QGC_EVENTLOOP_DEBUG) + qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__; +#endif + updateGL(); +} + +void +Q3DWidget::resizeGL(int width, int height) +{ + mHudProjectionMatrix->setMatrix(osg::Matrix::ortho(0.0, width, + 0.0, height, + -10.0, 10.0)); + + mOsgGW->getEventQueue()->windowResize(0, 0, width, height); + mOsgGW->resized(0 , 0, width, height); + + emit sizeChanged(width, height); +} + +void +Q3DWidget::keyPressEvent(QKeyEvent* event) +{ + QWidget::keyPressEvent(event); + + if (mHandleDeviceEvents) + { + handleKeyPressEvent(event); + } + else + { + event->ignore(); + } +} + +void +Q3DWidget::keyReleaseEvent(QKeyEvent* event) +{ + QWidget::keyReleaseEvent(event); + + if (mHandleDeviceEvents) + { + handleKeyReleaseEvent(event); + } + else + { + event->ignore(); + } +} + +void +Q3DWidget::mousePressEvent(QMouseEvent* event) +{ + QWidget::mousePressEvent(event); + + if (mHandleDeviceEvents) + { + handleMousePressEvent(event); + } + else + { + event->ignore(); + } +} + +void +Q3DWidget::mouseReleaseEvent(QMouseEvent* event) +{ + QWidget::mouseReleaseEvent(event); + + if (mHandleDeviceEvents) + { + handleMouseReleaseEvent(event); + } + else + { + event->ignore(); } - osgGW->getEventQueue()->mouseButtonRelease(event->x(), event->y(), button); } void Q3DWidget::mouseMoveEvent(QMouseEvent* event) { - osgGW->getEventQueue()->mouseMotion(event->x(), event->y()); + QWidget::mouseMoveEvent(event); + + if (mHandleDeviceEvents) + { + handleMouseMoveEvent(event); + } + else + { + event->ignore(); + } } void Q3DWidget::wheelEvent(QWheelEvent* event) { - osgGW->getEventQueue()->mouseScroll((event->delta() > 0) ? - osgGA::GUIEventAdapter::SCROLL_UP : - osgGA::GUIEventAdapter::SCROLL_DOWN); + QWidget::wheelEvent(event); + + if (mHandleDeviceEvents) + { + handleWheelEvent(event); + } + else + { + event->ignore(); + } +} + +void +Q3DWidget::showEvent(QShowEvent* event) +{ + // React only to internal (pre/post-display) + // events + Q_UNUSED(event) + mTimer.start(static_cast(floorf(1000.0f / mFps))); +} + +void +Q3DWidget::hideEvent(QHideEvent* event) +{ + // React only to internal (pre/post-display) + // events + Q_UNUSED(event) + mTimer.stop(); +} + +osg::ref_ptr +Q3DWidget::createHUD(void) +{ + mHudProjectionMatrix->setMatrix(osg::Matrix::ortho(0.0, width(), + 0.0, height(), + -10.0, 10.0)); + + osg::ref_ptr hudModelViewMatrix( + new osg::MatrixTransform); + hudModelViewMatrix->setMatrix(osg::Matrix::identity()); + hudModelViewMatrix->setReferenceFrame(osg::Transform::ABSOLUTE_RF); + + mHudProjectionMatrix->addChild(hudModelViewMatrix); + hudModelViewMatrix->addChild(mHudGroup); + + osg::ref_ptr hudStateSet(new osg::StateSet); + mHudGroup->setStateSet(hudStateSet); + hudStateSet->setMode(GL_DEPTH_TEST, osg::StateAttribute::OFF); + hudStateSet->setMode(GL_LIGHTING, osg::StateAttribute::OFF); + hudStateSet->setMode(GL_BLEND, osg::StateAttribute::ON); + hudStateSet->setRenderingHint(osg::StateSet::TRANSPARENT_BIN); + hudStateSet->setRenderBinDetails(11, "RenderBin"); + + return mHudProjectionMatrix; +} + +void +Q3DWidget::paintGL(void) +{ + setDisplayMode3D(); + + getCamera()->setClearColor(osg::Vec4f(0.0f, 0.0f, 0.0f, 0.0f)); + getCamera()->setClearMask(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + frame(); } osgGA::GUIEventAdapter::KeySymbol Q3DWidget::convertKey(int key) const { - switch (key) { + switch (key) + { case Qt::Key_Space : return osgGA::GUIEventAdapter::KEY_Space; case Qt::Key_Backspace : @@ -606,9 +713,9 @@ Q3DWidget::convertKey(int key) const } bool -Q3DWidget::getPlaneLineIntersection(const osg::Vec4d& plane, - const osg::LineSegment& line, - osg::Vec3d& isect) +Q3DWidget::planeLineIntersection(const osg::Vec4d& plane, + const osg::LineSegment& line, + osg::Vec3d& isect) const { osg::Vec3d lineStart = line.start(); osg::Vec3d lineEnd = line.end(); @@ -620,7 +727,8 @@ Q3DWidget::getPlaneLineIntersection(const osg::Vec4d& plane, const double denominator = plane[0] * deltaX + plane[1] * deltaY + plane[2] * deltaZ; - if (!denominator) { + if (!denominator) + { return false; } diff --git a/src/ui/map3D/Q3DWidget.h b/src/ui/map3D/Q3DWidget.h index f3b257a33..8367f030e 100644 --- a/src/ui/map3D/Q3DWidget.h +++ b/src/ui/map3D/Q3DWidget.h @@ -41,7 +41,9 @@ This file is part of the QGROUNDCONTROL project #include #include +#include "CameraParams.h" #include "GCManipulator.h" +#include "SystemGroupNode.h" /** * @brief Definition of the class Q3DWidget. @@ -96,11 +98,24 @@ public: */ void recenterCamera(double x, double y, double z); + /** + * @brief Rotates the camera. + */ + void rotateCamera(double roll, double pitch, double yaw); + /** * @brief Sets up 3D display mode. */ void setDisplayMode3D(void); + osg::ref_ptr& hudGroup(void); + + /** + * @brief Get screen coordinates of mouse cursor. + * @return screen coordinates of mouse cursor. + */ + QPoint mouseCursorCoords(void); + /** * @brief Gets the world 3D coordinates of the cursor. * The function projects the 2D cursor position to a line in world space @@ -108,50 +123,45 @@ public: * which contains the point (0, 0, z); * @param cursorX x-coordinate of the cursor. * @param cursorY y-coordinate of the cursor. - * @param z z-coordinate of the point in the plane. - * @return A pair of values containing the world 3D cursor coordinates. + * @param worldZ z-coordinate of the point in the plane. + * @return World (x,y) cursor coordinates. */ - std::pair getGlobalCursorPosition(int32_t cursorX, - int32_t cursorY, - double z); + QPointF worldCursorPosition(const QPoint& cursorPos, double worldZ) const; -protected slots: - /** - * @brief Updates the widget. - */ - void redraw(void); + CameraParams& cameraParams(void); -protected: + osg::ref_ptr& cameraManipulator(void); - /** @brief Start widget updating */ - void showEvent(QShowEvent* event); - /** @brief Stop widget updating */ - void hideEvent(QHideEvent* event); + osg::ref_ptr& font(void); - /** - * @brief Get base robot geode. - * @return Smart pointer to the geode. - */ - osg::ref_ptr createRobot(void); + osg::ref_ptr& worldMap(void); + osg::ref_ptr& systemGroup(int systemId); - /** - * @brief Get base HUD geode. - * @return Smart pointer to the geode. - */ - osg::ref_ptr createHUD(void); + bool& handleDeviceEvents(void); - /** - * @brief Get screen x-coordinate of mouse cursor. - * @return screen x-coordinate of mouse cursor. - */ - int getMouseX(void); + void handleKeyPressEvent(QKeyEvent* event); + + void handleKeyReleaseEvent(QKeyEvent* event); + + void handleMousePressEvent(QMouseEvent* event); + + void handleMouseReleaseEvent(QMouseEvent* event); + + void handleMouseMoveEvent(QMouseEvent* event); + void handleWheelEvent(QWheelEvent* event); + +protected slots: /** - * @brief Get screen y-coordinate of mouse cursor. - * @return screen y-coordinate of mouse cursor. + * @brief Updates the widget. */ - int getMouseY(void); + void redraw(void); +signals: + void sizeChanged(int width, int height); + void update(void); + +protected: /** * @brief Handle widget resize event. * @param width New width of widget. @@ -159,65 +169,58 @@ protected: */ virtual void resizeGL(int width, int height); - /** - * @brief Handle widget paint event. - */ - virtual void paintGL(void); - - /** - * @brief This function is a container for user-defined rendering. - * All code to render objects should be in this function. - */ - virtual void display(void); - /** * @brief Processes key press events. - * If this handler is reimplemented, it is very important that you call the - * base class implementation. * @param event Key press event. */ virtual void keyPressEvent(QKeyEvent* event); /** * @brief Processes key release events. - * If this handler is reimplemented, it is very important that you call the - * base class implementation. * @param event Key release event. */ virtual void keyReleaseEvent(QKeyEvent* event); /** * @brief Processes mouse press events. - * If this handler is reimplemented, it is very important that you call the - * base class implementation. * @param event Mouse press event. */ virtual void mousePressEvent(QMouseEvent* event); /** * @brief Processes mouse release events. - * If this handler is reimplemented, it is very important that you call the - * base class implementation. * @param event Mouse release event. */ virtual void mouseReleaseEvent(QMouseEvent* event); /** * @brief Processes mouse move events. - * If this handler is reimplemented, it is very important that you call the - * base class implementation. * @param event Mouse move event. */ virtual void mouseMoveEvent(QMouseEvent* event); /** * @brief Processes mouse wheel events. - * If this handler is reimplemented, it is very important that you call the - * base class implementation. * @param event Mouse wheel event. */ virtual void wheelEvent(QWheelEvent* event); + /** @brief Start widget updating */ + void showEvent(QShowEvent* event); + /** @brief Stop widget updating */ + void hideEvent(QHideEvent* event); + + /** + * @brief Get base HUD geode. + * @return Smart pointer to the geode. + */ + osg::ref_ptr createHUD(void); + + /** + * @brief Handle widget paint event. + */ + virtual void paintGL(void); + /** * @brief Converts Qt-defined key to OSG-defined key. * @param key Qt-defined key. @@ -231,37 +234,29 @@ protected: * @param line Line representation. * @return 3D point which lies at the intersection of the line and plane. */ - bool getPlaneLineIntersection(const osg::Vec4d& plane, - const osg::LineSegment& line, - osg::Vec3d& isect); + bool planeLineIntersection(const osg::Vec4d& plane, + const osg::LineSegment& line, + osg::Vec3d& isect) const; - osg::ref_ptr root; /**< Root node of scene graph. */ - osg::ref_ptr allocentricMap; - osg::ref_ptr rollingMap; - osg::ref_ptr egocentricMap; - osg::ref_ptr robotPosition; - osg::ref_ptr robotAttitude; + bool mHandleDeviceEvents; - osg::ref_ptr hudGroup; /**< A group which contains renderable HUD objects. */ - osg::ref_ptr hudProjectionMatrix; /**< An orthographic projection matrix for HUD display. */ + osg::ref_ptr mRoot; /**< Root node of scene graph. */ + osg::ref_ptr mWorldMap; + QMap > mSystemGroups; - osg::ref_ptr osgGW; /**< A class which manages OSG graphics windows and events. */ + osg::ref_ptr mHudGroup; /**< A group which contains renderable HUD objects. */ + osg::ref_ptr mHudProjectionMatrix; /**< An orthographic projection matrix for HUD display. */ - osg::ref_ptr cameraManipulator; /**< Camera manipulator. */ + osg::ref_ptr mOsgGW; /**< A class which manages OSG graphics windows and events. */ - QTimer timer; /**< Timer which draws graphics based on specified fps. */ + osg::ref_ptr mCameraManipulator; /**< Camera manipulator. */ - struct CameraParams { - float minZoomRange; - float cameraFov; - float minClipRange; - float maxClipRange; - }; + QTimer mTimer; /**< Timer which draws graphics based on specified fps. */ - CameraParams cameraParams; /**< Struct representing camera parameters. */ - float fps; + CameraParams mCameraParams; /**< Struct representing camera parameters. */ + float mFps; - osg::ref_ptr font; + osg::ref_ptr mFont; }; #endif // Q3DWIDGET_H diff --git a/src/ui/map3D/Q3DWidgetFactory.cc b/src/ui/map3D/Q3DWidgetFactory.cc index b41c6761d..c833eb5e7 100644 --- a/src/ui/map3D/Q3DWidgetFactory.cc +++ b/src/ui/map3D/Q3DWidgetFactory.cc @@ -37,10 +37,10 @@ This file is part of the QGROUNDCONTROL project #endif QPointer -Q3DWidgetFactory::get(const std::string& type) +Q3DWidgetFactory::get(const std::string& type, QWidget* parent) { if (type == "PIXHAWK") { - return QPointer(new Pixhawk3DWidget()); + return QPointer(new Pixhawk3DWidget(parent)); } #ifdef QGC_OSGEARTH_ENABLED else if (type == "MAP3D") { @@ -48,6 +48,6 @@ Q3DWidgetFactory::get(const std::string& type) } #endif else { - return QPointer(new Q3DWidget()); + return QPointer(new Q3DWidget(parent)); } } diff --git a/src/ui/map3D/Q3DWidgetFactory.h b/src/ui/map3D/Q3DWidgetFactory.h index 43e2283bc..836477955 100644 --- a/src/ui/map3D/Q3DWidgetFactory.h +++ b/src/ui/map3D/Q3DWidgetFactory.h @@ -50,7 +50,7 @@ public: * @return A smart pointer to the Q3DWidget instance. */ - static QPointer get(const std::string& type); + static QPointer get(const std::string& type, QWidget* parent); }; #endif // Q3DWIDGETFACTORY_H diff --git a/src/ui/map3D/SystemContainer.cc b/src/ui/map3D/SystemContainer.cc new file mode 100644 index 000000000..7e719b677 --- /dev/null +++ b/src/ui/map3D/SystemContainer.cc @@ -0,0 +1,96 @@ +#include "SystemContainer.h" + +#include + +SystemContainer::SystemContainer() +{ + +} + +QVector4D& +SystemContainer::target(void) +{ + return mTarget; +} + +QVector< osg::ref_ptr >& +SystemContainer::models(void) +{ + return mModels; +} + +QMap >& +SystemContainer::trailMap(void) +{ + return mTrailMap; +} + +QMap& +SystemContainer::trailIndexMap(void) +{ + return mTrailIndexMap; +} + +osg::ref_ptr& +SystemContainer::depthImageNode(void) +{ + return mDepthImageNode; +} + +osg::ref_ptr& +SystemContainer::localGridNode(void) +{ + return mLocalGridNode; +} + +osg::ref_ptr& +SystemContainer::modelNode(void) +{ + return mModelNode; +} + +osg::ref_ptr& +SystemContainer::pointCloudNode(void) +{ + return mPointCloudNode; +} + +osg::ref_ptr& +SystemContainer::rgbImageNode(void) +{ + return mRGBImageNode; +} + +osg::ref_ptr& +SystemContainer::targetNode(void) +{ + return mTargetNode; +} + +osg::ref_ptr& +SystemContainer::trailNode(void) +{ + return mTrailNode; +} + +osg::ref_ptr& +SystemContainer::waypointGroupNode(void) +{ + return mWaypointGroupNode; +} + +#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES) + +osg::ref_ptr& +SystemContainer::obstacleGroupNode(void) +{ + return mObstacleGroupNode; +} + +osg::ref_ptr& +SystemContainer::plannedPathNode(void) +{ + return mPlannedPathNode; +} + +#endif diff --git a/src/ui/map3D/SystemContainer.h b/src/ui/map3D/SystemContainer.h new file mode 100644 index 000000000..b6d830000 --- /dev/null +++ b/src/ui/map3D/SystemContainer.h @@ -0,0 +1,65 @@ +#ifndef SYSTEMCONTAINER_H +#define SYSTEMCONTAINER_H + +#include +#include +#include +#include + +#include "ImageWindowGeode.h" +#include "WaypointGroupNode.h" + +#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES) +#include "ObstacleGroupNode.h" +#endif + +class SystemContainer +{ +public: + SystemContainer(); + + QVector4D& target(void); + + QVector< osg::ref_ptr >& models(void); + + QMap >& trailMap(void); + QMap& trailIndexMap(void); + + osg::ref_ptr& depthImageNode(void); + osg::ref_ptr& localGridNode(void); + osg::ref_ptr& modelNode(void); + osg::ref_ptr& pointCloudNode(void); + osg::ref_ptr& rgbImageNode(void); + osg::ref_ptr& targetNode(void); + osg::ref_ptr& trailNode(void); + osg::ref_ptr& waypointGroupNode(void); +#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES) + osg::ref_ptr& obstacleGroupNode(void); + osg::ref_ptr& plannedPathNode(void); +#endif + +private: + QVector4D mTarget; + + QVector< osg::ref_ptr > mModels; + + // map component id to component-specific trail + QMap > mTrailMap; + QMap mTrailIndexMap; + + // osg structures + osg::ref_ptr mDepthImageNode; + osg::ref_ptr mLocalGridNode; + osg::ref_ptr mModelNode; + osg::ref_ptr mPointCloudNode; + osg::ref_ptr mRGBImageNode; + osg::ref_ptr mTargetNode; + osg::ref_ptr mTrailNode; + osg::ref_ptr mWaypointGroupNode; +#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES) + osg::ref_ptr mObstacleGroupNode; + osg::ref_ptr mPlannedPathNode; +#endif +}; + +#endif // SYSTEMCONTAINER_H diff --git a/src/ui/map3D/SystemGroupNode.cc b/src/ui/map3D/SystemGroupNode.cc new file mode 100644 index 000000000..5d64b4cae --- /dev/null +++ b/src/ui/map3D/SystemGroupNode.cc @@ -0,0 +1,101 @@ +#include "SystemGroupNode.h" + +#include +#include +#include + +SystemGroupNode::SystemGroupNode() + : mAllocentricMap(new osg::Switch()) + , mRollingMap(new osg::Switch()) + , mEgocentricMap(new osg::Switch()) + , mPosition(new osg::PositionAttitudeTransform()) + , mAttitude(new osg::PositionAttitudeTransform()) +{ + addChild(mAllocentricMap); + + // set up various maps + // allocentric - world map + // rolling - map aligned to the world axes and centered on the robot + // egocentric - vehicle-centric map + mAllocentricMap->addChild(mPosition); + mPosition->addChild(mRollingMap); + mRollingMap->addChild(mAttitude); + mAttitude->addChild(mEgocentricMap); + + // set up robot + mEgocentricMap->addChild(createAxisGeode()); +} + +osg::ref_ptr& +SystemGroupNode::allocentricMap(void) +{ + return mAllocentricMap; +} + +osg::ref_ptr& +SystemGroupNode::rollingMap(void) +{ + return mRollingMap; +} + +osg::ref_ptr& +SystemGroupNode::egocentricMap(void) +{ + return mEgocentricMap; +} + +osg::ref_ptr& +SystemGroupNode::position(void) +{ + return mPosition; +} + +osg::ref_ptr& +SystemGroupNode::attitude(void) +{ + return mAttitude; +} + +osg::ref_ptr +SystemGroupNode::createAxisGeode(void) +{ + // draw x,y,z-axes + osg::ref_ptr geode(new osg::Geode()); + osg::ref_ptr geometry(new osg::Geometry()); + geode->addDrawable(geometry.get()); + + osg::ref_ptr coords(new osg::Vec3Array(6)); + (*coords)[0] = (*coords)[2] = (*coords)[4] = + osg::Vec3(0.0f, 0.0f, 0.0f); + (*coords)[1] = osg::Vec3(0.0f, 0.3f, 0.0f); + (*coords)[3] = osg::Vec3(0.15f, 0.0f, 0.0f); + (*coords)[5] = osg::Vec3(0.0f, 0.0f, -0.15f); + + geometry->setVertexArray(coords); + + osg::Vec4 redColor(1.0f, 0.0f, 0.0f, 0.0f); + osg::Vec4 greenColor(0.0f, 1.0f, 0.0f, 0.0f); + osg::Vec4 blueColor(0.0f, 0.0f, 1.0f, 0.0f); + + osg::ref_ptr color(new osg::Vec4Array(6)); + (*color)[0] = redColor; + (*color)[1] = redColor; + (*color)[2] = greenColor; + (*color)[3] = greenColor; + (*color)[4] = blueColor; + (*color)[5] = blueColor; + + geometry->setColorArray(color); + geometry->setColorBinding(osg::Geometry::BIND_PER_VERTEX); + + geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, 6)); + + osg::ref_ptr stateset(new osg::StateSet); + osg::ref_ptr linewidth(new osg::LineWidth()); + linewidth->setWidth(3.0f); + stateset->setAttributeAndModes(linewidth, osg::StateAttribute::ON); + stateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF); + geometry->setStateSet(stateset); + + return geode; +} diff --git a/src/ui/map3D/SystemGroupNode.h b/src/ui/map3D/SystemGroupNode.h new file mode 100644 index 000000000..abc430e46 --- /dev/null +++ b/src/ui/map3D/SystemGroupNode.h @@ -0,0 +1,28 @@ +#ifndef SYSTEMGROUPNODE_H +#define SYSTEMGROUPNODE_H + +#include +#include + +class SystemGroupNode : public osg::Group +{ +public: + SystemGroupNode(); + + osg::ref_ptr& allocentricMap(void); + osg::ref_ptr& rollingMap(void); + osg::ref_ptr& egocentricMap(void); + osg::ref_ptr& position(void); + osg::ref_ptr& attitude(void); + +private: + osg::ref_ptr createAxisGeode(void); + + osg::ref_ptr mAllocentricMap; + osg::ref_ptr mRollingMap; + osg::ref_ptr mEgocentricMap; + osg::ref_ptr mPosition; + osg::ref_ptr mAttitude; +}; + +#endif // SYSTEMGROUPNODE_H diff --git a/src/ui/map3D/SystemViewParams.cc b/src/ui/map3D/SystemViewParams.cc new file mode 100644 index 000000000..c5308117e --- /dev/null +++ b/src/ui/map3D/SystemViewParams.cc @@ -0,0 +1,274 @@ +#include "SystemViewParams.h" + +SystemViewParams::SystemViewParams(int systemId) + : mSystemId(systemId) + , mColorPointCloudByDistance(false) + , mDisplayLocalGrid(false) + , mDisplayObstacleList(true) + , mDisplayPlannedPath(true) + , mDisplayPointCloud(true) + , mDisplayRGBD(false) + , mDisplayTarget(true) + , mDisplayTrails(true) + , mDisplayWaypoints(true) + , mModelIndex(-1) +{ + +} + +bool& +SystemViewParams::colorPointCloudByDistance(void) +{ + return mColorPointCloudByDistance; +} + +bool +SystemViewParams::colorPointCloudByDistance(void) const +{ + return mColorPointCloudByDistance; +} + +bool& +SystemViewParams::displayLocalGrid(void) +{ + return mDisplayLocalGrid; +} + +bool +SystemViewParams::displayLocalGrid(void) const +{ + return mDisplayLocalGrid; +} + +bool& +SystemViewParams::displayObstacleList(void) +{ + return mDisplayObstacleList; +} + +bool +SystemViewParams::displayObstacleList(void) const +{ + return mDisplayObstacleList; +} + +bool& +SystemViewParams::displayPlannedPath(void) +{ + return mDisplayPlannedPath; +} + +bool +SystemViewParams::displayPlannedPath(void) const +{ + return mDisplayPlannedPath; +} + +bool& +SystemViewParams::displayPointCloud(void) +{ + return mDisplayPointCloud; +} + +bool +SystemViewParams::displayPointCloud(void) const +{ + return mDisplayPointCloud; +} + +bool& +SystemViewParams::displayRGBD(void) +{ + return mDisplayRGBD; +} + +bool +SystemViewParams::displayRGBD(void) const +{ + return mDisplayRGBD; +} + +bool& +SystemViewParams::displayTarget(void) +{ + return mDisplayTarget; +} + +bool +SystemViewParams::displayTarget(void) const +{ + return mDisplayTarget; +} + +bool& +SystemViewParams::displayTrails(void) +{ + return mDisplayTrails; +} + +bool +SystemViewParams::displayTrails(void) const +{ + return mDisplayTrails; +} + +bool& +SystemViewParams::displayWaypoints(void) +{ + return mDisplayWaypoints; +} + +bool +SystemViewParams::displayWaypoints(void) const +{ + return mDisplayWaypoints; +} + +int& +SystemViewParams::modelIndex(void) +{ + return mModelIndex; +} + +int +SystemViewParams::modelIndex(void) const +{ + return mModelIndex; +} + +QVector& +SystemViewParams::modelNames(void) +{ + return mModelNames; +} + +const QVector& +SystemViewParams::modelNames(void) const +{ + return mModelNames; +} + +void +SystemViewParams::modelChanged(int index) +{ + mModelIndex = index; + + emit modelChangedSignal(mSystemId, index); +} + +void +SystemViewParams::toggleColorPointCloud(int state) +{ + if (state == Qt::Checked) + { + mColorPointCloudByDistance = true; + } + else + { + mColorPointCloudByDistance = false; + } +} + +void +SystemViewParams::toggleLocalGrid(int state) +{ + if (state == Qt::Checked) + { + mDisplayLocalGrid = true; + } + else + { + mDisplayLocalGrid = false; + } +} + +void +SystemViewParams::toggleObstacleList(int state) +{ + if (state == Qt::Checked) + { + mDisplayObstacleList = true; + } + else + { + mDisplayObstacleList = false; + } +} + +void +SystemViewParams::togglePlannedPath(int state) +{ + if (state == Qt::Checked) + { + mDisplayPlannedPath = true; + } + else + { + mDisplayPlannedPath = false; + } +} + +void +SystemViewParams::togglePointCloud(int state) +{ + if (state == Qt::Checked) + { + mDisplayPointCloud = true; + } + else + { + mDisplayPointCloud = false; + } +} + +void +SystemViewParams::toggleRGBD(int state) +{ + if (state == Qt::Checked) + { + mDisplayRGBD = true; + } + else + { + mDisplayRGBD = false; + } +} + +void +SystemViewParams::toggleTarget(int state) +{ + if (state == Qt::Checked) + { + mDisplayTarget = true; + } + else + { + mDisplayTarget = false; + } +} + +void +SystemViewParams::toggleTrails(int state) +{ + if (state == Qt::Checked) + { + mDisplayTrails = true; + } + else + { + mDisplayTrails = false; + } +} + +void +SystemViewParams::toggleWaypoints(int state) +{ + if (state == Qt::Checked) + { + mDisplayWaypoints = true; + } + else + { + mDisplayWaypoints = false; + } +} diff --git a/src/ui/map3D/SystemViewParams.h b/src/ui/map3D/SystemViewParams.h new file mode 100644 index 000000000..07d8789d9 --- /dev/null +++ b/src/ui/map3D/SystemViewParams.h @@ -0,0 +1,81 @@ +#ifndef SYSTEMVIEWPARAMS_H +#define SYSTEMVIEWPARAMS_H + +#include +#include +#include + +class SystemViewParams : public QObject +{ + Q_OBJECT + +public: + SystemViewParams(int systemId); + + bool& colorPointCloudByDistance(void); + bool colorPointCloudByDistance(void) const; + + bool& displayLocalGrid(void); + bool displayLocalGrid(void) const; + + bool& displayObstacleList(void); + bool displayObstacleList(void) const; + + bool& displayPlannedPath(void); + bool displayPlannedPath(void) const; + + bool& displayPointCloud(void); + bool displayPointCloud(void) const; + + bool& displayRGBD(void); + bool displayRGBD(void) const; + + bool& displayTarget(void); + bool displayTarget(void) const; + + bool& displayTrails(void); + bool displayTrails(void) const; + + bool& displayWaypoints(void); + bool displayWaypoints(void) const; + + int& modelIndex(void); + int modelIndex(void) const; + + QVector& modelNames(void); + const QVector& modelNames(void) const; + +public slots: + void modelChanged(int index); + void toggleColorPointCloud(int state); + void toggleLocalGrid(int state); + void toggleObstacleList(int state); + void togglePlannedPath(int state); + void togglePointCloud(int state); + void toggleRGBD(int state); + void toggleTarget(int state); + void toggleTrails(int state); + void toggleWaypoints(int state); + +signals: + void modelChangedSignal(int systemId, int index); + +private: + int mSystemId; + + bool mColorPointCloudByDistance; + bool mDisplayLocalGrid; + bool mDisplayObstacleList; + bool mDisplayPlannedPath; + bool mDisplayPointCloud; + bool mDisplayRGBD; + bool mDisplayTarget; + bool mDisplayTrails; + bool mDisplayWaypoints; + int mModelIndex; + QVector mModelNames; +}; + +typedef QSharedPointer SystemViewParamsPtr; + +#endif // SYSTEMVIEWPARAMS diff --git a/src/ui/map3D/ViewParamWidget.cc b/src/ui/map3D/ViewParamWidget.cc new file mode 100644 index 000000000..a1be92af6 --- /dev/null +++ b/src/ui/map3D/ViewParamWidget.cc @@ -0,0 +1,188 @@ +#include "ViewParamWidget.h" + +#include +#include +#include +#include +#include + +#include "UASInterface.h" + +ViewParamWidget::ViewParamWidget(GlobalViewParamsPtr& globalViewParams, + QMap& systemViewParamMap, + QWidget* parent, QWidget* mainWindow) + : QDockWidget(tr("View Parameters"), mainWindow) + , mGlobalViewParams(globalViewParams) + , mSystemViewParamMap(systemViewParamMap) + , mParent(parent) + , mFollowCameraComboBox(new QComboBox(this)) + , mTabWidget(new QTabWidget(this)) +{ + QVBoxLayout* layout = new QVBoxLayout; + QWidget* widget = new QWidget; + widget->setLayout(layout); + + setWidget(widget); + + buildLayout(layout); + + mTabWidget->setFocusPolicy(Qt::NoFocus); + + connect(parent, SIGNAL(systemCreatedSignal(UASInterface*)), + this, SLOT(systemCreated(UASInterface*))); +} + +void +ViewParamWidget::setFollowCameraId(int id) +{ + for (int i = 0; i < mFollowCameraComboBox->count(); ++i) + { + if (mFollowCameraComboBox->itemText(i).endsWith(QString::number(id))) + { + mFollowCameraComboBox->setCurrentIndex(i); + return; + } + } + + mFollowCameraComboBox->setCurrentIndex(0); +} + +void +ViewParamWidget::systemCreated(UASInterface *uas) +{ + addTab(uas->getUASID()); + + QString text("MAV "); + text += QString::number(uas->getUASID()); + mFollowCameraComboBox->addItem(text); +} + +void +ViewParamWidget::buildLayout(QVBoxLayout* layout) +{ + mFollowCameraComboBox->addItem("None"); + + QComboBox* frameComboBox = new QComboBox(this); + frameComboBox->addItem("Local"); + frameComboBox->addItem("Global"); + + QComboBox* imageryComboBox = new QComboBox(this); + imageryComboBox->addItem("None"); + imageryComboBox->addItem("Map (Google)"); + imageryComboBox->addItem("Satellite (Google)"); + + QCheckBox* worldGridCheckBox = new QCheckBox(this); + worldGridCheckBox->setChecked(mGlobalViewParams->displayWorldGrid()); + + QMapIterator it(mSystemViewParamMap); + while (it.hasNext()) + { + QString text("MAV "); + text += QString::number(it.key()); + mFollowCameraComboBox->addItem(text); + + addTab(it.key()); + } + + QFormLayout* formLayout = new QFormLayout; + formLayout->addRow(tr("Follow Camera"), mFollowCameraComboBox); + formLayout->addRow(tr("Frame"), frameComboBox); + formLayout->addRow(tr("Imagery"), imageryComboBox); + formLayout->addRow(tr("World Grid"), worldGridCheckBox); + + layout->addLayout(formLayout); + layout->addWidget(mTabWidget); + + // connect signals/slots + connect(mFollowCameraComboBox, SIGNAL(currentIndexChanged(const QString&)), + mGlobalViewParams.data(), SLOT(followCameraChanged(const QString&))); + connect(frameComboBox, SIGNAL(currentIndexChanged(const QString&)), + mGlobalViewParams.data(), SLOT(frameChanged(const QString&))); + connect(imageryComboBox, SIGNAL(currentIndexChanged(int)), + mGlobalViewParams.data(), SLOT(imageryTypeChanged(int))); + connect(worldGridCheckBox, SIGNAL(stateChanged(int)), + mGlobalViewParams.data(), SLOT(toggleWorldGrid(int))); +} + +void +ViewParamWidget::addTab(int systemId) +{ + // add widgets that configure system-specific parameters + SystemViewParamsPtr systemViewParams = mSystemViewParamMap[systemId]; + + QWidget* page = new QWidget; + + QCheckBox* colorPointCloudCheckBox = new QCheckBox(this); + colorPointCloudCheckBox->setChecked(systemViewParams->colorPointCloudByDistance()); + + QCheckBox* localGridCheckBox = new QCheckBox(this); + localGridCheckBox->setChecked(systemViewParams->displayLocalGrid()); + + QComboBox* modelComboBox = new QComboBox(this); + for (int i = 0; i < systemViewParams->modelNames().size(); ++i) + { + modelComboBox->addItem(systemViewParams->modelNames().at(i)); + } + + QCheckBox* obstacleListCheckBox = new QCheckBox(this); + obstacleListCheckBox->setChecked(systemViewParams->displayObstacleList()); + + QCheckBox* plannedPathCheckBox = new QCheckBox(this); + plannedPathCheckBox->setChecked(systemViewParams->displayPlannedPath()); + + QCheckBox* pointCloudCheckBox = new QCheckBox(this); + pointCloudCheckBox->setChecked(systemViewParams->displayPointCloud()); + + QCheckBox* rgbdCheckBox = new QCheckBox(this); + rgbdCheckBox->setChecked(systemViewParams->displayRGBD()); + + QCheckBox* targetCheckBox = new QCheckBox(this); + targetCheckBox->setChecked(systemViewParams->displayTarget()); + + QCheckBox* trailsCheckBox = new QCheckBox(this); + trailsCheckBox->setChecked(systemViewParams->displayTrails()); + + QCheckBox* waypointsCheckBox = new QCheckBox(this); + waypointsCheckBox->setChecked(systemViewParams->displayWaypoints()); + + QFormLayout* formLayout = new QFormLayout; + page->setLayout(formLayout); + + formLayout->addRow(tr("Color Point Cloud"), colorPointCloudCheckBox); + formLayout->addRow(tr("Local Grid"), localGridCheckBox); + formLayout->addRow(tr("Model"), modelComboBox); + formLayout->addRow(tr("Obstacles"), obstacleListCheckBox); + formLayout->addRow(tr("Planned Path"), plannedPathCheckBox); + formLayout->addRow(tr("Point Cloud"), pointCloudCheckBox); + formLayout->addRow(tr("RGBD"), rgbdCheckBox); + formLayout->addRow(tr("Target"), targetCheckBox); + formLayout->addRow(tr("Trails"), trailsCheckBox); + formLayout->addRow(tr("Waypoints"), waypointsCheckBox); + + QString label("MAV "); + label += QString::number(systemId); + + mTabWidget->addTab(page, label); + + // connect signals / slots + connect(colorPointCloudCheckBox, SIGNAL(stateChanged(int)), + systemViewParams.data(), SLOT(toggleColorPointCloud(int))); + connect(localGridCheckBox, SIGNAL(stateChanged(int)), + systemViewParams.data(), SLOT(toggleLocalGrid(int))); + connect(modelComboBox, SIGNAL(currentIndexChanged(int)), + systemViewParams.data(), SLOT(modelChanged(int))); + connect(obstacleListCheckBox, SIGNAL(stateChanged(int)), + systemViewParams.data(), SLOT(toggleObstacleList(int))); + connect(plannedPathCheckBox, SIGNAL(stateChanged(int)), + systemViewParams.data(), SLOT(togglePlannedPath(int))); + connect(pointCloudCheckBox, SIGNAL(stateChanged(int)), + systemViewParams.data(), SLOT(togglePointCloud(int))); + connect(rgbdCheckBox, SIGNAL(stateChanged(int)), + systemViewParams.data(), SLOT(toggleRGBD(int))); + connect(targetCheckBox, SIGNAL(stateChanged(int)), + systemViewParams.data(), SLOT(toggleTarget(int))); + connect(trailsCheckBox, SIGNAL(stateChanged(int)), + systemViewParams.data(), SLOT(toggleTrails(int))); + connect(waypointsCheckBox, SIGNAL(stateChanged(int)), + systemViewParams.data(), SLOT(toggleWaypoints(int))); +} diff --git a/src/ui/map3D/ViewParamWidget.h b/src/ui/map3D/ViewParamWidget.h new file mode 100644 index 000000000..73088d67b --- /dev/null +++ b/src/ui/map3D/ViewParamWidget.h @@ -0,0 +1,46 @@ +#ifndef VIEWPARAMWIDGET_H +#define VIEWPARAMWIDGET_H + +#include +#include +#include +#include + +#include "GlobalViewParams.h" +#include "SystemViewParams.h" + +class UASInterface; + +class ViewParamWidget : public QDockWidget +{ + Q_OBJECT + +public: + ViewParamWidget(GlobalViewParamsPtr& globalViewParams, + QMap& systemViewParamMap, + QWidget* parent = 0, QWidget* mainWindow = 0); + + void setFollowCameraId(int id); + +signals: + +private slots: + void systemCreated(UASInterface* uas); + +private: + void buildLayout(QVBoxLayout* layout); + void addTab(int systemId); + + // view parameters + GlobalViewParamsPtr mGlobalViewParams; + QMap& mSystemViewParamMap; + + // parent widget + QWidget* mParent; + + // child widgets + QComboBox* mFollowCameraComboBox; + QTabWidget* mTabWidget; +}; + +#endif // VIEWPARAMWIDGET_H diff --git a/src/ui/map3D/WaypointGroupNode.cc b/src/ui/map3D/WaypointGroupNode.cc index d19ea365e..15fe3f970 100644 --- a/src/ui/map3D/WaypointGroupNode.cc +++ b/src/ui/map3D/WaypointGroupNode.cc @@ -37,7 +37,8 @@ This file is part of the QGROUNDCONTROL project #include "Imagery.h" -WaypointGroupNode::WaypointGroupNode() +WaypointGroupNode::WaypointGroupNode(const QColor& color) + : mColor(color) { } @@ -49,7 +50,7 @@ WaypointGroupNode::init(void) } void -WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas) +WaypointGroupNode::update(UASInterface* uas, MAV_FRAME frame) { if (!uas) { @@ -105,11 +106,12 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas) if (wp->getCurrent()) { - sd->setColor(osg::Vec4(1.0f, 0.3f, 0.3f, 0.5f)); + sd->setColor(osg::Vec4(1.0f, 0.8f, 0.0f, 1.0f)); } else { - sd->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f)); + sd->setColor(osg::Vec4(mColor.redF(), mColor.greenF(), + mColor.blueF(), 0.5f)); } osg::ref_ptr geode = new osg::Geode; @@ -133,14 +135,8 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas) sd->setShape(cylinder); sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON); - if (wp->getCurrent()) - { - sd->setColor(osg::Vec4(1.0f, 0.3f, 0.3f, 0.5f)); - } - else - { - sd->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f)); - } + sd->setColor(osg::Vec4(mColor.redF(), mColor.greenF(), + mColor.blueF(), 0.5f)); geode = new osg::Geode; geode->addDrawable(sd); @@ -164,7 +160,8 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas) geometry->setVertexArray(vertices); osg::ref_ptr colors = new osg::Vec4Array; - colors->push_back(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f)); + colors->push_back(osg::Vec4(mColor.redF(), mColor.greenF(), + mColor.blueF(), 0.5f)); geometry->setColorArray(colors); geometry->setColorBinding(osg::Geometry::BIND_OVERALL); diff --git a/src/ui/map3D/WaypointGroupNode.h b/src/ui/map3D/WaypointGroupNode.h index e4df6049d..606accbac 100644 --- a/src/ui/map3D/WaypointGroupNode.h +++ b/src/ui/map3D/WaypointGroupNode.h @@ -39,13 +39,15 @@ This file is part of the QGROUNDCONTROL project class WaypointGroupNode : public osg::Group { public: - WaypointGroupNode(); + WaypointGroupNode(const QColor& color); void init(void); - void update(MAV_FRAME frame, UASInterface* uas); + void update(UASInterface* uas, MAV_FRAME frame); private: void getPosition(Waypoint* wp, double& x, double& y, double& z); + + QColor mColor; }; #endif // WAYPOINTGROUPNODE_H -- 2.22.0