Merge remote-tracking branch 'remotes/upstream/master'

qgroundcontrol.pri

@@ -246,11 +246,11 @@ message("Compiling for linux 32")
             -losgGA \
             -losgDB \
             -losgText \
+            -losgQt \
             -lOpenThreads
 
-            #-losgQt \
-
     DEFINES += QGC_OSG_ENABLED
+    DEFINES += QGC_OSG_QT_ENABLED
     }
 
     exists(/usr/local/include/google/protobuf) {
@@ -330,11 +330,11 @@ linux-g++-64 {
             -losgGA \
             -losgDB \
             -losgText \
+            -losgQt \
             -lOpenThreads
 
-#            -losgQt \
-
     DEFINES += QGC_OSG_ENABLED
+    DEFINES += QGC_OSG_QT_ENABLED
     }
 
     exists(/usr/local/include/google/protobuf) {

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

@@ -993,6 +993,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
@@ -1906,25 +1911,6 @@ void UAS::executeCommand(MAV_CMD command)
     sendMessage(msg);
 }
 
-void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, int component)
-{
-    mavlink_message_t msg;
-    mavlink_command_long_t cmd;
-    cmd.command = (uint8_t)command;
-    cmd.confirmation = confirmation;
-    cmd.param1 = param1;
-    cmd.param2 = param2;
-    cmd.param3 = param3;
-    cmd.param4 = param4;
-    cmd.param5 = 0.0f;
-    cmd.param6 = 0.0f;
-    cmd.param7 = 0.0f;
-    cmd.target_system = uasId;
-    cmd.target_component = component;
-    mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);
-    sendMessage(msg);
-}
-
 void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component)
 {
     mavlink_message_t msg;
@@ -2191,7 +2177,7 @@ void UAS::shutdown()
 void UAS::setTargetPosition(float x, float y, float z, float yaw)
 {
     mavlink_message_t msg;
-    mavlink_msg_set_local_position_setpoint_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_FRAME_LOCAL_NED, x, y, z, yaw);
+    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_PATHPLANNING, 1, 2, 3, 0, yaw, x, y, z);
     sendMessage(msg);
 }
 

src/uas/UAS.h

@@ -142,6 +142,10 @@ public:
     px::RGBDImage getRGBDImage() const {
         return rgbdImage;
     }
+
+    px::ObstacleList getObstacleList() const {
+        return obstacleList;
+    }
 #endif
 
     friend class UASWaypointManager;
@@ -230,6 +234,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
@@ -406,8 +411,6 @@ public slots:
     void setUASName(const QString& name);
     /** @brief Executes a command **/
     void executeCommand(MAV_CMD command);
-    /** @brief Executes a command **/
-    void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, int component);
     /** @brief Executes a command with 7 params */
     void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component);
     /** @brief Set the current battery type and voltages */
@@ -563,10 +566,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

@@ -46,7 +46,7 @@ This file is part of the QGROUNDCONTROL project
 
 #ifdef QGC_PROTOBUF_ENABLED
 #include <tr1/memory>
-#include <pixhawk.pb.h>
+#include <pixhawk/pixhawk.pb.h>
 #endif
 
 /**
@@ -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;
@@ -214,7 +215,7 @@ public slots:
     /** @brief Execute command immediately **/
     virtual void executeCommand(MAV_CMD command) = 0;
     /** @brief Executes a command **/
-    virtual void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, int component) = 0;
+    virtual void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component) = 0;
 
     /** @brief Selects the airframe */
     virtual void setAirframe(int airframe) = 0;

src/ui/HUD.cc

@@ -1431,10 +1431,13 @@ void HUD::setPixels(int imgid, const unsigned char* imageData, int length, int s
 
 void HUD::copyImage()
 {
-    qDebug() << "HUD::copyImage()";
-    UAS* u = dynamic_cast<UAS*>(this->uas);
-    if (u)
+    if (isVisible())
     {
-        this->glImage = QGLWidget::convertToGLFormat(u->getImage());
+        qDebug() << "HUD::copyImage()";
+        UAS* u = dynamic_cast<UAS*>(this->uas);
+        if (u)
+        {
+            this->glImage = QGLWidget::convertToGLFormat(u->getImage());
+        }
     }
 }

src/ui/MAVLinkDecoder.cc

@@ -27,8 +27,9 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) :
     messageFilter.insert(MAVLINK_MSG_ID_MISSION_ITEM, false);
     messageFilter.insert(MAVLINK_MSG_ID_MISSION_COUNT, false);
     messageFilter.insert(MAVLINK_MSG_ID_MISSION_ACK, false);
-    #ifdef MAVLINK_ENABLED_PIXHAWK
     messageFilter.insert(MAVLINK_MSG_ID_DATA_STREAM, false);
+    #ifdef MAVLINK_ENABLED_PIXHAWK
+    messageFilter.insert(MAVLINK_MSG_ID_ENCAPSULATED_DATA, false);
     messageFilter.insert(MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, false);
     #endif
     messageFilter.insert(MAVLINK_MSG_ID_EXTENDED_MESSAGE, false);

src/ui/QGCFirmwareUpdate.h

@@ -17,9 +17,23 @@ public:
 
 protected:
     void changeEvent(QEvent *e);
+    void showEvent(QShowEvent* event)
+    {
+        QWidget::showEvent(event);
+        emit visibilityChanged(true);
+    }
+
+    void hideEvent(QHideEvent* event)
+    {
+        QWidget::hideEvent(event);
+        emit visibilityChanged(false);
+    }
 
 private:
     Ui::QGCFirmwareUpdate *ui;
+
+signals:
+    void visibilityChanged(bool visible);
 };
 
 #endif // QGCFIRMWAREUPDATE_H

src/ui/QGCToolBar.cc

@@ -392,6 +392,6 @@ void QGCToolBar::receiveTextMessage(int uasid, int componentid, int severity, QS
 
 QGCToolBar::~QGCToolBar()
 {
-    delete toggleLoggingAction;
-    delete logReplayAction;
+    if (toggleLoggingAction) toggleLoggingAction->deleteLater();
+    if (logReplayAction) logReplayAction->deleteLater();
 }

src/ui/designer/QGCCommandButton.cc

@@ -79,7 +79,6 @@ QGCCommandButton::~QGCCommandButton()
 void QGCCommandButton::sendCommand()
 {
     if (QGCToolWidgetItem::uas) {
-        // FIXME
         int index = ui->editCommandComboBox->itemData(ui->editCommandComboBox->currentIndex()).toInt();
         MAV_CMD command = static_cast<MAV_CMD>(index);
         int confirm = (ui->editConfirmationCheckBox->isChecked()) ? 1 : 0;
@@ -87,9 +86,12 @@ void QGCCommandButton::sendCommand()
         float param2 = ui->editParam2SpinBox->value();
         float param3 = ui->editParam3SpinBox->value();
         float param4 = ui->editParam4SpinBox->value();
+        float param5 = ui->editParam5SpinBox->value();
+        float param6 = ui->editParam6SpinBox->value();
+        float param7 = ui->editParam7SpinBox->value();
         int component = ui->editComponentSpinBox->value();
 
-        QGCToolWidgetItem::uas->executeCommand(command, confirm, param1, param2, param3, param4, component);
+        QGCToolWidgetItem::uas->executeCommand(command, confirm, param1, param2, param3, param4, param5, param6, param7, component);
         qDebug() << __FILE__ << __LINE__ << "SENDING COMMAND" << index;
     } else {
         qDebug() << __FILE__ << __LINE__ << "NO UAS SET, DOING NOTHING";

src/ui/map/QGCMapWidget.cc

@@ -284,6 +284,68 @@ void QGCMapWidget::updateGlobalPosition()
     }
 }
 
+void QGCMapWidget::updateLocalPosition()
+{
+    QList<UASInterface*> systems = UASManager::instance()->getUASList();
+    foreach (UASInterface* system, systems)
+    {
+        // Get reference to graphic UAV item
+        mapcontrol::UAVItem* uav = GetUAV(system->getUASID());
+        // Check if reference is valid, else create a new one
+        if (uav == NULL)
+        {
+            MAV2DIcon* newUAV = new MAV2DIcon(map, this, system);
+            AddUAV(system->getUASID(), newUAV);
+            uav = newUAV;
+            uav->SetTrailTime(1);
+            uav->SetTrailDistance(5);
+            uav->SetTrailType(mapcontrol::UAVTrailType::ByTimeElapsed);
+        }
+
+        // Set new lat/lon position of UAV icon
+        double latitude = UASManager::instance()->getHomeLatitude();
+        double longitude = UASManager::instance()->getHomeLongitude();
+        double altitude = UASManager::instance()->getHomeAltitude();
+        internals::PointLatLng pos_lat_lon = internals::PointLatLng(system->getLatitude(), system->getLongitude());
+        uav->SetUAVPos(pos_lat_lon, system->getAltitude());
+        // Follow status
+        if (followUAVEnabled && system->getUASID() == followUAVID) SetCurrentPosition(pos_lat_lon);
+        // Convert from radians to degrees and apply
+        uav->SetUAVHeading((system->getYaw()/M_PI)*180.0f);
+    }
+}
+
+void QGCMapWidget::updateLocalPositionEstimates()
+{
+    QList<UASInterface*> systems = UASManager::instance()->getUASList();
+    foreach (UASInterface* system, systems)
+    {
+        // Get reference to graphic UAV item
+        mapcontrol::UAVItem* uav = GetUAV(system->getUASID());
+        // Check if reference is valid, else create a new one
+        if (uav == NULL)
+        {
+            MAV2DIcon* newUAV = new MAV2DIcon(map, this, system);
+            AddUAV(system->getUASID(), newUAV);
+            uav = newUAV;
+            uav->SetTrailTime(1);
+            uav->SetTrailDistance(5);
+            uav->SetTrailType(mapcontrol::UAVTrailType::ByTimeElapsed);
+        }
+
+        // Set new lat/lon position of UAV icon
+        double latitude = UASManager::instance()->getHomeLatitude();
+        double longitude = UASManager::instance()->getHomeLongitude();
+        double altitude = UASManager::instance()->getHomeAltitude();
+        internals::PointLatLng pos_lat_lon = internals::PointLatLng(system->getLatitude(), system->getLongitude());
+        uav->SetUAVPos(pos_lat_lon, system->getAltitude());
+        // Follow status
+        if (followUAVEnabled && system->getUASID() == followUAVID) SetCurrentPosition(pos_lat_lon);
+        // Convert from radians to degrees and apply
+        uav->SetUAVHeading((system->getYaw()/M_PI)*180.0f);
+    }
+}
+
 
 void QGCMapWidget::updateSystemSpecs(int uas)
 {

src/ui/map/QGCMapWidget.h

@@ -46,6 +46,10 @@ public slots:
     void updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec);
     /** @brief Update the global position of all systems */
     void updateGlobalPosition();
+    /** @brief Update the local position and draw it converted to GPS reference */
+    void updateLocalPosition();
+    /** @brief Update the local position estimates (individual sensors) and draw it converted to GPS reference */
+    void updateLocalPositionEstimates();
     /** @brief Update the type, size, etc. of this system */
     void updateSystemSpecs(int uas);
     /** @brief Change current system in focus / editing */

src/ui/map3D/HUDScaleGeode.cc

@@ -40,7 +40,7 @@ HUDScaleGeode::HUDScaleGeode()
 }
 
 void
-HUDScaleGeode::init(void)
+HUDScaleGeode::init(osg::ref_ptr<osgText::Font>& font)
 {
     osg::ref_ptr<osg::Vec2Array> outlineVertices(new osg::Vec2Array);
     outlineVertices->push_back(osg::Vec2(20.0f, 50.0f));
@@ -96,7 +96,7 @@ HUDScaleGeode::init(void)
 
     text = new osgText::Text;
     text->setCharacterSize(11);
-    text->setFont("images/Vera.ttf");
+    text->setFont(font);
     text->setAxisAlignment(osgText::Text::SCREEN);
     text->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f));
     text->setPosition(osg::Vec3(40.0f, 45.0f, -1.5f));

src/ui/map3D/HUDScaleGeode.h

@@ -41,7 +41,7 @@ class HUDScaleGeode : public osg::Geode
 public:
     HUDScaleGeode();
 
-    void init(void);
+    void init(osg::ref_ptr<osgText::Font>& font);
     void update(int windowHeight, float cameraFov, float cameraDistance,
                 bool darkBackground);
 

src/ui/map3D/ImageWindowGeode.cc

@@ -31,11 +31,17 @@
 
 #include "ImageWindowGeode.h"
 
-ImageWindowGeode::ImageWindowGeode(const QString& caption,
-                                   const osg::Vec4& backgroundColor,
-                                   osg::ref_ptr<osg::Image>& image)
+ImageWindowGeode::ImageWindowGeode()
     : border(5)
 {
+
+}
+
+void
+ImageWindowGeode::init(const QString& caption, const osg::Vec4& backgroundColor,
+                       osg::ref_ptr<osg::Image>& image,
+                       osg::ref_ptr<osgText::Font>& font)
+{
     // image
     osg::ref_ptr<osg::Geometry> imageGeometry = new osg::Geometry;
     imageVertices = new osg::Vec3Array(4);
@@ -82,7 +88,7 @@ ImageWindowGeode::ImageWindowGeode(const QString& caption,
     text = new osgText::Text;
     text->setText(caption.toStdString().c_str());
     text->setCharacterSize(11);
-    text->setFont("images/Vera.ttf");
+    text->setFont(font);
     text->setAxisAlignment(osgText::Text::SCREEN);
     text->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f));
 

src/ui/map3D/ImageWindowGeode.h

@@ -40,8 +40,10 @@
 class ImageWindowGeode : public osg::Geode
 {
 public:
-    ImageWindowGeode(const QString& caption, const osg::Vec4& backgroundColor,
-                     osg::ref_ptr<osg::Image>& image);
+    ImageWindowGeode();
+    void init(const QString& caption, const osg::Vec4& backgroundColor,
+              osg::ref_ptr<osg::Image>& image,
+              osg::ref_ptr<osgText::Font>& font);
 
     void setAttributes(int x, int y, int width, int height);
 

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.cc

@@ -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>
  *
  */
 
@@ -46,7 +46,7 @@
 
 #ifdef QGC_PROTOBUF_ENABLED
 #include <tr1/memory>
-#include <pixhawk.pb.h>
+#include <pixhawk/pixhawk.pb.h>
 #endif
 
 Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
@@ -60,6 +60,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     , displayWaypoints(true)
     , displayRGBD2D(false)
     , displayRGBD3D(true)
+    , displayObstacleList(true)
     , enableRGBDColor(false)
     , enableTarget(false)
     , followCamera(true)
@@ -98,7 +99,13 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
 
     // generate RGBD model
     rgbd3DNode = createRGBD3D();
-    egocentricMap->addChild(rgbd3DNode);
+    rollingMap->addChild(rgbd3DNode);
+
+#ifdef QGC_PROTOBUF_ENABLED
+    obstacleGroupNode = new ObstacleGroupNode;
+    obstacleGroupNode->init();
+    rollingMap->addChild(obstacleGroupNode);
+#endif
 
     setupHUD();
 
@@ -107,6 +114,8 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
 
     buildLayout();
 
+    updateHUD(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, "32N");
+
     connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
             this, SLOT(setActiveUAS(UASInterface*)));
 }
@@ -123,7 +132,8 @@ Pixhawk3DWidget::~Pixhawk3DWidget()
 void
 Pixhawk3DWidget::setActiveUAS(UASInterface* uas)
 {
-    if (this->uas != NULL && this->uas != uas) {
+    if (this->uas != NULL && this->uas != uas)
+    {
         // Disconnect any previously connected active MAV
         //disconnect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64)));
     }
@@ -134,9 +144,12 @@ Pixhawk3DWidget::setActiveUAS(UASInterface* uas)
 void
 Pixhawk3DWidget::selectFrame(QString text)
 {
-    if (text.compare("Global") == 0) {
+    if (text.compare("Global") == 0)
+    {
         frame = MAV_FRAME_GLOBAL;
-    } else if (text.compare("Local") == 0) {
+    }
+    else if (text.compare("Local") == 0)
+    {
         frame = MAV_FRAME_LOCAL_NED;
     }
 
@@ -148,9 +161,12 @@ Pixhawk3DWidget::selectFrame(QString text)
 void
 Pixhawk3DWidget::showGrid(int32_t state)
 {
-    if (state == Qt::Checked) {
+    if (state == Qt::Checked)
+    {
         displayGrid = true;
-    } else {
+    }
+    else
+    {
         displayGrid = false;
     }
 }
@@ -158,13 +174,17 @@ Pixhawk3DWidget::showGrid(int32_t state)
 void
 Pixhawk3DWidget::showTrail(int32_t state)
 {
-    if (state == Qt::Checked) {
-        if (!displayTrail) {
+    if (state == Qt::Checked)
+    {
+        if (!displayTrail)
+        {
             trail.clear();
         }
 
         displayTrail = true;
-    } else {
+    }
+    else
+    {
         displayTrail = false;
     }
 }
@@ -172,9 +192,12 @@ Pixhawk3DWidget::showTrail(int32_t state)
 void
 Pixhawk3DWidget::showWaypoints(int state)
 {
-    if (state == Qt::Checked) {
+    if (state == Qt::Checked)
+    {
         displayWaypoints = true;
-    } else {
+    }
+    else
+    {
         displayWaypoints = false;
     }
 }
@@ -184,9 +207,12 @@ Pixhawk3DWidget::selectMapSource(int index)
 {
     mapNode->setImageryType(static_cast<Imagery::ImageryType>(index));
 
-    if (mapNode->getImageryType() == Imagery::BLANK_MAP) {
+    if (mapNode->getImageryType() == Imagery::BLANK_MAP)
+    {
         displayImagery = false;
-    } else {
+    }
+    else
+    {
         displayImagery = true;
     }
 }
@@ -211,124 +237,234 @@ Pixhawk3DWidget::recenter(void)
 void
 Pixhawk3DWidget::toggleFollowCamera(int32_t state)
 {
-    if (state == Qt::Checked) {
+    if (state == Qt::Checked)
+    {
         followCamera = true;
-    } else {
+    }
+    else
+    {
         followCamera = false;
     }
 }
 
 void
-Pixhawk3DWidget::selectTarget(void)
+Pixhawk3DWidget::selectTargetHeading(void)
 {
-    if (uas) {
-        if (frame == MAV_FRAME_GLOBAL) {
-            double altitude = uas->getAltitude();
+    if (!uas)
+    {
+        return;
+    }
+
+    osg::Vec2d p;
 
-            std::pair<double,double> cursorWorldCoords =
-                getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
+    if (frame == MAV_FRAME_GLOBAL)
+    {
+        double altitude = uas->getAltitude();
 
-            target.set(cursorWorldCoords.first, cursorWorldCoords.second);
-        } else if (frame == MAV_FRAME_LOCAL_NED) {
-            double z = uas->getLocalZ();
+        std::pair<double,double> cursorWorldCoords =
+            getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
 
-            std::pair<double,double> cursorWorldCoords =
-                getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
+        p.set(cursorWorldCoords.first, cursorWorldCoords.second);
+    }
+    else if (frame == MAV_FRAME_LOCAL_NED)
+    {
+        double z = uas->getLocalZ();
 
-            target.set(cursorWorldCoords.first, cursorWorldCoords.second);
-        }
+        std::pair<double,double> cursorWorldCoords =
+            getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
+
+        p.set(cursorWorldCoords.first, cursorWorldCoords.second);
+    }
 
-        uas->setTargetPosition(target.x(), target.y(), 0.0, 0.0);
+    target.z() = atan2(p.y() - target.y(), p.x() - target.x());
+}
 
-        enableTarget = true;
+void
+Pixhawk3DWidget::selectTarget(void)
+{
+    if (!uas)
+    {
+        return;
     }
+
+    if (frame == MAV_FRAME_GLOBAL)
+    {
+        double altitude = uas->getAltitude();
+
+        std::pair<double,double> cursorWorldCoords =
+            getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(),
+                                    altitude);
+
+        target.set(cursorWorldCoords.first, cursorWorldCoords.second, 0.0);
+    }
+    else if (frame == MAV_FRAME_LOCAL_NED)
+    {
+        double z = uas->getLocalZ();
+
+        std::pair<double,double> cursorWorldCoords =
+            getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(), -z);
+
+        target.set(cursorWorldCoords.first, cursorWorldCoords.second, 0.0);
+    }
+
+    enableTarget = true;
+
+    mode = SELECT_TARGET_HEADING_MODE;
+}
+
+void
+Pixhawk3DWidget::setTarget(void)
+{
+    selectTargetHeading();
+
+    uas->setTargetPosition(target.x(), target.y(), 0.0,
+                           osg::RadiansToDegrees(target.z()));
 }
 
 void
 Pixhawk3DWidget::insertWaypoint(void)
 {
-    if (uas) {
-        Waypoint* wp = NULL;
-        if (frame == MAV_FRAME_GLOBAL) {
-            double latitude = uas->getLatitude();
-            double longitude = uas->getLongitude();
-            double altitude = uas->getAltitude();
-            double x, y;
-            QString utmZone;
-            Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
-
-            std::pair<double,double> cursorWorldCoords =
-                getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
-
-            Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone,
-                             latitude, longitude);
-
-            wp = new Waypoint(0, longitude, latitude, altitude);
-        } else if (frame == MAV_FRAME_LOCAL_NED) {
-            double z = uas->getLocalZ();
-
-            std::pair<double,double> cursorWorldCoords =
-                getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
-
-            wp = new Waypoint(0, cursorWorldCoords.first,
-                              cursorWorldCoords.second, z);
-        }
+    if (!uas)
+    {
+        return;
+    }
 
-        if (wp) {
-            wp->setFrame(frame);
-            uas->getWaypointManager()->addWaypointEditable(wp);
-        }
+    Waypoint* wp = NULL;
+    if (frame == MAV_FRAME_GLOBAL)
+    {
+        double latitude = uas->getLatitude();
+        double longitude = uas->getLongitude();
+        double altitude = uas->getAltitude();
+        double x, y;
+        QString utmZone;
+        Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
+
+        std::pair<double,double> cursorWorldCoords =
+            getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(),
+                                    altitude);
+
+        Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone,
+                         latitude, longitude);
+
+        wp = new Waypoint(0, longitude, latitude, altitude, 0.0, 0.25);
+    }
+    else if (frame == MAV_FRAME_LOCAL_NED)
+    {
+        double z = uas->getLocalZ();
+
+        std::pair<double,double> cursorWorldCoords =
+            getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(), -z);
+
+        wp = new Waypoint(0, cursorWorldCoords.first,
+                          cursorWorldCoords.second, z, 0.0, 0.25);
     }
+
+    if (wp)
+    {
+        wp->setFrame(frame);
+        uas->getWaypointManager()->addWaypointEditable(wp);
+    }
+
+    selectedWpIndex = wp->getId();
+    mode = MOVE_WAYPOINT_HEADING_MODE;
 }
 
 void
-Pixhawk3DWidget::moveWaypoint(void)
+Pixhawk3DWidget::moveWaypointPosition(void)
 {
-    mode = MOVE_WAYPOINT_MODE;
+    if (mode != MOVE_WAYPOINT_POSITION_MODE)
+    {
+        mode = MOVE_WAYPOINT_POSITION_MODE;
+        return;
+    }
+
+    if (!uas)
+    {
+        return;
+    }
+
+    const QVector<Waypoint *> waypoints =
+        uas->getWaypointManager()->getWaypointEditableList();
+    Waypoint* waypoint = waypoints.at(selectedWpIndex);
+
+    if (frame == MAV_FRAME_GLOBAL)
+    {
+        double latitude = uas->getLatitude();
+        double longitude = uas->getLongitude();
+        double altitude = uas->getAltitude();
+        double x, y;
+        QString utmZone;
+        Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
+
+        std::pair<double,double> cursorWorldCoords =
+            getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
+
+        Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second,
+                         utmZone, latitude, longitude);
+
+        waypoint->setX(longitude);
+        waypoint->setY(latitude);
+    }
+    else if (frame == MAV_FRAME_LOCAL_NED)
+    {
+        double z = uas->getLocalZ();
+
+        std::pair<double,double> cursorWorldCoords =
+            getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
+
+        waypoint->setX(cursorWorldCoords.first);
+        waypoint->setY(cursorWorldCoords.second);
+    }
 }
 
 void
-Pixhawk3DWidget::setWaypoint(void)
+Pixhawk3DWidget::moveWaypointHeading(void)
 {
-    if (uas) {
-        const QVector<Waypoint *> waypoints =
-            uas->getWaypointManager()->getWaypointEditableList();
-        Waypoint* waypoint = waypoints.at(selectedWpIndex);
-
-        if (frame == MAV_FRAME_GLOBAL) {
-            double latitude = uas->getLatitude();
-            double longitude = uas->getLongitude();
-            double altitude = uas->getAltitude();
-            double x, y;
-            QString utmZone;
-            Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
-
-            std::pair<double,double> cursorWorldCoords =
-                getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
-
-            Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone,
-                             latitude, longitude);
-
-            waypoint->setX(longitude);
-            waypoint->setY(latitude);
-            waypoint->setZ(altitude);
-        } else if (frame == MAV_FRAME_LOCAL_NED) {
-            double z = uas->getLocalZ();
-
-            std::pair<double,double> cursorWorldCoords =
-                getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
-
-            waypoint->setX(cursorWorldCoords.first);
-            waypoint->setY(cursorWorldCoords.second);
-            waypoint->setZ(z);
-        }
+    if (mode != MOVE_WAYPOINT_HEADING_MODE)
+    {
+        mode = MOVE_WAYPOINT_HEADING_MODE;
+        return;
+    }
+
+    if (!uas)
+    {
+        return;
+    }
+
+    const QVector<Waypoint *> waypoints =
+        uas->getWaypointManager()->getWaypointEditableList();
+    Waypoint* waypoint = waypoints.at(selectedWpIndex);
+
+    double x = 0.0, y = 0.0, z = 0.0;
+
+    if (frame == MAV_FRAME_GLOBAL)
+    {
+        double latitude = waypoint->getY();
+        double longitude = waypoint->getX();
+        z = -waypoint->getZ();
+        QString utmZone;
+        Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
+    }
+    else if (frame == MAV_FRAME_LOCAL_NED)
+    {
+        z = uas->getLocalZ();
     }
+
+    std::pair<double,double> cursorWorldCoords =
+        getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
+
+    double yaw = atan2(cursorWorldCoords.second - waypoint->getY(),
+                       cursorWorldCoords.first - waypoint->getX());
+    yaw = osg::RadiansToDegrees(yaw);
+
+    waypoint->setYaw(yaw);
 }
 
 void
 Pixhawk3DWidget::deleteWaypoint(void)
 {
-    if (uas) {
+    if (uas)
+    {
         uas->getWaypointManager()->removeWaypoint(selectedWpIndex);
     }
 }
@@ -336,26 +472,34 @@ Pixhawk3DWidget::deleteWaypoint(void)
 void
 Pixhawk3DWidget::setWaypointAltitude(void)
 {
-    if (uas) {
-        bool ok;
-        const QVector<Waypoint *> waypoints =
-            uas->getWaypointManager()->getWaypointEditableList();
-        Waypoint* waypoint = waypoints.at(selectedWpIndex);
+    if (!uas)
+    {
+        return;
+    }
 
-        double altitude = waypoint->getZ();
-        if (frame == MAV_FRAME_LOCAL_NED) {
-            altitude = -altitude;
-        }
+    bool ok;
+    const QVector<Waypoint *> waypoints =
+        uas->getWaypointManager()->getWaypointEditableList();
+    Waypoint* waypoint = waypoints.at(selectedWpIndex);
 
-        double newAltitude =
-            QInputDialog::getDouble(this, tr("Set altitude of waypoint %1").arg(selectedWpIndex),
-                                    tr("Altitude (m):"), waypoint->getZ(), -1000.0, 1000.0, 1, &ok);
-        if (ok) {
-            if (frame == MAV_FRAME_GLOBAL) {
-                waypoint->setZ(newAltitude);
-            } else if (frame == MAV_FRAME_LOCAL_NED) {
-                waypoint->setZ(-newAltitude);
-            }
+    double altitude = waypoint->getZ();
+    if (frame == MAV_FRAME_LOCAL_NED)
+    {
+        altitude = -altitude;
+    }
+
+    double newAltitude =
+        QInputDialog::getDouble(this, tr("Set altitude of waypoint %1").arg(selectedWpIndex),
+                                tr("Altitude (m):"), waypoint->getZ(), -1000.0, 1000.0, 1, &ok);
+    if (ok)
+    {
+        if (frame == MAV_FRAME_GLOBAL)
+        {
+            waypoint->setZ(newAltitude);
+        }
+        else if (frame == MAV_FRAME_LOCAL_NED)
+        {
+            waypoint->setZ(-newAltitude);
         }
     }
 }
@@ -363,10 +507,12 @@ Pixhawk3DWidget::setWaypointAltitude(void)
 void
 Pixhawk3DWidget::clearAllWaypoints(void)
 {
-    if (uas) {
+    if (uas)
+    {
         const QVector<Waypoint *> waypoints =
             uas->getWaypointManager()->getWaypointEditableList();
-        for (int i = waypoints.size() - 1; i >= 0; --i) {
+        for (int i = waypoints.size() - 1; i >= 0; --i)
+        {
             uas->getWaypointManager()->removeWaypoint(i);
         }
     }
@@ -393,13 +539,17 @@ Pixhawk3DWidget::findVehicleModels(void)
     nodes.push_back(sphereGeode);
 
     // add all other models in folder
-    for (int i = 0; i < files.size(); ++i) {
+    for (int i = 0; i < files.size(); ++i)
+    {
         osg::ref_ptr<osg::Node> node =
             osgDB::readNodeFile(directory.absoluteFilePath(files[i]).toStdString().c_str());
 
-        if (node) {
+        if (node)
+        {
             nodes.push_back(node);
-        } else {
+        }
+        else
+        {
             printf("%s\n", QString("ERROR: Could not load file " + directory.absoluteFilePath(files[i]) + "\n").toStdString().c_str());
         }
     }
@@ -457,7 +607,8 @@ Pixhawk3DWidget::buildLayout(void)
 
     QLabel* modelLabel = new QLabel("Vehicle", this);
     QComboBox* modelComboBox = new QComboBox(this);
-    for (int i = 0; i < vehicleModels.size(); ++i) {
+    for (int i = 0; i < vehicleModels.size(); ++i)
+    {
         modelComboBox->addItem(vehicleModels[i]->getName().c_str());
     }
 
@@ -505,10 +656,32 @@ Pixhawk3DWidget::buildLayout(void)
             this, SLOT(toggleFollowCamera(int)));
 }
 
+void
+Pixhawk3DWidget::resizeGL(int width, int height)
+{
+    Q3DWidget::resizeGL(width, height);
+
+    resizeHUD();
+}
+
 void
 Pixhawk3DWidget::display(void)
 {
-    if (uas == NULL) {
+    // set node visibility
+    rollingMap->setChildValue(gridNode, displayGrid);
+    rollingMap->setChildValue(trailNode, displayTrail);
+    rollingMap->setChildValue(mapNode, displayImagery);
+    rollingMap->setChildValue(waypointGroupNode, displayWaypoints);
+    rollingMap->setChildValue(targetNode, enableTarget);
+#ifdef QGC_PROTOBUF_ENABLED
+    rollingMap->setChildValue(obstacleGroupNode, displayObstacleList);
+#endif
+    rollingMap->setChildValue(rgbd3DNode, displayRGBD3D);
+    hudGroup->setChildValue(rgb2DGeode, displayRGBD2D);
+    hudGroup->setChildValue(depth2DGeode, displayRGBD2D);
+
+    if (!uas)
+    {
         return;
     }
 
@@ -516,7 +689,8 @@ Pixhawk3DWidget::display(void)
     QString utmZone;
     getPose(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw, utmZone);
 
-    if (lastRobotX == 0.0f && lastRobotY == 0.0f && lastRobotZ == 0.0f) {
+    if (lastRobotX == 0.0f && lastRobotY == 0.0f && lastRobotZ == 0.0f)
+    {
         lastRobotX = robotX;
         lastRobotY = robotY;
         lastRobotZ = robotZ;
@@ -524,7 +698,8 @@ Pixhawk3DWidget::display(void)
         recenterCamera(robotY, robotX, -robotZ);
     }
 
-    if (followCamera) {
+    if (followCamera)
+    {
         double dx = robotY - lastRobotY;
         double dy = robotX - lastRobotX;
         double dz = lastRobotZ - robotZ;
@@ -537,41 +712,40 @@ Pixhawk3DWidget::display(void)
                                          robotPitch, osg::Vec3d(1.0f, 0.0f, 0.0f),
                                          robotRoll, osg::Vec3d(0.0f, 1.0f, 0.0f)));
 
-    if (displayTrail) {
+    if (displayTrail)
+    {
         updateTrail(robotX, robotY, robotZ);
     }
 
-    if (frame == MAV_FRAME_GLOBAL && displayImagery) {
+    if (frame == MAV_FRAME_GLOBAL && displayImagery)
+    {
         updateImagery(robotX, robotY, robotZ, utmZone);
     }
 
-    if (displayWaypoints) {
+    if (displayWaypoints)
+    {
         updateWaypoints();
     }
 
-    if (enableTarget) {
+    if (enableTarget)
+    {
         updateTarget(robotX, robotY);
     }
 
 #ifdef QGC_PROTOBUF_ENABLED
-    if (displayRGBD2D || displayRGBD3D) {
+    if (displayRGBD2D || displayRGBD3D)
+    {
         updateRGBD(robotX, robotY, robotZ);
     }
+
+    if (displayObstacleList)
+    {
+        updateObstacles();
+    }
 #endif
 
     updateHUD(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw, utmZone);
 
-    // set node visibility
-
-    rollingMap->setChildValue(gridNode, displayGrid);
-    rollingMap->setChildValue(trailNode, displayTrail);
-    rollingMap->setChildValue(mapNode, displayImagery);
-    rollingMap->setChildValue(waypointGroupNode, displayWaypoints);
-    rollingMap->setChildValue(targetNode, enableTarget);
-    egocentricMap->setChildValue(rgbd3DNode, displayRGBD3D);
-    hudGroup->setChildValue(rgb2DGeode, displayRGBD2D);
-    hudGroup->setChildValue(depth2DGeode, displayRGBD2D);
-
     lastRobotX = robotX;
     lastRobotY = robotY;
     lastRobotZ = robotZ;
@@ -582,8 +756,10 @@ Pixhawk3DWidget::display(void)
 void
 Pixhawk3DWidget::keyPressEvent(QKeyEvent* event)
 {
-    if (!event->text().isEmpty()) {
-        switch (*(event->text().toAscii().data())) {
+    if (!event->text().isEmpty())
+    {
+        switch (*(event->text().toAscii().data()))
+        {
         case '1':
             displayRGBD2D = !displayRGBD2D;
             break;
@@ -594,6 +770,10 @@ Pixhawk3DWidget::keyPressEvent(QKeyEvent* event)
         case 'C':
             enableRGBDColor = !enableRGBDColor;
             break;
+        case 'o':
+        case 'O':
+            displayObstacleList = !displayObstacleList;
+            break;
         }
     }
 
@@ -603,19 +783,29 @@ Pixhawk3DWidget::keyPressEvent(QKeyEvent* event)
 void
 Pixhawk3DWidget::mousePressEvent(QMouseEvent* event)
 {
-    if (event->button() == Qt::LeftButton) {
-        if (mode == MOVE_WAYPOINT_MODE) {
-            setWaypoint();
-            mode = DEFAULT_MODE;
+    if (event->button() == Qt::LeftButton)
+    {
+        if (mode == SELECT_TARGET_HEADING_MODE)
+        {
+            setTarget();
+        }
 
-            return;
+        if (mode != DEFAULT_MODE)
+        {
+            mode = DEFAULT_MODE;
         }
 
-        if (event->modifiers() == Qt::ShiftModifier) {
-            selectedWpIndex = findWaypoint(event->x(), event->y());
-            if (selectedWpIndex == -1) {
+        if (event->modifiers() == Qt::ShiftModifier)
+        {
+            selectedWpIndex = findWaypoint(event->pos());
+            if (selectedWpIndex == -1)
+            {
+                cachedMousePos = event->pos();
+
                 showInsertWaypointMenu(event->globalPos());
-            } else {
+            }
+            else
+            {
                 showEditWaypointMenu(event->globalPos());
             }
 
@@ -626,29 +816,54 @@ Pixhawk3DWidget::mousePressEvent(QMouseEvent* event)
     Q3DWidget::mousePressEvent(event);
 }
 
+void
+Pixhawk3DWidget::mouseMoveEvent(QMouseEvent* event)
+{
+    if (mode == SELECT_TARGET_HEADING_MODE)
+    {
+        selectTargetHeading();
+    }
+    if (mode == MOVE_WAYPOINT_POSITION_MODE)
+    {
+        moveWaypointPosition();
+    }
+    if (mode == MOVE_WAYPOINT_HEADING_MODE)
+    {
+        moveWaypointHeading();
+    }
+
+    Q3DWidget::mouseMoveEvent(event);
+}
+
 void
 Pixhawk3DWidget::getPose(double& x, double& y, double& z,
                          double& roll, double& pitch, double& yaw,
                          QString& utmZone)
 {
-    if (uas) {
-        if (frame == MAV_FRAME_GLOBAL) {
-            double latitude = uas->getLatitude();
-            double longitude = uas->getLongitude();
-            double altitude = uas->getAltitude();
-
-            Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
-            z = -altitude;
-        } else if (frame == MAV_FRAME_LOCAL_NED) {
-            x = uas->getLocalX();
-            y = uas->getLocalY();
-            z = uas->getLocalZ();
-        }
+    if (!uas)
+    {
+        return;
+    }
 
-        roll = uas->getRoll();
-        pitch = uas->getPitch();
-        yaw = uas->getYaw();
+    if (frame == MAV_FRAME_GLOBAL)
+    {
+        double latitude = uas->getLatitude();
+        double longitude = uas->getLongitude();
+        double altitude = uas->getAltitude();
+
+        Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
+        z = -altitude;
     }
+    else if (frame == MAV_FRAME_LOCAL_NED)
+    {
+        x = uas->getLocalX();
+        y = uas->getLocalY();
+        z = uas->getLocalZ();
+    }
+
+    roll = uas->getRoll();
+    pitch = uas->getPitch();
+    yaw = uas->getYaw();
 }
 
 void
@@ -663,23 +878,25 @@ void
 Pixhawk3DWidget::getPosition(double& x, double& y, double& z,
                              QString& utmZone)
 {
-    if (uas)
+    if (!uas)
     {
-        if (frame == MAV_FRAME_GLOBAL)
-        {
-            double latitude = uas->getLatitude();
-            double longitude = uas->getLongitude();
-            double altitude = uas->getAltitude();
+        return;
+    }
 
-            Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
-            z = -altitude;
-        }
-        else if (frame == MAV_FRAME_LOCAL_NED)
-        {
-            x = uas->getLocalX();
-            y = uas->getLocalY();
-            z = uas->getLocalZ();
-        }
+    if (frame == MAV_FRAME_GLOBAL)
+    {
+        double latitude = uas->getLatitude();
+        double longitude = uas->getLongitude();
+        double altitude = uas->getAltitude();
+
+        Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
+        z = -altitude;
+    }
+    else if (frame == MAV_FRAME_LOCAL_NED)
+    {
+        x = uas->getLocalX();
+        y = uas->getLocalY();
+        z = uas->getLocalZ();
     }
 }
 
@@ -820,14 +1037,15 @@ Pixhawk3DWidget::createTarget(void)
 
     pat->setPosition(osg::Vec3d(0.0, 0.0, 0.0));
 
-    osg::ref_ptr<osg::Sphere> sphere = new osg::Sphere(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.1f);
-    osg::ref_ptr<osg::ShapeDrawable> sphereDrawable = new osg::ShapeDrawable(sphere);
-    sphereDrawable->setColor(osg::Vec4f(0.0f, 1.0f, 0.0f, 1.0f));
-    osg::ref_ptr<osg::Geode> sphereGeode = new osg::Geode;
-    sphereGeode->addDrawable(sphereDrawable);
-    sphereGeode->setName("Target");
+    osg::ref_ptr<osg::Cone> cone = new osg::Cone(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.2f, 0.6f);
+    osg::ref_ptr<osg::ShapeDrawable> coneDrawable = new osg::ShapeDrawable(cone);
+    coneDrawable->setColor(osg::Vec4f(0.0f, 1.0f, 0.0f, 1.0f));
+    coneDrawable->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
+    osg::ref_ptr<osg::Geode> coneGeode = new osg::Geode;
+    coneGeode->addDrawable(coneDrawable);
+    coneGeode->setName("Target");
 
-    pat->addChild(sphereGeode);
+    pat->addChild(coneGeode);
 
     return pat;
 }
@@ -850,31 +1068,29 @@ Pixhawk3DWidget::setupHUD(void)
 
     statusText = new osgText::Text;
     statusText->setCharacterSize(11);
-    statusText->setFont("images/Vera.ttf");
+    statusText->setFont(font);
     statusText->setAxisAlignment(osgText::Text::SCREEN);
     statusText->setColor(osg::Vec4(255, 255, 255, 1));
 
-    resizeHUD();
-
     osg::ref_ptr<osg::Geode> statusGeode = new osg::Geode;
     statusGeode->addDrawable(hudBackgroundGeometry);
     statusGeode->addDrawable(statusText);
     hudGroup->addChild(statusGeode);
 
     rgbImage = new osg::Image;
-    rgb2DGeode = new ImageWindowGeode("RGB Image",
-                                      osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f),
-                                      rgbImage);
+    rgb2DGeode = new ImageWindowGeode;
+    rgb2DGeode->init("RGB Image", osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f),
+                     rgbImage, font);
     hudGroup->addChild(rgb2DGeode);
 
     depthImage = new osg::Image;
-    depth2DGeode = new ImageWindowGeode("Depth Image",
-                                        osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f),
-                                        depthImage);
+    depth2DGeode = new ImageWindowGeode;
+    depth2DGeode->init("Depth Image", osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f),
+                       depthImage, font);
     hudGroup->addChild(depth2DGeode);
 
     scaleGeode = new HUDScaleGeode;
-    scaleGeode->init();
+    scaleGeode->init(font);
     hudGroup->addChild(scaleGeode);
 }
 
@@ -920,8 +1136,6 @@ Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ,
                            double robotRoll, double robotPitch, double robotYaw,
                            const QString& utmZone)
 {
-    resizeHUD();
-
     std::pair<double,double> cursorPosition =
         getGlobalCursorPosition(getMouseX(), getMouseY(), -robotZ);
 
@@ -1059,7 +1273,7 @@ Pixhawk3DWidget::updateImagery(double originX, double originY, double originZ,
         maxResolution = 0.25;
         break;
     default:
-    {}
+        {}
     }
 
     double resolution = minResolution;
@@ -1113,8 +1327,18 @@ void
 Pixhawk3DWidget::updateTarget(double robotX, double robotY)
 {
     osg::PositionAttitudeTransform* pat =
-        static_cast<osg::PositionAttitudeTransform*>(targetNode.get());
+        dynamic_cast<osg::PositionAttitudeTransform*>(targetNode.get());
+
     pat->setPosition(osg::Vec3d(target.y() - robotY, target.x() - robotX, 0.0));
+    pat->setAttitude(osg::Quat(target.z() - M_PI_2, osg::Vec3d(1.0f, 0.0f, 0.0f),
+                               M_PI_2, osg::Vec3d(0.0f, 1.0f, 0.0f),
+                               0.0, osg::Vec3d(0.0f, 0.0f, 1.0f)));
+
+    osg::Geode* geode = dynamic_cast<osg::Geode*>(pat->getChild(0));
+    osg::ShapeDrawable* sd = dynamic_cast<osg::ShapeDrawable*>(geode->getDrawable(0));
+
+
+    sd->setColor(osg::Vec4f(1.0f, 0.8f, 0.0f, 1.0f));
 }
 
 float colormap_jet[128][3] = {
@@ -1295,7 +1519,8 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
 
     osg::Vec3Array* vertices = static_cast<osg::Vec3Array*>(geometry->getVertexArray());
     osg::Vec4Array* colors = static_cast<osg::Vec4Array*>(geometry->getColorArray());
-    for (int i = 0; i < pointCloud.points_size(); ++i) {
+    for (int i = 0; i < pointCloud.points_size(); ++i)
+    {
         const px::PointCloudXYZRGB_PointXYZRGB& p = pointCloud.points(i);
 
         double x = p.x() - robotX;
@@ -1305,7 +1530,8 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
 
         (*vertices)[i].set(y, x, -z);
 
-        if (enableRGBDColor) {
+        if (enableRGBDColor)
+        {
             float rgb = p.rgb();
 
             float b = *(reinterpret_cast<unsigned char*>(&rgb)) / 255.0f;
@@ -1313,7 +1539,9 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
             float r = *(2 + reinterpret_cast<unsigned char*>(&rgb)) / 255.0f;
 
             (*colors)[i].set(r, g, b, 1.0f);
-        } else {
+        }
+        else
+        {
             double dist = sqrt(x * x + y * y + z * z);
             int colorIndex = static_cast<int>(fmin(dist / 7.0 * 127.0, 127.0));
             (*colors)[i].set(colormap_jet[colorIndex][0],
@@ -1323,28 +1551,44 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
         }
     }
 
-    if (geometry->getNumPrimitiveSets() == 0) {
+    if (geometry->getNumPrimitiveSets() == 0)
+    {
         geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS,
                                   0, pointCloud.points_size()));
-    } else {
+    }
+    else
+    {
         osg::DrawArrays* drawarrays = static_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
         drawarrays->setCount(pointCloud.points_size());
     }
 }
+
+void
+Pixhawk3DWidget::updateObstacles(void)
+{
+    obstacleGroupNode->update(frame, uas);
+}
+
 #endif
 
 int
-Pixhawk3DWidget::findWaypoint(int mouseX, int mouseY)
+Pixhawk3DWidget::findWaypoint(const QPoint& mousePos)
 {
-    if (getSceneData() != NULL) {
+    if (getSceneData())
+    {
         osgUtil::LineSegmentIntersector::Intersections intersections;
 
-        if (computeIntersections(mouseX, height() - mouseY, intersections)) {
+        if (computeIntersections(mousePos.x(), height() - mousePos.y(),
+                                 intersections))
+        {
             for (osgUtil::LineSegmentIntersector::Intersections::iterator
-                    it = intersections.begin(); it != intersections.end(); it++) {
-                for (uint i = 0 ; i < it->nodePath.size(); ++i) {
+                    it = intersections.begin(); it != intersections.end(); it++)
+            {
+                for (uint i = 0 ; i < it->nodePath.size(); ++i)
+                {
                     std::string nodeName = it->nodePath[i]->getName();
-                    if (nodeName.substr(0, 2).compare("wp") == 0) {
+                    if (nodeName.substr(0, 2).compare("wp") == 0)
+                    {
                         return atoi(nodeName.substr(2).c_str());
                     }
                 }
@@ -1358,15 +1602,20 @@ Pixhawk3DWidget::findWaypoint(int mouseX, int mouseY)
 bool
 Pixhawk3DWidget::findTarget(int mouseX, int mouseY)
 {
-    if (getSceneData() != NULL) {
+    if (getSceneData())
+    {
         osgUtil::LineSegmentIntersector::Intersections intersections;
 
-        if (computeIntersections(mouseX, height() - mouseY, intersections)) {
+        if (computeIntersections(mouseX, height() - mouseY, intersections))
+        {
             for (osgUtil::LineSegmentIntersector::Intersections::iterator
-                    it = intersections.begin(); it != intersections.end(); it++) {
-                for (uint i = 0 ; i < it->nodePath.size(); ++i) {
+                    it = intersections.begin(); it != intersections.end(); it++)
+            {
+                for (uint i = 0 ; i < it->nodePath.size(); ++i)
+                {
                     std::string nodeName = it->nodePath[i]->getName();
-                    if (nodeName.compare("Target") == 0) {
+                    if (nodeName.compare("Target") == 0)
+                    {
                         return true;
                     }
                 }
@@ -1394,7 +1643,10 @@ Pixhawk3DWidget::showEditWaypointMenu(const QPoint &cursorPos)
 
     QString text;
     text = QString("Move waypoint %1").arg(QString::number(selectedWpIndex));
-    menu.addAction(text, this, SLOT(moveWaypoint()));
+    menu.addAction(text, this, SLOT(moveWaypointPosition()));
+
+    text = QString("Change heading of waypoint %1").arg(QString::number(selectedWpIndex));
+    menu.addAction(text, this, SLOT(moveWaypointHeading()));
 
     text = QString("Change altitude of waypoint %1").arg(QString::number(selectedWpIndex));
     menu.addAction(text, this, SLOT(setWaypointAltitude()));

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"
 
@@ -67,10 +70,12 @@ private slots:
     void recenter(void);
     void toggleFollowCamera(int state);
 
+    void selectTargetHeading(void);
     void selectTarget(void);
+    void setTarget(void);
     void insertWaypoint(void);
-    void moveWaypoint(void);
-    void setWaypoint(void);
+    void moveWaypointPosition(void);
+    void moveWaypointHeading(void);
     void deleteWaypoint(void);
     void setWaypointAltitude(void);
     void clearAllWaypoints(void);
@@ -78,12 +83,28 @@ private slots:
 protected:
     QVector< osg::ref_ptr<osg::Node> > findVehicleModels(void);
     void buildLayout(void);
+    virtual void resizeGL(int width, int height);
     virtual void display(void);
     virtual void keyPressEvent(QKeyEvent* event);
     virtual void mousePressEvent(QMouseEvent* event);
+    void showEvent(QShowEvent* event)
+    {
+        QWidget::showEvent(event);
+        emit visibilityChanged(true);
+    }
+
+    void hideEvent(QHideEvent* event)
+    {
+        QWidget::hideEvent(event);
+        emit visibilityChanged(false);
+    }
+    virtual void mouseMoveEvent(QMouseEvent* event);
 
     UASInterface* uas;
 
+signals:
+    void visibilityChanged(bool visible);
+
 private:
     void getPose(double& x, double& y, double& z,
                  double& roll, double& pitch, double& yaw,
@@ -113,16 +134,19 @@ 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);
+    int findWaypoint(const QPoint& mousePos);
     bool findTarget(int mouseX, int mouseY);
     void showInsertWaypointMenu(const QPoint& cursorPos);
     void showEditWaypointMenu(const QPoint& cursorPos);
 
     enum Mode {
         DEFAULT_MODE,
-        MOVE_WAYPOINT_MODE
+        MOVE_WAYPOINT_POSITION_MODE,
+        MOVE_WAYPOINT_HEADING_MODE,
+        SELECT_TARGET_HEADING_MODE
     };
     Mode mode;
     int selectedWpIndex;
@@ -133,6 +157,7 @@ private:
     bool displayWaypoints;
     bool displayRGBD2D;
     bool displayRGBD3D;
+    bool displayObstacleList;
     bool enableRGBDColor;
     bool enableTarget;
 
@@ -157,11 +182,15 @@ 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;
 
     MAV_FRAME frame;
-    osg::Vec2d target;
+    osg::Vec3d target;
+    QPoint cachedMousePos;
     double lastRobotX, lastRobotY, lastRobotZ;
 };
 

src/ui/map3D/Q3DWidget.cc

@@ -35,6 +35,9 @@ This file is part of the QGROUNDCONTROL project
 #include <osg/Geometry>
 #include <osg/LineWidth>
 #include <osg/MatrixTransform>
+#ifdef QGC_OSG_QT_ENABLED
+#include <osgQt/QFontImplementation>
+#endif
 #ifdef Q_OS_MACX
 #include <Carbon/Carbon.h>
 #endif
@@ -56,12 +59,21 @@ Q3DWidget::Q3DWidget(QWidget* parent)
     cameraParams.cameraFov = 30.0f;
     cameraParams.minClipRange = 1.0f;
     cameraParams.maxClipRange = 10000.0f;
+#ifdef QGC_OSG_QT_ENABLED
+    osg::ref_ptr<osgText::Font::FontImplementation> fontImpl;
+    fontImpl = new osgQt::QFontImplementation(QFont(":/general/vera.ttf"));
+#else
+    osg::ref_ptr<osgText::Font::FontImplementation> fontImpl;
+    fontImpl = 0;//new osgText::Font::Font("images/Vera.ttf");
+#endif
+    font = new osgText::Font(fontImpl);
 
     osgGW = new osgViewer::GraphicsWindowEmbedded(0, 0, width(), height());
 
     setThreadingModel(osgViewer::Viewer::SingleThreaded);
 
     setFocusPolicy(Qt::StrongFocus);
+    setMouseTracking(true);
 }
 
 Q3DWidget::~Q3DWidget()

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,148 @@ 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 (!uas)
+    {
+        return;
+    }
 
-        if (getNumChildren() > 0) {
-            removeChild(0, getNumChildren());
-        }
+    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();
 
-        const QVector<Waypoint *>& list = uas->getWaypointManager()->getWaypointEditableList();
+        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();
+    }
 
-        for (int i = 0; i < list.size(); i++) {
-            Waypoint* wp = list.at(i);
+    if (getNumChildren() > 0)
+    {
+        removeChild(0, getNumChildren());
+    }
 
-            double wpX, wpY, wpZ;
-            getPosition(wp, wpX, wpY, wpZ);
+    const QVector<Waypoint *>& list = uas->getWaypointManager()->getWaypointEditableList();
 
-            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));
+    for (int i = 0; i < list.size(); i++)
+    {
+        Waypoint* wp = list.at(i);
 
-            sd->setShape(cylinder);
-            sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
+        double wpX, wpY, wpZ;
+        getPosition(wp, wpX, wpY, wpZ);
+        double wpYaw = osg::DegreesToRadians(wp->getYaw());
 
-            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::Group> group = new osg::Group;
 
-            osg::ref_ptr<osg::Geode> geode = new osg::Geode;
-            geode->addDrawable(sd);
+        // cone indicates waypoint orientation
+        osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
+        double coneRadius = wp->getAcceptanceRadius() / 2.0;
+        osg::ref_ptr<osg::Cone> cone =
+            new osg::Cone(osg::Vec3d(wpZ, 0.0, 0.0),
+                          coneRadius, wp->getAcceptanceRadius() * 2.0);
 
-            char wpLabel[10];
-            sprintf(wpLabel, "wp%d", i);
-            geode->setName(wpLabel);
+        sd->setShape(cone);
+        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::Geode> geode = new osg::Geode;
+        geode->addDrawable(sd);
+
+        osg::ref_ptr<osg::PositionAttitudeTransform> pat =
+            new osg::PositionAttitudeTransform;
+        pat->addChild(geode);
+        pat->setAttitude(osg::Quat(wpYaw - M_PI_2, osg::Vec3d(1.0f, 0.0f, 0.0f),
+                                   M_PI_2, osg::Vec3d(0.0f, 1.0f, 0.0f),
+                                   0.0, osg::Vec3d(0.0f, 0.0f, 1.0f)));
+        group->addChild(pat);
+
+        // cylinder indicates waypoint position
+        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));
+
+        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::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 = new osg::Geode;
+        geode->addDrawable(sd);
+        group->addChild(geode);
 
-                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);
+        group->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);
         }
+
+        pat = new osg::PositionAttitudeTransform;
+        pat->setPosition(osg::Vec3d(wpY - robotY,
+                                    wpX - robotX,
+                                    robotZ));
+
+        addChild(pat);
+        pat->addChild(group);
     }
 }
 
 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 +200,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>
  *
  */
 

thirdParty/mavlink/include/mavlink_protobuf_manager.hpp

@@ -42,6 +42,18 @@ public:
 			registerType(msg);
 		}
 
+		// register ObstacleList
+		{
+			std::tr1::shared_ptr<px::ObstacleList> msg(new px::ObstacleList);
+			registerType(msg);
+		}
+
+		// register ObstacleMap
+		{
+			std::tr1::shared_ptr<px::ObstacleMap> msg(new px::ObstacleMap);
+			registerType(msg);
+		}
+
 		srand(time(NULL));
 		mStreamID = rand() + 1;
 	}
@@ -186,6 +198,11 @@ public:
 				if (offset == 0)
 				{
 					queue.push_back(msg);
+
+					if ((flags & 0x1) != 0x1)
+					{
+						reassemble = true;
+					}
 				}
 				else
 				{

thirdParty/mavlink/include/mavlink_types.h

@@ -15,10 +15,21 @@
 
 #define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length
 
+
+#if (defined __GNUC__) || (defined _MSC_VER) || (defined __MINGW32__) || (defined __WATCOMC__) || (defined __CMB__) || (defined __BORLANDC__) || (defined __clang__)
+#define MAVLINK_EXTENDED_MESSAGES_ENABLED
+#endif
+
+// EXTENDED message definition - the extended message set is compatible to standard MAVLink message passing
+// but does NOT have to be supported by the platform. The extended message set will NOT consume
+// any memory if the messages are not explicitely used
+
+#ifdef MAVLINK_EXTENDED_MESSAGES_ENABLED
 #define MAVLINK_MSG_ID_EXTENDED_MESSAGE 255
 #define MAVLINK_EXTENDED_HEADER_LEN 14
 #define MAVLINK_MAX_EXTENDED_PACKET_LEN 65507
 #define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES)
+#endif
 
 typedef struct param_union {
 	union {
@@ -51,12 +62,13 @@ typedef struct __mavlink_message {
 	uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
 } mavlink_message_t;
 
-
+#ifdef MAVLINK_EXTENDED_MESSAGES_ENABLED
 typedef struct __mavlink_extended_message {
        mavlink_message_t base_msg;
        int32_t extended_payload_len;   ///< Length of extended payload if any
        uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN];
 } mavlink_extended_message_t;
+#endif
 
 
 typedef enum {

thirdParty/mavlink/include/pixhawk/pixhawk.pb.h

@@ -37,6 +37,9 @@ class PointCloudXYZI_PointXYZI;
 class PointCloudXYZRGB;
 class PointCloudXYZRGB_PointXYZRGB;
 class RGBDImage;
+class Obstacle;
+class ObstacleList;
+class ObstacleMap;
 
 // ===================================================================
 
@@ -729,6 +732,409 @@ class RGBDImage : public ::google::protobuf::Message {
   void InitAsDefaultInstance();
   static RGBDImage* default_instance_;
 };
+// -------------------------------------------------------------------
+
+class Obstacle : public ::google::protobuf::Message {
+ public:
+  Obstacle();
+  virtual ~Obstacle();
+  
+  Obstacle(const Obstacle& from);
+  
+  inline Obstacle& operator=(const Obstacle& from) {
+    CopyFrom(from);
+    return *this;
+  }
+  
+  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
+    return _unknown_fields_;
+  }
+  
+  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
+    return &_unknown_fields_;
+  }
+  
+  static const ::google::protobuf::Descriptor* descriptor();
+  static const Obstacle& default_instance();
+  
+  void Swap(Obstacle* other);
+  
+  // implements Message ----------------------------------------------
+  
+  Obstacle* New() const;
+  void CopyFrom(const ::google::protobuf::Message& from);
+  void MergeFrom(const ::google::protobuf::Message& from);
+  void CopyFrom(const Obstacle& from);
+  void MergeFrom(const Obstacle& from);
+  void Clear();
+  bool IsInitialized() const;
+  
+  int ByteSize() const;
+  bool MergePartialFromCodedStream(
+      ::google::protobuf::io::CodedInputStream* input);
+  void SerializeWithCachedSizes(
+      ::google::protobuf::io::CodedOutputStream* output) const;
+  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
+  int GetCachedSize() const { return _cached_size_; }
+  private:
+  void SharedCtor();
+  void SharedDtor();
+  void SetCachedSize(int size) const;
+  public:
+  
+  ::google::protobuf::Metadata GetMetadata() const;
+  
+  // nested types ----------------------------------------------------
+  
+  // accessors -------------------------------------------------------
+  
+  // optional float x = 1;
+  inline bool has_x() const;
+  inline void clear_x();
+  static const int kXFieldNumber = 1;
+  inline float x() const;
+  inline void set_x(float value);
+  
+  // optional float y = 2;
+  inline bool has_y() const;
+  inline void clear_y();
+  static const int kYFieldNumber = 2;
+  inline float y() const;
+  inline void set_y(float value);
+  
+  // optional float z = 3;
+  inline bool has_z() const;
+  inline void clear_z();
+  static const int kZFieldNumber = 3;
+  inline float z() const;
+  inline void set_z(float value);
+  
+  // optional float length = 4;
+  inline bool has_length() const;
+  inline void clear_length();
+  static const int kLengthFieldNumber = 4;
+  inline float length() const;
+  inline void set_length(float value);
+  
+  // optional float width = 5;
+  inline bool has_width() const;
+  inline void clear_width();
+  static const int kWidthFieldNumber = 5;
+  inline float width() const;
+  inline void set_width(float value);
+  
+  // optional float height = 6;
+  inline bool has_height() const;
+  inline void clear_height();
+  static const int kHeightFieldNumber = 6;
+  inline float height() const;
+  inline void set_height(float value);
+  
+  // @@protoc_insertion_point(class_scope:px.Obstacle)
+ private:
+  inline void set_has_x();
+  inline void clear_has_x();
+  inline void set_has_y();
+  inline void clear_has_y();
+  inline void set_has_z();
+  inline void clear_has_z();
+  inline void set_has_length();
+  inline void clear_has_length();
+  inline void set_has_width();
+  inline void clear_has_width();
+  inline void set_has_height();
+  inline void clear_has_height();
+  
+  ::google::protobuf::UnknownFieldSet _unknown_fields_;
+  
+  float x_;
+  float y_;
+  float z_;
+  float length_;
+  float width_;
+  float height_;
+  
+  mutable int _cached_size_;
+  ::google::protobuf::uint32 _has_bits_[(6 + 31) / 32];
+  
+  friend void  protobuf_AddDesc_pixhawk_2eproto();
+  friend void protobuf_AssignDesc_pixhawk_2eproto();
+  friend void protobuf_ShutdownFile_pixhawk_2eproto();
+  
+  void InitAsDefaultInstance();
+  static Obstacle* default_instance_;
+};
+// -------------------------------------------------------------------
+
+class ObstacleList : public ::google::protobuf::Message {
+ public:
+  ObstacleList();
+  virtual ~ObstacleList();
+  
+  ObstacleList(const ObstacleList& from);
+  
+  inline ObstacleList& operator=(const ObstacleList& from) {
+    CopyFrom(from);
+    return *this;
+  }
+  
+  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
+    return _unknown_fields_;
+  }
+  
+  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
+    return &_unknown_fields_;
+  }
+  
+  static const ::google::protobuf::Descriptor* descriptor();
+  static const ObstacleList& default_instance();
+  
+  void Swap(ObstacleList* other);
+  
+  // implements Message ----------------------------------------------
+  
+  ObstacleList* New() const;
+  void CopyFrom(const ::google::protobuf::Message& from);
+  void MergeFrom(const ::google::protobuf::Message& from);
+  void CopyFrom(const ObstacleList& from);
+  void MergeFrom(const ObstacleList& from);
+  void Clear();
+  bool IsInitialized() const;
+  
+  int ByteSize() const;
+  bool MergePartialFromCodedStream(
+      ::google::protobuf::io::CodedInputStream* input);
+  void SerializeWithCachedSizes(
+      ::google::protobuf::io::CodedOutputStream* output) const;
+  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
+  int GetCachedSize() const { return _cached_size_; }
+  private:
+  void SharedCtor();
+  void SharedDtor();
+  void SetCachedSize(int size) const;
+  public:
+  
+  ::google::protobuf::Metadata GetMetadata() const;
+  
+  // nested types ----------------------------------------------------
+  
+  // accessors -------------------------------------------------------
+  
+  // optional uint64 utime = 1;
+  inline bool has_utime() const;
+  inline void clear_utime();
+  static const int kUtimeFieldNumber = 1;
+  inline ::google::protobuf::uint64 utime() const;
+  inline void set_utime(::google::protobuf::uint64 value);
+  
+  // repeated .px.Obstacle obstacles = 2;
+  inline int obstacles_size() const;
+  inline void clear_obstacles();
+  static const int kObstaclesFieldNumber = 2;
+  inline const ::px::Obstacle& obstacles(int index) const;
+  inline ::px::Obstacle* mutable_obstacles(int index);
+  inline ::px::Obstacle* add_obstacles();
+  inline const ::google::protobuf::RepeatedPtrField< ::px::Obstacle >&
+      obstacles() const;
+  inline ::google::protobuf::RepeatedPtrField< ::px::Obstacle >*
+      mutable_obstacles();
+  
+  // @@protoc_insertion_point(class_scope:px.ObstacleList)
+ private:
+  inline void set_has_utime();
+  inline void clear_has_utime();
+  
+  ::google::protobuf::UnknownFieldSet _unknown_fields_;
+  
+  ::google::protobuf::uint64 utime_;
+  ::google::protobuf::RepeatedPtrField< ::px::Obstacle > obstacles_;
+  
+  mutable int _cached_size_;
+  ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32];
+  
+  friend void  protobuf_AddDesc_pixhawk_2eproto();
+  friend void protobuf_AssignDesc_pixhawk_2eproto();
+  friend void protobuf_ShutdownFile_pixhawk_2eproto();
+  
+  void InitAsDefaultInstance();
+  static ObstacleList* default_instance_;
+};
+// -------------------------------------------------------------------
+
+class ObstacleMap : public ::google::protobuf::Message {
+ public:
+  ObstacleMap();
+  virtual ~ObstacleMap();
+  
+  ObstacleMap(const ObstacleMap& from);
+  
+  inline ObstacleMap& operator=(const ObstacleMap& from) {
+    CopyFrom(from);
+    return *this;
+  }
+  
+  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
+    return _unknown_fields_;
+  }
+  
+  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
+    return &_unknown_fields_;
+  }
+  
+  static const ::google::protobuf::Descriptor* descriptor();
+  static const ObstacleMap& default_instance();
+  
+  void Swap(ObstacleMap* other);
+  
+  // implements Message ----------------------------------------------
+  
+  ObstacleMap* New() const;
+  void CopyFrom(const ::google::protobuf::Message& from);
+  void MergeFrom(const ::google::protobuf::Message& from);
+  void CopyFrom(const ObstacleMap& from);
+  void MergeFrom(const ObstacleMap& from);
+  void Clear();
+  bool IsInitialized() const;
+  
+  int ByteSize() const;
+  bool MergePartialFromCodedStream(
+      ::google::protobuf::io::CodedInputStream* input);
+  void SerializeWithCachedSizes(
+      ::google::protobuf::io::CodedOutputStream* output) const;
+  ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
+  int GetCachedSize() const { return _cached_size_; }
+  private:
+  void SharedCtor();
+  void SharedDtor();
+  void SetCachedSize(int size) const;
+  public:
+  
+  ::google::protobuf::Metadata GetMetadata() const;
+  
+  // nested types ----------------------------------------------------
+  
+  // accessors -------------------------------------------------------
+  
+  // required uint64 utime = 1;
+  inline bool has_utime() const;
+  inline void clear_utime();
+  static const int kUtimeFieldNumber = 1;
+  inline ::google::protobuf::uint64 utime() const;
+  inline void set_utime(::google::protobuf::uint64 value);
+  
+  // required int32 type = 2;
+  inline bool has_type() const;
+  inline void clear_type();
+  static const int kTypeFieldNumber = 2;
+  inline ::google::protobuf::int32 type() const;
+  inline void set_type(::google::protobuf::int32 value);
+  
+  // optional float resolution = 3;
+  inline bool has_resolution() const;
+  inline void clear_resolution();
+  static const int kResolutionFieldNumber = 3;
+  inline float resolution() const;
+  inline void set_resolution(float value);
+  
+  // optional int32 rows = 4;
+  inline bool has_rows() const;
+  inline void clear_rows();
+  static const int kRowsFieldNumber = 4;
+  inline ::google::protobuf::int32 rows() const;
+  inline void set_rows(::google::protobuf::int32 value);
+  
+  // optional int32 cols = 5;
+  inline bool has_cols() const;
+  inline void clear_cols();
+  static const int kColsFieldNumber = 5;
+  inline ::google::protobuf::int32 cols() const;
+  inline void set_cols(::google::protobuf::int32 value);
+  
+  // optional int32 mapR0 = 6;
+  inline bool has_mapr0() const;
+  inline void clear_mapr0();
+  static const int kMapR0FieldNumber = 6;
+  inline ::google::protobuf::int32 mapr0() const;
+  inline void set_mapr0(::google::protobuf::int32 value);
+  
+  // optional int32 mapC0 = 7;
+  inline bool has_mapc0() const;
+  inline void clear_mapc0();
+  static const int kMapC0FieldNumber = 7;
+  inline ::google::protobuf::int32 mapc0() const;
+  inline void set_mapc0(::google::protobuf::int32 value);
+  
+  // optional int32 arrayR0 = 8;
+  inline bool has_arrayr0() const;
+  inline void clear_arrayr0();
+  static const int kArrayR0FieldNumber = 8;
+  inline ::google::protobuf::int32 arrayr0() const;
+  inline void set_arrayr0(::google::protobuf::int32 value);
+  
+  // optional int32 arrayC0 = 9;
+  inline bool has_arrayc0() const;
+  inline void clear_arrayc0();
+  static const int kArrayC0FieldNumber = 9;
+  inline ::google::protobuf::int32 arrayc0() const;
+  inline void set_arrayc0(::google::protobuf::int32 value);
+  
+  // optional bytes data = 10;
+  inline bool has_data() const;
+  inline void clear_data();
+  static const int kDataFieldNumber = 10;
+  inline const ::std::string& data() const;
+  inline void set_data(const ::std::string& value);
+  inline void set_data(const char* value);
+  inline void set_data(const void* value, size_t size);
+  inline ::std::string* mutable_data();
+  inline ::std::string* release_data();
+  
+  // @@protoc_insertion_point(class_scope:px.ObstacleMap)
+ private:
+  inline void set_has_utime();
+  inline void clear_has_utime();
+  inline void set_has_type();
+  inline void clear_has_type();
+  inline void set_has_resolution();
+  inline void clear_has_resolution();
+  inline void set_has_rows();
+  inline void clear_has_rows();
+  inline void set_has_cols();
+  inline void clear_has_cols();
+  inline void set_has_mapr0();
+  inline void clear_has_mapr0();
+  inline void set_has_mapc0();
+  inline void clear_has_mapc0();
+  inline void set_has_arrayr0();
+  inline void clear_has_arrayr0();
+  inline void set_has_arrayc0();
+  inline void clear_has_arrayc0();
+  inline void set_has_data();
+  inline void clear_has_data();
+  
+  ::google::protobuf::UnknownFieldSet _unknown_fields_;
+  
+  ::google::protobuf::uint64 utime_;
+  ::google::protobuf::int32 type_;
+  float resolution_;
+  ::google::protobuf::int32 rows_;
+  ::google::protobuf::int32 cols_;
+  ::google::protobuf::int32 mapr0_;
+  ::google::protobuf::int32 mapc0_;
+  ::google::protobuf::int32 arrayr0_;
+  ::google::protobuf::int32 arrayc0_;
+  ::std::string* data_;
+  
+  mutable int _cached_size_;
+  ::google::protobuf::uint32 _has_bits_[(10 + 31) / 32];
+  
+  friend void  protobuf_AddDesc_pixhawk_2eproto();
+  friend void protobuf_AssignDesc_pixhawk_2eproto();
+  friend void protobuf_ShutdownFile_pixhawk_2eproto();
+  
+  void InitAsDefaultInstance();
+  static ObstacleMap* default_instance_;
+};
 // ===================================================================
 
 
@@ -1515,6 +1921,453 @@ RGBDImage::mutable_camera_matrix() {
   return &camera_matrix_;
 }
 
+// -------------------------------------------------------------------
+
+// Obstacle
+
+// optional float x = 1;
+inline bool Obstacle::has_x() const {
+  return (_has_bits_[0] & 0x00000001u) != 0;
+}
+inline void Obstacle::set_has_x() {
+  _has_bits_[0] |= 0x00000001u;
+}
+inline void Obstacle::clear_has_x() {
+  _has_bits_[0] &= ~0x00000001u;
+}
+inline void Obstacle::clear_x() {
+  x_ = 0;
+  clear_has_x();
+}
+inline float Obstacle::x() const {
+  return x_;
+}
+inline void Obstacle::set_x(float value) {
+  set_has_x();
+  x_ = value;
+}
+
+// optional float y = 2;
+inline bool Obstacle::has_y() const {
+  return (_has_bits_[0] & 0x00000002u) != 0;
+}
+inline void Obstacle::set_has_y() {
+  _has_bits_[0] |= 0x00000002u;
+}
+inline void Obstacle::clear_has_y() {
+  _has_bits_[0] &= ~0x00000002u;
+}
+inline void Obstacle::clear_y() {
+  y_ = 0;
+  clear_has_y();
+}
+inline float Obstacle::y() const {
+  return y_;
+}
+inline void Obstacle::set_y(float value) {
+  set_has_y();
+  y_ = value;
+}
+
+// optional float z = 3;
+inline bool Obstacle::has_z() const {
+  return (_has_bits_[0] & 0x00000004u) != 0;
+}
+inline void Obstacle::set_has_z() {
+  _has_bits_[0] |= 0x00000004u;
+}
+inline void Obstacle::clear_has_z() {
+  _has_bits_[0] &= ~0x00000004u;
+}
+inline void Obstacle::clear_z() {
+  z_ = 0;
+  clear_has_z();
+}
+inline float Obstacle::z() const {
+  return z_;
+}
+inline void Obstacle::set_z(float value) {
+  set_has_z();
+  z_ = value;
+}
+
+// optional float length = 4;
+inline bool Obstacle::has_length() const {
+  return (_has_bits_[0] & 0x00000008u) != 0;
+}
+inline void Obstacle::set_has_length() {
+  _has_bits_[0] |= 0x00000008u;
+}
+inline void Obstacle::clear_has_length() {
+  _has_bits_[0] &= ~0x00000008u;
+}
+inline void Obstacle::clear_length() {
+  length_ = 0;
+  clear_has_length();
+}
+inline float Obstacle::length() const {
+  return length_;
+}
+inline void Obstacle::set_length(float value) {
+  set_has_length();
+  length_ = value;
+}
+
+// optional float width = 5;
+inline bool Obstacle::has_width() const {
+  return (_has_bits_[0] & 0x00000010u) != 0;
+}
+inline void Obstacle::set_has_width() {
+  _has_bits_[0] |= 0x00000010u;
+}
+inline void Obstacle::clear_has_width() {
+  _has_bits_[0] &= ~0x00000010u;
+}
+inline void Obstacle::clear_width() {
+  width_ = 0;
+  clear_has_width();
+}
+inline float Obstacle::width() const {
+  return width_;
+}
+inline void Obstacle::set_width(float value) {
+  set_has_width();
+  width_ = value;
+}
+
+// optional float height = 6;
+inline bool Obstacle::has_height() const {
+  return (_has_bits_[0] & 0x00000020u) != 0;
+}
+inline void Obstacle::set_has_height() {
+  _has_bits_[0] |= 0x00000020u;
+}
+inline void Obstacle::clear_has_height() {
+  _has_bits_[0] &= ~0x00000020u;
+}
+inline void Obstacle::clear_height() {
+  height_ = 0;
+  clear_has_height();
+}
+inline float Obstacle::height() const {
+  return height_;
+}
+inline void Obstacle::set_height(float value) {
+  set_has_height();
+  height_ = value;
+}
+
+// -------------------------------------------------------------------
+
+// ObstacleList
+
+// optional uint64 utime = 1;
+inline bool ObstacleList::has_utime() const {
+  return (_has_bits_[0] & 0x00000001u) != 0;
+}
+inline void ObstacleList::set_has_utime() {
+  _has_bits_[0] |= 0x00000001u;
+}
+inline void ObstacleList::clear_has_utime() {
+  _has_bits_[0] &= ~0x00000001u;
+}
+inline void ObstacleList::clear_utime() {
+  utime_ = GOOGLE_ULONGLONG(0);
+  clear_has_utime();
+}
+inline ::google::protobuf::uint64 ObstacleList::utime() const {
+  return utime_;
+}
+inline void ObstacleList::set_utime(::google::protobuf::uint64 value) {
+  set_has_utime();
+  utime_ = value;
+}
+
+// repeated .px.Obstacle obstacles = 2;
+inline int ObstacleList::obstacles_size() const {
+  return obstacles_.size();
+}
+inline void ObstacleList::clear_obstacles() {
+  obstacles_.Clear();
+}
+inline const ::px::Obstacle& ObstacleList::obstacles(int index) const {
+  return obstacles_.Get(index);
+}
+inline ::px::Obstacle* ObstacleList::mutable_obstacles(int index) {
+  return obstacles_.Mutable(index);
+}
+inline ::px::Obstacle* ObstacleList::add_obstacles() {
+  return obstacles_.Add();
+}
+inline const ::google::protobuf::RepeatedPtrField< ::px::Obstacle >&
+ObstacleList::obstacles() const {
+  return obstacles_;
+}
+inline ::google::protobuf::RepeatedPtrField< ::px::Obstacle >*
+ObstacleList::mutable_obstacles() {
+  return &obstacles_;
+}
+
+// -------------------------------------------------------------------
+
+// ObstacleMap
+
+// required uint64 utime = 1;
+inline bool ObstacleMap::has_utime() const {
+  return (_has_bits_[0] & 0x00000001u) != 0;
+}
+inline void ObstacleMap::set_has_utime() {
+  _has_bits_[0] |= 0x00000001u;
+}
+inline void ObstacleMap::clear_has_utime() {
+  _has_bits_[0] &= ~0x00000001u;
+}
+inline void ObstacleMap::clear_utime() {
+  utime_ = GOOGLE_ULONGLONG(0);
+  clear_has_utime();
+}
+inline ::google::protobuf::uint64 ObstacleMap::utime() const {
+  return utime_;
+}
+inline void ObstacleMap::set_utime(::google::protobuf::uint64 value) {
+  set_has_utime();
+  utime_ = value;
+}
+
+// required int32 type = 2;
+inline bool ObstacleMap::has_type() const {
+  return (_has_bits_[0] & 0x00000002u) != 0;
+}
+inline void ObstacleMap::set_has_type() {
+  _has_bits_[0] |= 0x00000002u;
+}
+inline void ObstacleMap::clear_has_type() {
+  _has_bits_[0] &= ~0x00000002u;
+}
+inline void ObstacleMap::clear_type() {
+  type_ = 0;
+  clear_has_type();
+}
+inline ::google::protobuf::int32 ObstacleMap::type() const {
+  return type_;
+}
+inline void ObstacleMap::set_type(::google::protobuf::int32 value) {
+  set_has_type();
+  type_ = value;
+}
+
+// optional float resolution = 3;
+inline bool ObstacleMap::has_resolution() const {
+  return (_has_bits_[0] & 0x00000004u) != 0;
+}
+inline void ObstacleMap::set_has_resolution() {
+  _has_bits_[0] |= 0x00000004u;
+}
+inline void ObstacleMap::clear_has_resolution() {
+  _has_bits_[0] &= ~0x00000004u;
+}
+inline void ObstacleMap::clear_resolution() {
+  resolution_ = 0;
+  clear_has_resolution();
+}
+inline float ObstacleMap::resolution() const {
+  return resolution_;
+}
+inline void ObstacleMap::set_resolution(float value) {
+  set_has_resolution();
+  resolution_ = value;
+}
+
+// optional int32 rows = 4;
+inline bool ObstacleMap::has_rows() const {
+  return (_has_bits_[0] & 0x00000008u) != 0;
+}
+inline void ObstacleMap::set_has_rows() {
+  _has_bits_[0] |= 0x00000008u;
+}
+inline void ObstacleMap::clear_has_rows() {
+  _has_bits_[0] &= ~0x00000008u;
+}
+inline void ObstacleMap::clear_rows() {
+  rows_ = 0;
+  clear_has_rows();
+}
+inline ::google::protobuf::int32 ObstacleMap::rows() const {
+  return rows_;
+}
+inline void ObstacleMap::set_rows(::google::protobuf::int32 value) {
+  set_has_rows();
+  rows_ = value;
+}
+
+// optional int32 cols = 5;
+inline bool ObstacleMap::has_cols() const {
+  return (_has_bits_[0] & 0x00000010u) != 0;
+}
+inline void ObstacleMap::set_has_cols() {
+  _has_bits_[0] |= 0x00000010u;
+}
+inline void ObstacleMap::clear_has_cols() {
+  _has_bits_[0] &= ~0x00000010u;
+}
+inline void ObstacleMap::clear_cols() {
+  cols_ = 0;
+  clear_has_cols();
+}
+inline ::google::protobuf::int32 ObstacleMap::cols() const {
+  return cols_;
+}
+inline void ObstacleMap::set_cols(::google::protobuf::int32 value) {
+  set_has_cols();
+  cols_ = value;
+}
+
+// optional int32 mapR0 = 6;
+inline bool ObstacleMap::has_mapr0() const {
+  return (_has_bits_[0] & 0x00000020u) != 0;
+}
+inline void ObstacleMap::set_has_mapr0() {
+  _has_bits_[0] |= 0x00000020u;
+}
+inline void ObstacleMap::clear_has_mapr0() {
+  _has_bits_[0] &= ~0x00000020u;
+}
+inline void ObstacleMap::clear_mapr0() {
+  mapr0_ = 0;
+  clear_has_mapr0();
+}
+inline ::google::protobuf::int32 ObstacleMap::mapr0() const {
+  return mapr0_;
+}
+inline void ObstacleMap::set_mapr0(::google::protobuf::int32 value) {
+  set_has_mapr0();
+  mapr0_ = value;
+}
+
+// optional int32 mapC0 = 7;
+inline bool ObstacleMap::has_mapc0() const {
+  return (_has_bits_[0] & 0x00000040u) != 0;
+}
+inline void ObstacleMap::set_has_mapc0() {
+  _has_bits_[0] |= 0x00000040u;
+}
+inline void ObstacleMap::clear_has_mapc0() {
+  _has_bits_[0] &= ~0x00000040u;
+}
+inline void ObstacleMap::clear_mapc0() {
+  mapc0_ = 0;
+  clear_has_mapc0();
+}
+inline ::google::protobuf::int32 ObstacleMap::mapc0() const {
+  return mapc0_;
+}
+inline void ObstacleMap::set_mapc0(::google::protobuf::int32 value) {
+  set_has_mapc0();
+  mapc0_ = value;
+}
+
+// optional int32 arrayR0 = 8;
+inline bool ObstacleMap::has_arrayr0() const {
+  return (_has_bits_[0] & 0x00000080u) != 0;
+}
+inline void ObstacleMap::set_has_arrayr0() {
+  _has_bits_[0] |= 0x00000080u;
+}
+inline void ObstacleMap::clear_has_arrayr0() {
+  _has_bits_[0] &= ~0x00000080u;
+}
+inline void ObstacleMap::clear_arrayr0() {
+  arrayr0_ = 0;
+  clear_has_arrayr0();
+}
+inline ::google::protobuf::int32 ObstacleMap::arrayr0() const {
+  return arrayr0_;
+}
+inline void ObstacleMap::set_arrayr0(::google::protobuf::int32 value) {
+  set_has_arrayr0();
+  arrayr0_ = value;
+}
+
+// optional int32 arrayC0 = 9;
+inline bool ObstacleMap::has_arrayc0() const {
+  return (_has_bits_[0] & 0x00000100u) != 0;
+}
+inline void ObstacleMap::set_has_arrayc0() {
+  _has_bits_[0] |= 0x00000100u;
+}
+inline void ObstacleMap::clear_has_arrayc0() {
+  _has_bits_[0] &= ~0x00000100u;
+}
+inline void ObstacleMap::clear_arrayc0() {
+  arrayc0_ = 0;
+  clear_has_arrayc0();
+}
+inline ::google::protobuf::int32 ObstacleMap::arrayc0() const {
+  return arrayc0_;
+}
+inline void ObstacleMap::set_arrayc0(::google::protobuf::int32 value) {
+  set_has_arrayc0();
+  arrayc0_ = value;
+}
+
+// optional bytes data = 10;
+inline bool ObstacleMap::has_data() const {
+  return (_has_bits_[0] & 0x00000200u) != 0;
+}
+inline void ObstacleMap::set_has_data() {
+  _has_bits_[0] |= 0x00000200u;
+}
+inline void ObstacleMap::clear_has_data() {
+  _has_bits_[0] &= ~0x00000200u;
+}
+inline void ObstacleMap::clear_data() {
+  if (data_ != &::google::protobuf::internal::kEmptyString) {
+    data_->clear();
+  }
+  clear_has_data();
+}
+inline const ::std::string& ObstacleMap::data() const {
+  return *data_;
+}
+inline void ObstacleMap::set_data(const ::std::string& value) {
+  set_has_data();
+  if (data_ == &::google::protobuf::internal::kEmptyString) {
+    data_ = new ::std::string;
+  }
+  data_->assign(value);
+}
+inline void ObstacleMap::set_data(const char* value) {
+  set_has_data();
+  if (data_ == &::google::protobuf::internal::kEmptyString) {
+    data_ = new ::std::string;
+  }
+  data_->assign(value);
+}
+inline void ObstacleMap::set_data(const void* value, size_t size) {
+  set_has_data();
+  if (data_ == &::google::protobuf::internal::kEmptyString) {
+    data_ = new ::std::string;
+  }
+  data_->assign(reinterpret_cast<const char*>(value), size);
+}
+inline ::std::string* ObstacleMap::mutable_data() {
+  set_has_data();
+  if (data_ == &::google::protobuf::internal::kEmptyString) {
+    data_ = new ::std::string;
+  }
+  return data_;
+}
+inline ::std::string* ObstacleMap::release_data() {
+  clear_has_data();
+  if (data_ == &::google::protobuf::internal::kEmptyString) {
+    return NULL;
+  } else {
+    ::std::string* temp = data_;
+    data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
+    return temp;
+  }
+}
+
 
 // @@protoc_insertion_point(namespace_scope)