Commit ee74c2c3 authored by Lionel Heng's avatar Lionel Heng

Made imagery feature working again in 3D view.

parent 6b5c1e50
......@@ -391,7 +391,8 @@ contains(DEPENDENCIES_PRESENT, osg) {
src/ui/map3D/Imagery.h \
src/ui/map3D/HUDScaleGeode.h \
src/ui/map3D/WaypointGroupNode.h \
src/ui/map3D/TerrainParamDialog.h
src/ui/map3D/TerrainParamDialog.h \
src/ui/map3D/ImageryParamDialog.h
}
contains(DEPENDENCIES_PRESENT, protobuf):contains(MAVLINK_CONF, pixhawk) {
message("Including headers for Protocol Buffers")
......@@ -535,7 +536,8 @@ contains(DEPENDENCIES_PRESENT, osg) {
src/ui/map3D/Imagery.cc \
src/ui/map3D/HUDScaleGeode.cc \
src/ui/map3D/WaypointGroupNode.cc \
src/ui/map3D/TerrainParamDialog.cc
src/ui/map3D/TerrainParamDialog.cc \
src/ui/map3D/ImageryParamDialog.cc
contains(DEPENDENCIES_PRESENT, osgearth) {
message("Including sources for osgEarth")
......@@ -602,4 +604,4 @@ win32-msvc2008|win32-msvc2010|linux {
# TO DO: build library when it does not exist already
LIBS += -LthirdParty/libxbee/lib \
-llibxbee
}
\ No newline at end of file
}
......@@ -36,6 +36,30 @@ GlobalViewParams::displayWorldGrid(void) const
return mDisplayWorldGrid;
}
QVector3D&
GlobalViewParams::imageryOffset(void)
{
return mImageryOffset;
}
QVector3D
GlobalViewParams::imageryOffset(void) const
{
return mImageryOffset;
}
QString&
GlobalViewParams::imageryPath(void)
{
return mImageryPath;
}
QString
GlobalViewParams::imageryPath(void) const
{
return mImageryPath;
}
Imagery::Type&
GlobalViewParams::imageryType(void)
{
......@@ -72,6 +96,12 @@ GlobalViewParams::frame(void) const
return mFrame;
}
void
GlobalViewParams::signalImageryParamsChanged(void)
{
emit imageryParamsChanged();
}
QVector3D&
GlobalViewParams::terrainPositionOffset(void)
{
......@@ -132,12 +162,6 @@ GlobalViewParams::frameChanged(const QString& text)
}
}
void
GlobalViewParams::imageryTypeChanged(int index)
{
mImageryType = static_cast<Imagery::Type>(index);
}
void
GlobalViewParams::toggleWorldGrid(int state)
{
......
......@@ -21,6 +21,12 @@ public:
bool& displayWorldGrid(void);
bool displayWorldGrid(void) const;
QVector3D& imageryOffset(void);
QVector3D imageryOffset(void) const;
QString& imageryPath(void);
QString imageryPath(void) const;
Imagery::Type& imageryType(void);
Imagery::Type imageryType(void) const;
......@@ -30,6 +36,8 @@ public:
MAV_FRAME& frame(void);
MAV_FRAME frame(void) const;
void signalImageryParamsChanged(void);
QVector3D& terrainPositionOffset(void);
QVector3D terrainPositionOffset(void) const;
......@@ -39,16 +47,18 @@ public:
public slots:
void followCameraChanged(const QString& text);
void frameChanged(const QString &text);
void imageryTypeChanged(int index);
void toggleTerrain(int state);
void toggleWorldGrid(int state);
signals:
void followCameraChanged(int systemId);
void imageryParamsChanged(void);
private:
bool mDisplayTerrain;
bool mDisplayWorldGrid;
QVector3D mImageryOffset;
QString mImageryPath;
Imagery::Type mImageryType;
int mFollowCameraId;
MAV_FRAME mFrame;
......
This diff is collapsed.
......@@ -41,25 +41,26 @@ This file is part of the QGROUNDCONTROL project
class Imagery : public osg::Geode
{
public:
enum Type {
enum Type
{
BLANK_MAP = 0,
GOOGLE_MAP = 1,
GOOGLE_SATELLITE = 2,
SWISSTOPO_SATELLITE = 3
OFFLINE_SATELLITE = 3
};
Imagery();
Type getImageryType(void) const;
void setImageryType(Type type);
void setOffset(double xOffset, double yOffset);
void setOffset(double xOffset, double yOffset, double zOffset = 0.0);
void setPath(const QString& path);
void prefetch2D(double windowWidth, double windowHeight,
double zoom, double xOrigin, double yOrigin,
const QString& utmZone);
void draw2D(double windowWidth, double windowHeight,
double zoom, double xOrigin, double yOrigin,
double xOffset, double yOffset, double zOffset,
const QString& utmZone);
void prefetch3D(double radius, double tileResolution,
......@@ -67,7 +68,7 @@ public:
const QString& utmZone);
void draw3D(double radius, double tileResolution,
double xOrigin, double yOrigin,
double xOffset, double yOffset, double zOffset,
double xOffset, double yOffset,
const QString& utmZone);
bool update(void);
......@@ -102,12 +103,14 @@ private:
QString getTileLocation(int tileX, int tileY, int zoomLevel,
double tileResolution) const;
QScopedPointer<TextureCache> textureCache;
QScopedPointer<TextureCache> mTextureCache;
Type currentImageryType;
Type mImageryType;
std::string mImageryPath;
double xOffset;
double yOffset;
double mXOffset;
double mYOffset;
double mZOffset;
};
#endif // IMAGERY_H
#include "ImageryParamDialog.h"
#include <QDesktopServices>
#include <QFileDialog>
#include <QFormLayout>
#include <QGroupBox>
#include <QPushButton>
ImageryParamDialog::ImageryParamDialog(QWidget* parent)
: QDialog(parent)
{
QVBoxLayout* layout = new QVBoxLayout;
setLayout(layout);
setWindowTitle(tr("Imagery Parameters"));
setModal(true);
buildLayout(layout);
}
void
ImageryParamDialog::getImageryParams(GlobalViewParamsPtr &globalViewParams)
{
ImageryParamDialog dialog;
switch (globalViewParams->imageryType())
{
case Imagery::BLANK_MAP:
dialog.mImageryTypeComboBox->setCurrentIndex(0);
break;
case Imagery::GOOGLE_MAP:
dialog.mImageryTypeComboBox->setCurrentIndex(1);
break;
case Imagery::GOOGLE_SATELLITE:
dialog.mImageryTypeComboBox->setCurrentIndex(2);
break;
case Imagery::OFFLINE_SATELLITE:
dialog.mImageryTypeComboBox->setCurrentIndex(3);
break;
}
dialog.mPathLineEdit->setText(globalViewParams->imageryPath());
QVector3D& imageryOffset = globalViewParams->imageryOffset();
dialog.mXOffsetSpinBox->setValue(imageryOffset.x());
dialog.mYOffsetSpinBox->setValue(imageryOffset.y());
dialog.mZOffsetSpinBox->setValue(imageryOffset.z());
if (dialog.exec() == QDialog::Accepted)
{
switch (dialog.mImageryTypeComboBox->currentIndex())
{
case 0:
globalViewParams->imageryType() = Imagery::BLANK_MAP;
break;
case 1:
globalViewParams->imageryType() = Imagery::GOOGLE_MAP;
break;
case 2:
globalViewParams->imageryType() = Imagery::GOOGLE_SATELLITE;
break;
case 3:
globalViewParams->imageryType() = Imagery::OFFLINE_SATELLITE;
break;
}
globalViewParams->imageryPath() = dialog.mPathLineEdit->text();
imageryOffset.setX(dialog.mXOffsetSpinBox->value());
imageryOffset.setY(dialog.mYOffsetSpinBox->value());
imageryOffset.setZ(dialog.mZOffsetSpinBox->value());
globalViewParams->signalImageryParamsChanged();
}
}
void
ImageryParamDialog::selectPath(void)
{
QString filename = QFileDialog::getExistingDirectory(this, "Imagery path",
QDesktopServices::storageLocation(QDesktopServices::DesktopLocation));
if (filename.isNull())
{
return;
}
else
{
mPathLineEdit->setText(filename);
}
}
void
ImageryParamDialog::closeWithSaving(void)
{
done(QDialog::Accepted);
}
void
ImageryParamDialog::closeWithoutSaving(void)
{
done(QDialog::Rejected);
}
void
ImageryParamDialog::buildLayout(QVBoxLayout* layout)
{
QFormLayout* formLayout = new QFormLayout;
mImageryTypeComboBox = new QComboBox(this);
mImageryTypeComboBox->addItem("None");
mImageryTypeComboBox->addItem("Map (Google)");
mImageryTypeComboBox->addItem("Satellite (Google)");
mImageryTypeComboBox->addItem("Satellite (Offline)");
mPathLineEdit = new QLineEdit(this);
mPathLineEdit->setReadOnly(true);
QPushButton* pathButton = new QPushButton(this);
pathButton->setText(tr(".."));
QHBoxLayout* pathLayout = new QHBoxLayout;
pathLayout->addWidget(mPathLineEdit);
pathLayout->addWidget(pathButton);
formLayout->addRow(tr("Imagery Type"), mImageryTypeComboBox);
formLayout->addRow(tr("Path"), pathLayout);
layout->addLayout(formLayout);
QGroupBox* offsetGroupBox = new QGroupBox(tr("Offset"), this);
mXOffsetSpinBox = new QDoubleSpinBox(this);
mXOffsetSpinBox->setDecimals(1);
mXOffsetSpinBox->setRange(-1000.0, 1000.0);
mXOffsetSpinBox->setValue(0.0);
mYOffsetSpinBox = new QDoubleSpinBox(this);
mYOffsetSpinBox->setDecimals(1);
mYOffsetSpinBox->setRange(-1000.0, 1000.0);
mYOffsetSpinBox->setValue(0.0);
mZOffsetSpinBox = new QDoubleSpinBox(this);
mZOffsetSpinBox->setDecimals(1);
mZOffsetSpinBox->setRange(-1000.0, 1000.0);
mZOffsetSpinBox->setValue(0.0);
formLayout = new QFormLayout;
formLayout->addRow(tr("x (m)"), mXOffsetSpinBox);
formLayout->addRow(tr("y (m)"), mYOffsetSpinBox);
formLayout->addRow(tr("z (m)"), mZOffsetSpinBox);
offsetGroupBox->setLayout(formLayout);
layout->addWidget(offsetGroupBox);
layout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding));
QPushButton* cancelButton = new QPushButton(this);
cancelButton->setText("Cancel");
QPushButton* saveButton = new QPushButton(this);
saveButton->setText("Save");
QHBoxLayout* buttonLayout = new QHBoxLayout;
buttonLayout->addWidget(cancelButton);
buttonLayout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding));
buttonLayout->addWidget(saveButton);
layout->addLayout(buttonLayout);
connect(pathButton, SIGNAL(clicked()), this, SLOT(selectPath()));
connect(cancelButton, SIGNAL(clicked()), this, SLOT(closeWithoutSaving()));
connect(saveButton, SIGNAL(clicked()), this, SLOT(closeWithSaving()));
}
#ifndef IMAGERYPARAMDIALOG_H
#define IMAGERYPARAMDIALOG_H
#include <QComboBox>
#include <QDoubleSpinBox>
#include <QDialog>
#include <QLineEdit>
#include <QVBoxLayout>
#include "GlobalViewParams.h"
class ImageryParamDialog : public QDialog
{
Q_OBJECT
public:
ImageryParamDialog(QWidget* parent = 0);
static void getImageryParams(GlobalViewParamsPtr& globalViewParams);
private slots:
void selectPath(void);
void closeWithSaving(void);
void closeWithoutSaving(void);
private:
void buildLayout(QVBoxLayout* layout);
QComboBox* mImageryTypeComboBox;
QLineEdit* mPathLineEdit;
QDoubleSpinBox* mXOffsetSpinBox;
QDoubleSpinBox* mYOffsetSpinBox;
QDoubleSpinBox* mZOffsetSpinBox;
};
#endif // IMAGERYPARAMDIALOG_H
......@@ -91,6 +91,8 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
this, SLOT(systemCreated(UASInterface*)));
connect(mGlobalViewParams.data(), SIGNAL(followCameraChanged(int)),
this, SLOT(followCameraChanged(int)));
connect(mGlobalViewParams.data(), SIGNAL(imageryParamsChanged(void)),
this, SLOT(imageryParamsChanged(void)));
MainWindow* parentWindow = qobject_cast<MainWindow*>(parent);
parentWindow->addDockWidget(Qt::LeftDockWidgetArea, mViewParamWidget);
......@@ -139,11 +141,14 @@ Pixhawk3DWidget::systemCreated(UASInterface *uas)
this, SLOT(attitudeChanged(UASInterface*,double,double,double,quint64)));
connect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)),
this, SLOT(setpointChanged(int,float,float,float,float)));
connect(uas, SIGNAL(homePositionChanged(int,double,double,double)),
this, SLOT(homePositionChanged(int,double,double,double)));
#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
connect(uas, SIGNAL(overlayChanged(UASInterface*)),
this, SLOT(addOverlay(UASInterface*)));
#endif
// mSystemContainerMap[systemId].gpsLocalOrigin() = QVector3D(47.419182, 8.566980, 428);
initializeSystem(systemId, uas->getColor());
emit systemCreatedSignal(uas);
......@@ -333,6 +338,20 @@ Pixhawk3DWidget::attitudeChanged(UASInterface* uas,
m3DWidget->systemGroup(systemId)->attitude()->setAttitude(q);
}
void
Pixhawk3DWidget::homePositionChanged(int uasId, double lat, double lon,
double alt)
{
if (!mSystemContainerMap.contains(uasId))
{
return;
}
SystemContainer& systemData = mSystemContainerMap[uasId];
systemData.gpsLocalOrigin() = QVector3D(lat, lon, alt);
}
void
Pixhawk3DWidget::setpointChanged(int uasId, float x, float y, float z,
float yaw)
......@@ -465,6 +484,16 @@ Pixhawk3DWidget::followCameraChanged(int systemId)
}
}
void
Pixhawk3DWidget::imageryParamsChanged(void)
{
mImageryNode->setImageryType(mGlobalViewParams->imageryType());
mImageryNode->setPath(mGlobalViewParams->imageryPath());
const QVector3D& offset = mGlobalViewParams->imageryOffset();
mImageryNode->setOffset(offset.x(), offset.y(), offset.z());
}
void
Pixhawk3DWidget::recenterActiveCamera(void)
{
......@@ -981,8 +1010,6 @@ Pixhawk3DWidget::update(void)
#endif
}
mImageryNode->setImageryType(mGlobalViewParams->imageryType());
if (mFollowCameraId != -1)
{
UASInterface* uas = UASManager::instance()->getUASForId(mFollowCameraId);
......@@ -1091,13 +1118,27 @@ Pixhawk3DWidget::update(void)
updateRGBD(uas, frame, systemData.rgbImageNode(),
systemData.depthImageNode());
}
if (frame == MAV_FRAME_LOCAL_NED &&
mGlobalViewParams->imageryType() != Imagery::BLANK_MAP &&
!systemData.gpsLocalOrigin().isNull() &&
mActiveUAS->getUASID() == systemId)
{
const QVector3D& gpsLocalOrigin = systemData.gpsLocalOrigin();
double utmX, utmY;
QString utmZone;
Imagery::LLtoUTM(gpsLocalOrigin.x(), gpsLocalOrigin.y(), utmX, utmY, utmZone);
updateImagery(utmX, utmY, utmZone, frame);
}
#endif
}
if (frame == MAV_FRAME_GLOBAL &&
mGlobalViewParams->imageryType() != Imagery::BLANK_MAP)
{
// updateImagery(robotX, robotY, robotZ, utmZone);
// updateImagery(x, y, z, utmZone);
}
if (mActiveUAS)
......@@ -2003,8 +2044,8 @@ Pixhawk3DWidget::updateTrails(double robotX, double robotY, double robotZ,
}
void
Pixhawk3DWidget::updateImagery(double originX, double originY, double originZ,
const QString& zone)
Pixhawk3DWidget::updateImagery(double originX, double originY,
const QString& zone, MAV_FRAME frame)
{
if (mImageryNode->getImageryType() == Imagery::BLANK_MAP)
{
......@@ -2030,7 +2071,7 @@ Pixhawk3DWidget::updateImagery(double originX, double originY, double originZ,
case Imagery::GOOGLE_SATELLITE:
minResolution = 0.5;
break;
case Imagery::SWISSTOPO_SATELLITE:
case Imagery::OFFLINE_SATELLITE:
minResolution = 0.25;
maxResolution = 0.25;
break;
......@@ -2049,13 +2090,24 @@ Pixhawk3DWidget::updateImagery(double originX, double originY, double originZ,
resolution = maxResolution;
}
double x = m3DWidget->cameraManipulator()->getCenter().y();
double y = m3DWidget->cameraManipulator()->getCenter().x();
double xOffset = 0.0;
double yOffset = 0.0;
if (frame == MAV_FRAME_LOCAL_NED)
{
xOffset = originX;
yOffset = originY;
}
mImageryNode->draw3D(viewingRadius,
resolution,
m3DWidget->cameraManipulator()->getCenter().y(),
m3DWidget->cameraManipulator()->getCenter().x(),
originX,
originY,
originZ,
x + xOffset,
y + yOffset,
-xOffset,
-yOffset,
zone);
// prefetch map tiles
......@@ -2063,16 +2115,16 @@ Pixhawk3DWidget::updateImagery(double originX, double originY, double originZ,
{
mImageryNode->prefetch3D(viewingRadius / 2.0,
resolution / 2.0,
m3DWidget->cameraManipulator()->getCenter().y(),
m3DWidget->cameraManipulator()->getCenter().x(),
x + xOffset,
y + yOffset,
zone);
}
if (resolution * 2.0 <= maxResolution)
{
mImageryNode->prefetch3D(viewingRadius * 2.0,
resolution * 2.0,
m3DWidget->cameraManipulator()->getCenter().y(),
m3DWidget->cameraManipulator()->getCenter().x(),
x + xOffset,
y + yOffset,
zone);
}
......
......@@ -60,6 +60,7 @@ public slots:
void localPositionChanged(UASInterface* uas, double x, double y, double z, quint64 time);
void attitudeChanged(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 time);
void attitudeChanged(UASInterface* uas, double roll, double pitch, double yaw, quint64 time);
void homePositionChanged(int uasId, double lat, double lon, double alt);
void setpointChanged(int uasId, float x, float y, float z, float yaw);
signals:
......@@ -71,6 +72,7 @@ private slots:
void showTerrainParamWindow(void);
void showViewParamWindow(void);
void followCameraChanged(int systemId);
void imageryParamsChanged(void);
void recenterActiveCamera(void);
void modelChanged(int systemId, int index);
void setBirdEyeView(void);
......@@ -143,8 +145,8 @@ private:
void resizeHUD(int width, int height);
void updateHUD(UASInterface* uas, MAV_FRAME frame);
void updateImagery(double originX, double originY, double originZ,
const QString& zone);
void updateImagery(double originX, double originY,
const QString& zone, MAV_FRAME frame);
void updateTarget(UASInterface* uas, MAV_FRAME frame,
double robotX, double robotY, double robotZ,
QVector4D& target,
......
......@@ -7,6 +7,12 @@ SystemContainer::SystemContainer()
}
QVector3D&
SystemContainer::gpsLocalOrigin(void)
{
return mGPSLocalOrigin;
}
QVector4D&
SystemContainer::target(void)
{
......
......@@ -4,6 +4,7 @@
#include <osg/Geode>
#include <QMap>
#include <QVarLengthArray>
#include <QVector3D>
#include <QVector4D>
#include "ImageWindowGeode.h"
......@@ -19,6 +20,8 @@ class SystemContainer
public:
SystemContainer();
QVector3D& gpsLocalOrigin(void);
QVector4D& target(void);
QVector< osg::ref_ptr<osg::Node> >& models(void);
......@@ -43,6 +46,7 @@ public:
#endif
private:
QVector3D mGPSLocalOrigin;
QVector4D mTarget;
QVector< osg::ref_ptr<osg::Node> > mModels;
......
......@@ -6,6 +6,7 @@
#include <QLabel>
#include <QPushButton>
#include "ImageryParamDialog.h"
#include "UASInterface.h"
ViewParamWidget::ViewParamWidget(GlobalViewParamsPtr& globalViewParams,
......@@ -97,6 +98,12 @@ ViewParamWidget::setpointsCheckBoxToggled(int state)
}
}
void
ViewParamWidget::showImageryParamDialog(void)
{
ImageryParamDialog::getImageryParams(mGlobalViewParams);
}
void
ViewParamWidget::buildLayout(QVBoxLayout* layout)
{
......@@ -106,17 +113,15 @@ ViewParamWidget::buildLayout(QVBoxLayout* layout)
frameComboBox->addItem("Local");
frameComboBox->addItem("Global");
QComboBox* imageryComboBox = new QComboBox(this);
imageryComboBox->addItem("None");
imageryComboBox->addItem("Map (Google)");
imageryComboBox->addItem("Satellite (Google)");
QCheckBox* terrainModelCheckBox = new QCheckBox(this);
terrainModelCheckBox->setChecked(mGlobalViewParams->displayTerrain());
QCheckBox* worldGridCheckBox = new QCheckBox(this);
worldGridCheckBox->setChecked(mGlobalViewParams->displayWorldGrid());
QPushButton* imageryButton = new QPushButton(this);
imageryButton->setText("View");
QMapIterator<int, SystemViewParamsPtr> it(mSystemViewParamMap);
while (it.hasNext())
{
......@@ -130,9 +135,9 @@ ViewParamWidget::buildLayout(QVBoxLayout* layout)
QFormLayout* formLayout = new QFormLayout;
formLayout->addRow(tr("Follow Camera"), mFollowCameraComboBox);
formLayout->addRow(tr("Frame"), frameComboBox);
formLayout->addRow(tr("Imagery"), imageryComboBox);
formLayout->addRow(tr("Terrain"), terrainModelCheckBox);
formLayout->addRow(tr("World Grid"), worldGridCheckBox);
formLayout->addRow(tr("Imagery Options"), imageryButton);
layout->addLayout(formLayout);
layout->addWidget(mTabWidget);
......@@ -142,12 +147,14 @@ ViewParamWidget::buildLayout(QVBoxLayout* layout)
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(terrainModelCheckBox, SIGNAL(stateChanged(int)),
mGlobalViewParams.data(), SLOT(toggleTerrain(int)));
connect(worldGridCheckBox, SIGNAL(stateChanged(int)),
mGlobalViewParams.data(), SLOT(toggleWorldGrid(int)));
connect(imageryButton, SIGNAL(clicked()),
this, SLOT(showImageryParamDialog()));
// connect(imageryComboBox, SIGNAL(currentIndexChanged(int)),
// mGlobalViewParams.data(), SLOT(imageryTypeChanged(int)));
}
void
......
......@@ -31,6 +31,7 @@ private slots:
void overlayCreated(int systemId, const QString& name);
void systemCreated(UASInterface* uas);
void setpointsCheckBoxToggled(int state);
void showImageryParamDialog(void);
private:
void buildLayout(QVBoxLayout* layout);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment