diff --git a/src/ui/map3D/Imagery.cc b/src/ui/map3D/Imagery.cc index 9b32ba5a2cfb313449176e197132b26f62428441..14a1112241d1f325f25de366711916dc03fd16e5 100644 --- a/src/ui/map3D/Imagery.cc +++ b/src/ui/map3D/Imagery.cc @@ -7,6 +7,7 @@ const double WGS84_A = 6378137.0; const double WGS84_ECCSQ = 0.00669437999013; Imagery::Imagery() + : textureCache(new TextureCache(1000)) { } @@ -137,10 +138,13 @@ Imagery::draw2D(double windowWidth, double windowHeight, imageBounds(c, r, imageResolution, x1, y1, x2, y2, x3, y3, x4, y4); TexturePtr t = textureCache->get(tileURL); - t->draw(x1 - xOrigin, y1 - yOrigin, - x2 - xOrigin, y2 - yOrigin, - x3 - xOrigin, y3 - yOrigin, - x4 - xOrigin, y4 - yOrigin, true); + if (!t.isNull()) + { + t->draw(x1 - xOrigin, y1 - yOrigin, + x2 - xOrigin, y2 - yOrigin, + x3 - xOrigin, y3 - yOrigin, + x4 - xOrigin, y4 - yOrigin, true); + } } } } @@ -231,10 +235,13 @@ Imagery::draw3D(double radius, double imageResolution, imageBounds(c, r, imageResolution, x1, y1, x2, y2, x3, y3, x4, y4); TexturePtr t = textureCache->get(tileURL); - t->draw(x1 - xOrigin, y1 - yOrigin, - x2 - xOrigin, y2 - yOrigin, - x3 - xOrigin, y3 - yOrigin, - x4 - xOrigin, y4 - yOrigin, true); + if (!t.isNull()) + { + t->draw(x1 - xOrigin, y1 - yOrigin, + x2 - xOrigin, y2 - yOrigin, + x3 - xOrigin, y3 - yOrigin, + x4 - xOrigin, y4 - yOrigin, true); + } } } } @@ -499,18 +506,18 @@ Imagery::getTileURL(int32_t x, int32_t y, int32_t zoomLevel) switch (currentImageryType) { case MAP: - oss << "http://mt1.google.com/mt?&x=" << x << "&y=" << y << - "&z=" << zoomLevel; + oss << "http://mt0.google.com/vt/lyrs=m@120&x=" << x << "&y=" << y + << "&z=" << zoomLevel; break; case SATELLITE: - oss << "http://khm.google.com/kh?&x=" << x << "&y=" << y << - "&zoom=" << zoomLevel; + oss << "http://khm.google.com/vt/lbw/lyrs=y&x=" << x << "&y=" << y + << "&z=" << zoomLevel; break; default: {}; } - QString url; + QString url(oss.str().c_str()); return url; } diff --git a/src/ui/map3D/QMap3DWidget.cc b/src/ui/map3D/QMap3DWidget.cc index b6fcf0f3fdb40c6e2ab3a0cf249ccc0f987f9a57..2aaad841dabfdda323164e173a81407239194fff 100644 --- a/src/ui/map3D/QMap3DWidget.cc +++ b/src/ui/map3D/QMap3DWidget.cc @@ -45,11 +45,13 @@ QMap3DWidget::QMap3DWidget(QWidget* parent) , uas(NULL) , lastRedrawTime(0.0) , displayGrid(true) + , displayImagery(false) , displayTrail(false) , lockCamera(true) , updateLastUnlockedPose(true) , displayTarget(false) , displayWaypoints(true) + , imagery(new Imagery) { setFocusPolicy(Qt::StrongFocus); @@ -88,6 +90,14 @@ QMap3DWidget::buildLayout(void) waypointsCheckBox->setText("Waypoints"); waypointsCheckBox->setChecked(displayWaypoints); + QLabel* imageryLabel = new QLabel(this); + imageryLabel->setText("Imagery"); + + QComboBox* imageryComboBox = new QComboBox(this); + imageryComboBox->addItem("None"); + imageryComboBox->addItem("Map (Google)"); + imageryComboBox->addItem("Satellite (Google)"); + QPushButton* recenterButton = new QPushButton(this); recenterButton->setText("Recenter Camera"); @@ -105,18 +115,23 @@ QMap3DWidget::buildLayout(void) layout->addWidget(trailCheckBox, 1, 1); layout->addWidget(waypointsCheckBox, 1, 2); layout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 3); - layout->addWidget(recenterButton, 1, 4); - layout->addWidget(lockCameraCheckBox, 1, 5); + layout->addWidget(imageryLabel, 1, 4); + layout->addWidget(imageryComboBox, 1, 5); + layout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 6); + layout->addWidget(recenterButton, 1, 7); + layout->addWidget(lockCameraCheckBox, 1, 8); layout->setRowStretch(0, 100); layout->setRowStretch(1, 1); - //layout->setColumnStretch(0, 1); - layout->setColumnStretch(2, 50); + layout->setColumnStretch(3, 50); + layout->setColumnStretch(6, 50); setLayout(layout); connect(gridCheckBox, SIGNAL(stateChanged(int)), this, SLOT(showGrid(int))); connect(trailCheckBox, SIGNAL(stateChanged(int)), this, SLOT(showTrail(int))); + connect(imageryComboBox, SIGNAL(currentIndexChanged(const QString &)), + this, SLOT(showImagery(const QString &))); connect(recenterButton, SIGNAL(clicked()), this, SLOT(recenterCamera())); connect(lockCameraCheckBox, SIGNAL(stateChanged(int)), this, SLOT(toggleLockCamera(int))); @@ -206,6 +221,11 @@ QMap3DWidget::displayHandler(void) drawWaypoints(); } + if (displayImagery) + { + drawImagery(robotX, robotY, "32N", true); + } + glPopMatrix(); // switch to 2D @@ -441,6 +461,28 @@ QMap3DWidget::showGrid(int32_t state) } } +void +QMap3DWidget::showImagery(const QString& text) +{ + if (text.compare("None") == 0) + { + displayImagery = false; + } + else + { + if (text.compare("Map (Google)") == 0) + { + imagery->setImageryType(Imagery::MAP); + } + else if (text.compare("Satellite (Google)") == 0) + { + imagery->setImageryType(Imagery::SATELLITE); + } + displayImagery = true; + } +} + + void QMap3DWidget::showTrail(int32_t state) { @@ -549,6 +591,56 @@ QMap3DWidget::drawGrid(void) glPopMatrix(); } +void +QMap3DWidget::drawImagery(double originX, double originY, const QString& zone, + bool prefetch) +{ + glPushMatrix(); + glEnable(GL_BLEND); + + CameraPose camPose = getCameraPose(); + double viewingRadius = camPose.distance / 4000.0 * 3000.0; + if (viewingRadius < 100.0) + { + viewingRadius = 100.0; + } + + double minResolution = 0.25; + double centerResolution = camPose.distance / 160.0 * 0.25; + double maxResolution = 2.0; + + double resolution = minResolution; + while (resolution * 2.0 < centerResolution) + { + resolution *= 2.0; + } + if (resolution > maxResolution) + { + resolution = maxResolution; + } + + imagery->draw3D(viewingRadius, resolution, originX, originY, camPose.yOffset, camPose.xOffset, zone); + + if (prefetch) + { + if (resolution / 2.0 >= minResolution) + { + imagery->prefetch3D(viewingRadius / 2.0, resolution / 2.0, + originX, originY, + camPose.yOffset, camPose.xOffset, zone); + } + if (resolution * 2.0 <= maxResolution) + { + imagery->prefetch3D(viewingRadius * 2.0, resolution * 2.0, + originX, originY, + camPose.yOffset, camPose.xOffset, zone); + } + } + + glDisable(GL_BLEND); + glPopMatrix(); +} + void QMap3DWidget::drawTrail(float x, float y, float z) { diff --git a/src/ui/map3D/QMap3DWidget.h b/src/ui/map3D/QMap3DWidget.h index 52a9531a25b5d4254a26659abae8a84c9d2ad18c..038f1b1a474983af96f15471024a902cd7e4bff2 100644 --- a/src/ui/map3D/QMap3DWidget.h +++ b/src/ui/map3D/QMap3DWidget.h @@ -34,6 +34,7 @@ This file is part of the QGROUNDCONTROL project #include +#include "Imagery.h" #include "Q3DWidget.h" class CheetahModel; @@ -72,23 +73,28 @@ public slots: private slots: void showGrid(int state); void showTrail(int state); + void showImagery(const QString& text); void recenterCamera(void); void toggleLockCamera(int state); protected: UASInterface* uas; - void paintText(QString text, QColor color, float fontSize, float refX, float refY, QPainter* painter); + void paintText(QString text, QColor color, float fontSize, + float refX, float refY, QPainter* painter); void drawWaypoints(); private: void drawPlatform(float roll, float pitch, float yaw); void drawGrid(void); + void drawImagery(double originX, double originY, const QString& zone, + bool prefetch); void drawTrail(float x, float y, float z); void drawTarget(float x, float y, float z); double lastRedrawTime; bool displayGrid; + bool displayImagery; bool displayTrail; typedef struct @@ -110,6 +116,7 @@ private: Pose3D targetPosition; QScopedPointer cheetahModel; + QScopedPointer imagery; }; #endif // QMAP3DWIDGET_H diff --git a/src/ui/map3D/Texture.cc b/src/ui/map3D/Texture.cc index 9a2fcebfd1b7addc10c8195b1cd1af3e50e03cd1..0a073227718173dc20aa2e4d98034737883d50e2 100644 --- a/src/ui/map3D/Texture.cc +++ b/src/ui/map3D/Texture.cc @@ -5,8 +5,8 @@ Texture::Texture() } -QString -Texture::getSourceURL(void) +const QString& +Texture::getSourceURL(void) const { return sourceURL; } diff --git a/src/ui/map3D/Texture.h b/src/ui/map3D/Texture.h index 331dd3aa79c19ebb221f3591774274b516f3240c..d4c8395644f68828b6900d70141fda22852935c2 100644 --- a/src/ui/map3D/Texture.h +++ b/src/ui/map3D/Texture.h @@ -16,7 +16,7 @@ class Texture public: Texture(); - QString getSourceURL(void); + const QString& getSourceURL(void) const; void setID(GLuint id); diff --git a/src/ui/map3D/TextureCache.cc b/src/ui/map3D/TextureCache.cc index ef0cf38197c52450a482a1c355f551fd1d3e4b5f..34b2311bff860109f7045bf92461c9bc20664ca6 100644 --- a/src/ui/map3D/TextureCache.cc +++ b/src/ui/map3D/TextureCache.cc @@ -4,10 +4,9 @@ TextureCache::TextureCache(uint32_t _cacheSize) : cacheSize(_cacheSize) , imageCache(new WebImageCache(0, cacheSize)) { - textures.resize(cacheSize); - TexturePtr t; - foreach(t, textures) + for (uint32_t i = 0; i < cacheSize; ++i) { + TexturePtr t(new Texture); GLuint id; glGenTextures(1, &id); t->setID(id); @@ -15,6 +14,8 @@ TextureCache::TextureCache(uint32_t _cacheSize) glPixelStorei(GL_UNPACK_ALIGNMENT, 1); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + + textures.push_back(t); } } diff --git a/src/ui/map3D/WebImage.cc b/src/ui/map3D/WebImage.cc index 638f4684feefa4602bd25462e6a99c281d3b8143..107579df6c929667e79d24efb61dbfcf03bf79b4 100644 --- a/src/ui/map3D/WebImage.cc +++ b/src/ui/map3D/WebImage.cc @@ -2,6 +2,8 @@ WebImage::WebImage() : state(WebImage::UNINITIALIZED) + , sourceURL("") + , image(new QImage) , lastReference(0) , syncFlag(false) { @@ -29,7 +31,7 @@ WebImage::setState(State state) this->state = state; } -QString +const QString& WebImage::getSourceURL(void) const { return sourceURL; @@ -47,6 +49,12 @@ WebImage::getData(void) const return image->bits(); } +void +WebImage::setData(const QByteArray& data) +{ + image->loadFromData(data); +} + int32_t WebImage::getWidth(void) const { diff --git a/src/ui/map3D/WebImage.h b/src/ui/map3D/WebImage.h index b39919ec49056e6a2cde598933ec824aec81443c..ec489a9a3d97a5c139c72e2545d4aa93de44b75a 100644 --- a/src/ui/map3D/WebImage.h +++ b/src/ui/map3D/WebImage.h @@ -22,10 +22,12 @@ public: State getState(void) const; void setState(State state); - QString getSourceURL(void) const; + const QString& getSourceURL(void) const; void setSourceURL(const QString& url); const uint8_t* getData(void) const; + void setData(const QByteArray& data); + int32_t getWidth(void) const; int32_t getHeight(void) const; diff --git a/src/ui/map3D/WebImageCache.cc b/src/ui/map3D/WebImageCache.cc index 622224666e6d0b57e74fc650a162509bac2e3598..ce85266fcd299ee230673ec13eb65545eef08c39 100644 --- a/src/ui/map3D/WebImageCache.cc +++ b/src/ui/map3D/WebImageCache.cc @@ -9,7 +9,12 @@ WebImageCache::WebImageCache(QObject* parent, uint32_t _cacheSize) , currentReference(0) , networkManager(new QNetworkAccessManager) { - webImages.resize(cacheSize); + for (uint32_t i = 0; i < cacheSize; ++i) + { + WebImagePtr image(new WebImage); + + webImages.push_back(image); + } connect(networkManager.data(), SIGNAL(finished(QNetworkReply*)), this, SLOT(downloadFinished(QNetworkReply*))); @@ -104,9 +109,16 @@ WebImageCache::downloadFinished(QNetworkReply* reply) return; } - QByteArray imageData = reply->readAll(); - QPixmap pixmap; - pixmap.loadFromData(imageData); + WebImagePtr image; + foreach(image, webImages) + { + if (reply->url().toString() == image->getSourceURL()) + { + image->setData(reply->readAll()); + image->setSyncFlag(true); + image->setState(WebImage::READY); - // set image, needsSync to true, and state to READY + return; + } + } }