Added to 3D view user switching between local and global coordinate system....

Added to 3D view user switching between local and global coordinate system. Fixed bug in WaypointView: new instance used to assume frame is global.

qgroundcontrol.pro

@@ -237,7 +237,6 @@ HEADERS += src/MG.h \
     src/ui/QGCWebView.h \
     src/ui/map3D/QGCGoogleEarthView.h \
     src/ui/map3D/QGCWebPage.h
-
 contains(DEPENDENCIES_PRESENT, osg) { 
     message("Including headers for OpenSceneGraph")
     
@@ -254,7 +253,8 @@ contains(DEPENDENCIES_PRESENT, osg) {
         src/ui/map3D/TextureCache.h \
         src/ui/map3D/Texture.h \
         src/ui/map3D/Imagery.h \
-        src/ui/map3D/HUDScaleGeode.h
+        src/ui/map3D/HUDScaleGeode.h \
+        src/ui/map3D/WaypointGroupNode.h
     contains(DEPENDENCIES_PRESENT, osgearth) { 
         message("Including headers for OSGEARTH")
         
@@ -356,7 +356,8 @@ contains(DEPENDENCIES_PRESENT, osg) {
         src/ui/map3D/TextureCache.cc \
         src/ui/map3D/Texture.cc \
         src/ui/map3D/Imagery.cc \
-        src/ui/map3D/HUDScaleGeode.cc
+        src/ui/map3D/HUDScaleGeode.cc \
+        src/ui/map3D/WaypointGroupNode.cc
     contains(DEPENDENCIES_PRESENT, osgearth) { 
         message("Including sources for osgEarth")
         

src/ui/WaypointView.cc

@@ -49,7 +49,6 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) :
     m_ui->setupUi(this);
 
     this->wp = wp;
-    wp->setFrame(MAV_FRAME_LOCAL);
 
     // add actions
     m_ui->comboBox_action->addItem("Navigate",MAV_ACTION_NAVIGATE);
@@ -63,7 +62,7 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) :
 
     // defaults
     changedAction(0);
-    changedFrame(0);
+    changedFrame(wp->getFrame());
 
     // Read values and set user interface
     updateValues();

src/ui/map3D/Pixhawk3DWidget.h

@@ -37,6 +37,7 @@
 #include "HUDScaleGeode.h"
 #include "Imagery.h"
 #include "ImageWindowGeode.h"
+#include "WaypointGroupNode.h"
 
 #ifdef QGC_LIBFREENECT_ENABLED
 #include "Freenect.h"
@@ -61,6 +62,7 @@ public slots:
     void setActiveUAS(UASInterface* uas);
 
 private slots:
+    void selectFrame(QString text);
     void showGrid(int state);
     void showTrail(int state);
     void showWaypoints(int state);
@@ -86,29 +88,34 @@ protected:
     UASInterface* uas;
 
 private:
+    void getPose(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);
+
     osg::ref_ptr<osg::Geode> createGrid(void);
     osg::ref_ptr<osg::Geode> createTrail(void);
     osg::ref_ptr<Imagery> createMap(void);
-    osg::ref_ptr<osg::Node> createTarget(void);
-    osg::ref_ptr<osg::Group> createWaypoints(void);
     osg::ref_ptr<osg::Geode> createRGBD3D(void);
 
     void setupHUD(void);
     void resizeHUD(void);
 
     void updateHUD(double robotX, double robotY, double robotZ,
-                   double robotRoll, double robotPitch, double robotYaw);
+                   double robotRoll, double robotPitch, double robotYaw,
+                   const QString& utmZone);
     void updateTrail(double robotX, double robotY, double robotZ);
     void updateImagery(double originX, double originY, double originZ,
                        const QString& zone);
-    void updateTarget(void);
     void updateWaypoints(void);
 #ifdef QGC_LIBFREENECT_ENABLED
     void updateRGBD(void);
 #endif
 
-    void markTarget(void);
-
     int findWaypoint(int mouseX, int mouseY);
     void showInsertWaypointMenu(const QPoint& cursorPos);
     void showEditWaypointMenu(const QPoint& cursorPos);
@@ -123,7 +130,6 @@ private:
     bool displayGrid;
     bool displayTrail;
     bool displayImagery;
-    bool displayTarget;
     bool displayWaypoints;
     bool displayRGBD2D;
     bool displayRGBD3D;
@@ -147,9 +153,7 @@ private:
     osg::ref_ptr<osg::Geometry> trailGeometry;
     osg::ref_ptr<osg::DrawArrays> trailDrawArrays;
     osg::ref_ptr<Imagery> mapNode;
-    osg::ref_ptr<osg::Geode> targetNode;
-    osg::ref_ptr<osg::PositionAttitudeTransform> targetPosition;
-    osg::ref_ptr<osg::Group> waypointsNode;
+    osg::ref_ptr<WaypointGroupNode> waypointGroupNode;
     osg::ref_ptr<osg::Geode> rgbd3DNode;
 #ifdef QGC_LIBFREENECT_ENABLED
     QScopedPointer<Freenect> freenect;
@@ -161,8 +165,7 @@ private:
 
     QVector< osg::ref_ptr<osg::Node> > vehicleModels;
 
-    QPushButton* targetButton;
-
+    MAV_FRAME frame;
     double lastRobotX, lastRobotY, lastRobotZ;
 };
 

src/ui/map3D/Q3DWidget.cc

@@ -168,6 +168,7 @@ Q3DWidget::createHUD(void)
     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");
 

src/ui/map3D/WaypointGroupNode.cc

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class WaypointGroupNode.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
+#include "WaypointGroupNode.h"
+
+#include <osg/LineWidth>
+#include <osg/PositionAttitudeTransform>
+#include <osg/ShapeDrawable>
+
+#include "Imagery.h"
+
+WaypointGroupNode::WaypointGroupNode()
+{
+
+}
+
+void
+WaypointGroupNode::init(void)
+{
+
+}
+
+void
+WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
+{
+    if (uas)
+    {
+        double robotX, robotY, robotZ;
+        if (frame == MAV_FRAME_GLOBAL)
+        {
+            double latitude = uas->getLatitude();
+            double longitude = uas->getLongitude();
+            double altitude = uas->getAltitude();
+
+            QString utmZone;
+            Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
+            robotZ = -altitude;
+        }
+        else if (frame == MAV_FRAME_LOCAL)
+        {
+            robotX = uas->getLocalX();
+            robotY = uas->getLocalY();
+            robotZ = uas->getLocalZ();
+        }
+
+        if (getNumChildren() > 0)
+        {
+            removeChild(0, getNumChildren());
+        }
+
+        const QVector<Waypoint *>& list = uas->getWaypointManager().getWaypointList();
+
+        for (int i = 0; i < list.size(); i++)
+        {
+            Waypoint* wp = list.at(i);
+
+            double wpX, wpY, wpZ;
+            getPosition(wp, wpX, wpY, wpZ);
+
+            osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
+            osg::ref_ptr<osg::Cylinder> cylinder =
+                    new osg::Cylinder(osg::Vec3d(0.0, 0.0, -wpZ / 2.0),
+                                      wp->getOrbit(),
+                                      fabs(wpZ));
+
+            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));
+            }
+
+            osg::ref_ptr<osg::Geode> geode = new osg::Geode;
+            geode->addDrawable(sd);
+
+            char wpLabel[10];
+            sprintf(wpLabel, "wp%d", i);
+            geode->setName(wpLabel);
+
+            if (i < list.size() - 1)
+            {
+                double nextWpX, nextWpY, nextWpZ;
+                getPosition(list.at(i + 1), nextWpX, nextWpY, nextWpZ);
+
+                osg::ref_ptr<osg::Geometry> geometry = new osg::Geometry;
+                osg::ref_ptr<osg::Vec3dArray> vertices = new osg::Vec3dArray;
+                vertices->push_back(osg::Vec3d(0.0, 0.0, -wpZ));
+                vertices->push_back(osg::Vec3d(nextWpY - wpY,
+                                               nextWpX - wpX,
+                                               -nextWpZ));
+                geometry->setVertexArray(vertices);
+
+                osg::ref_ptr<osg::Vec4Array> colors = new osg::Vec4Array;
+                colors->push_back(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
+                geometry->setColorArray(colors);
+                geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
+
+                geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, 2));
+
+                osg::ref_ptr<osg::LineWidth> linewidth(new osg::LineWidth());
+                linewidth->setWidth(2.0f);
+                geometry->getOrCreateStateSet()->setAttributeAndModes(linewidth, osg::StateAttribute::ON);
+                geometry->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
+
+                geode->addDrawable(geometry);
+            }
+
+            osg::ref_ptr<osg::PositionAttitudeTransform> pat =
+                    new osg::PositionAttitudeTransform;
+
+            pat->setPosition(osg::Vec3d(wpY - robotY,
+                                        wpX - robotX,
+                                        robotZ));
+
+            addChild(pat);
+            pat->addChild(geode);
+        }
+    }
+}
+
+void
+WaypointGroupNode::getPosition(Waypoint* wp, double &x, double &y, double &z)
+{
+    if (wp->getFrame() == MAV_FRAME_GLOBAL)
+    {
+        double latitude = wp->getY();
+        double longitude = wp->getX();
+        double altitude = wp->getZ();
+
+        QString utmZone;
+        Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
+        z = -altitude;
+    }
+    else if (wp->getFrame() == MAV_FRAME_LOCAL)
+    {
+        x = wp->getX();
+        y = wp->getY();
+        z = wp->getZ();
+    }
+}

src/ui/map3D/WaypointGroupNode.h

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class WaypointGroupNode.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
+#ifndef WAYPOINTGROUPNODE_H
+#define WAYPOINTGROUPNODE_H
+
+#include <osg/Group>
+
+#include "UASInterface.h"
+
+class WaypointGroupNode : public osg::Group
+{
+public:
+    WaypointGroupNode();
+
+    void init(void);
+    void update(MAV_FRAME frame, UASInterface* uas);
+
+private:
+    void getPosition(Waypoint* wp, double& x, double& y, double& z);
+};
+
+#endif // WAYPOINTGROUPNODE_H