Added box displaying pose information to 3D View widget --Lionel

qgroundcontrol.pro

@@ -43,7 +43,9 @@ DEPENDPATH += . \
 INCLUDEPATH += . \
     lib/QMapControl \
     $$BASEDIR/../mavlink/contrib/slugs/include \
-    $$BASEDIR/../mavlink/include
+    $$BASEDIR/../mavlink/include \
+    /usr/include/freetype2
+LIBS += /usr/lib/libftgl.so
 
 # ../mavlink/include \
 # MAVLink/include \

src/ui/map3D/Q3DWidget.cc

@@ -241,8 +241,21 @@ Q3DWidget::set2DRotation(bool onoff)
 	allow2DRotation = onoff;
 }
 
+void
+Q3DWidget::setDisplayMode2D(void)
+{
+        glDisable(GL_DEPTH_TEST);
+        glMatrixMode(GL_PROJECTION);
+        glLoadIdentity();
+        glOrtho(0.0, static_cast<GLfloat>(getWindowWidth()),
+                0.0, static_cast<GLfloat>(getWindowHeight()),
+                -10.0, 10.0);
+        glMatrixMode(GL_MODELVIEW);
+        glLoadIdentity();
+}
+
 std::pair<float,float>
-Q3DWidget::pickPoint(int32_t mouseX, int32_t mouseY)
+Q3DWidget::getPositionIn3DMode(int32_t mouseX, int32_t mouseY)
 {
 	float cx = windowWidth / 2.0f;
 	float cy = windowHeight / 2.0f;
@@ -264,10 +277,10 @@ Q3DWidget::pickPoint(int32_t mouseX, int32_t mouseY)
 }
 
 std::pair<float,float>
-Q3DWidget::get2DPosition(int32_t x, int32_t y)
+Q3DWidget::getPositionIn2DMode(int32_t mouseX, int32_t mouseY)
 {
-	float dx = (x - windowWidth / 2.0f) / cameraPose.zoom;
-	float dy = (windowHeight / 2.0f - y) / cameraPose.zoom;
+        float dx = (mouseX - windowWidth / 2.0f) / cameraPose.zoom;
+        float dy = (windowHeight / 2.0f - mouseY) / cameraPose.zoom;
 	float ctheta = cosf(-cameraPose.rotation2D);
 	float stheta = sinf(-cameraPose.rotation2D);
 
@@ -404,7 +417,7 @@ void Q3DWidget::switchTo3DMode(void)
 	float cameraX = cameraPose.distance * cosf(cpan) * cosf(ctilt);
 	float cameraY = cameraPose.distance * sinf(cpan) * cosf(ctilt);
 	float cameraZ = cameraPose.distance * sinf(ctilt);
-	setDisplayMode3D(windowWidth, windowHeight);
+        setDisplayMode3D();
 	glViewport(0, 0, static_cast<GLsizei>(windowWidth),
 			   static_cast<GLsizei>(windowHeight));
 	gluLookAt(cameraX + cameraPose.xOffset, cameraY + cameraPose.yOffset,
@@ -413,21 +426,10 @@ void Q3DWidget::switchTo3DMode(void)
 }
 
 void
-Q3DWidget::setDisplayMode2D(int32_t width, int32_t height)
-{
-	glDisable(GL_DEPTH_TEST);
-	glMatrixMode(GL_PROJECTION);
-	glLoadIdentity();
-	glOrtho(0.0, static_cast<GLfloat>(width), 0.0, static_cast<GLfloat>(height),
-			-10.0, 10.0);
-	glMatrixMode(GL_MODELVIEW);
-	glLoadIdentity();
-}
-
-void
-Q3DWidget::setDisplayMode3D(int32_t width, int32_t height)
+Q3DWidget::setDisplayMode3D()
 {
-	float aspect = static_cast<float>(width) / static_cast<float>(height);
+        float aspect = static_cast<float>(getWindowWidth()) /
+                       static_cast<float>(getWindowHeight());
 
 	glEnable(GL_DEPTH_TEST);
 	glMatrixMode(GL_PROJECTION);
@@ -480,7 +482,7 @@ Q3DWidget::paintGL(void)
 	}
 	else
 	{
-		setDisplayMode2D(windowWidth, windowHeight);
+                setDisplayMode2D();
 		// do camera control
 		glTranslatef(static_cast<float>(windowWidth) / 2.0f,
 					 static_cast<float>(windowHeight) / 2.0f,
@@ -517,11 +519,11 @@ Q3DWidget::resizeGL(int32_t width, int32_t height)
 
 	if (_is3D)
 	{
-		setDisplayMode3D(width, height);
+                setDisplayMode3D();
 	}
 	else
 	{
-		setDisplayMode2D(width, height);
+                setDisplayMode2D();
 	}
 }
 

src/ui/map3D/Q3DWidget.h

@@ -98,9 +98,13 @@ public:
 
 	void set2DRotation(bool onoff);
 
-	std::pair<float,float> pickPoint(int32_t mouseX, int32_t mouseY);
+        void setDisplayMode2D(void);
 
-	std::pair<float,float> get2DPosition(int32_t x, int32_t y);
+        std::pair<float,float> getPositionIn3DMode(int32_t mouseX,
+                                                   int32_t mouseY);
+
+        std::pair<float,float> getPositionIn2DMode(int32_t mouseX,
+                                                   int32_t mouseY);
 
 	int32_t getWindowWidth(void);
 	int32_t getWindowHeight(void);
@@ -121,8 +125,7 @@ private:
 	void moveCamera2D(float dx, float dy);
 
 	void switchTo3DMode(void);
-	void setDisplayMode2D(int32_t width, int32_t height);
-	void setDisplayMode3D(int32_t width, int32_t height);
+        void setDisplayMode3D(void);
 
 	float r2d(float angle);
 	float d2r(float angle);

src/ui/map3D/QMap3DWidget.cc

 #include "QMap3DWidget.h"
-#include <QPushButton>
 
+#include <FTGL/ftgl.h>
+#include <QPushButton>
 #include <sys/time.h>
 
+#include "CheetahModel.h"
 #include "UASManager.h"
 #include "UASInterface.h"
 
 QMap3DWidget::QMap3DWidget(QWidget* parent)
-     : Q3DWidget(parent),
-     uas(NULL),
-     lastRedrawTime(0.0)
+     : Q3DWidget(parent)
+     , uas(NULL)
+     , lastRedrawTime(0.0)
 {
     setFocusPolicy(Qt::StrongFocus);
 
@@ -35,6 +37,8 @@ QMap3DWidget::QMap3DWidget(QWidget* parent)
     layout->setColumnStretch(1, 50);
     setLayout(layout);
 
+    font.reset(new FTTextureFont("images/Vera.ttf"));
+
     connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
             this, SLOT(setActiveUAS(UASInterface*)));
 }
@@ -44,6 +48,12 @@ QMap3DWidget::~QMap3DWidget()
 
 }
 
+void
+QMap3DWidget::init(void)
+{
+
+}
+
 void
 QMap3DWidget::display(void* clientData)
 {
@@ -92,6 +102,38 @@ QMap3DWidget::displayHandler(void)
     cheetahModel->draw();
 
     glPopMatrix();
+
+    setDisplayMode2D();
+
+    // display pose information
+    glColor4f(0.0f, 0.0f, 0.0f, 0.5f);
+    glBegin(GL_POLYGON);
+    glVertex2f(0.0f, 0.0f);
+    glVertex2f(0.0f, 20.0f);
+    glVertex2f(getWindowWidth(), 20.0f);
+    glVertex2f(getWindowWidth(), 0.0f);
+    glEnd();
+
+    char buffer[6][255];
+
+    sprintf(buffer[0], "x = %.2f", uas->getLocalX());
+    sprintf(buffer[1], "y = %.2f", uas->getLocalY());
+    sprintf(buffer[2], "z = %.2f", uas->getLocalZ());
+    sprintf(buffer[3], "r = %.2f", uas->getRoll());
+    sprintf(buffer[4], "p = %.2f", uas->getPitch());
+    sprintf(buffer[5], "y = %.2f", uas->getYaw());
+
+    font->FaceSize(10);
+    glColor3f(1.0f, 1.0f, 1.0f);
+    glPushMatrix();
+
+    glTranslatef(0.0f, 5.0f, 0.0f);
+    for (int32_t i = 0; i < 6; ++i)
+    {
+        glTranslatef(60.0f, 0.0f, 0.0f);
+        font->Render(buffer[i]);
+    }
+    glPopMatrix();
 }
 
 /**

src/ui/map3D/QMap3DWidget.h

@@ -2,8 +2,9 @@
 #define QMAP3DWIDGET_H
 
 #include "Q3DWidget.h"
-#include "CheetahModel.h"
 
+class CheetahModel;
+class FTTextureFont;
 class UASInterface;
 
 class QMap3DWidget : public Q3DWidget
@@ -14,6 +15,8 @@ public:
     explicit QMap3DWidget(QWidget* parent);
     ~QMap3DWidget();
 
+    void init(void);
+
     static void display(void* clientData);
     void displayHandler(void);
 
@@ -32,6 +35,7 @@ private:
     double lastRedrawTime;
 
     boost::scoped_ptr<CheetahModel> cheetahModel;
+    boost::scoped_ptr<FTTextureFont> font;
 };
 
 #endif // QMAP3DWIDGET_H