Added visualization of obstacles. Fixed relative font path issue in 3D widgets...

Added visualization of obstacles. Fixed relative font path issue in 3D widgets by using QFont within osgText::Font instead of using static font filename.

qgroundcontrol.pri

@@ -234,10 +234,9 @@ message("Compiling for linux 32")
             -losgGA \
             -losgDB \
             -losgText \
+            -losgQt \
             -lOpenThreads
 
-            #-losgQt \
-
     DEFINES += QGC_OSG_ENABLED
     }
 
@@ -318,10 +317,9 @@ linux-g++-64 {
             -losgGA \
             -losgDB \
             -losgText \
+            -losgQt \
             -lOpenThreads
 
-#            -losgQt \
-
     DEFINES += QGC_OSG_ENABLED
     }
 

qgroundcontrol.pro

@@ -387,7 +387,8 @@ contains(DEPENDENCIES_PRESENT, protobuf):contains(MAVLINK_CONF, pixhawk) {
     message("Including headers for Protocol Buffers")
 
     # Enable only if protobuf is available
-    HEADERS += ../mavlink/include/pixhawk/pixhawk.pb.h
+    HEADERS += ../mavlink/include/pixhawk/pixhawk.pb.h \
+               src/ui/map3D/ObstacleGroupNode.h
 }
 contains(DEPENDENCIES_PRESENT, libfreenect) { 
     message("Including headers for libfreenect")
@@ -527,7 +528,8 @@ contains(DEPENDENCIES_PRESENT, protobuf):contains(MAVLINK_CONF, pixhawk) {
     message("Including sources for Protocol Buffers")
 
     # Enable only if protobuf is available
-    SOURCES += ../mavlink/src/pixhawk/pixhawk.pb.cc
+    SOURCES += ../mavlink/src/pixhawk/pixhawk.pb.cc \
+               src/ui/map3D/ObstacleGroupNode.cc
 }
 contains(DEPENDENCIES_PRESENT, libfreenect) { 
     message("Including sources for libfreenect")

src/uas/UAS.cc

@@ -961,6 +961,11 @@ void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<googl
         rgbdImage.CopyFrom(*message);
         emit rgbdImageChanged(this);
     }
+    else if (message->GetTypeName() == obstacleList.GetTypeName())
+    {
+        obstacleList.CopyFrom(*message);
+        emit obstacleListChanged(this);
+    }
 }
 
 #endif

src/uas/UAS.h

@@ -136,6 +136,10 @@ public:
     px::RGBDImage getRGBDImage() const {
         return rgbdImage;
     }
+
+    px::ObstacleList getObstacleList() const {
+        return obstacleList;
+    }
 #endif
 
     friend class UASWaypointManager;
@@ -223,6 +227,7 @@ protected: //COMMENTS FOR TEST UNIT
 #ifdef QGC_PROTOBUF_ENABLED
     px::PointCloudXYZRGB pointCloud;
     px::RGBDImage rgbdImage;
+    px::ObstacleList obstacleList;
 #endif
 
     QMap<int, QMap<QString, QVariant>* > parameters; ///< All parameters
@@ -556,10 +561,14 @@ signals:
     void imageStarted(quint64 timestamp);
     /** @brief A new camera image has arrived */
     void imageReady(UASInterface* uas);
+#ifdef QGC_PROTOBUF_ENABLED
     /** @brief Point cloud data has been changed */
     void pointCloudChanged(UASInterface* uas);
     /** @brief RGBD image data has been changed */
     void rgbdImageChanged(UASInterface* uas);
+    /** @brief Obstacle list data has been changed */
+    void obstacleListChanged(UASInterface* uas);
+#endif
     /** @brief HIL controls have changed */
     void hilControlsChanged(uint64_t time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, uint8_t systemMode, uint8_t navMode);
 

src/uas/UASInterface.h

@@ -97,6 +97,7 @@ public:
 #ifdef QGC_PROTOBUF_ENABLED
     virtual px::PointCloudXYZRGB getPointCloud() const = 0;
     virtual px::RGBDImage getRGBDImage() const = 0;
+    virtual px::ObstacleList getObstacleList() const = 0;
 #endif
 
     virtual bool isArmed() const = 0;

src/ui/map3D/ObstacleGroupNode.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 ObstacleGroupNode.
+ *
+ *   @author Lionel Heng <hengli@inf.ethz.ch>
+ *
+ */
+
+#include "ObstacleGroupNode.h"
+
+#include <osg/PositionAttitudeTransform>
+#include <osg/ShapeDrawable>
+
+#include "Imagery.h"
+
+ObstacleGroupNode::ObstacleGroupNode()
+{
+
+}
+
+void
+ObstacleGroupNode::init(void)
+{
+
+}
+
+void
+ObstacleGroupNode::update(MAV_FRAME frame, UASInterface *uas)
+{
+    if (!uas || frame == MAV_FRAME_GLOBAL)
+    {
+        return;
+    }
+
+    double robotX = uas->getLocalX();
+    double robotY = uas->getLocalY();
+    double robotZ = uas->getLocalZ();
+
+    if (getNumChildren() > 0)
+    {
+        removeChild(0, getNumChildren());
+    }
+
+    osg::ref_ptr<osg::Geode> geode = new osg::Geode;
+
+    px::ObstacleList obstacleList = uas->getObstacleList();
+
+    for (int i = 0; i < obstacleList.obstacles_size(); ++i)
+    {
+        const px::Obstacle& obs = obstacleList.obstacles(i);
+
+        osg::Vec3d obsPos(obs.y() - robotY, obs.x() - robotX, robotZ - obs.z());
+
+        osg::ref_ptr<osg::Box> box =
+            new osg::Box(obsPos, obs.width(), obs.width(), obs.height());
+        osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable(box);
+
+        sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
+        sd->setColor(osg::Vec4(0.0f, 0.0f, 1.0f, 1.0f));
+
+        geode->addDrawable(sd);
+    }
+
+    addChild(geode);
+}
+

src/ui/map3D/ObstacleGroupNode.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 ObstacleGroupNode.
+ *
+ *   @author Lionel Heng <hengli@inf.ethz.ch>
+ *
+ */
+
+#ifndef OBSTACLEGROUPNODE_H
+#define OBSTACLEGROUPNODE_H
+
+#include <osg/Group>
+
+#include "UASInterface.h"
+
+class ObstacleGroupNode : public osg::Group
+{
+public:
+    ObstacleGroupNode();
+
+    void init(void);
+    void update(MAV_FRAME frame, UASInterface* uas);
+};
+
+#endif // OBSTACLEGROUPNODE_H

src/ui/map3D/Pixhawk3DWidget.h

@@ -25,7 +25,7 @@
  * @file
  *   @brief Definition of the class Pixhawk3DWidget.
  *
- *   @author Lionel Heng <hengli@student.ethz.ch>
+ *   @author Lionel Heng <hengli@inf.ethz.ch>
  *
  */
 
@@ -38,6 +38,9 @@
 #include "Imagery.h"
 #include "ImageWindowGeode.h"
 #include "WaypointGroupNode.h"
+#ifdef QGC_PROTOBUF_ENABLED
+    #include "ObstacleGroupNode.h"
+#endif
 
 #include "Q3DWidget.h"
 
@@ -113,6 +116,7 @@ private:
     void updateTarget(double robotX, double robotY);
 #ifdef QGC_PROTOBUF_ENABLED
     void updateRGBD(double robotX, double robotY, double robotZ);
+    void updateObstacles(void);
 #endif
 
     int findWaypoint(int mouseX, int mouseY);
@@ -133,6 +137,7 @@ private:
     bool displayWaypoints;
     bool displayRGBD2D;
     bool displayRGBD3D;
+    bool displayObstacleList;
     bool enableRGBDColor;
     bool enableTarget;
 
@@ -157,6 +162,9 @@ private:
     osg::ref_ptr<WaypointGroupNode> waypointGroupNode;
     osg::ref_ptr<osg::Node> targetNode;
     osg::ref_ptr<osg::Geode> rgbd3DNode;
+#ifdef QGC_PROTOBUF_ENABLED
+    osg::ref_ptr<ObstacleGroupNode> obstacleGroupNode;
+#endif
 
     QVector< osg::ref_ptr<osg::Node> > vehicleModels;
 

src/ui/map3D/Q3DWidget.cc

@@ -35,6 +35,7 @@ This file is part of the QGROUNDCONTROL project
 #include <osg/Geometry>
 #include <osg/LineWidth>
 #include <osg/MatrixTransform>
+#include <osgQt/QFontImplementation>
 #ifdef Q_OS_MACX
 #include <Carbon/Carbon.h>
 #endif
@@ -57,6 +58,10 @@ Q3DWidget::Q3DWidget(QWidget* parent)
     cameraParams.minClipRange = 1.0f;
     cameraParams.maxClipRange = 10000.0f;
 
+    osg::ref_ptr<osgText::Font::FontImplementation> fontImpl;
+    fontImpl = new osgQt::QFontImplementation(QFont(":/general/vera.ttf"));
+    font = new osgText::Font(fontImpl);
+
     osgGW = new osgViewer::GraphicsWindowEmbedded(0, 0, width(), height());
 
     setThreadingModel(osgViewer::Viewer::SingleThreaded);

src/ui/map3D/Q3DWidget.h

@@ -38,6 +38,7 @@ This file is part of the QGROUNDCONTROL project
 #include <osg/LineSegment>
 #include <osg/PositionAttitudeTransform>
 #include <osgGA/TrackballManipulator>
+#include <osgText/Font>
 #include <osgViewer/Viewer>
 
 #include "GCManipulator.h"
@@ -259,6 +260,8 @@ protected:
 
     CameraParams cameraParams; /**< Struct representing camera parameters. */
     float fps;
+
+    osg::ref_ptr<osgText::Font> font;
 };
 
 #endif // Q3DWIDGET_H

src/ui/map3D/WaypointGroupNode.cc

@@ -25,7 +25,7 @@ This file is part of the QGROUNDCONTROL project
  * @file
  *   @brief Definition of the class WaypointGroupNode.
  *
- *   @author Lionel Heng <hengli@student.ethz.ch>
+ *   @author Lionel Heng <hengli@inf.ethz.ch>
  *
  */
 
@@ -51,102 +51,115 @@ WaypointGroupNode::init(void)
 void
 WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
 {
-    if (uas) {
-        double robotX = 0.0;
-        double robotY = 0.0;
-        double robotZ = 0.0;
-        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_NED) {
-            robotX = uas->getLocalX();
-            robotY = uas->getLocalY();
-            robotZ = uas->getLocalZ();
-        }
-
-        if (getNumChildren() > 0) {
-            removeChild(0, getNumChildren());
-        }
+    if (!uas)
+    {
+        return;
+    }
 
-        const QVector<Waypoint *>& list = uas->getWaypointManager()->getWaypointEditableList();
+    double robotX = 0.0;
+    double robotY = 0.0;
+    double robotZ = 0.0;
+    if (frame == MAV_FRAME_GLOBAL)
+    {
+        double latitude = uas->getLatitude();
+        double longitude = uas->getLongitude();
+        double altitude = uas->getAltitude();
 
-        for (int i = 0; i < list.size(); i++) {
-            Waypoint* wp = list.at(i);
+        QString utmZone;
+        Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
+        robotZ = -altitude;
+    }
+    else if (frame == MAV_FRAME_LOCAL_NED)
+    {
+        robotX = uas->getLocalX();
+        robotY = uas->getLocalY();
+        robotZ = uas->getLocalZ();
+    }
 
-            double wpX, wpY, wpZ;
-            getPosition(wp, wpX, wpY, wpZ);
+    if (getNumChildren() > 0)
+    {
+        removeChild(0, getNumChildren());
+    }
 
-            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->getAcceptanceRadius(),
-                                  fabs(wpZ));
+    const QVector<Waypoint *>& list = uas->getWaypointManager()->getWaypointEditableList();
 
-            sd->setShape(cylinder);
-            sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
+    for (int i = 0; i < list.size(); i++)
+    {
+        Waypoint* wp = list.at(i);
 
-            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));
-            }
+        double wpX, wpY, wpZ;
+        getPosition(wp, wpX, wpY, wpZ);
 
-            osg::ref_ptr<osg::Geode> geode = new osg::Geode;
-            geode->addDrawable(sd);
+        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->getAcceptanceRadius(),
+                              fabs(wpZ));
 
-            char wpLabel[10];
-            sprintf(wpLabel, "wp%d", i);
-            geode->setName(wpLabel);
+        sd->setShape(cylinder);
+        sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
 
-            if (i < list.size() - 1) {
-                double nextWpX, nextWpY, nextWpZ;
-                getPosition(list.at(i + 1), nextWpX, nextWpY, nextWpZ);
+        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::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::Geode> geode = new osg::Geode;
+        geode->addDrawable(sd);
 
-                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);
+        char wpLabel[10];
+        sprintf(wpLabel, "wp%d", i);
+        geode->setName(wpLabel);
 
-                geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, 2));
+        if (i < list.size() - 1)
+        {
+            double nextWpX, nextWpY, nextWpZ;
+            getPosition(list.at(i + 1), nextWpX, nextWpY, nextWpZ);
 
-                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);
+            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);
 
-                geode->addDrawable(geometry);
-            }
+            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);
 
-            osg::ref_ptr<osg::PositionAttitudeTransform> pat =
-                new osg::PositionAttitudeTransform;
+            geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, 2));
 
-            pat->setPosition(osg::Vec3d(wpY - robotY,
-                                        wpX - robotX,
-                                        robotZ));
+            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);
 
-            addChild(pat);
-            pat->addChild(geode);
+            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) {
+    if (wp->getFrame() == MAV_FRAME_GLOBAL)
+    {
         double latitude = wp->getY();
         double longitude = wp->getX();
         double altitude = wp->getZ();
@@ -154,7 +167,9 @@ WaypointGroupNode::getPosition(Waypoint* wp, double &x, double &y, double &z)
         QString utmZone;
         Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
         z = -altitude;
-    } else if (wp->getFrame() == MAV_FRAME_LOCAL_NED) {
+    }
+    else if (wp->getFrame() == MAV_FRAME_LOCAL_NED)
+    {
         x = wp->getX();
         y = wp->getY();
         z = wp->getZ();

src/ui/map3D/WaypointGroupNode.h

@@ -25,7 +25,7 @@ This file is part of the QGROUNDCONTROL project
  * @file
  *   @brief Definition of the class WaypointGroupNode.
  *
- *   @author Lionel Heng <hengli@student.ethz.ch>
+ *   @author Lionel Heng <hengli@inf.ethz.ch>
  *
  */