diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index 480323272401b5d6dcdc995bbeaee96a9b670e7f..746e28476ec9868574d8ed0abecced48f8fcadd3 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -364,7 +364,8 @@ HEADERS += src/MG.h \
     src/ui/firmwareupdate/QGCFirmwareUpdateWidget.h \
     src/ui/QGCPluginHost.h \
     src/ui/firmwareupdate/QGCPX4FirmwareUpdate.h \
-    src/ui/map3D/TerrainParamDialog.h
+    src/ui/map3D/TerrainParamDialog.h \
+    src/ui/map3D/GLOverlayGeode.h
 
 # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler
 macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010::HEADERS += src/ui/map3D/QGCGoogleEarthView.h
@@ -505,7 +506,8 @@ SOURCES += src/main.cc \
     src/ui/firmwareupdate/QGCFirmwareUpdateWidget.cc \
     src/ui/QGCPluginHost.cc \
     src/ui/firmwareupdate/QGCPX4FirmwareUpdate.cc \
-    src/ui/map3D/TerrainParamDialog.cc
+    src/ui/map3D/TerrainParamDialog.cc \
+    src/ui/map3D/GLOverlayGeode.cc
 
 # Enable Google Earth only on Mac OS and Windows with Visual Studio compiler
 macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010::SOURCES += src/ui/map3D/QGCGoogleEarthView.cc
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index b08f0868221ebdbe75e276265d80bce0d90d27a0..ad8decc5ae7a018362b25ba4faca74d350bf46c0 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -76,10 +76,11 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
     yaw(0.0),
     statusTimeout(new QTimer(this)),
     #if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
-    receivedPointCloudTimestamp(0.0),
-    receivedRGBDImageTimestamp(0.0),
+    receivedOverlayTimestamp(0.0),
     receivedObstacleListTimestamp(0.0),
     receivedPathTimestamp(0.0),
+    receivedPointCloudTimestamp(0.0),
+    receivedRGBDImageTimestamp(0.0),
     #endif
     paramsOnceRequested(false),
     airframe(QGC_AIRFRAME_EASYSTAR),
@@ -1125,21 +1126,13 @@ void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<googl
     }
 
 #ifdef QGC_USE_PIXHAWK_MESSAGES
-    if (message->GetTypeName() == pointCloud.GetTypeName())
+    if (message->GetTypeName() == overlay.GetTypeName())
     {
-        receivedPointCloudTimestamp = QGC::groundTimeSeconds();
-        pointCloudMutex.lock();
-        pointCloud.CopyFrom(*message);
-        pointCloudMutex.unlock();
-        emit pointCloudChanged(this);
-    }
-    else if (message->GetTypeName() == rgbdImage.GetTypeName())
-    {
-        receivedRGBDImageTimestamp = QGC::groundTimeSeconds();
-        rgbdImageMutex.lock();
-        rgbdImage.CopyFrom(*message);
-        rgbdImageMutex.unlock();
-        emit rgbdImageChanged(this);
+        receivedOverlayTimestamp = QGC::groundTimeSeconds();
+        overlayMutex.lock();
+        overlay.CopyFrom(*message);
+        overlayMutex.unlock();
+        emit overlayChanged(this);
     }
     else if (message->GetTypeName() == obstacleList.GetTypeName())
     {
@@ -1157,6 +1150,22 @@ void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<googl
         pathMutex.unlock();
         emit pathChanged(this);
     }
+    else if (message->GetTypeName() == pointCloud.GetTypeName())
+    {
+        receivedPointCloudTimestamp = QGC::groundTimeSeconds();
+        pointCloudMutex.lock();
+        pointCloud.CopyFrom(*message);
+        pointCloudMutex.unlock();
+        emit pointCloudChanged(this);
+    }
+    else if (message->GetTypeName() == rgbdImage.GetTypeName())
+    {
+        receivedRGBDImageTimestamp = QGC::groundTimeSeconds();
+        rgbdImageMutex.lock();
+        rgbdImage.CopyFrom(*message);
+        rgbdImageMutex.unlock();
+        emit rgbdImageChanged(this);
+    }
 #endif
 }
 
@@ -2329,9 +2338,7 @@ void UAS::shutdown()
 void UAS::setTargetPosition(float x, float y, float z, float yaw)
 {
     mavlink_message_t msg;
-    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_PATHPLANNING, 1, 1, 0, 0, yaw, x, y, z);
-    sendMessage(msg);
-    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_PATHPLANNING, 1, 0, 1, 0, yaw, x, y, z);
+    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_PATHPLANNING, 1, 1, 1, 0, yaw, x, y, z);
     sendMessage(msg);
 }
 
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index d6284d005c763795ff79639a5f76840f83f396eb..2cbdb486bbc5102fa7631d612c879b42d8391be9 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -135,26 +135,15 @@ public:
     bool getSelected() const;
 
 #if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
-    px::PointCloudXYZRGB getPointCloud() {
-        QMutexLocker locker(&pointCloudMutex);
-        return pointCloud;
+    px::GLOverlay getOverlay() {
+        QMutexLocker locker(&overlayMutex);
+        return overlay;
     }
 
-    px::PointCloudXYZRGB getPointCloud(qreal& receivedTimestamp) {
-        receivedTimestamp = receivedPointCloudTimestamp;
-        QMutexLocker locker(&pointCloudMutex);
-        return pointCloud;
-    }
-
-    px::RGBDImage getRGBDImage() {
-        QMutexLocker locker(&rgbdImageMutex);
-        return rgbdImage;
-    }
-
-    px::RGBDImage getRGBDImage(qreal& receivedTimestamp) {
-        receivedTimestamp = receivedRGBDImageTimestamp;
-        QMutexLocker locker(&rgbdImageMutex);
-        return rgbdImage;
+    px::GLOverlay getOverlay(qreal& receivedTimestamp) {
+        receivedTimestamp = receivedOverlayTimestamp;
+        QMutexLocker locker(&overlayMutex);
+        return overlay;
     }
 
     px::ObstacleList getObstacleList() {
@@ -178,6 +167,28 @@ public:
         QMutexLocker locker(&pathMutex);
         return path;
     }
+
+    px::PointCloudXYZRGB getPointCloud() {
+        QMutexLocker locker(&pointCloudMutex);
+        return pointCloud;
+    }
+
+    px::PointCloudXYZRGB getPointCloud(qreal& receivedTimestamp) {
+        receivedTimestamp = receivedPointCloudTimestamp;
+        QMutexLocker locker(&pointCloudMutex);
+        return pointCloud;
+    }
+
+    px::RGBDImage getRGBDImage() {
+        QMutexLocker locker(&rgbdImageMutex);
+        return rgbdImage;
+    }
+
+    px::RGBDImage getRGBDImage(qreal& receivedTimestamp) {
+        receivedTimestamp = receivedRGBDImageTimestamp;
+        QMutexLocker locker(&rgbdImageMutex);
+        return rgbdImage;
+    }
 #endif
 
     friend class UASWaypointManager;
@@ -265,13 +276,9 @@ protected: //COMMENTS FOR TEST UNIT
     quint64 imageStart;
 
 #if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
-    px::PointCloudXYZRGB pointCloud;
-    QMutex pointCloudMutex;
-    qreal receivedPointCloudTimestamp;
-
-    px::RGBDImage rgbdImage;
-    QMutex rgbdImageMutex;
-    qreal receivedRGBDImageTimestamp;
+    px::GLOverlay overlay;
+    QMutex overlayMutex;
+    qreal receivedOverlayTimestamp;
 
     px::ObstacleList obstacleList;
     QMutex obstacleListMutex;
@@ -280,6 +287,14 @@ protected: //COMMENTS FOR TEST UNIT
     px::Path path;
     QMutex pathMutex;
     qreal receivedPathTimestamp;
+
+    px::PointCloudXYZRGB pointCloud;
+    QMutex pointCloudMutex;
+    qreal receivedPointCloudTimestamp;
+
+    px::RGBDImage rgbdImage;
+    QMutex rgbdImageMutex;
+    qreal receivedRGBDImageTimestamp;
 #endif
 
     QMap<int, QMap<QString, QVariant>* > parameters; ///< All parameters
@@ -611,16 +626,6 @@ signals:
     void imageStarted(quint64 timestamp);
     /** @brief A new camera image has arrived */
     void imageReady(UASInterface* uas);
-#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
-    /** @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);
-    /** @brief Path data has been changed */
-    void pathChanged(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);
 
diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h
index 4aab4d3c9b103335655239591f81d10f629ef9ad..cb083ab95f165aecc683a30894b90bf25fcf45da 100644
--- a/src/uas/UASInterface.h
+++ b/src/uas/UASInterface.h
@@ -97,14 +97,16 @@ public:
     virtual bool getSelected() const = 0;
 
 #if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
-    virtual px::PointCloudXYZRGB getPointCloud() = 0;
-    virtual px::PointCloudXYZRGB getPointCloud(qreal& receivedTimestamp) = 0;
-    virtual px::RGBDImage getRGBDImage() = 0;
-    virtual px::RGBDImage getRGBDImage(qreal& receivedTimestamp) = 0;
+    virtual px::GLOverlay getOverlay() = 0;
+    virtual px::GLOverlay getOverlay(qreal& receivedTimestamp) = 0;
     virtual px::ObstacleList getObstacleList() = 0;
     virtual px::ObstacleList getObstacleList(qreal& receivedTimestamp) = 0;
     virtual px::Path getPath() = 0;
     virtual px::Path getPath(qreal& receivedTimestamp) = 0;
+    virtual px::PointCloudXYZRGB getPointCloud() = 0;
+    virtual px::PointCloudXYZRGB getPointCloud(qreal& receivedTimestamp) = 0;
+    virtual px::RGBDImage getRGBDImage() = 0;
+    virtual px::RGBDImage getRGBDImage(qreal& receivedTimestamp) = 0;
 #endif
 
     virtual bool isArmed() const = 0;
@@ -489,6 +491,19 @@ signals:
     /** @brief Radio Calibration Data has been received from the MAV*/
     void radioCalibrationReceived(const QPointer<RadioCalibrationData>&);
 
+#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
+    /** @brief Overlay data has been changed */
+    void overlayChanged(UASInterface* uas);
+    /** @brief Obstacle list data has been changed */
+    void obstacleListChanged(UASInterface* uas);
+    /** @brief Path data has been changed */
+    void pathChanged(UASInterface* uas);
+    /** @brief Point cloud data has been changed */
+    void pointCloudChanged(UASInterface* uas);
+    /** @brief RGBD image data has been changed */
+    void rgbdImageChanged(UASInterface* uas);
+#endif
+
     /**
      * @brief Localization quality changed
      * @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization
diff --git a/src/ui/map3D/GLOverlayGeode.cc b/src/ui/map3D/GLOverlayGeode.cc
new file mode 100644
index 0000000000000000000000000000000000000000..88aea6eedb987cbe09b861d1d618c6d6465160e9
--- /dev/null
+++ b/src/ui/map3D/GLOverlayGeode.cc
@@ -0,0 +1,410 @@
+#include "GLOverlayGeode.h"
+
+GLOverlayGeode::GLOverlayGeode()
+ : mDrawable()
+{
+
+}
+
+void
+GLOverlayGeode::setOverlay(px::GLOverlay &overlay)
+{
+    mDrawable.setOverlay(overlay);
+
+    dirtyBound();
+}
+
+px::GLOverlay::CoordinateFrameType
+GLOverlayGeode::coordinateFrameType(void) const
+{
+    return mCoordinateFrameType;
+}
+
+GLOverlayGeode::GLOverlayDrawable::GLOverlayDrawable()
+{
+    setUseDisplayList(true);
+}
+
+GLOverlayGeode::GLOverlayDrawable::GLOverlayDrawable(const GLOverlayDrawable& drawable,
+                                                     const osg::CopyOp& copyop)
+ : osg::Drawable(drawable,copyop)
+{
+    setUseDisplayList(true);
+}
+
+void
+GLOverlayGeode::GLOverlayDrawable::setOverlay(px::GLOverlay &overlay)
+{
+    if (!overlay.IsInitialized())
+    {
+        return;
+    }
+
+    mOverlay = overlay;
+
+    mBBox.init();
+
+    const std::string& data = mOverlay.data();
+
+    for (size_t i = 0; i < data.size(); ++i)
+    {
+        switch (data.at(i)) {
+        case px::GLOverlay::POINTS:
+            break;
+        case px::GLOverlay::LINES:
+            break;
+        case px::GLOverlay::LINE_STRIP:
+            break;
+        case px::GLOverlay::LINE_LOOP:
+            break;
+        case px::GLOverlay::TRIANGLES:
+            break;
+        case px::GLOverlay::TRIANGLE_STRIP:
+            break;
+        case px::GLOverlay::TRIANGLE_FAN:
+            break;
+        case px::GLOverlay::QUADS:
+            break;
+        case px::GLOverlay::QUAD_STRIP:
+            break;
+        case px::GLOverlay::POLYGON:
+            break;
+        case px::GLOverlay::WIRE_CIRCLE:
+            i += sizeof(float) * 4;
+            break;
+        case px::GLOverlay::SOLID_CIRCLE:
+            i += sizeof(float) * 4;
+            break;
+        case px::GLOverlay::SOLID_CUBE:
+            i += sizeof(float) * 5;
+            break;
+        case px::GLOverlay::WIRE_CUBE:
+            i += sizeof(float) * 5;
+            break;
+        case px::GLOverlay::END:
+            break;
+        case px::GLOverlay::VERTEX2F:
+            i += sizeof(float) * 2;
+            break;
+        case px::GLOverlay::VERTEX3F:
+        {
+            float x = getFloatValue(data, i);
+            float y = getFloatValue(data, i);
+            float z = getFloatValue(data, i);
+
+            mBBox.expandBy(x, y, z);
+        }
+            break;
+        case px::GLOverlay::ROTATEF:
+            i += sizeof(float) * 4;
+            break;
+        case px::GLOverlay::TRANSLATEF:
+            i += sizeof(float) * 3;
+            break;
+        case px::GLOverlay::SCALEF:
+            i += sizeof(float) * 3;
+            break;
+        case px::GLOverlay::PUSH_MATRIX:
+            break;
+        case px::GLOverlay::POP_MATRIX:
+            break;
+        case px::GLOverlay::COLOR3F:
+            i += sizeof(float) * 3;
+            break;
+        case px::GLOverlay::COLOR4F:
+            i += sizeof(float) * 4;
+            break;
+        case px::GLOverlay::POINTSIZE:
+            i += sizeof(float);
+            break;
+        case px::GLOverlay::LINEWIDTH:
+            i += sizeof(float);
+            break;
+        }
+    }
+
+    dirtyDisplayList();
+}
+
+void
+GLOverlayGeode::GLOverlayDrawable::drawImplementation(osg::RenderInfo&) const
+{
+    if (!mOverlay.IsInitialized())
+    {
+        return;
+    }
+
+    glPushMatrix();
+
+    const std::string& data = mOverlay.data();
+
+    for (size_t i = 0; i < data.size(); ++i)
+    {
+        switch (data.at(i)) {
+        case px::GLOverlay::POINTS:
+            glBegin(GL_POINTS);
+            break;
+        case px::GLOverlay::LINES:
+            glBegin(GL_LINES);
+            break;
+        case px::GLOverlay::LINE_STRIP:
+            glBegin(GL_LINE_STRIP);
+            break;
+        case px::GLOverlay::LINE_LOOP:
+            glBegin(GL_LINE_LOOP);
+            break;
+        case px::GLOverlay::TRIANGLES:
+            glBegin(GL_TRIANGLES);
+            break;
+        case px::GLOverlay::TRIANGLE_STRIP:
+            glBegin(GL_TRIANGLE_STRIP);
+            break;
+        case px::GLOverlay::TRIANGLE_FAN:
+            glBegin(GL_TRIANGLE_FAN);
+            break;
+        case px::GLOverlay::QUADS:
+            glBegin(GL_QUADS);
+            break;
+        case px::GLOverlay::QUAD_STRIP:
+            glBegin(GL_QUAD_STRIP);
+            break;
+        case px::GLOverlay::POLYGON:
+            glBegin(GL_POLYGON);
+            break;
+        case px::GLOverlay::WIRE_CIRCLE:
+        {
+            float x = getFloatValue(data, i);
+            float y = getFloatValue(data, i);
+            float z = getFloatValue(data, i);
+            float r = getFloatValue(data, i);
+
+            glBegin(GL_LINE_LOOP);
+            for (int i = 0; i < 20; i++)
+            {
+                float angle = i / 20.0f * M_PI * 2.0f;
+                glVertex3f(x + r * cosf(angle), y + r * sinf(angle), z);
+            }
+            glEnd();
+        }
+            break;
+        case px::GLOverlay::SOLID_CIRCLE:
+        {
+            float x = getFloatValue(data, i);
+            float y = getFloatValue(data, i);
+            float z = getFloatValue(data, i);
+            float r = getFloatValue(data, i);
+
+            glBegin(GL_POLYGON);
+            for (int i = 0; i < 20; i++)
+            {
+                float angle = i / 20.0f * M_PI * 2.0f;
+                glVertex3f(x + r * cosf(angle), y + r * sinf(angle), z);
+            }
+            glEnd();
+        }
+            break;
+        case px::GLOverlay::SOLID_CUBE:
+        {
+            float x = getFloatValue(data, i);
+            float y = getFloatValue(data, i);
+            float z = getFloatValue(data, i);
+            float w = getFloatValue(data, i);
+            float h = getFloatValue(data, i);
+
+            float w_2 = w / 2.0f;
+            float h_2 = h / 2.0f;
+            glBegin(GL_QUADS);
+            // face 1
+            glNormal3f(1.0f, 0.0f, 0.0f);
+            glVertex3f(x + w_2, y - w_2, z + h_2);
+            glVertex3f(x + w_2, y - w_2, z - h_2);
+            glVertex3f(x + w_2, y + w_2, z - h_2);
+            glVertex3f(x + w_2, y + w_2, z + h_2);
+            // face 2
+            glNormal3f(0.0f, 1.0f, 0.0f);
+            glVertex3f(x + w_2, y + w_2, z + h_2);
+            glVertex3f(x + w_2, y + w_2, z - h_2);
+            glVertex3f(x - w_2, y + w_2, z - h_2);
+            glVertex3f(x - w_2, y + w_2, z + h_2);
+            // face 3
+            glNormal3f(0.0f, 0.0f, 1.0f);
+            glVertex3f(x + w_2, y + w_2, z + h_2);
+            glVertex3f(x - w_2, y + w_2, z + h_2);
+            glVertex3f(x - w_2, y - w_2, z + h_2);
+            glVertex3f(x + w_2, y - w_2, z + h_2);
+            // face 4
+            glNormal3f(-1.0f, 0.0f, 0.0f);
+            glVertex3f(x - w_2, y - w_2, z + h_2);
+            glVertex3f(x - w_2, y + w_2, z + h_2);
+            glVertex3f(x - w_2, y + w_2, z - h_2);
+            glVertex3f(x - w_2, y - w_2, z - h_2);
+            // face 5
+            glNormal3f(0.0f, -1.0f, 0.0f);
+            glVertex3f(x - w_2, y - w_2, z + h_2);
+            glVertex3f(x - w_2, y - w_2, z - h_2);
+            glVertex3f(x + w_2, y - w_2, z - h_2);
+            glVertex3f(x + w_2, y - w_2, z + h_2);
+            // face 6
+            glNormal3f(0.0f, 0.0f, -1.0f);
+            glVertex3f(x - w_2, y - w_2, z - h_2);
+            glVertex3f(x - w_2, y + w_2, z - h_2);
+            glVertex3f(x + w_2, y + w_2, z - h_2);
+            glVertex3f(x + w_2, y - w_2, z - h_2);
+
+            glEnd();
+        }
+            break;
+        case px::GLOverlay::WIRE_CUBE:
+        {
+            float x = getFloatValue(data, i);
+            float y = getFloatValue(data, i);
+            float z = getFloatValue(data, i);
+            float w = getFloatValue(data, i);
+            float h = getFloatValue(data, i);
+
+            float w_2 = w / 2.0f;
+            float h_2 = h / 2.0f;
+            // face 1
+            glBegin(GL_LINE_LOOP);
+            glVertex3f(x + w_2, y - w_2, z + h_2);
+            glVertex3f(x + w_2, y - w_2, z - h_2);
+            glVertex3f(x + w_2, y + w_2, z - h_2);
+            glVertex3f(x + w_2, y + w_2, z + h_2);
+            glEnd();
+            // face 2
+            glBegin(GL_LINE_LOOP);
+            glVertex3f(x + w_2, y + w_2, z + h_2);
+            glVertex3f(x + w_2, y + w_2, z - h_2);
+            glVertex3f(x - w_2, y + w_2, z - h_2);
+            glVertex3f(x - w_2, y + w_2, z + h_2);
+            glEnd();
+            // face 3
+            glBegin(GL_LINE_LOOP);
+            glVertex3f(x + w_2, y + w_2, z + h_2);
+            glVertex3f(x - w_2, y + w_2, z + h_2);
+            glVertex3f(x - w_2, y - w_2, z + h_2);
+            glVertex3f(x + w_2, y - w_2, z + h_2);
+            glEnd();
+            // face 4
+            glBegin(GL_LINE_LOOP);
+            glVertex3f(x - w_2, y - w_2, z + h_2);
+            glVertex3f(x - w_2, y + w_2, z + h_2);
+            glVertex3f(x - w_2, y + w_2, z - h_2);
+            glVertex3f(x - w_2, y - w_2, z - h_2);
+            glEnd();
+            // face 5
+            glBegin(GL_LINE_LOOP);
+            glVertex3f(x - w_2, y - w_2, z + h_2);
+            glVertex3f(x - w_2, y - w_2, z - h_2);
+            glVertex3f(x + w_2, y - w_2, z - h_2);
+            glVertex3f(x + w_2, y - w_2, z + h_2);
+            glEnd();
+            // face 6
+            glBegin(GL_LINE_LOOP);
+            glVertex3f(x - w_2, y - w_2, z - h_2);
+            glVertex3f(x - w_2, y + w_2, z - h_2);
+            glVertex3f(x + w_2, y + w_2, z - h_2);
+            glVertex3f(x + w_2, y - w_2, z - h_2);
+            glEnd();
+        }
+            break;
+        case px::GLOverlay::END:
+            glEnd();
+            break;
+        case px::GLOverlay::VERTEX2F:
+        {
+            float x = getFloatValue(data, i);
+            float y = getFloatValue(data, i);
+            glVertex2f(x, y);
+        }
+            break;
+        case px::GLOverlay::VERTEX3F:
+        {
+            float x = getFloatValue(data, i);
+            float y = getFloatValue(data, i);
+            float z = getFloatValue(data, i);
+            glVertex3f(x, y, z);
+        }
+            break;
+        case px::GLOverlay::ROTATEF:
+        {
+            float angle = getFloatValue(data, i);
+            float x = getFloatValue(data, i);
+            float y = getFloatValue(data, i);
+            float z = getFloatValue(data, i);
+            glRotatef(angle, x, y, z);
+        }
+            break;
+        case px::GLOverlay::TRANSLATEF:
+        {
+            float x = getFloatValue(data, i);
+            float y = getFloatValue(data, i);
+            float z = getFloatValue(data, i);
+            glTranslatef(x, y, z);
+        }
+            break;
+        case px::GLOverlay::SCALEF:
+        {
+            float x = getFloatValue(data, i);
+            float y = getFloatValue(data, i);
+            float z = getFloatValue(data, i);
+            glScalef(x, y, z);
+        }
+            break;
+        case px::GLOverlay::PUSH_MATRIX:
+            glPushMatrix();
+            break;
+        case px::GLOverlay::POP_MATRIX:
+            glPopMatrix();
+            break;
+        case px::GLOverlay::COLOR3F:
+        {
+            float red = getFloatValue(data, i);
+            float green = getFloatValue(data, i);
+            float blue = getFloatValue(data, i);
+            glColor3f(red, green, blue);
+        }
+            break;
+        case px::GLOverlay::COLOR4F:
+        {
+            float red = getFloatValue(data, i);
+            float green = getFloatValue(data, i);
+            float blue = getFloatValue(data, i);
+            float alpha = getFloatValue(data, i);
+            glColor4f(red, green, blue, alpha);
+        }
+            break;
+        case px::GLOverlay::POINTSIZE:
+            glPointSize(getFloatValue(data, i));
+            break;
+        case px::GLOverlay::LINEWIDTH:
+            glLineWidth(getFloatValue(data, i));
+            break;
+        }
+    }
+
+    glPopMatrix();
+}
+
+osg::BoundingBox
+GLOverlayGeode::GLOverlayDrawable::computeBound() const
+{
+    return mBBox;
+}
+
+float
+GLOverlayGeode::GLOverlayDrawable::getFloatValue(const std::string& data,
+                                                 size_t& mark) const
+{
+    char temp[4];
+    for (int i = 0; i < 4; ++i)
+    {
+        ++mark;
+        temp[i] = data.at(mark);
+    }
+
+    char *cp = &(temp[0]);
+    float *fp = reinterpret_cast<float *>(cp);
+
+    return *fp;
+}
diff --git a/src/ui/map3D/GLOverlayGeode.h b/src/ui/map3D/GLOverlayGeode.h
new file mode 100644
index 0000000000000000000000000000000000000000..cbd84309760906f3fe83cbe8533d5aa3febc682d
--- /dev/null
+++ b/src/ui/map3D/GLOverlayGeode.h
@@ -0,0 +1,44 @@
+#ifndef GLOVERLAYGEODE_H
+#define GLOVERLAYGEODE_H
+
+#include <mavlink_protobuf_manager.hpp>
+#include <osg/Geode>
+
+class GLOverlayGeode : public osg::Geode
+{
+public:
+    GLOverlayGeode();
+
+    void setOverlay(px::GLOverlay& overlay);
+
+    px::GLOverlay::CoordinateFrameType coordinateFrameType(void) const;
+
+private:
+    class GLOverlayDrawable : public osg::Drawable
+    {
+    public:
+        GLOverlayDrawable();
+
+        GLOverlayDrawable(const GLOverlayDrawable& drawable,
+                          const osg::CopyOp& copyop = osg::CopyOp::SHALLOW_COPY);
+
+        void setOverlay(px::GLOverlay& overlay);
+
+        META_Object(GLOverlayDrawableApp, GLOverlayDrawable)
+
+        virtual void drawImplementation(osg::RenderInfo&) const;
+
+        osg::BoundingBox computeBound() const;
+
+    private:
+        float getFloatValue(const std::string& data, size_t& mark) const;
+
+        px::GLOverlay mOverlay;
+        osg::BoundingBox mBBox;
+    };
+
+    GLOverlayDrawable mDrawable;
+    px::GLOverlay::CoordinateFrameType mCoordinateFrameType;
+};
+
+#endif // GLOVERLAYGEODE_H
diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc
index 1de0b56b21a5e7e48310182d1ff1cafb32e33c68..081ede5dc87464635723c75fd2079aa94a3d686d 100644
--- a/src/ui/map3D/Pixhawk3DWidget.cc
+++ b/src/ui/map3D/Pixhawk3DWidget.cc
@@ -139,6 +139,10 @@ Pixhawk3DWidget::systemCreated(UASInterface *uas)
             this, SLOT(attitudeChanged(UASInterface*,double,double,double,quint64)));
     connect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)),
             this, SLOT(setpointChanged(int,float,float,float,float)));
+#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
+    connect(uas, SIGNAL(overlayChanged(UASInterface*)),
+            this, SLOT(addOverlay(UASInterface*)));
+#endif
 
     initializeSystem(systemId, uas->getColor());
 
@@ -546,6 +550,40 @@ Pixhawk3DWidget::loadTerrainModel(void)
     }
 }
 
+#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
+void
+Pixhawk3DWidget::addOverlay(UASInterface *uas)
+{
+    int systemId = uas->getUASID();
+
+    if (!mSystemContainerMap.contains(systemId))
+    {
+        return;
+    }
+
+    SystemContainer& systemData = mSystemContainerMap[systemId];
+
+    px::GLOverlay overlay = uas->getOverlay();
+
+    QString overlayName = QString::fromStdString(overlay.name());
+
+    osg::ref_ptr<SystemGroupNode>& systemNode = m3DWidget->systemGroup(systemId);
+
+    if (!systemData.overlayNodeMap().contains(overlayName))
+    {
+        osg::ref_ptr<GLOverlayGeode> overlayNode = new GLOverlayGeode;
+        systemData.overlayNodeMap().insert(overlayName, overlayNode);
+
+        systemNode->allocentricMap()->addChild(overlayNode, false);
+        systemNode->rollingMap()->addChild(overlayNode, false);
+
+        emit overlayCreatedSignal(systemId, overlayName);
+    }
+
+    systemData.overlayNodeMap()[overlayName]->setOverlay(overlay);
+}
+#endif
+
 void
 Pixhawk3DWidget::selectTargetHeading(void)
 {
@@ -904,6 +942,26 @@ Pixhawk3DWidget::update(void)
 #if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
         rollingMap->setChildValue(systemData.obstacleGroupNode(),
                                   systemViewParams->displayObstacleList());
+
+        QMutableMapIterator<QString, osg::ref_ptr<GLOverlayGeode> > itOverlay(systemData.overlayNodeMap());
+        while (itOverlay.hasNext())
+        {
+            itOverlay.next();
+
+            osg::ref_ptr<GLOverlayGeode>& overlayNode = itOverlay.value();
+
+            bool displayOverlay = systemViewParams->displayOverlay().value(itOverlay.key());
+
+            bool visible;
+            visible = (overlayNode->coordinateFrameType() == px::GLOverlay::GLOBAL) && displayOverlay;
+
+            allocentricMap->setChildValue(overlayNode, visible);
+
+            visible = (overlayNode->coordinateFrameType() == px::GLOverlay::LOCAL) && displayOverlay;
+
+            rollingMap->setChildValue(overlayNode, visible);
+        }
+
         rollingMap->setChildValue(systemData.plannedPathNode(),
                                   systemViewParams->displayPlannedPath());
 
diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h
index f04b3d3390c0a829f5dd964c6222e2e6a4f095ff..4a521997d8512fab4db1c36c7089fc0bded23072 100644
--- a/src/ui/map3D/Pixhawk3DWidget.h
+++ b/src/ui/map3D/Pixhawk3DWidget.h
@@ -64,6 +64,7 @@ public slots:
 
 signals:
     void systemCreatedSignal(UASInterface* uas);
+    void overlayCreatedSignal(int systemId, const QString& name);
 
 private slots:
     void clearData(void);
@@ -75,6 +76,10 @@ private slots:
     void setBirdEyeView(void);
     void loadTerrainModel(void);
 
+#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
+    void addOverlay(UASInterface* uas);
+#endif
+
     void selectTargetHeading(void);
     void selectTarget(void);
     void setTarget(void);
diff --git a/src/ui/map3D/SystemContainer.cc b/src/ui/map3D/SystemContainer.cc
index 9a9025a3dfb6916597765232ecf5676f8dc01696..72cac8eee21eb6c9496c2b1e8525f04ef5cd454e 100644
--- a/src/ui/map3D/SystemContainer.cc
+++ b/src/ui/map3D/SystemContainer.cc
@@ -99,6 +99,12 @@ SystemContainer::obstacleGroupNode(void)
     return mObstacleGroupNode;
 }
 
+QMap<QString,osg::ref_ptr<GLOverlayGeode> >&
+SystemContainer::overlayNodeMap(void)
+{
+    return mOverlayNodeMap;
+}
+
 osg::ref_ptr<osg::Geode>&
 SystemContainer::plannedPathNode(void)
 {
diff --git a/src/ui/map3D/SystemContainer.h b/src/ui/map3D/SystemContainer.h
index 25ec67933804da96c802bc1f179ba24566617fb5..92d7ea68bf8a213a46dc450c109748cf4dea5094 100644
--- a/src/ui/map3D/SystemContainer.h
+++ b/src/ui/map3D/SystemContainer.h
@@ -10,6 +10,7 @@
 #include "WaypointGroupNode.h"
 
 #if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
+#include "GLOverlayGeode.h"
 #include "ObstacleGroupNode.h"
 #endif
 
@@ -37,6 +38,7 @@ public:
     osg::ref_ptr<WaypointGroupNode>& waypointGroupNode(void);
 #if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
     osg::ref_ptr<ObstacleGroupNode>& obstacleGroupNode(void);
+    QMap<QString,osg::ref_ptr<GLOverlayGeode> >& overlayNodeMap(void);
     osg::ref_ptr<osg::Geode>& plannedPathNode(void);
 #endif
 
@@ -62,6 +64,7 @@ private:
     osg::ref_ptr<WaypointGroupNode> mWaypointGroupNode;
 #if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
     osg::ref_ptr<ObstacleGroupNode> mObstacleGroupNode;
+    QMap<QString,osg::ref_ptr<GLOverlayGeode> > mOverlayNodeMap;
     osg::ref_ptr<osg::Geode> mPlannedPathNode;
 #endif
 };
diff --git a/src/ui/map3D/SystemViewParams.cc b/src/ui/map3D/SystemViewParams.cc
index 1c83050fad6f5185ee6dc5e5d4300118862af37f..d131a812ab28751ac459defa1a92b7cc993b3eae 100644
--- a/src/ui/map3D/SystemViewParams.cc
+++ b/src/ui/map3D/SystemViewParams.cc
@@ -54,6 +54,18 @@ SystemViewParams::displayObstacleList(void) const
     return mDisplayObstacleList;
 }
 
+QMap<QString,bool>&
+SystemViewParams::displayOverlay(void)
+{
+    return mDisplayOverlay;
+}
+
+QMap<QString,bool>
+SystemViewParams::displayOverlay(void) const
+{
+    return mDisplayOverlay;
+}
+
 bool&
 SystemViewParams::displayPlannedPath(void)
 {
@@ -227,6 +239,17 @@ SystemViewParams::toggleObstacleList(int state)
     }
 }
 
+void
+SystemViewParams::toggleOverlay(const QString& name)
+{
+    if (!mDisplayOverlay.contains(name))
+    {
+        return;
+    }
+
+    mDisplayOverlay[name] = !mDisplayOverlay[name];
+}
+
 void
 SystemViewParams::togglePlannedPath(int state)
 {
diff --git a/src/ui/map3D/SystemViewParams.h b/src/ui/map3D/SystemViewParams.h
index f3701e86f3002be361111a4c8381922999a490ff..7926b1d6457b0ffd8b227814878e0b2531cbc6d3 100644
--- a/src/ui/map3D/SystemViewParams.h
+++ b/src/ui/map3D/SystemViewParams.h
@@ -1,6 +1,7 @@
 #ifndef SYSTEMVIEWPARAMS_H
 #define SYSTEMVIEWPARAMS_H
 
+#include <QMap>
 #include <QObject>
 #include <QSharedPointer>
 #include <QVector>
@@ -21,6 +22,9 @@ public:
     bool& displayObstacleList(void);
     bool displayObstacleList(void) const;
 
+    QMap<QString,bool>& displayOverlay(void);
+    QMap<QString,bool> displayOverlay(void) const;
+
     bool& displayPlannedPath(void);
     bool displayPlannedPath(void) const;
 
@@ -57,6 +61,7 @@ public slots:
     void toggleColorPointCloud(int state);
     void toggleLocalGrid(int state);
     void toggleObstacleList(int state);
+    void toggleOverlay(const QString& name);
     void togglePlannedPath(int state);
     void togglePointCloud(int state);
     void toggleRGBD(int state);
@@ -74,6 +79,7 @@ private:
     bool mColorPointCloudByDistance;
     bool mDisplayLocalGrid;
     bool mDisplayObstacleList;
+    QMap<QString,bool> mDisplayOverlay;
     bool mDisplayPlannedPath;
     bool mDisplayPointCloud;
     bool mDisplayRGBD;
diff --git a/src/ui/map3D/ViewParamWidget.cc b/src/ui/map3D/ViewParamWidget.cc
index 516187b9c0d8c62c984dd20e8fafe4b5e57cf03a..067b3f2a53da8128cb9954c3241d6adec302d9c2 100644
--- a/src/ui/map3D/ViewParamWidget.cc
+++ b/src/ui/map3D/ViewParamWidget.cc
@@ -2,7 +2,7 @@
 
 #include <osg/LineWidth>
 #include <QCheckBox>
-#include <QFormLayout>
+#include <QGroupBox>
 #include <QLabel>
 #include <QPushButton>
 
@@ -28,8 +28,13 @@ ViewParamWidget::ViewParamWidget(GlobalViewParamsPtr& globalViewParams,
 
     mTabWidget->setFocusPolicy(Qt::NoFocus);
 
+    mOverlaySignalMapper = new QSignalMapper(this);
+
     connect(parent, SIGNAL(systemCreatedSignal(UASInterface*)),
             this, SLOT(systemCreated(UASInterface*)));
+
+    connect(parent, SIGNAL(overlayCreatedSignal(int,QString)),
+            this, SLOT(overlayCreated(int,QString)));
 }
 
 void
@@ -47,6 +52,28 @@ ViewParamWidget::setFollowCameraId(int id)
     mFollowCameraComboBox->setCurrentIndex(0);
 }
 
+void
+ViewParamWidget::overlayCreated(int systemId, const QString& name)
+{
+    if (!mOverlayLayout.contains(systemId))
+    {
+        return;
+    }
+
+    SystemViewParamsPtr systemViewParams = mSystemViewParamMap[systemId];
+    systemViewParams->displayOverlay().insert(name, true);
+
+    QCheckBox* checkbox = new QCheckBox(this);
+    checkbox->setChecked(systemViewParams->displayOverlay().value(name));
+    mOverlayLayout[systemId]->addRow(name, checkbox);
+
+    mOverlaySignalMapper->setMapping(checkbox, name);
+    connect(checkbox, SIGNAL(clicked()),
+            mOverlaySignalMapper, SLOT(map()));
+    connect(mOverlaySignalMapper, SIGNAL(mapped(QString)),
+            systemViewParams.data(), SLOT(toggleOverlay(QString)));
+}
+
 void
 ViewParamWidget::systemCreated(UASInterface *uas)
 {
@@ -173,6 +200,10 @@ ViewParamWidget::addTab(int systemId)
     QCheckBox* waypointsCheckBox = new QCheckBox(this);
     waypointsCheckBox->setChecked(systemViewParams->displayWaypoints());
 
+    QGroupBox* overlayGroupBox = new QGroupBox(tr("Overlays"), this);
+    mOverlayLayout[systemId] = new QFormLayout;
+    overlayGroupBox->setLayout(mOverlayLayout[systemId]);
+
     QFormLayout* formLayout = new QFormLayout;
     page->setLayout(formLayout);
 
@@ -188,6 +219,7 @@ ViewParamWidget::addTab(int systemId)
     formLayout->addRow(tr("Target"), targetCheckBox);
     formLayout->addRow(tr("Trails"), trailsCheckBox);
     formLayout->addRow(tr("Waypoints"), waypointsCheckBox);
+    formLayout->addRow(overlayGroupBox);
 
     QString label("MAV ");
     label += QString::number(systemId);
diff --git a/src/ui/map3D/ViewParamWidget.h b/src/ui/map3D/ViewParamWidget.h
index aacae5a512202277e53e27eb8faeaead907d162b..69142f090441dedf1594814e2eedc47af3ec6264 100644
--- a/src/ui/map3D/ViewParamWidget.h
+++ b/src/ui/map3D/ViewParamWidget.h
@@ -3,6 +3,8 @@
 
 #include <QComboBox>
 #include <QDockWidget>
+#include <QFormLayout>
+#include <QSignalMapper>
 #include <QSpinBox>
 #include <QTabWidget>
 #include <QVBoxLayout>
@@ -26,6 +28,7 @@ public:
 signals:
 
 private slots:
+    void overlayCreated(int systemId, const QString& name);
     void systemCreated(UASInterface* uas);
     void setpointsCheckBoxToggled(int state);
 
@@ -43,7 +46,10 @@ private:
     // child widgets
     QComboBox* mFollowCameraComboBox;
     QSpinBox* mSetpointHistoryLengthSpinBox;
+    QMap<int, QFormLayout*> mOverlayLayout;
     QTabWidget* mTabWidget;
+
+    QSignalMapper* mOverlaySignalMapper;
 };
 
 #endif // VIEWPARAMWIDGET_H
diff --git a/thirdParty/mavlink/include/mavlink_protobuf_manager.hpp b/thirdParty/mavlink/include/mavlink_protobuf_manager.hpp
index d16668ed8e9c94861a69525b912e1b96cec5a419..fd3ddd026f709c1acb8c31d7493523892924ab49 100644
--- a/thirdParty/mavlink/include/mavlink_protobuf_manager.hpp
+++ b/thirdParty/mavlink/include/mavlink_protobuf_manager.hpp
@@ -24,40 +24,46 @@ public:
 	 , kExtendedHeaderSize(MAVLINK_EXTENDED_HEADER_LEN)
 	 , kExtendedPayloadMaxSize(MAVLINK_MAX_EXTENDED_PAYLOAD_LEN)
 	{
-		// register PointCloudXYZI
+		// register GLOverlay
 		{
-			std::tr1::shared_ptr<px::PointCloudXYZI> msg(new px::PointCloudXYZI);
+			std::tr1::shared_ptr<px::GLOverlay> msg(new px::GLOverlay);
 			registerType(msg);
 		}
 
-		// register PointCloudXYZRGB
+		// register ObstacleList
 		{
-			std::tr1::shared_ptr<px::PointCloudXYZRGB> msg(new px::PointCloudXYZRGB);
+			std::tr1::shared_ptr<px::ObstacleList> msg(new px::ObstacleList);
 			registerType(msg);
 		}
 
-		// register RGBDImage
+		// register ObstacleMap
 		{
-			std::tr1::shared_ptr<px::RGBDImage> msg(new px::RGBDImage);
+			std::tr1::shared_ptr<px::ObstacleMap> msg(new px::ObstacleMap);
 			registerType(msg);
 		}
 
-		// register ObstacleList
+		// register Path
 		{
-			std::tr1::shared_ptr<px::ObstacleList> msg(new px::ObstacleList);
+			std::tr1::shared_ptr<px::Path> msg(new px::Path);
+                        registerType(msg);
+		}
+
+		// register PointCloudXYZI
+		{
+			std::tr1::shared_ptr<px::PointCloudXYZI> msg(new px::PointCloudXYZI);
 			registerType(msg);
 		}
 
-		// register ObstacleMap
+		// register PointCloudXYZRGB
 		{
-			std::tr1::shared_ptr<px::ObstacleMap> msg(new px::ObstacleMap);
+			std::tr1::shared_ptr<px::PointCloudXYZRGB> msg(new px::PointCloudXYZRGB);
 			registerType(msg);
 		}
 
-		// register Path
+		// register RGBDImage
 		{
-			std::tr1::shared_ptr<px::Path> msg(new px::Path);
-                        registerType(msg);
+			std::tr1::shared_ptr<px::RGBDImage> msg(new px::RGBDImage);
+			registerType(msg);
 		}
 
 		srand(time(NULL));
diff --git a/thirdParty/mavlink/include/pixhawk/pixhawk.pb.h b/thirdParty/mavlink/include/pixhawk/pixhawk.pb.h
index 63279d3bb123b1e808b9a3636e662728c4b27555..7556606e98e26066d539950e6a43037145937770 100644
--- a/thirdParty/mavlink/include/pixhawk/pixhawk.pb.h
+++ b/thirdParty/mavlink/include/pixhawk/pixhawk.pb.h
@@ -33,17 +33,97 @@ void protobuf_AssignDesc_pixhawk_2eproto();
 void protobuf_ShutdownFile_pixhawk_2eproto();
 
 class HeaderInfo;
+class GLOverlay;
+class Obstacle;
+class ObstacleList;
+class ObstacleMap;
+class Path;
 class PointCloudXYZI;
 class PointCloudXYZI_PointXYZI;
 class PointCloudXYZRGB;
 class PointCloudXYZRGB_PointXYZRGB;
 class RGBDImage;
-class Obstacle;
-class ObstacleList;
-class ObstacleMap;
 class Waypoint;
-class Path;
 
+enum GLOverlay_CoordinateFrameType {
+  GLOverlay_CoordinateFrameType_GLOBAL = 0,
+  GLOverlay_CoordinateFrameType_LOCAL = 1
+};
+bool GLOverlay_CoordinateFrameType_IsValid(int value);
+const GLOverlay_CoordinateFrameType GLOverlay_CoordinateFrameType_CoordinateFrameType_MIN = GLOverlay_CoordinateFrameType_GLOBAL;
+const GLOverlay_CoordinateFrameType GLOverlay_CoordinateFrameType_CoordinateFrameType_MAX = GLOverlay_CoordinateFrameType_LOCAL;
+const int GLOverlay_CoordinateFrameType_CoordinateFrameType_ARRAYSIZE = GLOverlay_CoordinateFrameType_CoordinateFrameType_MAX + 1;
+
+const ::google::protobuf::EnumDescriptor* GLOverlay_CoordinateFrameType_descriptor();
+inline const ::std::string& GLOverlay_CoordinateFrameType_Name(GLOverlay_CoordinateFrameType value) {
+  return ::google::protobuf::internal::NameOfEnum(
+    GLOverlay_CoordinateFrameType_descriptor(), value);
+}
+inline bool GLOverlay_CoordinateFrameType_Parse(
+    const ::std::string& name, GLOverlay_CoordinateFrameType* value) {
+  return ::google::protobuf::internal::ParseNamedEnum<GLOverlay_CoordinateFrameType>(
+    GLOverlay_CoordinateFrameType_descriptor(), name, value);
+}
+enum GLOverlay_Mode {
+  GLOverlay_Mode_POINTS = 0,
+  GLOverlay_Mode_LINES = 1,
+  GLOverlay_Mode_LINE_STRIP = 2,
+  GLOverlay_Mode_LINE_LOOP = 3,
+  GLOverlay_Mode_TRIANGLES = 4,
+  GLOverlay_Mode_TRIANGLE_STRIP = 5,
+  GLOverlay_Mode_TRIANGLE_FAN = 6,
+  GLOverlay_Mode_QUADS = 7,
+  GLOverlay_Mode_QUAD_STRIP = 8,
+  GLOverlay_Mode_POLYGON = 9,
+  GLOverlay_Mode_SOLID_CIRCLE = 10,
+  GLOverlay_Mode_WIRE_CIRCLE = 11,
+  GLOverlay_Mode_SOLID_CUBE = 12,
+  GLOverlay_Mode_WIRE_CUBE = 13
+};
+bool GLOverlay_Mode_IsValid(int value);
+const GLOverlay_Mode GLOverlay_Mode_Mode_MIN = GLOverlay_Mode_POINTS;
+const GLOverlay_Mode GLOverlay_Mode_Mode_MAX = GLOverlay_Mode_WIRE_CUBE;
+const int GLOverlay_Mode_Mode_ARRAYSIZE = GLOverlay_Mode_Mode_MAX + 1;
+
+const ::google::protobuf::EnumDescriptor* GLOverlay_Mode_descriptor();
+inline const ::std::string& GLOverlay_Mode_Name(GLOverlay_Mode value) {
+  return ::google::protobuf::internal::NameOfEnum(
+    GLOverlay_Mode_descriptor(), value);
+}
+inline bool GLOverlay_Mode_Parse(
+    const ::std::string& name, GLOverlay_Mode* value) {
+  return ::google::protobuf::internal::ParseNamedEnum<GLOverlay_Mode>(
+    GLOverlay_Mode_descriptor(), name, value);
+}
+enum GLOverlay_Identifier {
+  GLOverlay_Identifier_END = 14,
+  GLOverlay_Identifier_VERTEX2F = 15,
+  GLOverlay_Identifier_VERTEX3F = 16,
+  GLOverlay_Identifier_ROTATEF = 17,
+  GLOverlay_Identifier_TRANSLATEF = 18,
+  GLOverlay_Identifier_SCALEF = 19,
+  GLOverlay_Identifier_PUSH_MATRIX = 20,
+  GLOverlay_Identifier_POP_MATRIX = 21,
+  GLOverlay_Identifier_COLOR3F = 22,
+  GLOverlay_Identifier_COLOR4F = 23,
+  GLOverlay_Identifier_POINTSIZE = 24,
+  GLOverlay_Identifier_LINEWIDTH = 25
+};
+bool GLOverlay_Identifier_IsValid(int value);
+const GLOverlay_Identifier GLOverlay_Identifier_Identifier_MIN = GLOverlay_Identifier_END;
+const GLOverlay_Identifier GLOverlay_Identifier_Identifier_MAX = GLOverlay_Identifier_LINEWIDTH;
+const int GLOverlay_Identifier_Identifier_ARRAYSIZE = GLOverlay_Identifier_Identifier_MAX + 1;
+
+const ::google::protobuf::EnumDescriptor* GLOverlay_Identifier_descriptor();
+inline const ::std::string& GLOverlay_Identifier_Name(GLOverlay_Identifier value) {
+  return ::google::protobuf::internal::NameOfEnum(
+    GLOverlay_Identifier_descriptor(), value);
+}
+inline bool GLOverlay_Identifier_Parse(
+    const ::std::string& name, GLOverlay_Identifier* value) {
+  return ::google::protobuf::internal::ParseNamedEnum<GLOverlay_Identifier>(
+    GLOverlay_Identifier_descriptor(), name, value);
+}
 // ===================================================================
 
 class HeaderInfo : public ::google::protobuf::Message {
@@ -148,14 +228,14 @@ class HeaderInfo : public ::google::protobuf::Message {
 };
 // -------------------------------------------------------------------
 
-class PointCloudXYZI_PointXYZI : public ::google::protobuf::Message {
+class GLOverlay : public ::google::protobuf::Message {
  public:
-  PointCloudXYZI_PointXYZI();
-  virtual ~PointCloudXYZI_PointXYZI();
+  GLOverlay();
+  virtual ~GLOverlay();
   
-  PointCloudXYZI_PointXYZI(const PointCloudXYZI_PointXYZI& from);
+  GLOverlay(const GLOverlay& from);
   
-  inline PointCloudXYZI_PointXYZI& operator=(const PointCloudXYZI_PointXYZI& from) {
+  inline GLOverlay& operator=(const GLOverlay& from) {
     CopyFrom(from);
     return *this;
   }
@@ -169,17 +249,17 @@ class PointCloudXYZI_PointXYZI : public ::google::protobuf::Message {
   }
   
   static const ::google::protobuf::Descriptor* descriptor();
-  static const PointCloudXYZI_PointXYZI& default_instance();
+  static const GLOverlay& default_instance();
   
-  void Swap(PointCloudXYZI_PointXYZI* other);
+  void Swap(GLOverlay* other);
   
   // implements Message ----------------------------------------------
   
-  PointCloudXYZI_PointXYZI* New() const;
+  GLOverlay* New() const;
   void CopyFrom(const ::google::protobuf::Message& from);
   void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const PointCloudXYZI_PointXYZI& from);
-  void MergeFrom(const PointCloudXYZI_PointXYZI& from);
+  void CopyFrom(const GLOverlay& from);
+  void MergeFrom(const GLOverlay& from);
   void Clear();
   bool IsInitialized() const;
   
@@ -200,37 +280,296 @@ class PointCloudXYZI_PointXYZI : public ::google::protobuf::Message {
   
   // nested types ----------------------------------------------------
   
+  typedef GLOverlay_CoordinateFrameType CoordinateFrameType;
+  static const CoordinateFrameType GLOBAL = GLOverlay_CoordinateFrameType_GLOBAL;
+  static const CoordinateFrameType LOCAL = GLOverlay_CoordinateFrameType_LOCAL;
+  static inline bool CoordinateFrameType_IsValid(int value) {
+    return GLOverlay_CoordinateFrameType_IsValid(value);
+  }
+  static const CoordinateFrameType CoordinateFrameType_MIN =
+    GLOverlay_CoordinateFrameType_CoordinateFrameType_MIN;
+  static const CoordinateFrameType CoordinateFrameType_MAX =
+    GLOverlay_CoordinateFrameType_CoordinateFrameType_MAX;
+  static const int CoordinateFrameType_ARRAYSIZE =
+    GLOverlay_CoordinateFrameType_CoordinateFrameType_ARRAYSIZE;
+  static inline const ::google::protobuf::EnumDescriptor*
+  CoordinateFrameType_descriptor() {
+    return GLOverlay_CoordinateFrameType_descriptor();
+  }
+  static inline const ::std::string& CoordinateFrameType_Name(CoordinateFrameType value) {
+    return GLOverlay_CoordinateFrameType_Name(value);
+  }
+  static inline bool CoordinateFrameType_Parse(const ::std::string& name,
+      CoordinateFrameType* value) {
+    return GLOverlay_CoordinateFrameType_Parse(name, value);
+  }
+  
+  typedef GLOverlay_Mode Mode;
+  static const Mode POINTS = GLOverlay_Mode_POINTS;
+  static const Mode LINES = GLOverlay_Mode_LINES;
+  static const Mode LINE_STRIP = GLOverlay_Mode_LINE_STRIP;
+  static const Mode LINE_LOOP = GLOverlay_Mode_LINE_LOOP;
+  static const Mode TRIANGLES = GLOverlay_Mode_TRIANGLES;
+  static const Mode TRIANGLE_STRIP = GLOverlay_Mode_TRIANGLE_STRIP;
+  static const Mode TRIANGLE_FAN = GLOverlay_Mode_TRIANGLE_FAN;
+  static const Mode QUADS = GLOverlay_Mode_QUADS;
+  static const Mode QUAD_STRIP = GLOverlay_Mode_QUAD_STRIP;
+  static const Mode POLYGON = GLOverlay_Mode_POLYGON;
+  static const Mode SOLID_CIRCLE = GLOverlay_Mode_SOLID_CIRCLE;
+  static const Mode WIRE_CIRCLE = GLOverlay_Mode_WIRE_CIRCLE;
+  static const Mode SOLID_CUBE = GLOverlay_Mode_SOLID_CUBE;
+  static const Mode WIRE_CUBE = GLOverlay_Mode_WIRE_CUBE;
+  static inline bool Mode_IsValid(int value) {
+    return GLOverlay_Mode_IsValid(value);
+  }
+  static const Mode Mode_MIN =
+    GLOverlay_Mode_Mode_MIN;
+  static const Mode Mode_MAX =
+    GLOverlay_Mode_Mode_MAX;
+  static const int Mode_ARRAYSIZE =
+    GLOverlay_Mode_Mode_ARRAYSIZE;
+  static inline const ::google::protobuf::EnumDescriptor*
+  Mode_descriptor() {
+    return GLOverlay_Mode_descriptor();
+  }
+  static inline const ::std::string& Mode_Name(Mode value) {
+    return GLOverlay_Mode_Name(value);
+  }
+  static inline bool Mode_Parse(const ::std::string& name,
+      Mode* value) {
+    return GLOverlay_Mode_Parse(name, value);
+  }
+  
+  typedef GLOverlay_Identifier Identifier;
+  static const Identifier END = GLOverlay_Identifier_END;
+  static const Identifier VERTEX2F = GLOverlay_Identifier_VERTEX2F;
+  static const Identifier VERTEX3F = GLOverlay_Identifier_VERTEX3F;
+  static const Identifier ROTATEF = GLOverlay_Identifier_ROTATEF;
+  static const Identifier TRANSLATEF = GLOverlay_Identifier_TRANSLATEF;
+  static const Identifier SCALEF = GLOverlay_Identifier_SCALEF;
+  static const Identifier PUSH_MATRIX = GLOverlay_Identifier_PUSH_MATRIX;
+  static const Identifier POP_MATRIX = GLOverlay_Identifier_POP_MATRIX;
+  static const Identifier COLOR3F = GLOverlay_Identifier_COLOR3F;
+  static const Identifier COLOR4F = GLOverlay_Identifier_COLOR4F;
+  static const Identifier POINTSIZE = GLOverlay_Identifier_POINTSIZE;
+  static const Identifier LINEWIDTH = GLOverlay_Identifier_LINEWIDTH;
+  static inline bool Identifier_IsValid(int value) {
+    return GLOverlay_Identifier_IsValid(value);
+  }
+  static const Identifier Identifier_MIN =
+    GLOverlay_Identifier_Identifier_MIN;
+  static const Identifier Identifier_MAX =
+    GLOverlay_Identifier_Identifier_MAX;
+  static const int Identifier_ARRAYSIZE =
+    GLOverlay_Identifier_Identifier_ARRAYSIZE;
+  static inline const ::google::protobuf::EnumDescriptor*
+  Identifier_descriptor() {
+    return GLOverlay_Identifier_descriptor();
+  }
+  static inline const ::std::string& Identifier_Name(Identifier value) {
+    return GLOverlay_Identifier_Name(value);
+  }
+  static inline bool Identifier_Parse(const ::std::string& name,
+      Identifier* value) {
+    return GLOverlay_Identifier_Parse(name, value);
+  }
+  
   // accessors -------------------------------------------------------
   
-  // required float x = 1;
+  // required .px.HeaderInfo header = 1;
+  inline bool has_header() const;
+  inline void clear_header();
+  static const int kHeaderFieldNumber = 1;
+  inline const ::px::HeaderInfo& header() const;
+  inline ::px::HeaderInfo* mutable_header();
+  inline ::px::HeaderInfo* release_header();
+  
+  // optional string name = 2;
+  inline bool has_name() const;
+  inline void clear_name();
+  static const int kNameFieldNumber = 2;
+  inline const ::std::string& name() const;
+  inline void set_name(const ::std::string& value);
+  inline void set_name(const char* value);
+  inline void set_name(const char* value, size_t size);
+  inline ::std::string* mutable_name();
+  inline ::std::string* release_name();
+  
+  // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3;
+  inline bool has_coordinateframetype() const;
+  inline void clear_coordinateframetype();
+  static const int kCoordinateFrameTypeFieldNumber = 3;
+  inline ::px::GLOverlay_CoordinateFrameType coordinateframetype() const;
+  inline void set_coordinateframetype(::px::GLOverlay_CoordinateFrameType value);
+  
+  // optional double origin_x = 4;
+  inline bool has_origin_x() const;
+  inline void clear_origin_x();
+  static const int kOriginXFieldNumber = 4;
+  inline double origin_x() const;
+  inline void set_origin_x(double value);
+  
+  // optional double origin_y = 5;
+  inline bool has_origin_y() const;
+  inline void clear_origin_y();
+  static const int kOriginYFieldNumber = 5;
+  inline double origin_y() const;
+  inline void set_origin_y(double value);
+  
+  // optional double origin_z = 6;
+  inline bool has_origin_z() const;
+  inline void clear_origin_z();
+  static const int kOriginZFieldNumber = 6;
+  inline double origin_z() const;
+  inline void set_origin_z(double value);
+  
+  // optional bytes data = 7;
+  inline bool has_data() const;
+  inline void clear_data();
+  static const int kDataFieldNumber = 7;
+  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.GLOverlay)
+ private:
+  inline void set_has_header();
+  inline void clear_has_header();
+  inline void set_has_name();
+  inline void clear_has_name();
+  inline void set_has_coordinateframetype();
+  inline void clear_has_coordinateframetype();
+  inline void set_has_origin_x();
+  inline void clear_has_origin_x();
+  inline void set_has_origin_y();
+  inline void clear_has_origin_y();
+  inline void set_has_origin_z();
+  inline void clear_has_origin_z();
+  inline void set_has_data();
+  inline void clear_has_data();
+  
+  ::google::protobuf::UnknownFieldSet _unknown_fields_;
+  
+  ::px::HeaderInfo* header_;
+  ::std::string* name_;
+  double origin_x_;
+  double origin_y_;
+  double origin_z_;
+  ::std::string* data_;
+  int coordinateframetype_;
+  
+  mutable int _cached_size_;
+  ::google::protobuf::uint32 _has_bits_[(7 + 31) / 32];
+  
+  friend void  protobuf_AddDesc_pixhawk_2eproto();
+  friend void protobuf_AssignDesc_pixhawk_2eproto();
+  friend void protobuf_ShutdownFile_pixhawk_2eproto();
+  
+  void InitAsDefaultInstance();
+  static GLOverlay* 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);
   
-  // required float y = 2;
+  // 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);
   
-  // required float z = 3;
+  // 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);
   
-  // required float intensity = 4;
-  inline bool has_intensity() const;
-  inline void clear_intensity();
-  static const int kIntensityFieldNumber = 4;
-  inline float intensity() const;
-  inline void set_intensity(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);
   
-  // @@protoc_insertion_point(class_scope:px.PointCloudXYZI.PointXYZI)
+  // 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();
@@ -238,36 +577,42 @@ class PointCloudXYZI_PointXYZI : public ::google::protobuf::Message {
   inline void clear_has_y();
   inline void set_has_z();
   inline void clear_has_z();
-  inline void set_has_intensity();
-  inline void clear_has_intensity();
+  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 intensity_;
+  float length_;
+  float width_;
+  float height_;
   
   mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(4 + 31) / 32];
+  ::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 PointCloudXYZI_PointXYZI* default_instance_;
+  static Obstacle* default_instance_;
 };
 // -------------------------------------------------------------------
 
-class PointCloudXYZI : public ::google::protobuf::Message {
+class ObstacleList : public ::google::protobuf::Message {
  public:
-  PointCloudXYZI();
-  virtual ~PointCloudXYZI();
+  ObstacleList();
+  virtual ~ObstacleList();
   
-  PointCloudXYZI(const PointCloudXYZI& from);
+  ObstacleList(const ObstacleList& from);
   
-  inline PointCloudXYZI& operator=(const PointCloudXYZI& from) {
+  inline ObstacleList& operator=(const ObstacleList& from) {
     CopyFrom(from);
     return *this;
   }
@@ -281,17 +626,17 @@ class PointCloudXYZI : public ::google::protobuf::Message {
   }
   
   static const ::google::protobuf::Descriptor* descriptor();
-  static const PointCloudXYZI& default_instance();
+  static const ObstacleList& default_instance();
   
-  void Swap(PointCloudXYZI* other);
+  void Swap(ObstacleList* other);
   
   // implements Message ----------------------------------------------
   
-  PointCloudXYZI* New() const;
+  ObstacleList* New() const;
   void CopyFrom(const ::google::protobuf::Message& from);
   void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const PointCloudXYZI& from);
-  void MergeFrom(const PointCloudXYZI& from);
+  void CopyFrom(const ObstacleList& from);
+  void MergeFrom(const ObstacleList& from);
   void Clear();
   bool IsInitialized() const;
   
@@ -312,8 +657,6 @@ class PointCloudXYZI : public ::google::protobuf::Message {
   
   // nested types ----------------------------------------------------
   
-  typedef PointCloudXYZI_PointXYZI PointXYZI;
-  
   // accessors -------------------------------------------------------
   
   // required .px.HeaderInfo header = 1;
@@ -324,19 +667,19 @@ class PointCloudXYZI : public ::google::protobuf::Message {
   inline ::px::HeaderInfo* mutable_header();
   inline ::px::HeaderInfo* release_header();
   
-  // repeated .px.PointCloudXYZI.PointXYZI points = 2;
-  inline int points_size() const;
-  inline void clear_points();
-  static const int kPointsFieldNumber = 2;
-  inline const ::px::PointCloudXYZI_PointXYZI& points(int index) const;
-  inline ::px::PointCloudXYZI_PointXYZI* mutable_points(int index);
-  inline ::px::PointCloudXYZI_PointXYZI* add_points();
-  inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >&
-      points() const;
-  inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >*
-      mutable_points();
+  // 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.PointCloudXYZI)
+  // @@protoc_insertion_point(class_scope:px.ObstacleList)
  private:
   inline void set_has_header();
   inline void clear_has_header();
@@ -344,7 +687,7 @@ class PointCloudXYZI : public ::google::protobuf::Message {
   ::google::protobuf::UnknownFieldSet _unknown_fields_;
   
   ::px::HeaderInfo* header_;
-  ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI > points_;
+  ::google::protobuf::RepeatedPtrField< ::px::Obstacle > obstacles_;
   
   mutable int _cached_size_;
   ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32];
@@ -354,18 +697,18 @@ class PointCloudXYZI : public ::google::protobuf::Message {
   friend void protobuf_ShutdownFile_pixhawk_2eproto();
   
   void InitAsDefaultInstance();
-  static PointCloudXYZI* default_instance_;
+  static ObstacleList* default_instance_;
 };
 // -------------------------------------------------------------------
 
-class PointCloudXYZRGB_PointXYZRGB : public ::google::protobuf::Message {
+class ObstacleMap : public ::google::protobuf::Message {
  public:
-  PointCloudXYZRGB_PointXYZRGB();
-  virtual ~PointCloudXYZRGB_PointXYZRGB();
+  ObstacleMap();
+  virtual ~ObstacleMap();
   
-  PointCloudXYZRGB_PointXYZRGB(const PointCloudXYZRGB_PointXYZRGB& from);
+  ObstacleMap(const ObstacleMap& from);
   
-  inline PointCloudXYZRGB_PointXYZRGB& operator=(const PointCloudXYZRGB_PointXYZRGB& from) {
+  inline ObstacleMap& operator=(const ObstacleMap& from) {
     CopyFrom(from);
     return *this;
   }
@@ -379,17 +722,17 @@ class PointCloudXYZRGB_PointXYZRGB : public ::google::protobuf::Message {
   }
   
   static const ::google::protobuf::Descriptor* descriptor();
-  static const PointCloudXYZRGB_PointXYZRGB& default_instance();
+  static const ObstacleMap& default_instance();
   
-  void Swap(PointCloudXYZRGB_PointXYZRGB* other);
+  void Swap(ObstacleMap* other);
   
   // implements Message ----------------------------------------------
   
-  PointCloudXYZRGB_PointXYZRGB* New() const;
+  ObstacleMap* New() const;
   void CopyFrom(const ::google::protobuf::Message& from);
   void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const PointCloudXYZRGB_PointXYZRGB& from);
-  void MergeFrom(const PointCloudXYZRGB_PointXYZRGB& from);
+  void CopyFrom(const ObstacleMap& from);
+  void MergeFrom(const ObstacleMap& from);
   void Clear();
   bool IsInitialized() const;
   
@@ -412,72 +755,137 @@ class PointCloudXYZRGB_PointXYZRGB : public ::google::protobuf::Message {
   
   // accessors -------------------------------------------------------
   
-  // required 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);
+  // required .px.HeaderInfo header = 1;
+  inline bool has_header() const;
+  inline void clear_header();
+  static const int kHeaderFieldNumber = 1;
+  inline const ::px::HeaderInfo& header() const;
+  inline ::px::HeaderInfo* mutable_header();
+  inline ::px::HeaderInfo* release_header();
   
-  // required 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);
+  // 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);
   
-  // required 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 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);
   
-  // required float rgb = 4;
-  inline bool has_rgb() const;
-  inline void clear_rgb();
-  static const int kRgbFieldNumber = 4;
-  inline float rgb() const;
-  inline void set_rgb(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);
   
-  // @@protoc_insertion_point(class_scope:px.PointCloudXYZRGB.PointXYZRGB)
+  // 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_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_rgb();
-  inline void clear_has_rgb();
+  inline void set_has_header();
+  inline void clear_has_header();
+  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_;
   
-  float x_;
-  float y_;
-  float z_;
-  float rgb_;
+  ::px::HeaderInfo* header_;
+  ::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_[(4 + 31) / 32];
+  ::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 PointCloudXYZRGB_PointXYZRGB* default_instance_;
+  static ObstacleMap* default_instance_;
 };
 // -------------------------------------------------------------------
 
-class PointCloudXYZRGB : public ::google::protobuf::Message {
+class Path : public ::google::protobuf::Message {
  public:
-  PointCloudXYZRGB();
-  virtual ~PointCloudXYZRGB();
+  Path();
+  virtual ~Path();
   
-  PointCloudXYZRGB(const PointCloudXYZRGB& from);
+  Path(const Path& from);
   
-  inline PointCloudXYZRGB& operator=(const PointCloudXYZRGB& from) {
+  inline Path& operator=(const Path& from) {
     CopyFrom(from);
     return *this;
   }
@@ -491,17 +899,17 @@ class PointCloudXYZRGB : public ::google::protobuf::Message {
   }
   
   static const ::google::protobuf::Descriptor* descriptor();
-  static const PointCloudXYZRGB& default_instance();
+  static const Path& default_instance();
   
-  void Swap(PointCloudXYZRGB* other);
+  void Swap(Path* other);
   
   // implements Message ----------------------------------------------
   
-  PointCloudXYZRGB* New() const;
+  Path* New() const;
   void CopyFrom(const ::google::protobuf::Message& from);
   void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const PointCloudXYZRGB& from);
-  void MergeFrom(const PointCloudXYZRGB& from);
+  void CopyFrom(const Path& from);
+  void MergeFrom(const Path& from);
   void Clear();
   bool IsInitialized() const;
   
@@ -522,8 +930,6 @@ class PointCloudXYZRGB : public ::google::protobuf::Message {
   
   // nested types ----------------------------------------------------
   
-  typedef PointCloudXYZRGB_PointXYZRGB PointXYZRGB;
-  
   // accessors -------------------------------------------------------
   
   // required .px.HeaderInfo header = 1;
@@ -534,19 +940,19 @@ class PointCloudXYZRGB : public ::google::protobuf::Message {
   inline ::px::HeaderInfo* mutable_header();
   inline ::px::HeaderInfo* release_header();
   
-  // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
-  inline int points_size() const;
-  inline void clear_points();
-  static const int kPointsFieldNumber = 2;
-  inline const ::px::PointCloudXYZRGB_PointXYZRGB& points(int index) const;
-  inline ::px::PointCloudXYZRGB_PointXYZRGB* mutable_points(int index);
-  inline ::px::PointCloudXYZRGB_PointXYZRGB* add_points();
-  inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >&
-      points() const;
-  inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >*
-      mutable_points();
+  // repeated .px.Waypoint waypoints = 2;
+  inline int waypoints_size() const;
+  inline void clear_waypoints();
+  static const int kWaypointsFieldNumber = 2;
+  inline const ::px::Waypoint& waypoints(int index) const;
+  inline ::px::Waypoint* mutable_waypoints(int index);
+  inline ::px::Waypoint* add_waypoints();
+  inline const ::google::protobuf::RepeatedPtrField< ::px::Waypoint >&
+      waypoints() const;
+  inline ::google::protobuf::RepeatedPtrField< ::px::Waypoint >*
+      mutable_waypoints();
   
-  // @@protoc_insertion_point(class_scope:px.PointCloudXYZRGB)
+  // @@protoc_insertion_point(class_scope:px.Path)
  private:
   inline void set_has_header();
   inline void clear_has_header();
@@ -554,7 +960,7 @@ class PointCloudXYZRGB : public ::google::protobuf::Message {
   ::google::protobuf::UnknownFieldSet _unknown_fields_;
   
   ::px::HeaderInfo* header_;
-  ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB > points_;
+  ::google::protobuf::RepeatedPtrField< ::px::Waypoint > waypoints_;
   
   mutable int _cached_size_;
   ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32];
@@ -564,18 +970,18 @@ class PointCloudXYZRGB : public ::google::protobuf::Message {
   friend void protobuf_ShutdownFile_pixhawk_2eproto();
   
   void InitAsDefaultInstance();
-  static PointCloudXYZRGB* default_instance_;
+  static Path* default_instance_;
 };
 // -------------------------------------------------------------------
 
-class RGBDImage : public ::google::protobuf::Message {
+class PointCloudXYZI_PointXYZI : public ::google::protobuf::Message {
  public:
-  RGBDImage();
-  virtual ~RGBDImage();
+  PointCloudXYZI_PointXYZI();
+  virtual ~PointCloudXYZI_PointXYZI();
   
-  RGBDImage(const RGBDImage& from);
+  PointCloudXYZI_PointXYZI(const PointCloudXYZI_PointXYZI& from);
   
-  inline RGBDImage& operator=(const RGBDImage& from) {
+  inline PointCloudXYZI_PointXYZI& operator=(const PointCloudXYZI_PointXYZI& from) {
     CopyFrom(from);
     return *this;
   }
@@ -589,17 +995,17 @@ class RGBDImage : public ::google::protobuf::Message {
   }
   
   static const ::google::protobuf::Descriptor* descriptor();
-  static const RGBDImage& default_instance();
+  static const PointCloudXYZI_PointXYZI& default_instance();
   
-  void Swap(RGBDImage* other);
+  void Swap(PointCloudXYZI_PointXYZI* other);
   
   // implements Message ----------------------------------------------
   
-  RGBDImage* New() const;
+  PointCloudXYZI_PointXYZI* New() const;
   void CopyFrom(const ::google::protobuf::Message& from);
   void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const RGBDImage& from);
-  void MergeFrom(const RGBDImage& from);
+  void CopyFrom(const PointCloudXYZI_PointXYZI& from);
+  void MergeFrom(const PointCloudXYZI_PointXYZI& from);
   void Clear();
   bool IsInitialized() const;
   
@@ -622,254 +1028,72 @@ class RGBDImage : public ::google::protobuf::Message {
   
   // accessors -------------------------------------------------------
   
-  // required .px.HeaderInfo header = 1;
-  inline bool has_header() const;
-  inline void clear_header();
-  static const int kHeaderFieldNumber = 1;
-  inline const ::px::HeaderInfo& header() const;
-  inline ::px::HeaderInfo* mutable_header();
-  inline ::px::HeaderInfo* release_header();
-  
-  // required uint32 cols = 2;
-  inline bool has_cols() const;
-  inline void clear_cols();
-  static const int kColsFieldNumber = 2;
-  inline ::google::protobuf::uint32 cols() const;
-  inline void set_cols(::google::protobuf::uint32 value);
+  // required 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);
   
-  // required uint32 rows = 3;
-  inline bool has_rows() const;
-  inline void clear_rows();
-  static const int kRowsFieldNumber = 3;
-  inline ::google::protobuf::uint32 rows() const;
-  inline void set_rows(::google::protobuf::uint32 value);
+  // required 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);
   
-  // required uint32 step1 = 4;
-  inline bool has_step1() const;
-  inline void clear_step1();
-  static const int kStep1FieldNumber = 4;
-  inline ::google::protobuf::uint32 step1() const;
-  inline void set_step1(::google::protobuf::uint32 value);
+  // required 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);
   
-  // required uint32 type1 = 5;
-  inline bool has_type1() const;
-  inline void clear_type1();
-  static const int kType1FieldNumber = 5;
-  inline ::google::protobuf::uint32 type1() const;
-  inline void set_type1(::google::protobuf::uint32 value);
-  
-  // required bytes imageData1 = 6;
-  inline bool has_imagedata1() const;
-  inline void clear_imagedata1();
-  static const int kImageData1FieldNumber = 6;
-  inline const ::std::string& imagedata1() const;
-  inline void set_imagedata1(const ::std::string& value);
-  inline void set_imagedata1(const char* value);
-  inline void set_imagedata1(const void* value, size_t size);
-  inline ::std::string* mutable_imagedata1();
-  inline ::std::string* release_imagedata1();
-  
-  // required uint32 step2 = 7;
-  inline bool has_step2() const;
-  inline void clear_step2();
-  static const int kStep2FieldNumber = 7;
-  inline ::google::protobuf::uint32 step2() const;
-  inline void set_step2(::google::protobuf::uint32 value);
-  
-  // required uint32 type2 = 8;
-  inline bool has_type2() const;
-  inline void clear_type2();
-  static const int kType2FieldNumber = 8;
-  inline ::google::protobuf::uint32 type2() const;
-  inline void set_type2(::google::protobuf::uint32 value);
-  
-  // required bytes imageData2 = 9;
-  inline bool has_imagedata2() const;
-  inline void clear_imagedata2();
-  static const int kImageData2FieldNumber = 9;
-  inline const ::std::string& imagedata2() const;
-  inline void set_imagedata2(const ::std::string& value);
-  inline void set_imagedata2(const char* value);
-  inline void set_imagedata2(const void* value, size_t size);
-  inline ::std::string* mutable_imagedata2();
-  inline ::std::string* release_imagedata2();
-  
-  // optional uint32 camera_config = 10;
-  inline bool has_camera_config() const;
-  inline void clear_camera_config();
-  static const int kCameraConfigFieldNumber = 10;
-  inline ::google::protobuf::uint32 camera_config() const;
-  inline void set_camera_config(::google::protobuf::uint32 value);
-  
-  // optional uint32 camera_type = 11;
-  inline bool has_camera_type() const;
-  inline void clear_camera_type();
-  static const int kCameraTypeFieldNumber = 11;
-  inline ::google::protobuf::uint32 camera_type() const;
-  inline void set_camera_type(::google::protobuf::uint32 value);
-  
-  // optional float roll = 12;
-  inline bool has_roll() const;
-  inline void clear_roll();
-  static const int kRollFieldNumber = 12;
-  inline float roll() const;
-  inline void set_roll(float value);
-  
-  // optional float pitch = 13;
-  inline bool has_pitch() const;
-  inline void clear_pitch();
-  static const int kPitchFieldNumber = 13;
-  inline float pitch() const;
-  inline void set_pitch(float value);
-  
-  // optional float yaw = 14;
-  inline bool has_yaw() const;
-  inline void clear_yaw();
-  static const int kYawFieldNumber = 14;
-  inline float yaw() const;
-  inline void set_yaw(float value);
-  
-  // optional float lon = 15;
-  inline bool has_lon() const;
-  inline void clear_lon();
-  static const int kLonFieldNumber = 15;
-  inline float lon() const;
-  inline void set_lon(float value);
-  
-  // optional float lat = 16;
-  inline bool has_lat() const;
-  inline void clear_lat();
-  static const int kLatFieldNumber = 16;
-  inline float lat() const;
-  inline void set_lat(float value);
-  
-  // optional float alt = 17;
-  inline bool has_alt() const;
-  inline void clear_alt();
-  static const int kAltFieldNumber = 17;
-  inline float alt() const;
-  inline void set_alt(float value);
-  
-  // optional float ground_x = 18;
-  inline bool has_ground_x() const;
-  inline void clear_ground_x();
-  static const int kGroundXFieldNumber = 18;
-  inline float ground_x() const;
-  inline void set_ground_x(float value);
-  
-  // optional float ground_y = 19;
-  inline bool has_ground_y() const;
-  inline void clear_ground_y();
-  static const int kGroundYFieldNumber = 19;
-  inline float ground_y() const;
-  inline void set_ground_y(float value);
-  
-  // optional float ground_z = 20;
-  inline bool has_ground_z() const;
-  inline void clear_ground_z();
-  static const int kGroundZFieldNumber = 20;
-  inline float ground_z() const;
-  inline void set_ground_z(float value);
-  
-  // repeated float camera_matrix = 21;
-  inline int camera_matrix_size() const;
-  inline void clear_camera_matrix();
-  static const int kCameraMatrixFieldNumber = 21;
-  inline float camera_matrix(int index) const;
-  inline void set_camera_matrix(int index, float value);
-  inline void add_camera_matrix(float value);
-  inline const ::google::protobuf::RepeatedField< float >&
-      camera_matrix() const;
-  inline ::google::protobuf::RepeatedField< float >*
-      mutable_camera_matrix();
+  // required float intensity = 4;
+  inline bool has_intensity() const;
+  inline void clear_intensity();
+  static const int kIntensityFieldNumber = 4;
+  inline float intensity() const;
+  inline void set_intensity(float value);
   
-  // @@protoc_insertion_point(class_scope:px.RGBDImage)
+  // @@protoc_insertion_point(class_scope:px.PointCloudXYZI.PointXYZI)
  private:
-  inline void set_has_header();
-  inline void clear_has_header();
-  inline void set_has_cols();
-  inline void clear_has_cols();
-  inline void set_has_rows();
-  inline void clear_has_rows();
-  inline void set_has_step1();
-  inline void clear_has_step1();
-  inline void set_has_type1();
-  inline void clear_has_type1();
-  inline void set_has_imagedata1();
-  inline void clear_has_imagedata1();
-  inline void set_has_step2();
-  inline void clear_has_step2();
-  inline void set_has_type2();
-  inline void clear_has_type2();
-  inline void set_has_imagedata2();
-  inline void clear_has_imagedata2();
-  inline void set_has_camera_config();
-  inline void clear_has_camera_config();
-  inline void set_has_camera_type();
-  inline void clear_has_camera_type();
-  inline void set_has_roll();
-  inline void clear_has_roll();
-  inline void set_has_pitch();
-  inline void clear_has_pitch();
-  inline void set_has_yaw();
-  inline void clear_has_yaw();
-  inline void set_has_lon();
-  inline void clear_has_lon();
-  inline void set_has_lat();
-  inline void clear_has_lat();
-  inline void set_has_alt();
-  inline void clear_has_alt();
-  inline void set_has_ground_x();
-  inline void clear_has_ground_x();
-  inline void set_has_ground_y();
-  inline void clear_has_ground_y();
-  inline void set_has_ground_z();
-  inline void clear_has_ground_z();
+  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_intensity();
+  inline void clear_has_intensity();
   
   ::google::protobuf::UnknownFieldSet _unknown_fields_;
   
-  ::px::HeaderInfo* header_;
-  ::google::protobuf::uint32 cols_;
-  ::google::protobuf::uint32 rows_;
-  ::google::protobuf::uint32 step1_;
-  ::google::protobuf::uint32 type1_;
-  ::std::string* imagedata1_;
-  ::google::protobuf::uint32 step2_;
-  ::google::protobuf::uint32 type2_;
-  ::std::string* imagedata2_;
-  ::google::protobuf::uint32 camera_config_;
-  ::google::protobuf::uint32 camera_type_;
-  float roll_;
-  float pitch_;
-  float yaw_;
-  float lon_;
-  float lat_;
-  float alt_;
-  float ground_x_;
-  float ground_y_;
-  ::google::protobuf::RepeatedField< float > camera_matrix_;
-  float ground_z_;
+  float x_;
+  float y_;
+  float z_;
+  float intensity_;
   
   mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(21 + 31) / 32];
+  ::google::protobuf::uint32 _has_bits_[(4 + 31) / 32];
   
   friend void  protobuf_AddDesc_pixhawk_2eproto();
   friend void protobuf_AssignDesc_pixhawk_2eproto();
   friend void protobuf_ShutdownFile_pixhawk_2eproto();
   
   void InitAsDefaultInstance();
-  static RGBDImage* default_instance_;
+  static PointCloudXYZI_PointXYZI* default_instance_;
 };
 // -------------------------------------------------------------------
 
-class Obstacle : public ::google::protobuf::Message {
+class PointCloudXYZI : public ::google::protobuf::Message {
  public:
-  Obstacle();
-  virtual ~Obstacle();
+  PointCloudXYZI();
+  virtual ~PointCloudXYZI();
   
-  Obstacle(const Obstacle& from);
+  PointCloudXYZI(const PointCloudXYZI& from);
   
-  inline Obstacle& operator=(const Obstacle& from) {
+  inline PointCloudXYZI& operator=(const PointCloudXYZI& from) {
     CopyFrom(from);
     return *this;
   }
@@ -883,17 +1107,17 @@ class Obstacle : public ::google::protobuf::Message {
   }
   
   static const ::google::protobuf::Descriptor* descriptor();
-  static const Obstacle& default_instance();
+  static const PointCloudXYZI& default_instance();
   
-  void Swap(Obstacle* other);
+  void Swap(PointCloudXYZI* other);
   
   // implements Message ----------------------------------------------
   
-  Obstacle* New() const;
+  PointCloudXYZI* 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 CopyFrom(const PointCloudXYZI& from);
+  void MergeFrom(const PointCloudXYZI& from);
   void Clear();
   bool IsInitialized() const;
   
@@ -914,51 +1138,135 @@ class Obstacle : public ::google::protobuf::Message {
   
   // nested types ----------------------------------------------------
   
-  // accessors -------------------------------------------------------
+  typedef PointCloudXYZI_PointXYZI PointXYZI;
   
-  // 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);
+  // accessors -------------------------------------------------------
   
-  // 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);
+  // required .px.HeaderInfo header = 1;
+  inline bool has_header() const;
+  inline void clear_header();
+  static const int kHeaderFieldNumber = 1;
+  inline const ::px::HeaderInfo& header() const;
+  inline ::px::HeaderInfo* mutable_header();
+  inline ::px::HeaderInfo* release_header();
   
-  // optional float z = 3;
-  inline bool has_z() const;
+  // repeated .px.PointCloudXYZI.PointXYZI points = 2;
+  inline int points_size() const;
+  inline void clear_points();
+  static const int kPointsFieldNumber = 2;
+  inline const ::px::PointCloudXYZI_PointXYZI& points(int index) const;
+  inline ::px::PointCloudXYZI_PointXYZI* mutable_points(int index);
+  inline ::px::PointCloudXYZI_PointXYZI* add_points();
+  inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >&
+      points() const;
+  inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >*
+      mutable_points();
+  
+  // @@protoc_insertion_point(class_scope:px.PointCloudXYZI)
+ private:
+  inline void set_has_header();
+  inline void clear_has_header();
+  
+  ::google::protobuf::UnknownFieldSet _unknown_fields_;
+  
+  ::px::HeaderInfo* header_;
+  ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI > points_;
+  
+  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 PointCloudXYZI* default_instance_;
+};
+// -------------------------------------------------------------------
+
+class PointCloudXYZRGB_PointXYZRGB : public ::google::protobuf::Message {
+ public:
+  PointCloudXYZRGB_PointXYZRGB();
+  virtual ~PointCloudXYZRGB_PointXYZRGB();
+  
+  PointCloudXYZRGB_PointXYZRGB(const PointCloudXYZRGB_PointXYZRGB& from);
+  
+  inline PointCloudXYZRGB_PointXYZRGB& operator=(const PointCloudXYZRGB_PointXYZRGB& 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 PointCloudXYZRGB_PointXYZRGB& default_instance();
+  
+  void Swap(PointCloudXYZRGB_PointXYZRGB* other);
+  
+  // implements Message ----------------------------------------------
+  
+  PointCloudXYZRGB_PointXYZRGB* New() const;
+  void CopyFrom(const ::google::protobuf::Message& from);
+  void MergeFrom(const ::google::protobuf::Message& from);
+  void CopyFrom(const PointCloudXYZRGB_PointXYZRGB& from);
+  void MergeFrom(const PointCloudXYZRGB_PointXYZRGB& 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 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);
+  
+  // required 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);
+  
+  // required 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);
+  // required float rgb = 4;
+  inline bool has_rgb() const;
+  inline void clear_rgb();
+  static const int kRgbFieldNumber = 4;
+  inline float rgb() const;
+  inline void set_rgb(float value);
   
-  // @@protoc_insertion_point(class_scope:px.Obstacle)
+  // @@protoc_insertion_point(class_scope:px.PointCloudXYZRGB.PointXYZRGB)
  private:
   inline void set_has_x();
   inline void clear_has_x();
@@ -966,42 +1274,36 @@ class Obstacle : public ::google::protobuf::Message {
   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();
+  inline void set_has_rgb();
+  inline void clear_has_rgb();
   
   ::google::protobuf::UnknownFieldSet _unknown_fields_;
   
   float x_;
   float y_;
   float z_;
-  float length_;
-  float width_;
-  float height_;
+  float rgb_;
   
   mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(6 + 31) / 32];
+  ::google::protobuf::uint32 _has_bits_[(4 + 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_;
+  static PointCloudXYZRGB_PointXYZRGB* default_instance_;
 };
 // -------------------------------------------------------------------
 
-class ObstacleList : public ::google::protobuf::Message {
+class PointCloudXYZRGB : public ::google::protobuf::Message {
  public:
-  ObstacleList();
-  virtual ~ObstacleList();
+  PointCloudXYZRGB();
+  virtual ~PointCloudXYZRGB();
   
-  ObstacleList(const ObstacleList& from);
+  PointCloudXYZRGB(const PointCloudXYZRGB& from);
   
-  inline ObstacleList& operator=(const ObstacleList& from) {
+  inline PointCloudXYZRGB& operator=(const PointCloudXYZRGB& from) {
     CopyFrom(from);
     return *this;
   }
@@ -1015,17 +1317,17 @@ class ObstacleList : public ::google::protobuf::Message {
   }
   
   static const ::google::protobuf::Descriptor* descriptor();
-  static const ObstacleList& default_instance();
+  static const PointCloudXYZRGB& default_instance();
   
-  void Swap(ObstacleList* other);
+  void Swap(PointCloudXYZRGB* other);
   
   // implements Message ----------------------------------------------
   
-  ObstacleList* New() const;
+  PointCloudXYZRGB* 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 CopyFrom(const PointCloudXYZRGB& from);
+  void MergeFrom(const PointCloudXYZRGB& from);
   void Clear();
   bool IsInitialized() const;
   
@@ -1046,6 +1348,8 @@ class ObstacleList : public ::google::protobuf::Message {
   
   // nested types ----------------------------------------------------
   
+  typedef PointCloudXYZRGB_PointXYZRGB PointXYZRGB;
+  
   // accessors -------------------------------------------------------
   
   // required .px.HeaderInfo header = 1;
@@ -1056,19 +1360,19 @@ class ObstacleList : public ::google::protobuf::Message {
   inline ::px::HeaderInfo* mutable_header();
   inline ::px::HeaderInfo* release_header();
   
-  // 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();
+  // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
+  inline int points_size() const;
+  inline void clear_points();
+  static const int kPointsFieldNumber = 2;
+  inline const ::px::PointCloudXYZRGB_PointXYZRGB& points(int index) const;
+  inline ::px::PointCloudXYZRGB_PointXYZRGB* mutable_points(int index);
+  inline ::px::PointCloudXYZRGB_PointXYZRGB* add_points();
+  inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >&
+      points() const;
+  inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >*
+      mutable_points();
   
-  // @@protoc_insertion_point(class_scope:px.ObstacleList)
+  // @@protoc_insertion_point(class_scope:px.PointCloudXYZRGB)
  private:
   inline void set_has_header();
   inline void clear_has_header();
@@ -1076,7 +1380,7 @@ class ObstacleList : public ::google::protobuf::Message {
   ::google::protobuf::UnknownFieldSet _unknown_fields_;
   
   ::px::HeaderInfo* header_;
-  ::google::protobuf::RepeatedPtrField< ::px::Obstacle > obstacles_;
+  ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB > points_;
   
   mutable int _cached_size_;
   ::google::protobuf::uint32 _has_bits_[(2 + 31) / 32];
@@ -1086,18 +1390,18 @@ class ObstacleList : public ::google::protobuf::Message {
   friend void protobuf_ShutdownFile_pixhawk_2eproto();
   
   void InitAsDefaultInstance();
-  static ObstacleList* default_instance_;
+  static PointCloudXYZRGB* default_instance_;
 };
 // -------------------------------------------------------------------
 
-class ObstacleMap : public ::google::protobuf::Message {
+class RGBDImage : public ::google::protobuf::Message {
  public:
-  ObstacleMap();
-  virtual ~ObstacleMap();
+  RGBDImage();
+  virtual ~RGBDImage();
   
-  ObstacleMap(const ObstacleMap& from);
+  RGBDImage(const RGBDImage& from);
   
-  inline ObstacleMap& operator=(const ObstacleMap& from) {
+  inline RGBDImage& operator=(const RGBDImage& from) {
     CopyFrom(from);
     return *this;
   }
@@ -1111,17 +1415,17 @@ class ObstacleMap : public ::google::protobuf::Message {
   }
   
   static const ::google::protobuf::Descriptor* descriptor();
-  static const ObstacleMap& default_instance();
+  static const RGBDImage& default_instance();
   
-  void Swap(ObstacleMap* other);
+  void Swap(RGBDImage* other);
   
   // implements Message ----------------------------------------------
   
-  ObstacleMap* New() const;
+  RGBDImage* 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 CopyFrom(const RGBDImage& from);
+  void MergeFrom(const RGBDImage& from);
   void Clear();
   bool IsInitialized() const;
   
@@ -1152,118 +1456,235 @@ class ObstacleMap : public ::google::protobuf::Message {
   inline ::px::HeaderInfo* mutable_header();
   inline ::px::HeaderInfo* release_header();
   
-  // 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);
+  // required uint32 cols = 2;
+  inline bool has_cols() const;
+  inline void clear_cols();
+  static const int kColsFieldNumber = 2;
+  inline ::google::protobuf::uint32 cols() const;
+  inline void set_cols(::google::protobuf::uint32 value);
   
-  // optional int32 rows = 4;
+  // required uint32 rows = 3;
   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);
+  static const int kRowsFieldNumber = 3;
+  inline ::google::protobuf::uint32 rows() const;
+  inline void set_rows(::google::protobuf::uint32 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);
+  // required uint32 step1 = 4;
+  inline bool has_step1() const;
+  inline void clear_step1();
+  static const int kStep1FieldNumber = 4;
+  inline ::google::protobuf::uint32 step1() const;
+  inline void set_step1(::google::protobuf::uint32 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);
+  // required uint32 type1 = 5;
+  inline bool has_type1() const;
+  inline void clear_type1();
+  static const int kType1FieldNumber = 5;
+  inline ::google::protobuf::uint32 type1() const;
+  inline void set_type1(::google::protobuf::uint32 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);
+  // required bytes imageData1 = 6;
+  inline bool has_imagedata1() const;
+  inline void clear_imagedata1();
+  static const int kImageData1FieldNumber = 6;
+  inline const ::std::string& imagedata1() const;
+  inline void set_imagedata1(const ::std::string& value);
+  inline void set_imagedata1(const char* value);
+  inline void set_imagedata1(const void* value, size_t size);
+  inline ::std::string* mutable_imagedata1();
+  inline ::std::string* release_imagedata1();
   
-  // 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);
+  // required uint32 step2 = 7;
+  inline bool has_step2() const;
+  inline void clear_step2();
+  static const int kStep2FieldNumber = 7;
+  inline ::google::protobuf::uint32 step2() const;
+  inline void set_step2(::google::protobuf::uint32 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();
+  // required uint32 type2 = 8;
+  inline bool has_type2() const;
+  inline void clear_type2();
+  static const int kType2FieldNumber = 8;
+  inline ::google::protobuf::uint32 type2() const;
+  inline void set_type2(::google::protobuf::uint32 value);
   
-  // @@protoc_insertion_point(class_scope:px.ObstacleMap)
+  // required bytes imageData2 = 9;
+  inline bool has_imagedata2() const;
+  inline void clear_imagedata2();
+  static const int kImageData2FieldNumber = 9;
+  inline const ::std::string& imagedata2() const;
+  inline void set_imagedata2(const ::std::string& value);
+  inline void set_imagedata2(const char* value);
+  inline void set_imagedata2(const void* value, size_t size);
+  inline ::std::string* mutable_imagedata2();
+  inline ::std::string* release_imagedata2();
+  
+  // optional uint32 camera_config = 10;
+  inline bool has_camera_config() const;
+  inline void clear_camera_config();
+  static const int kCameraConfigFieldNumber = 10;
+  inline ::google::protobuf::uint32 camera_config() const;
+  inline void set_camera_config(::google::protobuf::uint32 value);
+  
+  // optional uint32 camera_type = 11;
+  inline bool has_camera_type() const;
+  inline void clear_camera_type();
+  static const int kCameraTypeFieldNumber = 11;
+  inline ::google::protobuf::uint32 camera_type() const;
+  inline void set_camera_type(::google::protobuf::uint32 value);
+  
+  // optional float roll = 12;
+  inline bool has_roll() const;
+  inline void clear_roll();
+  static const int kRollFieldNumber = 12;
+  inline float roll() const;
+  inline void set_roll(float value);
+  
+  // optional float pitch = 13;
+  inline bool has_pitch() const;
+  inline void clear_pitch();
+  static const int kPitchFieldNumber = 13;
+  inline float pitch() const;
+  inline void set_pitch(float value);
+  
+  // optional float yaw = 14;
+  inline bool has_yaw() const;
+  inline void clear_yaw();
+  static const int kYawFieldNumber = 14;
+  inline float yaw() const;
+  inline void set_yaw(float value);
+  
+  // optional float lon = 15;
+  inline bool has_lon() const;
+  inline void clear_lon();
+  static const int kLonFieldNumber = 15;
+  inline float lon() const;
+  inline void set_lon(float value);
+  
+  // optional float lat = 16;
+  inline bool has_lat() const;
+  inline void clear_lat();
+  static const int kLatFieldNumber = 16;
+  inline float lat() const;
+  inline void set_lat(float value);
+  
+  // optional float alt = 17;
+  inline bool has_alt() const;
+  inline void clear_alt();
+  static const int kAltFieldNumber = 17;
+  inline float alt() const;
+  inline void set_alt(float value);
+  
+  // optional float ground_x = 18;
+  inline bool has_ground_x() const;
+  inline void clear_ground_x();
+  static const int kGroundXFieldNumber = 18;
+  inline float ground_x() const;
+  inline void set_ground_x(float value);
+  
+  // optional float ground_y = 19;
+  inline bool has_ground_y() const;
+  inline void clear_ground_y();
+  static const int kGroundYFieldNumber = 19;
+  inline float ground_y() const;
+  inline void set_ground_y(float value);
+  
+  // optional float ground_z = 20;
+  inline bool has_ground_z() const;
+  inline void clear_ground_z();
+  static const int kGroundZFieldNumber = 20;
+  inline float ground_z() const;
+  inline void set_ground_z(float value);
+  
+  // repeated float camera_matrix = 21;
+  inline int camera_matrix_size() const;
+  inline void clear_camera_matrix();
+  static const int kCameraMatrixFieldNumber = 21;
+  inline float camera_matrix(int index) const;
+  inline void set_camera_matrix(int index, float value);
+  inline void add_camera_matrix(float value);
+  inline const ::google::protobuf::RepeatedField< float >&
+      camera_matrix() const;
+  inline ::google::protobuf::RepeatedField< float >*
+      mutable_camera_matrix();
+  
+  // @@protoc_insertion_point(class_scope:px.RGBDImage)
  private:
   inline void set_has_header();
   inline void clear_has_header();
-  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();
+  inline void set_has_rows();
+  inline void clear_has_rows();
+  inline void set_has_step1();
+  inline void clear_has_step1();
+  inline void set_has_type1();
+  inline void clear_has_type1();
+  inline void set_has_imagedata1();
+  inline void clear_has_imagedata1();
+  inline void set_has_step2();
+  inline void clear_has_step2();
+  inline void set_has_type2();
+  inline void clear_has_type2();
+  inline void set_has_imagedata2();
+  inline void clear_has_imagedata2();
+  inline void set_has_camera_config();
+  inline void clear_has_camera_config();
+  inline void set_has_camera_type();
+  inline void clear_has_camera_type();
+  inline void set_has_roll();
+  inline void clear_has_roll();
+  inline void set_has_pitch();
+  inline void clear_has_pitch();
+  inline void set_has_yaw();
+  inline void clear_has_yaw();
+  inline void set_has_lon();
+  inline void clear_has_lon();
+  inline void set_has_lat();
+  inline void clear_has_lat();
+  inline void set_has_alt();
+  inline void clear_has_alt();
+  inline void set_has_ground_x();
+  inline void clear_has_ground_x();
+  inline void set_has_ground_y();
+  inline void clear_has_ground_y();
+  inline void set_has_ground_z();
+  inline void clear_has_ground_z();
   
   ::google::protobuf::UnknownFieldSet _unknown_fields_;
   
   ::px::HeaderInfo* header_;
-  ::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_;
+  ::google::protobuf::uint32 cols_;
+  ::google::protobuf::uint32 rows_;
+  ::google::protobuf::uint32 step1_;
+  ::google::protobuf::uint32 type1_;
+  ::std::string* imagedata1_;
+  ::google::protobuf::uint32 step2_;
+  ::google::protobuf::uint32 type2_;
+  ::std::string* imagedata2_;
+  ::google::protobuf::uint32 camera_config_;
+  ::google::protobuf::uint32 camera_type_;
+  float roll_;
+  float pitch_;
+  float yaw_;
+  float lon_;
+  float lat_;
+  float alt_;
+  float ground_x_;
+  float ground_y_;
+  ::google::protobuf::RepeatedField< float > camera_matrix_;
+  float ground_z_;
   
   mutable int _cached_size_;
-  ::google::protobuf::uint32 _has_bits_[(10 + 31) / 32];
+  ::google::protobuf::uint32 _has_bits_[(21 + 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_;
+  static RGBDImage* default_instance_;
 };
 // -------------------------------------------------------------------
 
@@ -1397,103 +1818,7 @@ class Waypoint : public ::google::protobuf::Message {
   void InitAsDefaultInstance();
   static Waypoint* default_instance_;
 };
-// -------------------------------------------------------------------
-
-class Path : public ::google::protobuf::Message {
- public:
-  Path();
-  virtual ~Path();
-  
-  Path(const Path& from);
-  
-  inline Path& operator=(const Path& 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 Path& default_instance();
-  
-  void Swap(Path* other);
-  
-  // implements Message ----------------------------------------------
-  
-  Path* New() const;
-  void CopyFrom(const ::google::protobuf::Message& from);
-  void MergeFrom(const ::google::protobuf::Message& from);
-  void CopyFrom(const Path& from);
-  void MergeFrom(const Path& 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 .px.HeaderInfo header = 1;
-  inline bool has_header() const;
-  inline void clear_header();
-  static const int kHeaderFieldNumber = 1;
-  inline const ::px::HeaderInfo& header() const;
-  inline ::px::HeaderInfo* mutable_header();
-  inline ::px::HeaderInfo* release_header();
-  
-  // repeated .px.Waypoint waypoints = 2;
-  inline int waypoints_size() const;
-  inline void clear_waypoints();
-  static const int kWaypointsFieldNumber = 2;
-  inline const ::px::Waypoint& waypoints(int index) const;
-  inline ::px::Waypoint* mutable_waypoints(int index);
-  inline ::px::Waypoint* add_waypoints();
-  inline const ::google::protobuf::RepeatedPtrField< ::px::Waypoint >&
-      waypoints() const;
-  inline ::google::protobuf::RepeatedPtrField< ::px::Waypoint >*
-      mutable_waypoints();
-  
-  // @@protoc_insertion_point(class_scope:px.Path)
- private:
-  inline void set_has_header();
-  inline void clear_has_header();
-  
-  ::google::protobuf::UnknownFieldSet _unknown_fields_;
-  
-  ::px::HeaderInfo* header_;
-  ::google::protobuf::RepeatedPtrField< ::px::Waypoint > waypoints_;
-  
-  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 Path* default_instance_;
-};
-// ===================================================================
+// ===================================================================
 
 
 // ===================================================================
@@ -1568,1311 +1893,1607 @@ inline void HeaderInfo::set_timestamp(double value) {
 
 // -------------------------------------------------------------------
 
-// PointCloudXYZI_PointXYZI
+// GLOverlay
 
-// required float x = 1;
-inline bool PointCloudXYZI_PointXYZI::has_x() const {
+// required .px.HeaderInfo header = 1;
+inline bool GLOverlay::has_header() const {
   return (_has_bits_[0] & 0x00000001u) != 0;
 }
-inline void PointCloudXYZI_PointXYZI::set_has_x() {
+inline void GLOverlay::set_has_header() {
   _has_bits_[0] |= 0x00000001u;
 }
-inline void PointCloudXYZI_PointXYZI::clear_has_x() {
+inline void GLOverlay::clear_has_header() {
   _has_bits_[0] &= ~0x00000001u;
 }
-inline void PointCloudXYZI_PointXYZI::clear_x() {
-  x_ = 0;
-  clear_has_x();
+inline void GLOverlay::clear_header() {
+  if (header_ != NULL) header_->::px::HeaderInfo::Clear();
+  clear_has_header();
 }
-inline float PointCloudXYZI_PointXYZI::x() const {
-  return x_;
+inline const ::px::HeaderInfo& GLOverlay::header() const {
+  return header_ != NULL ? *header_ : *default_instance_->header_;
 }
-inline void PointCloudXYZI_PointXYZI::set_x(float value) {
-  set_has_x();
-  x_ = value;
+inline ::px::HeaderInfo* GLOverlay::mutable_header() {
+  set_has_header();
+  if (header_ == NULL) header_ = new ::px::HeaderInfo;
+  return header_;
+}
+inline ::px::HeaderInfo* GLOverlay::release_header() {
+  clear_has_header();
+  ::px::HeaderInfo* temp = header_;
+  header_ = NULL;
+  return temp;
 }
 
-// required float y = 2;
-inline bool PointCloudXYZI_PointXYZI::has_y() const {
+// optional string name = 2;
+inline bool GLOverlay::has_name() const {
   return (_has_bits_[0] & 0x00000002u) != 0;
 }
-inline void PointCloudXYZI_PointXYZI::set_has_y() {
+inline void GLOverlay::set_has_name() {
   _has_bits_[0] |= 0x00000002u;
 }
-inline void PointCloudXYZI_PointXYZI::clear_has_y() {
+inline void GLOverlay::clear_has_name() {
   _has_bits_[0] &= ~0x00000002u;
 }
-inline void PointCloudXYZI_PointXYZI::clear_y() {
-  y_ = 0;
-  clear_has_y();
+inline void GLOverlay::clear_name() {
+  if (name_ != &::google::protobuf::internal::kEmptyString) {
+    name_->clear();
+  }
+  clear_has_name();
 }
-inline float PointCloudXYZI_PointXYZI::y() const {
-  return y_;
+inline const ::std::string& GLOverlay::name() const {
+  return *name_;
 }
-inline void PointCloudXYZI_PointXYZI::set_y(float value) {
-  set_has_y();
-  y_ = value;
+inline void GLOverlay::set_name(const ::std::string& value) {
+  set_has_name();
+  if (name_ == &::google::protobuf::internal::kEmptyString) {
+    name_ = new ::std::string;
+  }
+  name_->assign(value);
+}
+inline void GLOverlay::set_name(const char* value) {
+  set_has_name();
+  if (name_ == &::google::protobuf::internal::kEmptyString) {
+    name_ = new ::std::string;
+  }
+  name_->assign(value);
+}
+inline void GLOverlay::set_name(const char* value, size_t size) {
+  set_has_name();
+  if (name_ == &::google::protobuf::internal::kEmptyString) {
+    name_ = new ::std::string;
+  }
+  name_->assign(reinterpret_cast<const char*>(value), size);
+}
+inline ::std::string* GLOverlay::mutable_name() {
+  set_has_name();
+  if (name_ == &::google::protobuf::internal::kEmptyString) {
+    name_ = new ::std::string;
+  }
+  return name_;
+}
+inline ::std::string* GLOverlay::release_name() {
+  clear_has_name();
+  if (name_ == &::google::protobuf::internal::kEmptyString) {
+    return NULL;
+  } else {
+    ::std::string* temp = name_;
+    name_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
+    return temp;
+  }
 }
 
-// required float z = 3;
-inline bool PointCloudXYZI_PointXYZI::has_z() const {
+// optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3;
+inline bool GLOverlay::has_coordinateframetype() const {
   return (_has_bits_[0] & 0x00000004u) != 0;
 }
-inline void PointCloudXYZI_PointXYZI::set_has_z() {
+inline void GLOverlay::set_has_coordinateframetype() {
   _has_bits_[0] |= 0x00000004u;
 }
-inline void PointCloudXYZI_PointXYZI::clear_has_z() {
+inline void GLOverlay::clear_has_coordinateframetype() {
   _has_bits_[0] &= ~0x00000004u;
 }
-inline void PointCloudXYZI_PointXYZI::clear_z() {
-  z_ = 0;
-  clear_has_z();
+inline void GLOverlay::clear_coordinateframetype() {
+  coordinateframetype_ = 0;
+  clear_has_coordinateframetype();
 }
-inline float PointCloudXYZI_PointXYZI::z() const {
-  return z_;
+inline ::px::GLOverlay_CoordinateFrameType GLOverlay::coordinateframetype() const {
+  return static_cast< ::px::GLOverlay_CoordinateFrameType >(coordinateframetype_);
 }
-inline void PointCloudXYZI_PointXYZI::set_z(float value) {
-  set_has_z();
-  z_ = value;
+inline void GLOverlay::set_coordinateframetype(::px::GLOverlay_CoordinateFrameType value) {
+  GOOGLE_DCHECK(::px::GLOverlay_CoordinateFrameType_IsValid(value));
+  set_has_coordinateframetype();
+  coordinateframetype_ = value;
 }
 
-// required float intensity = 4;
-inline bool PointCloudXYZI_PointXYZI::has_intensity() const {
+// optional double origin_x = 4;
+inline bool GLOverlay::has_origin_x() const {
   return (_has_bits_[0] & 0x00000008u) != 0;
 }
-inline void PointCloudXYZI_PointXYZI::set_has_intensity() {
+inline void GLOverlay::set_has_origin_x() {
   _has_bits_[0] |= 0x00000008u;
 }
-inline void PointCloudXYZI_PointXYZI::clear_has_intensity() {
+inline void GLOverlay::clear_has_origin_x() {
   _has_bits_[0] &= ~0x00000008u;
 }
-inline void PointCloudXYZI_PointXYZI::clear_intensity() {
-  intensity_ = 0;
-  clear_has_intensity();
+inline void GLOverlay::clear_origin_x() {
+  origin_x_ = 0;
+  clear_has_origin_x();
 }
-inline float PointCloudXYZI_PointXYZI::intensity() const {
-  return intensity_;
+inline double GLOverlay::origin_x() const {
+  return origin_x_;
 }
-inline void PointCloudXYZI_PointXYZI::set_intensity(float value) {
-  set_has_intensity();
-  intensity_ = value;
+inline void GLOverlay::set_origin_x(double value) {
+  set_has_origin_x();
+  origin_x_ = value;
 }
 
-// -------------------------------------------------------------------
-
-// PointCloudXYZI
-
-// required .px.HeaderInfo header = 1;
-inline bool PointCloudXYZI::has_header() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void PointCloudXYZI::set_has_header() {
-  _has_bits_[0] |= 0x00000001u;
+// optional double origin_y = 5;
+inline bool GLOverlay::has_origin_y() const {
+  return (_has_bits_[0] & 0x00000010u) != 0;
 }
-inline void PointCloudXYZI::clear_has_header() {
-  _has_bits_[0] &= ~0x00000001u;
+inline void GLOverlay::set_has_origin_y() {
+  _has_bits_[0] |= 0x00000010u;
 }
-inline void PointCloudXYZI::clear_header() {
-  if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-  clear_has_header();
+inline void GLOverlay::clear_has_origin_y() {
+  _has_bits_[0] &= ~0x00000010u;
 }
-inline const ::px::HeaderInfo& PointCloudXYZI::header() const {
-  return header_ != NULL ? *header_ : *default_instance_->header_;
+inline void GLOverlay::clear_origin_y() {
+  origin_y_ = 0;
+  clear_has_origin_y();
 }
-inline ::px::HeaderInfo* PointCloudXYZI::mutable_header() {
-  set_has_header();
-  if (header_ == NULL) header_ = new ::px::HeaderInfo;
-  return header_;
+inline double GLOverlay::origin_y() const {
+  return origin_y_;
 }
-inline ::px::HeaderInfo* PointCloudXYZI::release_header() {
-  clear_has_header();
-  ::px::HeaderInfo* temp = header_;
-  header_ = NULL;
-  return temp;
+inline void GLOverlay::set_origin_y(double value) {
+  set_has_origin_y();
+  origin_y_ = value;
 }
 
-// repeated .px.PointCloudXYZI.PointXYZI points = 2;
-inline int PointCloudXYZI::points_size() const {
-  return points_.size();
-}
-inline void PointCloudXYZI::clear_points() {
-  points_.Clear();
+// optional double origin_z = 6;
+inline bool GLOverlay::has_origin_z() const {
+  return (_has_bits_[0] & 0x00000020u) != 0;
 }
-inline const ::px::PointCloudXYZI_PointXYZI& PointCloudXYZI::points(int index) const {
-  return points_.Get(index);
+inline void GLOverlay::set_has_origin_z() {
+  _has_bits_[0] |= 0x00000020u;
 }
-inline ::px::PointCloudXYZI_PointXYZI* PointCloudXYZI::mutable_points(int index) {
-  return points_.Mutable(index);
+inline void GLOverlay::clear_has_origin_z() {
+  _has_bits_[0] &= ~0x00000020u;
 }
-inline ::px::PointCloudXYZI_PointXYZI* PointCloudXYZI::add_points() {
-  return points_.Add();
+inline void GLOverlay::clear_origin_z() {
+  origin_z_ = 0;
+  clear_has_origin_z();
 }
-inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >&
-PointCloudXYZI::points() const {
-  return points_;
+inline double GLOverlay::origin_z() const {
+  return origin_z_;
 }
-inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >*
-PointCloudXYZI::mutable_points() {
-  return &points_;
+inline void GLOverlay::set_origin_z(double value) {
+  set_has_origin_z();
+  origin_z_ = value;
 }
 
-// -------------------------------------------------------------------
-
-// PointCloudXYZRGB_PointXYZRGB
-
-// required float x = 1;
-inline bool PointCloudXYZRGB_PointXYZRGB::has_x() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void PointCloudXYZRGB_PointXYZRGB::set_has_x() {
-  _has_bits_[0] |= 0x00000001u;
+// optional bytes data = 7;
+inline bool GLOverlay::has_data() const {
+  return (_has_bits_[0] & 0x00000040u) != 0;
 }
-inline void PointCloudXYZRGB_PointXYZRGB::clear_has_x() {
-  _has_bits_[0] &= ~0x00000001u;
+inline void GLOverlay::set_has_data() {
+  _has_bits_[0] |= 0x00000040u;
 }
-inline void PointCloudXYZRGB_PointXYZRGB::clear_x() {
-  x_ = 0;
-  clear_has_x();
+inline void GLOverlay::clear_has_data() {
+  _has_bits_[0] &= ~0x00000040u;
 }
-inline float PointCloudXYZRGB_PointXYZRGB::x() const {
-  return x_;
+inline void GLOverlay::clear_data() {
+  if (data_ != &::google::protobuf::internal::kEmptyString) {
+    data_->clear();
+  }
+  clear_has_data();
 }
-inline void PointCloudXYZRGB_PointXYZRGB::set_x(float value) {
-  set_has_x();
-  x_ = value;
+inline const ::std::string& GLOverlay::data() const {
+  return *data_;
 }
-
-// required float y = 2;
-inline bool PointCloudXYZRGB_PointXYZRGB::has_y() const {
-  return (_has_bits_[0] & 0x00000002u) != 0;
+inline void GLOverlay::set_data(const ::std::string& value) {
+  set_has_data();
+  if (data_ == &::google::protobuf::internal::kEmptyString) {
+    data_ = new ::std::string;
+  }
+  data_->assign(value);
 }
-inline void PointCloudXYZRGB_PointXYZRGB::set_has_y() {
-  _has_bits_[0] |= 0x00000002u;
+inline void GLOverlay::set_data(const char* value) {
+  set_has_data();
+  if (data_ == &::google::protobuf::internal::kEmptyString) {
+    data_ = new ::std::string;
+  }
+  data_->assign(value);
 }
-inline void PointCloudXYZRGB_PointXYZRGB::clear_has_y() {
+inline void GLOverlay::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* GLOverlay::mutable_data() {
+  set_has_data();
+  if (data_ == &::google::protobuf::internal::kEmptyString) {
+    data_ = new ::std::string;
+  }
+  return data_;
+}
+inline ::std::string* GLOverlay::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;
+  }
+}
+
+// -------------------------------------------------------------------
+
+// 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 PointCloudXYZRGB_PointXYZRGB::clear_y() {
+inline void Obstacle::clear_y() {
   y_ = 0;
   clear_has_y();
 }
-inline float PointCloudXYZRGB_PointXYZRGB::y() const {
+inline float Obstacle::y() const {
   return y_;
 }
-inline void PointCloudXYZRGB_PointXYZRGB::set_y(float value) {
+inline void Obstacle::set_y(float value) {
   set_has_y();
   y_ = value;
 }
 
-// required float z = 3;
-inline bool PointCloudXYZRGB_PointXYZRGB::has_z() const {
+// optional float z = 3;
+inline bool Obstacle::has_z() const {
   return (_has_bits_[0] & 0x00000004u) != 0;
 }
-inline void PointCloudXYZRGB_PointXYZRGB::set_has_z() {
+inline void Obstacle::set_has_z() {
   _has_bits_[0] |= 0x00000004u;
 }
-inline void PointCloudXYZRGB_PointXYZRGB::clear_has_z() {
+inline void Obstacle::clear_has_z() {
   _has_bits_[0] &= ~0x00000004u;
 }
-inline void PointCloudXYZRGB_PointXYZRGB::clear_z() {
+inline void Obstacle::clear_z() {
   z_ = 0;
   clear_has_z();
 }
-inline float PointCloudXYZRGB_PointXYZRGB::z() const {
+inline float Obstacle::z() const {
   return z_;
 }
-inline void PointCloudXYZRGB_PointXYZRGB::set_z(float value) {
+inline void Obstacle::set_z(float value) {
   set_has_z();
   z_ = value;
 }
 
-// required float rgb = 4;
-inline bool PointCloudXYZRGB_PointXYZRGB::has_rgb() const {
+// optional float length = 4;
+inline bool Obstacle::has_length() const {
   return (_has_bits_[0] & 0x00000008u) != 0;
 }
-inline void PointCloudXYZRGB_PointXYZRGB::set_has_rgb() {
+inline void Obstacle::set_has_length() {
   _has_bits_[0] |= 0x00000008u;
 }
-inline void PointCloudXYZRGB_PointXYZRGB::clear_has_rgb() {
+inline void Obstacle::clear_has_length() {
   _has_bits_[0] &= ~0x00000008u;
 }
-inline void PointCloudXYZRGB_PointXYZRGB::clear_rgb() {
-  rgb_ = 0;
-  clear_has_rgb();
+inline void Obstacle::clear_length() {
+  length_ = 0;
+  clear_has_length();
 }
-inline float PointCloudXYZRGB_PointXYZRGB::rgb() const {
-  return rgb_;
+inline float Obstacle::length() const {
+  return length_;
 }
-inline void PointCloudXYZRGB_PointXYZRGB::set_rgb(float value) {
-  set_has_rgb();
-  rgb_ = value;
+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;
 }
 
 // -------------------------------------------------------------------
 
-// PointCloudXYZRGB
+// ObstacleList
 
 // required .px.HeaderInfo header = 1;
-inline bool PointCloudXYZRGB::has_header() const {
+inline bool ObstacleList::has_header() const {
   return (_has_bits_[0] & 0x00000001u) != 0;
 }
-inline void PointCloudXYZRGB::set_has_header() {
+inline void ObstacleList::set_has_header() {
   _has_bits_[0] |= 0x00000001u;
 }
-inline void PointCloudXYZRGB::clear_has_header() {
+inline void ObstacleList::clear_has_header() {
   _has_bits_[0] &= ~0x00000001u;
 }
-inline void PointCloudXYZRGB::clear_header() {
+inline void ObstacleList::clear_header() {
   if (header_ != NULL) header_->::px::HeaderInfo::Clear();
   clear_has_header();
 }
-inline const ::px::HeaderInfo& PointCloudXYZRGB::header() const {
+inline const ::px::HeaderInfo& ObstacleList::header() const {
   return header_ != NULL ? *header_ : *default_instance_->header_;
 }
-inline ::px::HeaderInfo* PointCloudXYZRGB::mutable_header() {
+inline ::px::HeaderInfo* ObstacleList::mutable_header() {
   set_has_header();
   if (header_ == NULL) header_ = new ::px::HeaderInfo;
   return header_;
 }
-inline ::px::HeaderInfo* PointCloudXYZRGB::release_header() {
+inline ::px::HeaderInfo* ObstacleList::release_header() {
   clear_has_header();
   ::px::HeaderInfo* temp = header_;
   header_ = NULL;
   return temp;
 }
 
-// repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
-inline int PointCloudXYZRGB::points_size() const {
-  return points_.size();
+// repeated .px.Obstacle obstacles = 2;
+inline int ObstacleList::obstacles_size() const {
+  return obstacles_.size();
 }
-inline void PointCloudXYZRGB::clear_points() {
-  points_.Clear();
+inline void ObstacleList::clear_obstacles() {
+  obstacles_.Clear();
 }
-inline const ::px::PointCloudXYZRGB_PointXYZRGB& PointCloudXYZRGB::points(int index) const {
-  return points_.Get(index);
+inline const ::px::Obstacle& ObstacleList::obstacles(int index) const {
+  return obstacles_.Get(index);
 }
-inline ::px::PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB::mutable_points(int index) {
-  return points_.Mutable(index);
+inline ::px::Obstacle* ObstacleList::mutable_obstacles(int index) {
+  return obstacles_.Mutable(index);
 }
-inline ::px::PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB::add_points() {
-  return points_.Add();
+inline ::px::Obstacle* ObstacleList::add_obstacles() {
+  return obstacles_.Add();
 }
-inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >&
-PointCloudXYZRGB::points() const {
-  return points_;
+inline const ::google::protobuf::RepeatedPtrField< ::px::Obstacle >&
+ObstacleList::obstacles() const {
+  return obstacles_;
 }
-inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >*
-PointCloudXYZRGB::mutable_points() {
-  return &points_;
+inline ::google::protobuf::RepeatedPtrField< ::px::Obstacle >*
+ObstacleList::mutable_obstacles() {
+  return &obstacles_;
 }
 
 // -------------------------------------------------------------------
 
-// RGBDImage
+// ObstacleMap
 
 // required .px.HeaderInfo header = 1;
-inline bool RGBDImage::has_header() const {
+inline bool ObstacleMap::has_header() const {
   return (_has_bits_[0] & 0x00000001u) != 0;
 }
-inline void RGBDImage::set_has_header() {
+inline void ObstacleMap::set_has_header() {
   _has_bits_[0] |= 0x00000001u;
 }
-inline void RGBDImage::clear_has_header() {
+inline void ObstacleMap::clear_has_header() {
   _has_bits_[0] &= ~0x00000001u;
 }
-inline void RGBDImage::clear_header() {
+inline void ObstacleMap::clear_header() {
   if (header_ != NULL) header_->::px::HeaderInfo::Clear();
   clear_has_header();
 }
-inline const ::px::HeaderInfo& RGBDImage::header() const {
+inline const ::px::HeaderInfo& ObstacleMap::header() const {
   return header_ != NULL ? *header_ : *default_instance_->header_;
 }
-inline ::px::HeaderInfo* RGBDImage::mutable_header() {
+inline ::px::HeaderInfo* ObstacleMap::mutable_header() {
   set_has_header();
   if (header_ == NULL) header_ = new ::px::HeaderInfo;
   return header_;
 }
-inline ::px::HeaderInfo* RGBDImage::release_header() {
+inline ::px::HeaderInfo* ObstacleMap::release_header() {
   clear_has_header();
   ::px::HeaderInfo* temp = header_;
   header_ = NULL;
   return temp;
 }
 
-// required uint32 cols = 2;
-inline bool RGBDImage::has_cols() const {
+// required int32 type = 2;
+inline bool ObstacleMap::has_type() const {
   return (_has_bits_[0] & 0x00000002u) != 0;
 }
-inline void RGBDImage::set_has_cols() {
+inline void ObstacleMap::set_has_type() {
   _has_bits_[0] |= 0x00000002u;
 }
-inline void RGBDImage::clear_has_cols() {
+inline void ObstacleMap::clear_has_type() {
   _has_bits_[0] &= ~0x00000002u;
 }
-inline void RGBDImage::clear_cols() {
-  cols_ = 0u;
-  clear_has_cols();
+inline void ObstacleMap::clear_type() {
+  type_ = 0;
+  clear_has_type();
 }
-inline ::google::protobuf::uint32 RGBDImage::cols() const {
-  return cols_;
+inline ::google::protobuf::int32 ObstacleMap::type() const {
+  return type_;
 }
-inline void RGBDImage::set_cols(::google::protobuf::uint32 value) {
-  set_has_cols();
-  cols_ = value;
+inline void ObstacleMap::set_type(::google::protobuf::int32 value) {
+  set_has_type();
+  type_ = value;
 }
 
-// required uint32 rows = 3;
-inline bool RGBDImage::has_rows() const {
+// optional float resolution = 3;
+inline bool ObstacleMap::has_resolution() const {
   return (_has_bits_[0] & 0x00000004u) != 0;
 }
-inline void RGBDImage::set_has_rows() {
+inline void ObstacleMap::set_has_resolution() {
   _has_bits_[0] |= 0x00000004u;
 }
-inline void RGBDImage::clear_has_rows() {
+inline void ObstacleMap::clear_has_resolution() {
   _has_bits_[0] &= ~0x00000004u;
 }
-inline void RGBDImage::clear_rows() {
-  rows_ = 0u;
-  clear_has_rows();
+inline void ObstacleMap::clear_resolution() {
+  resolution_ = 0;
+  clear_has_resolution();
 }
-inline ::google::protobuf::uint32 RGBDImage::rows() const {
-  return rows_;
+inline float ObstacleMap::resolution() const {
+  return resolution_;
 }
-inline void RGBDImage::set_rows(::google::protobuf::uint32 value) {
-  set_has_rows();
-  rows_ = value;
+inline void ObstacleMap::set_resolution(float value) {
+  set_has_resolution();
+  resolution_ = value;
 }
 
-// required uint32 step1 = 4;
-inline bool RGBDImage::has_step1() const {
+// optional int32 rows = 4;
+inline bool ObstacleMap::has_rows() const {
   return (_has_bits_[0] & 0x00000008u) != 0;
 }
-inline void RGBDImage::set_has_step1() {
+inline void ObstacleMap::set_has_rows() {
   _has_bits_[0] |= 0x00000008u;
 }
-inline void RGBDImage::clear_has_step1() {
+inline void ObstacleMap::clear_has_rows() {
   _has_bits_[0] &= ~0x00000008u;
 }
-inline void RGBDImage::clear_step1() {
-  step1_ = 0u;
-  clear_has_step1();
+inline void ObstacleMap::clear_rows() {
+  rows_ = 0;
+  clear_has_rows();
 }
-inline ::google::protobuf::uint32 RGBDImage::step1() const {
-  return step1_;
+inline ::google::protobuf::int32 ObstacleMap::rows() const {
+  return rows_;
 }
-inline void RGBDImage::set_step1(::google::protobuf::uint32 value) {
-  set_has_step1();
-  step1_ = value;
+inline void ObstacleMap::set_rows(::google::protobuf::int32 value) {
+  set_has_rows();
+  rows_ = value;
 }
 
-// required uint32 type1 = 5;
-inline bool RGBDImage::has_type1() const {
+// optional int32 cols = 5;
+inline bool ObstacleMap::has_cols() const {
   return (_has_bits_[0] & 0x00000010u) != 0;
 }
-inline void RGBDImage::set_has_type1() {
+inline void ObstacleMap::set_has_cols() {
   _has_bits_[0] |= 0x00000010u;
 }
-inline void RGBDImage::clear_has_type1() {
+inline void ObstacleMap::clear_has_cols() {
   _has_bits_[0] &= ~0x00000010u;
 }
-inline void RGBDImage::clear_type1() {
-  type1_ = 0u;
-  clear_has_type1();
+inline void ObstacleMap::clear_cols() {
+  cols_ = 0;
+  clear_has_cols();
 }
-inline ::google::protobuf::uint32 RGBDImage::type1() const {
-  return type1_;
+inline ::google::protobuf::int32 ObstacleMap::cols() const {
+  return cols_;
 }
-inline void RGBDImage::set_type1(::google::protobuf::uint32 value) {
-  set_has_type1();
-  type1_ = value;
+inline void ObstacleMap::set_cols(::google::protobuf::int32 value) {
+  set_has_cols();
+  cols_ = value;
 }
 
-// required bytes imageData1 = 6;
-inline bool RGBDImage::has_imagedata1() const {
+// optional int32 mapR0 = 6;
+inline bool ObstacleMap::has_mapr0() const {
   return (_has_bits_[0] & 0x00000020u) != 0;
 }
-inline void RGBDImage::set_has_imagedata1() {
+inline void ObstacleMap::set_has_mapr0() {
   _has_bits_[0] |= 0x00000020u;
 }
-inline void RGBDImage::clear_has_imagedata1() {
+inline void ObstacleMap::clear_has_mapr0() {
   _has_bits_[0] &= ~0x00000020u;
 }
-inline void RGBDImage::clear_imagedata1() {
-  if (imagedata1_ != &::google::protobuf::internal::kEmptyString) {
-    imagedata1_->clear();
-  }
-  clear_has_imagedata1();
-}
-inline const ::std::string& RGBDImage::imagedata1() const {
-  return *imagedata1_;
-}
-inline void RGBDImage::set_imagedata1(const ::std::string& value) {
-  set_has_imagedata1();
-  if (imagedata1_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata1_ = new ::std::string;
-  }
-  imagedata1_->assign(value);
-}
-inline void RGBDImage::set_imagedata1(const char* value) {
-  set_has_imagedata1();
-  if (imagedata1_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata1_ = new ::std::string;
-  }
-  imagedata1_->assign(value);
-}
-inline void RGBDImage::set_imagedata1(const void* value, size_t size) {
-  set_has_imagedata1();
-  if (imagedata1_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata1_ = new ::std::string;
-  }
-  imagedata1_->assign(reinterpret_cast<const char*>(value), size);
+inline void ObstacleMap::clear_mapr0() {
+  mapr0_ = 0;
+  clear_has_mapr0();
 }
-inline ::std::string* RGBDImage::mutable_imagedata1() {
-  set_has_imagedata1();
-  if (imagedata1_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata1_ = new ::std::string;
-  }
-  return imagedata1_;
+inline ::google::protobuf::int32 ObstacleMap::mapr0() const {
+  return mapr0_;
 }
-inline ::std::string* RGBDImage::release_imagedata1() {
-  clear_has_imagedata1();
-  if (imagedata1_ == &::google::protobuf::internal::kEmptyString) {
-    return NULL;
-  } else {
-    ::std::string* temp = imagedata1_;
-    imagedata1_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-    return temp;
-  }
+inline void ObstacleMap::set_mapr0(::google::protobuf::int32 value) {
+  set_has_mapr0();
+  mapr0_ = value;
 }
 
-// required uint32 step2 = 7;
-inline bool RGBDImage::has_step2() const {
+// optional int32 mapC0 = 7;
+inline bool ObstacleMap::has_mapc0() const {
   return (_has_bits_[0] & 0x00000040u) != 0;
 }
-inline void RGBDImage::set_has_step2() {
+inline void ObstacleMap::set_has_mapc0() {
   _has_bits_[0] |= 0x00000040u;
 }
-inline void RGBDImage::clear_has_step2() {
+inline void ObstacleMap::clear_has_mapc0() {
   _has_bits_[0] &= ~0x00000040u;
 }
-inline void RGBDImage::clear_step2() {
-  step2_ = 0u;
-  clear_has_step2();
+inline void ObstacleMap::clear_mapc0() {
+  mapc0_ = 0;
+  clear_has_mapc0();
 }
-inline ::google::protobuf::uint32 RGBDImage::step2() const {
-  return step2_;
+inline ::google::protobuf::int32 ObstacleMap::mapc0() const {
+  return mapc0_;
 }
-inline void RGBDImage::set_step2(::google::protobuf::uint32 value) {
-  set_has_step2();
-  step2_ = value;
+inline void ObstacleMap::set_mapc0(::google::protobuf::int32 value) {
+  set_has_mapc0();
+  mapc0_ = value;
 }
 
-// required uint32 type2 = 8;
-inline bool RGBDImage::has_type2() const {
+// optional int32 arrayR0 = 8;
+inline bool ObstacleMap::has_arrayr0() const {
   return (_has_bits_[0] & 0x00000080u) != 0;
 }
-inline void RGBDImage::set_has_type2() {
+inline void ObstacleMap::set_has_arrayr0() {
   _has_bits_[0] |= 0x00000080u;
 }
-inline void RGBDImage::clear_has_type2() {
+inline void ObstacleMap::clear_has_arrayr0() {
   _has_bits_[0] &= ~0x00000080u;
 }
-inline void RGBDImage::clear_type2() {
-  type2_ = 0u;
-  clear_has_type2();
+inline void ObstacleMap::clear_arrayr0() {
+  arrayr0_ = 0;
+  clear_has_arrayr0();
 }
-inline ::google::protobuf::uint32 RGBDImage::type2() const {
-  return type2_;
+inline ::google::protobuf::int32 ObstacleMap::arrayr0() const {
+  return arrayr0_;
 }
-inline void RGBDImage::set_type2(::google::protobuf::uint32 value) {
-  set_has_type2();
-  type2_ = value;
+inline void ObstacleMap::set_arrayr0(::google::protobuf::int32 value) {
+  set_has_arrayr0();
+  arrayr0_ = value;
 }
 
-// required bytes imageData2 = 9;
-inline bool RGBDImage::has_imagedata2() const {
+// optional int32 arrayC0 = 9;
+inline bool ObstacleMap::has_arrayc0() const {
   return (_has_bits_[0] & 0x00000100u) != 0;
 }
-inline void RGBDImage::set_has_imagedata2() {
+inline void ObstacleMap::set_has_arrayc0() {
   _has_bits_[0] |= 0x00000100u;
 }
-inline void RGBDImage::clear_has_imagedata2() {
+inline void ObstacleMap::clear_has_arrayc0() {
   _has_bits_[0] &= ~0x00000100u;
 }
-inline void RGBDImage::clear_imagedata2() {
-  if (imagedata2_ != &::google::protobuf::internal::kEmptyString) {
-    imagedata2_->clear();
+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_imagedata2();
+  clear_has_data();
 }
-inline const ::std::string& RGBDImage::imagedata2() const {
-  return *imagedata2_;
+inline const ::std::string& ObstacleMap::data() const {
+  return *data_;
 }
-inline void RGBDImage::set_imagedata2(const ::std::string& value) {
-  set_has_imagedata2();
-  if (imagedata2_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata2_ = new ::std::string;
+inline void ObstacleMap::set_data(const ::std::string& value) {
+  set_has_data();
+  if (data_ == &::google::protobuf::internal::kEmptyString) {
+    data_ = new ::std::string;
   }
-  imagedata2_->assign(value);
+  data_->assign(value);
 }
-inline void RGBDImage::set_imagedata2(const char* value) {
-  set_has_imagedata2();
-  if (imagedata2_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata2_ = new ::std::string;
+inline void ObstacleMap::set_data(const char* value) {
+  set_has_data();
+  if (data_ == &::google::protobuf::internal::kEmptyString) {
+    data_ = new ::std::string;
   }
-  imagedata2_->assign(value);
+  data_->assign(value);
 }
-inline void RGBDImage::set_imagedata2(const void* value, size_t size) {
-  set_has_imagedata2();
-  if (imagedata2_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata2_ = new ::std::string;
+inline void ObstacleMap::set_data(const void* value, size_t size) {
+  set_has_data();
+  if (data_ == &::google::protobuf::internal::kEmptyString) {
+    data_ = new ::std::string;
   }
-  imagedata2_->assign(reinterpret_cast<const char*>(value), size);
+  data_->assign(reinterpret_cast<const char*>(value), size);
 }
-inline ::std::string* RGBDImage::mutable_imagedata2() {
-  set_has_imagedata2();
-  if (imagedata2_ == &::google::protobuf::internal::kEmptyString) {
-    imagedata2_ = new ::std::string;
+inline ::std::string* ObstacleMap::mutable_data() {
+  set_has_data();
+  if (data_ == &::google::protobuf::internal::kEmptyString) {
+    data_ = new ::std::string;
   }
-  return imagedata2_;
+  return data_;
 }
-inline ::std::string* RGBDImage::release_imagedata2() {
-  clear_has_imagedata2();
-  if (imagedata2_ == &::google::protobuf::internal::kEmptyString) {
+inline ::std::string* ObstacleMap::release_data() {
+  clear_has_data();
+  if (data_ == &::google::protobuf::internal::kEmptyString) {
     return NULL;
   } else {
-    ::std::string* temp = imagedata2_;
-    imagedata2_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
+    ::std::string* temp = data_;
+    data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
     return temp;
   }
 }
 
-// optional uint32 camera_config = 10;
-inline bool RGBDImage::has_camera_config() const {
-  return (_has_bits_[0] & 0x00000200u) != 0;
+// -------------------------------------------------------------------
+
+// Path
+
+// required .px.HeaderInfo header = 1;
+inline bool Path::has_header() const {
+  return (_has_bits_[0] & 0x00000001u) != 0;
 }
-inline void RGBDImage::set_has_camera_config() {
-  _has_bits_[0] |= 0x00000200u;
+inline void Path::set_has_header() {
+  _has_bits_[0] |= 0x00000001u;
 }
-inline void RGBDImage::clear_has_camera_config() {
-  _has_bits_[0] &= ~0x00000200u;
+inline void Path::clear_has_header() {
+  _has_bits_[0] &= ~0x00000001u;
 }
-inline void RGBDImage::clear_camera_config() {
-  camera_config_ = 0u;
-  clear_has_camera_config();
+inline void Path::clear_header() {
+  if (header_ != NULL) header_->::px::HeaderInfo::Clear();
+  clear_has_header();
 }
-inline ::google::protobuf::uint32 RGBDImage::camera_config() const {
-  return camera_config_;
+inline const ::px::HeaderInfo& Path::header() const {
+  return header_ != NULL ? *header_ : *default_instance_->header_;
 }
-inline void RGBDImage::set_camera_config(::google::protobuf::uint32 value) {
-  set_has_camera_config();
-  camera_config_ = value;
+inline ::px::HeaderInfo* Path::mutable_header() {
+  set_has_header();
+  if (header_ == NULL) header_ = new ::px::HeaderInfo;
+  return header_;
+}
+inline ::px::HeaderInfo* Path::release_header() {
+  clear_has_header();
+  ::px::HeaderInfo* temp = header_;
+  header_ = NULL;
+  return temp;
 }
 
-// optional uint32 camera_type = 11;
-inline bool RGBDImage::has_camera_type() const {
-  return (_has_bits_[0] & 0x00000400u) != 0;
+// repeated .px.Waypoint waypoints = 2;
+inline int Path::waypoints_size() const {
+  return waypoints_.size();
 }
-inline void RGBDImage::set_has_camera_type() {
-  _has_bits_[0] |= 0x00000400u;
+inline void Path::clear_waypoints() {
+  waypoints_.Clear();
 }
-inline void RGBDImage::clear_has_camera_type() {
-  _has_bits_[0] &= ~0x00000400u;
+inline const ::px::Waypoint& Path::waypoints(int index) const {
+  return waypoints_.Get(index);
 }
-inline void RGBDImage::clear_camera_type() {
-  camera_type_ = 0u;
-  clear_has_camera_type();
+inline ::px::Waypoint* Path::mutable_waypoints(int index) {
+  return waypoints_.Mutable(index);
 }
-inline ::google::protobuf::uint32 RGBDImage::camera_type() const {
-  return camera_type_;
+inline ::px::Waypoint* Path::add_waypoints() {
+  return waypoints_.Add();
 }
-inline void RGBDImage::set_camera_type(::google::protobuf::uint32 value) {
-  set_has_camera_type();
-  camera_type_ = value;
+inline const ::google::protobuf::RepeatedPtrField< ::px::Waypoint >&
+Path::waypoints() const {
+  return waypoints_;
+}
+inline ::google::protobuf::RepeatedPtrField< ::px::Waypoint >*
+Path::mutable_waypoints() {
+  return &waypoints_;
 }
 
-// optional float roll = 12;
-inline bool RGBDImage::has_roll() const {
-  return (_has_bits_[0] & 0x00000800u) != 0;
+// -------------------------------------------------------------------
+
+// PointCloudXYZI_PointXYZI
+
+// required float x = 1;
+inline bool PointCloudXYZI_PointXYZI::has_x() const {
+  return (_has_bits_[0] & 0x00000001u) != 0;
 }
-inline void RGBDImage::set_has_roll() {
-  _has_bits_[0] |= 0x00000800u;
+inline void PointCloudXYZI_PointXYZI::set_has_x() {
+  _has_bits_[0] |= 0x00000001u;
 }
-inline void RGBDImage::clear_has_roll() {
-  _has_bits_[0] &= ~0x00000800u;
+inline void PointCloudXYZI_PointXYZI::clear_has_x() {
+  _has_bits_[0] &= ~0x00000001u;
 }
-inline void RGBDImage::clear_roll() {
-  roll_ = 0;
-  clear_has_roll();
+inline void PointCloudXYZI_PointXYZI::clear_x() {
+  x_ = 0;
+  clear_has_x();
 }
-inline float RGBDImage::roll() const {
-  return roll_;
+inline float PointCloudXYZI_PointXYZI::x() const {
+  return x_;
 }
-inline void RGBDImage::set_roll(float value) {
-  set_has_roll();
-  roll_ = value;
+inline void PointCloudXYZI_PointXYZI::set_x(float value) {
+  set_has_x();
+  x_ = value;
 }
 
-// optional float pitch = 13;
-inline bool RGBDImage::has_pitch() const {
-  return (_has_bits_[0] & 0x00001000u) != 0;
+// required float y = 2;
+inline bool PointCloudXYZI_PointXYZI::has_y() const {
+  return (_has_bits_[0] & 0x00000002u) != 0;
 }
-inline void RGBDImage::set_has_pitch() {
-  _has_bits_[0] |= 0x00001000u;
+inline void PointCloudXYZI_PointXYZI::set_has_y() {
+  _has_bits_[0] |= 0x00000002u;
 }
-inline void RGBDImage::clear_has_pitch() {
-  _has_bits_[0] &= ~0x00001000u;
+inline void PointCloudXYZI_PointXYZI::clear_has_y() {
+  _has_bits_[0] &= ~0x00000002u;
 }
-inline void RGBDImage::clear_pitch() {
-  pitch_ = 0;
-  clear_has_pitch();
+inline void PointCloudXYZI_PointXYZI::clear_y() {
+  y_ = 0;
+  clear_has_y();
 }
-inline float RGBDImage::pitch() const {
-  return pitch_;
+inline float PointCloudXYZI_PointXYZI::y() const {
+  return y_;
 }
-inline void RGBDImage::set_pitch(float value) {
-  set_has_pitch();
-  pitch_ = value;
+inline void PointCloudXYZI_PointXYZI::set_y(float value) {
+  set_has_y();
+  y_ = value;
 }
 
-// optional float yaw = 14;
-inline bool RGBDImage::has_yaw() const {
-  return (_has_bits_[0] & 0x00002000u) != 0;
+// required float z = 3;
+inline bool PointCloudXYZI_PointXYZI::has_z() const {
+  return (_has_bits_[0] & 0x00000004u) != 0;
 }
-inline void RGBDImage::set_has_yaw() {
-  _has_bits_[0] |= 0x00002000u;
+inline void PointCloudXYZI_PointXYZI::set_has_z() {
+  _has_bits_[0] |= 0x00000004u;
 }
-inline void RGBDImage::clear_has_yaw() {
-  _has_bits_[0] &= ~0x00002000u;
+inline void PointCloudXYZI_PointXYZI::clear_has_z() {
+  _has_bits_[0] &= ~0x00000004u;
 }
-inline void RGBDImage::clear_yaw() {
-  yaw_ = 0;
-  clear_has_yaw();
+inline void PointCloudXYZI_PointXYZI::clear_z() {
+  z_ = 0;
+  clear_has_z();
 }
-inline float RGBDImage::yaw() const {
-  return yaw_;
+inline float PointCloudXYZI_PointXYZI::z() const {
+  return z_;
 }
-inline void RGBDImage::set_yaw(float value) {
-  set_has_yaw();
-  yaw_ = value;
+inline void PointCloudXYZI_PointXYZI::set_z(float value) {
+  set_has_z();
+  z_ = value;
 }
 
-// optional float lon = 15;
-inline bool RGBDImage::has_lon() const {
-  return (_has_bits_[0] & 0x00004000u) != 0;
+// required float intensity = 4;
+inline bool PointCloudXYZI_PointXYZI::has_intensity() const {
+  return (_has_bits_[0] & 0x00000008u) != 0;
 }
-inline void RGBDImage::set_has_lon() {
-  _has_bits_[0] |= 0x00004000u;
+inline void PointCloudXYZI_PointXYZI::set_has_intensity() {
+  _has_bits_[0] |= 0x00000008u;
 }
-inline void RGBDImage::clear_has_lon() {
-  _has_bits_[0] &= ~0x00004000u;
+inline void PointCloudXYZI_PointXYZI::clear_has_intensity() {
+  _has_bits_[0] &= ~0x00000008u;
 }
-inline void RGBDImage::clear_lon() {
-  lon_ = 0;
-  clear_has_lon();
+inline void PointCloudXYZI_PointXYZI::clear_intensity() {
+  intensity_ = 0;
+  clear_has_intensity();
 }
-inline float RGBDImage::lon() const {
-  return lon_;
+inline float PointCloudXYZI_PointXYZI::intensity() const {
+  return intensity_;
 }
-inline void RGBDImage::set_lon(float value) {
-  set_has_lon();
-  lon_ = value;
+inline void PointCloudXYZI_PointXYZI::set_intensity(float value) {
+  set_has_intensity();
+  intensity_ = value;
 }
 
-// optional float lat = 16;
-inline bool RGBDImage::has_lat() const {
-  return (_has_bits_[0] & 0x00008000u) != 0;
+// -------------------------------------------------------------------
+
+// PointCloudXYZI
+
+// required .px.HeaderInfo header = 1;
+inline bool PointCloudXYZI::has_header() const {
+  return (_has_bits_[0] & 0x00000001u) != 0;
 }
-inline void RGBDImage::set_has_lat() {
-  _has_bits_[0] |= 0x00008000u;
+inline void PointCloudXYZI::set_has_header() {
+  _has_bits_[0] |= 0x00000001u;
 }
-inline void RGBDImage::clear_has_lat() {
-  _has_bits_[0] &= ~0x00008000u;
+inline void PointCloudXYZI::clear_has_header() {
+  _has_bits_[0] &= ~0x00000001u;
 }
-inline void RGBDImage::clear_lat() {
-  lat_ = 0;
-  clear_has_lat();
+inline void PointCloudXYZI::clear_header() {
+  if (header_ != NULL) header_->::px::HeaderInfo::Clear();
+  clear_has_header();
 }
-inline float RGBDImage::lat() const {
-  return lat_;
+inline const ::px::HeaderInfo& PointCloudXYZI::header() const {
+  return header_ != NULL ? *header_ : *default_instance_->header_;
 }
-inline void RGBDImage::set_lat(float value) {
-  set_has_lat();
-  lat_ = value;
+inline ::px::HeaderInfo* PointCloudXYZI::mutable_header() {
+  set_has_header();
+  if (header_ == NULL) header_ = new ::px::HeaderInfo;
+  return header_;
+}
+inline ::px::HeaderInfo* PointCloudXYZI::release_header() {
+  clear_has_header();
+  ::px::HeaderInfo* temp = header_;
+  header_ = NULL;
+  return temp;
 }
 
-// optional float alt = 17;
-inline bool RGBDImage::has_alt() const {
-  return (_has_bits_[0] & 0x00010000u) != 0;
+// repeated .px.PointCloudXYZI.PointXYZI points = 2;
+inline int PointCloudXYZI::points_size() const {
+  return points_.size();
 }
-inline void RGBDImage::set_has_alt() {
-  _has_bits_[0] |= 0x00010000u;
+inline void PointCloudXYZI::clear_points() {
+  points_.Clear();
 }
-inline void RGBDImage::clear_has_alt() {
-  _has_bits_[0] &= ~0x00010000u;
+inline const ::px::PointCloudXYZI_PointXYZI& PointCloudXYZI::points(int index) const {
+  return points_.Get(index);
 }
-inline void RGBDImage::clear_alt() {
-  alt_ = 0;
-  clear_has_alt();
-}
-inline float RGBDImage::alt() const {
-  return alt_;
-}
-inline void RGBDImage::set_alt(float value) {
-  set_has_alt();
-  alt_ = value;
-}
-
-// optional float ground_x = 18;
-inline bool RGBDImage::has_ground_x() const {
-  return (_has_bits_[0] & 0x00020000u) != 0;
-}
-inline void RGBDImage::set_has_ground_x() {
-  _has_bits_[0] |= 0x00020000u;
-}
-inline void RGBDImage::clear_has_ground_x() {
-  _has_bits_[0] &= ~0x00020000u;
-}
-inline void RGBDImage::clear_ground_x() {
-  ground_x_ = 0;
-  clear_has_ground_x();
-}
-inline float RGBDImage::ground_x() const {
-  return ground_x_;
-}
-inline void RGBDImage::set_ground_x(float value) {
-  set_has_ground_x();
-  ground_x_ = value;
-}
-
-// optional float ground_y = 19;
-inline bool RGBDImage::has_ground_y() const {
-  return (_has_bits_[0] & 0x00040000u) != 0;
-}
-inline void RGBDImage::set_has_ground_y() {
-  _has_bits_[0] |= 0x00040000u;
-}
-inline void RGBDImage::clear_has_ground_y() {
-  _has_bits_[0] &= ~0x00040000u;
-}
-inline void RGBDImage::clear_ground_y() {
-  ground_y_ = 0;
-  clear_has_ground_y();
-}
-inline float RGBDImage::ground_y() const {
-  return ground_y_;
-}
-inline void RGBDImage::set_ground_y(float value) {
-  set_has_ground_y();
-  ground_y_ = value;
-}
-
-// optional float ground_z = 20;
-inline bool RGBDImage::has_ground_z() const {
-  return (_has_bits_[0] & 0x00080000u) != 0;
-}
-inline void RGBDImage::set_has_ground_z() {
-  _has_bits_[0] |= 0x00080000u;
-}
-inline void RGBDImage::clear_has_ground_z() {
-  _has_bits_[0] &= ~0x00080000u;
-}
-inline void RGBDImage::clear_ground_z() {
-  ground_z_ = 0;
-  clear_has_ground_z();
-}
-inline float RGBDImage::ground_z() const {
-  return ground_z_;
-}
-inline void RGBDImage::set_ground_z(float value) {
-  set_has_ground_z();
-  ground_z_ = value;
-}
-
-// repeated float camera_matrix = 21;
-inline int RGBDImage::camera_matrix_size() const {
-  return camera_matrix_.size();
-}
-inline void RGBDImage::clear_camera_matrix() {
-  camera_matrix_.Clear();
-}
-inline float RGBDImage::camera_matrix(int index) const {
-  return camera_matrix_.Get(index);
-}
-inline void RGBDImage::set_camera_matrix(int index, float value) {
-  camera_matrix_.Set(index, value);
+inline ::px::PointCloudXYZI_PointXYZI* PointCloudXYZI::mutable_points(int index) {
+  return points_.Mutable(index);
 }
-inline void RGBDImage::add_camera_matrix(float value) {
-  camera_matrix_.Add(value);
+inline ::px::PointCloudXYZI_PointXYZI* PointCloudXYZI::add_points() {
+  return points_.Add();
 }
-inline const ::google::protobuf::RepeatedField< float >&
-RGBDImage::camera_matrix() const {
-  return camera_matrix_;
+inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >&
+PointCloudXYZI::points() const {
+  return points_;
 }
-inline ::google::protobuf::RepeatedField< float >*
-RGBDImage::mutable_camera_matrix() {
-  return &camera_matrix_;
+inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZI_PointXYZI >*
+PointCloudXYZI::mutable_points() {
+  return &points_;
 }
 
 // -------------------------------------------------------------------
 
-// Obstacle
+// PointCloudXYZRGB_PointXYZRGB
 
-// optional float x = 1;
-inline bool Obstacle::has_x() const {
+// required float x = 1;
+inline bool PointCloudXYZRGB_PointXYZRGB::has_x() const {
   return (_has_bits_[0] & 0x00000001u) != 0;
 }
-inline void Obstacle::set_has_x() {
+inline void PointCloudXYZRGB_PointXYZRGB::set_has_x() {
   _has_bits_[0] |= 0x00000001u;
 }
-inline void Obstacle::clear_has_x() {
+inline void PointCloudXYZRGB_PointXYZRGB::clear_has_x() {
   _has_bits_[0] &= ~0x00000001u;
 }
-inline void Obstacle::clear_x() {
+inline void PointCloudXYZRGB_PointXYZRGB::clear_x() {
   x_ = 0;
   clear_has_x();
 }
-inline float Obstacle::x() const {
+inline float PointCloudXYZRGB_PointXYZRGB::x() const {
   return x_;
 }
-inline void Obstacle::set_x(float value) {
+inline void PointCloudXYZRGB_PointXYZRGB::set_x(float value) {
   set_has_x();
   x_ = value;
 }
 
-// optional float y = 2;
-inline bool Obstacle::has_y() const {
+// required float y = 2;
+inline bool PointCloudXYZRGB_PointXYZRGB::has_y() const {
   return (_has_bits_[0] & 0x00000002u) != 0;
 }
-inline void Obstacle::set_has_y() {
+inline void PointCloudXYZRGB_PointXYZRGB::set_has_y() {
   _has_bits_[0] |= 0x00000002u;
 }
-inline void Obstacle::clear_has_y() {
+inline void PointCloudXYZRGB_PointXYZRGB::clear_has_y() {
   _has_bits_[0] &= ~0x00000002u;
 }
-inline void Obstacle::clear_y() {
+inline void PointCloudXYZRGB_PointXYZRGB::clear_y() {
   y_ = 0;
   clear_has_y();
 }
-inline float Obstacle::y() const {
+inline float PointCloudXYZRGB_PointXYZRGB::y() const {
   return y_;
 }
-inline void Obstacle::set_y(float value) {
+inline void PointCloudXYZRGB_PointXYZRGB::set_y(float value) {
   set_has_y();
   y_ = value;
 }
 
-// optional float z = 3;
-inline bool Obstacle::has_z() const {
+// required float z = 3;
+inline bool PointCloudXYZRGB_PointXYZRGB::has_z() const {
   return (_has_bits_[0] & 0x00000004u) != 0;
 }
-inline void Obstacle::set_has_z() {
+inline void PointCloudXYZRGB_PointXYZRGB::set_has_z() {
   _has_bits_[0] |= 0x00000004u;
 }
-inline void Obstacle::clear_has_z() {
+inline void PointCloudXYZRGB_PointXYZRGB::clear_has_z() {
   _has_bits_[0] &= ~0x00000004u;
 }
-inline void Obstacle::clear_z() {
+inline void PointCloudXYZRGB_PointXYZRGB::clear_z() {
   z_ = 0;
   clear_has_z();
 }
-inline float Obstacle::z() const {
+inline float PointCloudXYZRGB_PointXYZRGB::z() const {
   return z_;
 }
-inline void Obstacle::set_z(float value) {
+inline void PointCloudXYZRGB_PointXYZRGB::set_z(float value) {
   set_has_z();
   z_ = value;
 }
 
-// optional float length = 4;
-inline bool Obstacle::has_length() const {
+// required float rgb = 4;
+inline bool PointCloudXYZRGB_PointXYZRGB::has_rgb() const {
   return (_has_bits_[0] & 0x00000008u) != 0;
 }
-inline void Obstacle::set_has_length() {
+inline void PointCloudXYZRGB_PointXYZRGB::set_has_rgb() {
   _has_bits_[0] |= 0x00000008u;
 }
-inline void Obstacle::clear_has_length() {
+inline void PointCloudXYZRGB_PointXYZRGB::clear_has_rgb() {
   _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 void PointCloudXYZRGB_PointXYZRGB::clear_rgb() {
+  rgb_ = 0;
+  clear_has_rgb();
 }
-inline float Obstacle::height() const {
-  return height_;
+inline float PointCloudXYZRGB_PointXYZRGB::rgb() const {
+  return rgb_;
 }
-inline void Obstacle::set_height(float value) {
-  set_has_height();
-  height_ = value;
+inline void PointCloudXYZRGB_PointXYZRGB::set_rgb(float value) {
+  set_has_rgb();
+  rgb_ = value;
 }
 
 // -------------------------------------------------------------------
 
-// ObstacleList
+// PointCloudXYZRGB
 
 // required .px.HeaderInfo header = 1;
-inline bool ObstacleList::has_header() const {
+inline bool PointCloudXYZRGB::has_header() const {
   return (_has_bits_[0] & 0x00000001u) != 0;
 }
-inline void ObstacleList::set_has_header() {
+inline void PointCloudXYZRGB::set_has_header() {
   _has_bits_[0] |= 0x00000001u;
 }
-inline void ObstacleList::clear_has_header() {
+inline void PointCloudXYZRGB::clear_has_header() {
   _has_bits_[0] &= ~0x00000001u;
 }
-inline void ObstacleList::clear_header() {
+inline void PointCloudXYZRGB::clear_header() {
   if (header_ != NULL) header_->::px::HeaderInfo::Clear();
   clear_has_header();
 }
-inline const ::px::HeaderInfo& ObstacleList::header() const {
+inline const ::px::HeaderInfo& PointCloudXYZRGB::header() const {
   return header_ != NULL ? *header_ : *default_instance_->header_;
 }
-inline ::px::HeaderInfo* ObstacleList::mutable_header() {
+inline ::px::HeaderInfo* PointCloudXYZRGB::mutable_header() {
   set_has_header();
   if (header_ == NULL) header_ = new ::px::HeaderInfo;
   return header_;
 }
-inline ::px::HeaderInfo* ObstacleList::release_header() {
+inline ::px::HeaderInfo* PointCloudXYZRGB::release_header() {
   clear_has_header();
   ::px::HeaderInfo* temp = header_;
   header_ = NULL;
   return temp;
 }
 
-// repeated .px.Obstacle obstacles = 2;
-inline int ObstacleList::obstacles_size() const {
-  return obstacles_.size();
+// repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
+inline int PointCloudXYZRGB::points_size() const {
+  return points_.size();
 }
-inline void ObstacleList::clear_obstacles() {
-  obstacles_.Clear();
+inline void PointCloudXYZRGB::clear_points() {
+  points_.Clear();
 }
-inline const ::px::Obstacle& ObstacleList::obstacles(int index) const {
-  return obstacles_.Get(index);
+inline const ::px::PointCloudXYZRGB_PointXYZRGB& PointCloudXYZRGB::points(int index) const {
+  return points_.Get(index);
 }
-inline ::px::Obstacle* ObstacleList::mutable_obstacles(int index) {
-  return obstacles_.Mutable(index);
+inline ::px::PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB::mutable_points(int index) {
+  return points_.Mutable(index);
 }
-inline ::px::Obstacle* ObstacleList::add_obstacles() {
-  return obstacles_.Add();
+inline ::px::PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB::add_points() {
+  return points_.Add();
 }
-inline const ::google::protobuf::RepeatedPtrField< ::px::Obstacle >&
-ObstacleList::obstacles() const {
-  return obstacles_;
+inline const ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >&
+PointCloudXYZRGB::points() const {
+  return points_;
 }
-inline ::google::protobuf::RepeatedPtrField< ::px::Obstacle >*
-ObstacleList::mutable_obstacles() {
-  return &obstacles_;
+inline ::google::protobuf::RepeatedPtrField< ::px::PointCloudXYZRGB_PointXYZRGB >*
+PointCloudXYZRGB::mutable_points() {
+  return &points_;
 }
 
 // -------------------------------------------------------------------
 
-// ObstacleMap
+// RGBDImage
 
 // required .px.HeaderInfo header = 1;
-inline bool ObstacleMap::has_header() const {
+inline bool RGBDImage::has_header() const {
   return (_has_bits_[0] & 0x00000001u) != 0;
 }
-inline void ObstacleMap::set_has_header() {
+inline void RGBDImage::set_has_header() {
   _has_bits_[0] |= 0x00000001u;
 }
-inline void ObstacleMap::clear_has_header() {
+inline void RGBDImage::clear_has_header() {
   _has_bits_[0] &= ~0x00000001u;
 }
-inline void ObstacleMap::clear_header() {
+inline void RGBDImage::clear_header() {
   if (header_ != NULL) header_->::px::HeaderInfo::Clear();
   clear_has_header();
 }
-inline const ::px::HeaderInfo& ObstacleMap::header() const {
+inline const ::px::HeaderInfo& RGBDImage::header() const {
   return header_ != NULL ? *header_ : *default_instance_->header_;
 }
-inline ::px::HeaderInfo* ObstacleMap::mutable_header() {
+inline ::px::HeaderInfo* RGBDImage::mutable_header() {
   set_has_header();
   if (header_ == NULL) header_ = new ::px::HeaderInfo;
   return header_;
 }
-inline ::px::HeaderInfo* ObstacleMap::release_header() {
+inline ::px::HeaderInfo* RGBDImage::release_header() {
   clear_has_header();
   ::px::HeaderInfo* temp = header_;
   header_ = NULL;
   return temp;
 }
 
-// required int32 type = 2;
-inline bool ObstacleMap::has_type() const {
+// required uint32 cols = 2;
+inline bool RGBDImage::has_cols() const {
   return (_has_bits_[0] & 0x00000002u) != 0;
 }
-inline void ObstacleMap::set_has_type() {
+inline void RGBDImage::set_has_cols() {
   _has_bits_[0] |= 0x00000002u;
 }
-inline void ObstacleMap::clear_has_type() {
+inline void RGBDImage::clear_has_cols() {
   _has_bits_[0] &= ~0x00000002u;
 }
-inline void ObstacleMap::clear_type() {
-  type_ = 0;
-  clear_has_type();
+inline void RGBDImage::clear_cols() {
+  cols_ = 0u;
+  clear_has_cols();
 }
-inline ::google::protobuf::int32 ObstacleMap::type() const {
-  return type_;
+inline ::google::protobuf::uint32 RGBDImage::cols() const {
+  return cols_;
 }
-inline void ObstacleMap::set_type(::google::protobuf::int32 value) {
-  set_has_type();
-  type_ = value;
+inline void RGBDImage::set_cols(::google::protobuf::uint32 value) {
+  set_has_cols();
+  cols_ = value;
 }
 
-// optional float resolution = 3;
-inline bool ObstacleMap::has_resolution() const {
+// required uint32 rows = 3;
+inline bool RGBDImage::has_rows() const {
   return (_has_bits_[0] & 0x00000004u) != 0;
 }
-inline void ObstacleMap::set_has_resolution() {
+inline void RGBDImage::set_has_rows() {
   _has_bits_[0] |= 0x00000004u;
 }
-inline void ObstacleMap::clear_has_resolution() {
+inline void RGBDImage::clear_has_rows() {
   _has_bits_[0] &= ~0x00000004u;
 }
-inline void ObstacleMap::clear_resolution() {
-  resolution_ = 0;
-  clear_has_resolution();
+inline void RGBDImage::clear_rows() {
+  rows_ = 0u;
+  clear_has_rows();
 }
-inline float ObstacleMap::resolution() const {
-  return resolution_;
+inline ::google::protobuf::uint32 RGBDImage::rows() const {
+  return rows_;
 }
-inline void ObstacleMap::set_resolution(float value) {
-  set_has_resolution();
-  resolution_ = value;
+inline void RGBDImage::set_rows(::google::protobuf::uint32 value) {
+  set_has_rows();
+  rows_ = value;
 }
 
-// optional int32 rows = 4;
-inline bool ObstacleMap::has_rows() const {
+// required uint32 step1 = 4;
+inline bool RGBDImage::has_step1() const {
   return (_has_bits_[0] & 0x00000008u) != 0;
 }
-inline void ObstacleMap::set_has_rows() {
+inline void RGBDImage::set_has_step1() {
   _has_bits_[0] |= 0x00000008u;
 }
-inline void ObstacleMap::clear_has_rows() {
+inline void RGBDImage::clear_has_step1() {
   _has_bits_[0] &= ~0x00000008u;
 }
-inline void ObstacleMap::clear_rows() {
-  rows_ = 0;
-  clear_has_rows();
+inline void RGBDImage::clear_step1() {
+  step1_ = 0u;
+  clear_has_step1();
 }
-inline ::google::protobuf::int32 ObstacleMap::rows() const {
-  return rows_;
+inline ::google::protobuf::uint32 RGBDImage::step1() const {
+  return step1_;
 }
-inline void ObstacleMap::set_rows(::google::protobuf::int32 value) {
-  set_has_rows();
-  rows_ = value;
+inline void RGBDImage::set_step1(::google::protobuf::uint32 value) {
+  set_has_step1();
+  step1_ = value;
 }
 
-// optional int32 cols = 5;
-inline bool ObstacleMap::has_cols() const {
+// required uint32 type1 = 5;
+inline bool RGBDImage::has_type1() const {
   return (_has_bits_[0] & 0x00000010u) != 0;
 }
-inline void ObstacleMap::set_has_cols() {
+inline void RGBDImage::set_has_type1() {
   _has_bits_[0] |= 0x00000010u;
 }
-inline void ObstacleMap::clear_has_cols() {
+inline void RGBDImage::clear_has_type1() {
   _has_bits_[0] &= ~0x00000010u;
 }
-inline void ObstacleMap::clear_cols() {
-  cols_ = 0;
-  clear_has_cols();
+inline void RGBDImage::clear_type1() {
+  type1_ = 0u;
+  clear_has_type1();
 }
-inline ::google::protobuf::int32 ObstacleMap::cols() const {
-  return cols_;
+inline ::google::protobuf::uint32 RGBDImage::type1() const {
+  return type1_;
+}
+inline void RGBDImage::set_type1(::google::protobuf::uint32 value) {
+  set_has_type1();
+  type1_ = value;
+}
+
+// required bytes imageData1 = 6;
+inline bool RGBDImage::has_imagedata1() const {
+  return (_has_bits_[0] & 0x00000020u) != 0;
+}
+inline void RGBDImage::set_has_imagedata1() {
+  _has_bits_[0] |= 0x00000020u;
+}
+inline void RGBDImage::clear_has_imagedata1() {
+  _has_bits_[0] &= ~0x00000020u;
+}
+inline void RGBDImage::clear_imagedata1() {
+  if (imagedata1_ != &::google::protobuf::internal::kEmptyString) {
+    imagedata1_->clear();
+  }
+  clear_has_imagedata1();
+}
+inline const ::std::string& RGBDImage::imagedata1() const {
+  return *imagedata1_;
+}
+inline void RGBDImage::set_imagedata1(const ::std::string& value) {
+  set_has_imagedata1();
+  if (imagedata1_ == &::google::protobuf::internal::kEmptyString) {
+    imagedata1_ = new ::std::string;
+  }
+  imagedata1_->assign(value);
+}
+inline void RGBDImage::set_imagedata1(const char* value) {
+  set_has_imagedata1();
+  if (imagedata1_ == &::google::protobuf::internal::kEmptyString) {
+    imagedata1_ = new ::std::string;
+  }
+  imagedata1_->assign(value);
+}
+inline void RGBDImage::set_imagedata1(const void* value, size_t size) {
+  set_has_imagedata1();
+  if (imagedata1_ == &::google::protobuf::internal::kEmptyString) {
+    imagedata1_ = new ::std::string;
+  }
+  imagedata1_->assign(reinterpret_cast<const char*>(value), size);
+}
+inline ::std::string* RGBDImage::mutable_imagedata1() {
+  set_has_imagedata1();
+  if (imagedata1_ == &::google::protobuf::internal::kEmptyString) {
+    imagedata1_ = new ::std::string;
+  }
+  return imagedata1_;
+}
+inline ::std::string* RGBDImage::release_imagedata1() {
+  clear_has_imagedata1();
+  if (imagedata1_ == &::google::protobuf::internal::kEmptyString) {
+    return NULL;
+  } else {
+    ::std::string* temp = imagedata1_;
+    imagedata1_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
+    return temp;
+  }
+}
+
+// required uint32 step2 = 7;
+inline bool RGBDImage::has_step2() const {
+  return (_has_bits_[0] & 0x00000040u) != 0;
+}
+inline void RGBDImage::set_has_step2() {
+  _has_bits_[0] |= 0x00000040u;
+}
+inline void RGBDImage::clear_has_step2() {
+  _has_bits_[0] &= ~0x00000040u;
+}
+inline void RGBDImage::clear_step2() {
+  step2_ = 0u;
+  clear_has_step2();
+}
+inline ::google::protobuf::uint32 RGBDImage::step2() const {
+  return step2_;
+}
+inline void RGBDImage::set_step2(::google::protobuf::uint32 value) {
+  set_has_step2();
+  step2_ = value;
+}
+
+// required uint32 type2 = 8;
+inline bool RGBDImage::has_type2() const {
+  return (_has_bits_[0] & 0x00000080u) != 0;
+}
+inline void RGBDImage::set_has_type2() {
+  _has_bits_[0] |= 0x00000080u;
+}
+inline void RGBDImage::clear_has_type2() {
+  _has_bits_[0] &= ~0x00000080u;
+}
+inline void RGBDImage::clear_type2() {
+  type2_ = 0u;
+  clear_has_type2();
+}
+inline ::google::protobuf::uint32 RGBDImage::type2() const {
+  return type2_;
+}
+inline void RGBDImage::set_type2(::google::protobuf::uint32 value) {
+  set_has_type2();
+  type2_ = value;
+}
+
+// required bytes imageData2 = 9;
+inline bool RGBDImage::has_imagedata2() const {
+  return (_has_bits_[0] & 0x00000100u) != 0;
+}
+inline void RGBDImage::set_has_imagedata2() {
+  _has_bits_[0] |= 0x00000100u;
+}
+inline void RGBDImage::clear_has_imagedata2() {
+  _has_bits_[0] &= ~0x00000100u;
+}
+inline void RGBDImage::clear_imagedata2() {
+  if (imagedata2_ != &::google::protobuf::internal::kEmptyString) {
+    imagedata2_->clear();
+  }
+  clear_has_imagedata2();
+}
+inline const ::std::string& RGBDImage::imagedata2() const {
+  return *imagedata2_;
+}
+inline void RGBDImage::set_imagedata2(const ::std::string& value) {
+  set_has_imagedata2();
+  if (imagedata2_ == &::google::protobuf::internal::kEmptyString) {
+    imagedata2_ = new ::std::string;
+  }
+  imagedata2_->assign(value);
+}
+inline void RGBDImage::set_imagedata2(const char* value) {
+  set_has_imagedata2();
+  if (imagedata2_ == &::google::protobuf::internal::kEmptyString) {
+    imagedata2_ = new ::std::string;
+  }
+  imagedata2_->assign(value);
+}
+inline void RGBDImage::set_imagedata2(const void* value, size_t size) {
+  set_has_imagedata2();
+  if (imagedata2_ == &::google::protobuf::internal::kEmptyString) {
+    imagedata2_ = new ::std::string;
+  }
+  imagedata2_->assign(reinterpret_cast<const char*>(value), size);
+}
+inline ::std::string* RGBDImage::mutable_imagedata2() {
+  set_has_imagedata2();
+  if (imagedata2_ == &::google::protobuf::internal::kEmptyString) {
+    imagedata2_ = new ::std::string;
+  }
+  return imagedata2_;
+}
+inline ::std::string* RGBDImage::release_imagedata2() {
+  clear_has_imagedata2();
+  if (imagedata2_ == &::google::protobuf::internal::kEmptyString) {
+    return NULL;
+  } else {
+    ::std::string* temp = imagedata2_;
+    imagedata2_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
+    return temp;
+  }
+}
+
+// optional uint32 camera_config = 10;
+inline bool RGBDImage::has_camera_config() const {
+  return (_has_bits_[0] & 0x00000200u) != 0;
+}
+inline void RGBDImage::set_has_camera_config() {
+  _has_bits_[0] |= 0x00000200u;
+}
+inline void RGBDImage::clear_has_camera_config() {
+  _has_bits_[0] &= ~0x00000200u;
+}
+inline void RGBDImage::clear_camera_config() {
+  camera_config_ = 0u;
+  clear_has_camera_config();
+}
+inline ::google::protobuf::uint32 RGBDImage::camera_config() const {
+  return camera_config_;
+}
+inline void RGBDImage::set_camera_config(::google::protobuf::uint32 value) {
+  set_has_camera_config();
+  camera_config_ = value;
+}
+
+// optional uint32 camera_type = 11;
+inline bool RGBDImage::has_camera_type() const {
+  return (_has_bits_[0] & 0x00000400u) != 0;
+}
+inline void RGBDImage::set_has_camera_type() {
+  _has_bits_[0] |= 0x00000400u;
+}
+inline void RGBDImage::clear_has_camera_type() {
+  _has_bits_[0] &= ~0x00000400u;
+}
+inline void RGBDImage::clear_camera_type() {
+  camera_type_ = 0u;
+  clear_has_camera_type();
+}
+inline ::google::protobuf::uint32 RGBDImage::camera_type() const {
+  return camera_type_;
+}
+inline void RGBDImage::set_camera_type(::google::protobuf::uint32 value) {
+  set_has_camera_type();
+  camera_type_ = value;
+}
+
+// optional float roll = 12;
+inline bool RGBDImage::has_roll() const {
+  return (_has_bits_[0] & 0x00000800u) != 0;
+}
+inline void RGBDImage::set_has_roll() {
+  _has_bits_[0] |= 0x00000800u;
+}
+inline void RGBDImage::clear_has_roll() {
+  _has_bits_[0] &= ~0x00000800u;
+}
+inline void RGBDImage::clear_roll() {
+  roll_ = 0;
+  clear_has_roll();
+}
+inline float RGBDImage::roll() const {
+  return roll_;
+}
+inline void RGBDImage::set_roll(float value) {
+  set_has_roll();
+  roll_ = value;
+}
+
+// optional float pitch = 13;
+inline bool RGBDImage::has_pitch() const {
+  return (_has_bits_[0] & 0x00001000u) != 0;
+}
+inline void RGBDImage::set_has_pitch() {
+  _has_bits_[0] |= 0x00001000u;
+}
+inline void RGBDImage::clear_has_pitch() {
+  _has_bits_[0] &= ~0x00001000u;
+}
+inline void RGBDImage::clear_pitch() {
+  pitch_ = 0;
+  clear_has_pitch();
+}
+inline float RGBDImage::pitch() const {
+  return pitch_;
+}
+inline void RGBDImage::set_pitch(float value) {
+  set_has_pitch();
+  pitch_ = value;
+}
+
+// optional float yaw = 14;
+inline bool RGBDImage::has_yaw() const {
+  return (_has_bits_[0] & 0x00002000u) != 0;
+}
+inline void RGBDImage::set_has_yaw() {
+  _has_bits_[0] |= 0x00002000u;
+}
+inline void RGBDImage::clear_has_yaw() {
+  _has_bits_[0] &= ~0x00002000u;
+}
+inline void RGBDImage::clear_yaw() {
+  yaw_ = 0;
+  clear_has_yaw();
+}
+inline float RGBDImage::yaw() const {
+  return yaw_;
+}
+inline void RGBDImage::set_yaw(float value) {
+  set_has_yaw();
+  yaw_ = value;
+}
+
+// optional float lon = 15;
+inline bool RGBDImage::has_lon() const {
+  return (_has_bits_[0] & 0x00004000u) != 0;
+}
+inline void RGBDImage::set_has_lon() {
+  _has_bits_[0] |= 0x00004000u;
+}
+inline void RGBDImage::clear_has_lon() {
+  _has_bits_[0] &= ~0x00004000u;
+}
+inline void RGBDImage::clear_lon() {
+  lon_ = 0;
+  clear_has_lon();
+}
+inline float RGBDImage::lon() const {
+  return lon_;
+}
+inline void RGBDImage::set_lon(float value) {
+  set_has_lon();
+  lon_ = value;
+}
+
+// optional float lat = 16;
+inline bool RGBDImage::has_lat() const {
+  return (_has_bits_[0] & 0x00008000u) != 0;
+}
+inline void RGBDImage::set_has_lat() {
+  _has_bits_[0] |= 0x00008000u;
+}
+inline void RGBDImage::clear_has_lat() {
+  _has_bits_[0] &= ~0x00008000u;
+}
+inline void RGBDImage::clear_lat() {
+  lat_ = 0;
+  clear_has_lat();
+}
+inline float RGBDImage::lat() const {
+  return lat_;
 }
-inline void ObstacleMap::set_cols(::google::protobuf::int32 value) {
-  set_has_cols();
-  cols_ = value;
+inline void RGBDImage::set_lat(float value) {
+  set_has_lat();
+  lat_ = value;
 }
 
-// optional int32 mapR0 = 6;
-inline bool ObstacleMap::has_mapr0() const {
-  return (_has_bits_[0] & 0x00000020u) != 0;
+// optional float alt = 17;
+inline bool RGBDImage::has_alt() const {
+  return (_has_bits_[0] & 0x00010000u) != 0;
 }
-inline void ObstacleMap::set_has_mapr0() {
-  _has_bits_[0] |= 0x00000020u;
+inline void RGBDImage::set_has_alt() {
+  _has_bits_[0] |= 0x00010000u;
 }
-inline void ObstacleMap::clear_has_mapr0() {
-  _has_bits_[0] &= ~0x00000020u;
+inline void RGBDImage::clear_has_alt() {
+  _has_bits_[0] &= ~0x00010000u;
 }
-inline void ObstacleMap::clear_mapr0() {
-  mapr0_ = 0;
-  clear_has_mapr0();
+inline void RGBDImage::clear_alt() {
+  alt_ = 0;
+  clear_has_alt();
 }
-inline ::google::protobuf::int32 ObstacleMap::mapr0() const {
-  return mapr0_;
+inline float RGBDImage::alt() const {
+  return alt_;
 }
-inline void ObstacleMap::set_mapr0(::google::protobuf::int32 value) {
-  set_has_mapr0();
-  mapr0_ = value;
+inline void RGBDImage::set_alt(float value) {
+  set_has_alt();
+  alt_ = value;
 }
 
-// optional int32 mapC0 = 7;
-inline bool ObstacleMap::has_mapc0() const {
-  return (_has_bits_[0] & 0x00000040u) != 0;
+// optional float ground_x = 18;
+inline bool RGBDImage::has_ground_x() const {
+  return (_has_bits_[0] & 0x00020000u) != 0;
 }
-inline void ObstacleMap::set_has_mapc0() {
-  _has_bits_[0] |= 0x00000040u;
+inline void RGBDImage::set_has_ground_x() {
+  _has_bits_[0] |= 0x00020000u;
 }
-inline void ObstacleMap::clear_has_mapc0() {
-  _has_bits_[0] &= ~0x00000040u;
+inline void RGBDImage::clear_has_ground_x() {
+  _has_bits_[0] &= ~0x00020000u;
 }
-inline void ObstacleMap::clear_mapc0() {
-  mapc0_ = 0;
-  clear_has_mapc0();
+inline void RGBDImage::clear_ground_x() {
+  ground_x_ = 0;
+  clear_has_ground_x();
 }
-inline ::google::protobuf::int32 ObstacleMap::mapc0() const {
-  return mapc0_;
+inline float RGBDImage::ground_x() const {
+  return ground_x_;
 }
-inline void ObstacleMap::set_mapc0(::google::protobuf::int32 value) {
-  set_has_mapc0();
-  mapc0_ = value;
+inline void RGBDImage::set_ground_x(float value) {
+  set_has_ground_x();
+  ground_x_ = value;
 }
 
-// optional int32 arrayR0 = 8;
-inline bool ObstacleMap::has_arrayr0() const {
-  return (_has_bits_[0] & 0x00000080u) != 0;
+// optional float ground_y = 19;
+inline bool RGBDImage::has_ground_y() const {
+  return (_has_bits_[0] & 0x00040000u) != 0;
 }
-inline void ObstacleMap::set_has_arrayr0() {
-  _has_bits_[0] |= 0x00000080u;
+inline void RGBDImage::set_has_ground_y() {
+  _has_bits_[0] |= 0x00040000u;
 }
-inline void ObstacleMap::clear_has_arrayr0() {
-  _has_bits_[0] &= ~0x00000080u;
+inline void RGBDImage::clear_has_ground_y() {
+  _has_bits_[0] &= ~0x00040000u;
 }
-inline void ObstacleMap::clear_arrayr0() {
-  arrayr0_ = 0;
-  clear_has_arrayr0();
+inline void RGBDImage::clear_ground_y() {
+  ground_y_ = 0;
+  clear_has_ground_y();
 }
-inline ::google::protobuf::int32 ObstacleMap::arrayr0() const {
-  return arrayr0_;
+inline float RGBDImage::ground_y() const {
+  return ground_y_;
 }
-inline void ObstacleMap::set_arrayr0(::google::protobuf::int32 value) {
-  set_has_arrayr0();
-  arrayr0_ = value;
+inline void RGBDImage::set_ground_y(float value) {
+  set_has_ground_y();
+  ground_y_ = value;
 }
 
-// optional int32 arrayC0 = 9;
-inline bool ObstacleMap::has_arrayc0() const {
-  return (_has_bits_[0] & 0x00000100u) != 0;
+// optional float ground_z = 20;
+inline bool RGBDImage::has_ground_z() const {
+  return (_has_bits_[0] & 0x00080000u) != 0;
 }
-inline void ObstacleMap::set_has_arrayc0() {
-  _has_bits_[0] |= 0x00000100u;
+inline void RGBDImage::set_has_ground_z() {
+  _has_bits_[0] |= 0x00080000u;
 }
-inline void ObstacleMap::clear_has_arrayc0() {
-  _has_bits_[0] &= ~0x00000100u;
+inline void RGBDImage::clear_has_ground_z() {
+  _has_bits_[0] &= ~0x00080000u;
 }
-inline void ObstacleMap::clear_arrayc0() {
-  arrayc0_ = 0;
-  clear_has_arrayc0();
+inline void RGBDImage::clear_ground_z() {
+  ground_z_ = 0;
+  clear_has_ground_z();
 }
-inline ::google::protobuf::int32 ObstacleMap::arrayc0() const {
-  return arrayc0_;
+inline float RGBDImage::ground_z() const {
+  return ground_z_;
 }
-inline void ObstacleMap::set_arrayc0(::google::protobuf::int32 value) {
-  set_has_arrayc0();
-  arrayc0_ = value;
+inline void RGBDImage::set_ground_z(float value) {
+  set_has_ground_z();
+  ground_z_ = 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();
+// repeated float camera_matrix = 21;
+inline int RGBDImage::camera_matrix_size() const {
+  return camera_matrix_.size();
 }
-inline const ::std::string& ObstacleMap::data() const {
-  return *data_;
+inline void RGBDImage::clear_camera_matrix() {
+  camera_matrix_.Clear();
 }
-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 float RGBDImage::camera_matrix(int index) const {
+  return camera_matrix_.Get(index);
 }
-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 RGBDImage::set_camera_matrix(int index, float value) {
+  camera_matrix_.Set(index, 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 void RGBDImage::add_camera_matrix(float value) {
+  camera_matrix_.Add(value);
 }
-inline ::std::string* ObstacleMap::mutable_data() {
-  set_has_data();
-  if (data_ == &::google::protobuf::internal::kEmptyString) {
-    data_ = new ::std::string;
-  }
-  return data_;
+inline const ::google::protobuf::RepeatedField< float >&
+RGBDImage::camera_matrix() const {
+  return camera_matrix_;
 }
-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;
-  }
+inline ::google::protobuf::RepeatedField< float >*
+RGBDImage::mutable_camera_matrix() {
+  return &camera_matrix_;
 }
 
 // -------------------------------------------------------------------
@@ -3011,64 +3632,6 @@ inline void Waypoint::set_yaw(double value) {
   yaw_ = value;
 }
 
-// -------------------------------------------------------------------
-
-// Path
-
-// required .px.HeaderInfo header = 1;
-inline bool Path::has_header() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-inline void Path::set_has_header() {
-  _has_bits_[0] |= 0x00000001u;
-}
-inline void Path::clear_has_header() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline void Path::clear_header() {
-  if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-  clear_has_header();
-}
-inline const ::px::HeaderInfo& Path::header() const {
-  return header_ != NULL ? *header_ : *default_instance_->header_;
-}
-inline ::px::HeaderInfo* Path::mutable_header() {
-  set_has_header();
-  if (header_ == NULL) header_ = new ::px::HeaderInfo;
-  return header_;
-}
-inline ::px::HeaderInfo* Path::release_header() {
-  clear_has_header();
-  ::px::HeaderInfo* temp = header_;
-  header_ = NULL;
-  return temp;
-}
-
-// repeated .px.Waypoint waypoints = 2;
-inline int Path::waypoints_size() const {
-  return waypoints_.size();
-}
-inline void Path::clear_waypoints() {
-  waypoints_.Clear();
-}
-inline const ::px::Waypoint& Path::waypoints(int index) const {
-  return waypoints_.Get(index);
-}
-inline ::px::Waypoint* Path::mutable_waypoints(int index) {
-  return waypoints_.Mutable(index);
-}
-inline ::px::Waypoint* Path::add_waypoints() {
-  return waypoints_.Add();
-}
-inline const ::google::protobuf::RepeatedPtrField< ::px::Waypoint >&
-Path::waypoints() const {
-  return waypoints_;
-}
-inline ::google::protobuf::RepeatedPtrField< ::px::Waypoint >*
-Path::mutable_waypoints() {
-  return &waypoints_;
-}
-
 
 // @@protoc_insertion_point(namespace_scope)
 
@@ -3078,6 +3641,18 @@ Path::mutable_waypoints() {
 namespace google {
 namespace protobuf {
 
+template <>
+inline const EnumDescriptor* GetEnumDescriptor< ::px::GLOverlay_CoordinateFrameType>() {
+  return ::px::GLOverlay_CoordinateFrameType_descriptor();
+}
+template <>
+inline const EnumDescriptor* GetEnumDescriptor< ::px::GLOverlay_Mode>() {
+  return ::px::GLOverlay_Mode_descriptor();
+}
+template <>
+inline const EnumDescriptor* GetEnumDescriptor< ::px::GLOverlay_Identifier>() {
+  return ::px::GLOverlay_Identifier_descriptor();
+}
 
 }  // namespace google
 }  // namespace protobuf
diff --git a/thirdParty/mavlink/message_definitions/pixhawk.proto b/thirdParty/mavlink/message_definitions/pixhawk.proto
index e658cf575c1175c60a0020099d9bb85e652d717d..991dcee8a241f6070025db4b4eebf899a71c957b 100644
--- a/thirdParty/mavlink/message_definitions/pixhawk.proto
+++ b/thirdParty/mavlink/message_definitions/pixhawk.proto
@@ -1,11 +1,107 @@
 package px;
 
-message HeaderInfo {
+message HeaderInfo
+{
     required int32 source_sysid = 1;
     required int32 source_compid = 2;
     required double timestamp = 3; // in seconds
 }
 
+message GLOverlay
+{
+    required HeaderInfo header = 1;
+
+    optional string name = 2;
+    
+    enum CoordinateFrameType
+    {
+        GLOBAL = 0;
+        LOCAL = 1;
+    }
+    optional CoordinateFrameType coordinateFrameType = 3;
+
+    optional double origin_x = 4;
+    optional double origin_y = 5;
+    optional double origin_z = 6;
+    
+    optional bytes data = 7;
+
+    enum Mode
+    {
+        POINTS = 0;
+        LINES = 1;
+        LINE_STRIP = 2;
+        LINE_LOOP = 3;
+        TRIANGLES = 4;
+        TRIANGLE_STRIP = 5;
+        TRIANGLE_FAN = 6;
+        QUADS = 7;
+        QUAD_STRIP = 8;
+        POLYGON = 9;
+        SOLID_CIRCLE = 10;
+        WIRE_CIRCLE = 11;
+        SOLID_CUBE = 12;
+        WIRE_CUBE = 13;
+    }
+
+    enum Identifier
+    {
+        END = 14;
+        VERTEX2F = 15;
+        VERTEX3F = 16;
+        ROTATEF = 17;
+        TRANSLATEF = 18;
+        SCALEF = 19;
+        PUSH_MATRIX = 20;
+        POP_MATRIX = 21;
+        COLOR3F = 22;
+        COLOR4F = 23;
+        POINTSIZE = 24;
+        LINEWIDTH = 25;
+    }
+}
+
+message Obstacle
+{
+    optional float x = 1;
+    optional float y = 2;
+    optional float z = 3;
+    optional float length = 4;
+    optional float width = 5;
+    optional float height = 6;
+}
+
+message ObstacleList
+{
+    required HeaderInfo header = 1;
+
+    repeated Obstacle obstacles = 2;
+}
+
+message ObstacleMap
+{
+    required HeaderInfo header = 1;
+
+    required int32 type = 2;
+
+    optional float resolution = 3;
+    optional int32 rows = 4;
+    optional int32 cols = 5;
+    optional int32 mapR0 = 6;
+    optional int32 mapC0 = 7;
+    optional int32 arrayR0 = 8;
+    optional int32 arrayC0 = 9;
+    
+    optional bytes data = 10;
+}
+
+message Path
+{
+    required HeaderInfo header = 1;
+
+    repeated Waypoint waypoints = 2;
+}
+
 message PointCloudXYZI {
     message PointXYZI {
         required float x = 1;
@@ -34,16 +130,16 @@ message RGBDImage
 {
     required HeaderInfo header = 1;
 
-    required uint32 cols = 2;		///< Number of columns in image(s)
-    required uint32 rows = 3;		///< Number of rows in image(s)
-    required uint32 step1 = 4;		///< Step (stride) of image 1
-    required uint32 type1 = 5;		///< Type of image 1
+    required uint32 cols = 2;        ///< Number of columns in image(s)
+    required uint32 rows = 3;        ///< Number of rows in image(s)
+    required uint32 step1 = 4;        ///< Step (stride) of image 1
+    required uint32 type1 = 5;        ///< Type of image 1
     required bytes imageData1 = 6;
-    required uint32 step2 = 7;		///< Step (stride) of image 2
-    required uint32 type2 = 8;		///< Type of image 2
+    required uint32 step2 = 7;        ///< Step (stride) of image 2
+    required uint32 type2 = 8;        ///< Type of image 2
     required bytes imageData2 = 9;
-    optional uint32 camera_config = 10;	///< PxSHM::Camera enumeration
-    optional uint32 camera_type = 11;	///< PxSHM::CameraType enumeration
+    optional uint32 camera_config = 10;    ///< PxSHM::Camera enumeration
+    optional uint32 camera_type = 11;    ///< PxSHM::CameraType enumeration
     optional float roll = 12;
     optional float pitch = 13;
     optional float yaw = 14;
@@ -56,40 +152,6 @@ message RGBDImage
     repeated float camera_matrix = 21;
 }
 
-message Obstacle
-{
-    optional float x = 1;
-    optional float y = 2;
-    optional float z = 3;
-    optional float length = 4;
-    optional float width = 5;
-    optional float height = 6;
-}
-
-message ObstacleList
-{
-    required HeaderInfo header = 1;
-
-    repeated Obstacle obstacles = 2;
-}
-
-message ObstacleMap
-{
-    required HeaderInfo header = 1;
-
-    required int32 type = 2;
-
-    optional float resolution = 3;
-    optional int32 rows = 4;
-    optional int32 cols = 5;
-    optional int32 mapR0 = 6;
-    optional int32 mapC0 = 7;
-    optional int32 arrayR0 = 8;
-    optional int32 arrayC0 = 9;
-    
-    optional bytes data = 10;
-}
-
 message Waypoint
 {
     required double x = 1;
@@ -99,10 +161,3 @@ message Waypoint
     optional double pitch = 5;
     optional double yaw = 6;
 }
-
-message Path
-{
-    required HeaderInfo header = 1;
-
-    repeated Waypoint waypoints = 2;
-}
diff --git a/thirdParty/mavlink/message_definitions/sensesoar.xml b/thirdParty/mavlink/message_definitions/sensesoar.xml
new file mode 100644
index 0000000000000000000000000000000000000000..4c7140f93d690128b2e0feaf51f8ec9292875110
--- /dev/null
+++ b/thirdParty/mavlink/message_definitions/sensesoar.xml
@@ -0,0 +1,83 @@
+<?xml version='1.0'?>
+<mavlink>
+     <include>common.xml</include>
+     <enums>
+          <enum name="SENSESOAR_MODE">
+               <description> Different flight modes </description>
+               <entry name="SENSESOAR_MODE_GLIDING">  Gliding mode with motors off</entry>
+               <entry name="SENSESOAR_MODE_AUTONOMOUS"> Autonomous flight</entry>
+               <entry name="SENSESOAR_MODE_MANUAL"> RC controlled</entry>
+          </enum>
+     </enums>
+     <messages>
+          <message id="170" name="OBS_POSITION">
+     Position estimate of the observer in global frame
+     <field type="int32_t" name="lon">Longitude expressed in 1E7</field>
+               <field type="int32_t" name="lat">Latitude expressed in 1E7</field>
+               <field type="int32_t" name="alt">Altitude expressed in milimeters</field>
+          </message>
+          <message id="172" name="OBS_VELOCITY">
+     velocity estimate of the observer in NED inertial frame
+     <field type="float[3]" name="vel">Velocity</field>
+          </message>
+          <message id="174" name="OBS_ATTITUDE">
+     attitude estimate of the observer
+     <field type="double[4]" name="quat">Quaternion re;im</field>
+          </message>
+          <message id="176" name="OBS_WIND">
+     Wind estimate in NED inertial frame
+     <field type="float[3]" name="wind">Wind</field>
+          </message>
+          <message id="178" name="OBS_AIR_VELOCITY">
+     Estimate of the air velocity
+     <field type="float" name="magnitude">Air speed</field>
+               <field type="float" name="aoa">angle of attack</field>
+               <field type="float" name="slip">slip angle</field>
+          </message>
+          <message id="180" name="OBS_BIAS">
+     IMU biases
+     <field type="float[3]" name="accBias">accelerometer bias</field>
+               <field type="float[3]" name="gyroBias">gyroscope bias</field>
+          </message>
+          <message id="182" name="OBS_QFF">
+     estimate of the pressure at sea level
+     <field type="float" name="qff">Wind</field>
+          </message>
+          <message id="183" name="OBS_AIR_TEMP">
+     ambient air temperature
+     <field type="float" name="airT">Air Temperatur</field>
+          </message>
+          <message id="184" name="FILT_ROT_VEL">
+     filtered rotational velocity
+     <field type="float[3]" name="rotVel">rotational velocity</field>
+          </message>
+          <message id="186" name="LLC_OUT">
+     low level control output
+     <field type="int16_t[4]" name="servoOut">Servo signal</field>
+               <field type="int16_t[2]" name="MotorOut">motor signal</field>
+          </message>
+          <message id="188" name="PM_ELEC">
+     Power managment
+     <field type="float" name="PwCons">current power consumption</field>
+               <field type="float" name="BatStat">battery status</field>
+               <field type="float[3]" name="PwGen">Power generation from each module</field>
+          </message>
+          <message id="190" name="SYS_Stat">
+     system status
+     <field type="uint8_t" name="gps">gps status</field>
+               <field type="uint8_t" name="act">actuator status</field>
+               <field type="uint8_t" name="mod">module status</field>
+               <field type="uint8_t" name="commRssi">module status</field>
+          </message>
+          <message id="192" name="CMD_AIRSPEED_CHNG">
+     change commanded air speed
+     <field type="uint8_t" name="target">Target ID</field>
+               <field type="float" name="spCmd">commanded airspeed</field>
+          </message>
+          <message id="194" name="CMD_AIRSPEED_ACK">
+     accept change of airspeed
+     <field type="float" name="spCmd">commanded airspeed</field>
+               <field type="uint8_t" name="ack">0:ack, 1:nack</field>
+          </message>
+     </messages>
+</mavlink>
diff --git a/thirdParty/mavlink/src/pixhawk/pixhawk.pb.cc b/thirdParty/mavlink/src/pixhawk/pixhawk.pb.cc
index 2e2058224a421351b8c5cff77601cc322272d30c..e984f512af21c5887512715425f30a1ea27303aa 100644
--- a/thirdParty/mavlink/src/pixhawk/pixhawk.pb.cc
+++ b/thirdParty/mavlink/src/pixhawk/pixhawk.pb.cc
@@ -20,6 +20,24 @@ namespace {
 const ::google::protobuf::Descriptor* HeaderInfo_descriptor_ = NULL;
 const ::google::protobuf::internal::GeneratedMessageReflection*
   HeaderInfo_reflection_ = NULL;
+const ::google::protobuf::Descriptor* GLOverlay_descriptor_ = NULL;
+const ::google::protobuf::internal::GeneratedMessageReflection*
+  GLOverlay_reflection_ = NULL;
+const ::google::protobuf::EnumDescriptor* GLOverlay_CoordinateFrameType_descriptor_ = NULL;
+const ::google::protobuf::EnumDescriptor* GLOverlay_Mode_descriptor_ = NULL;
+const ::google::protobuf::EnumDescriptor* GLOverlay_Identifier_descriptor_ = NULL;
+const ::google::protobuf::Descriptor* Obstacle_descriptor_ = NULL;
+const ::google::protobuf::internal::GeneratedMessageReflection*
+  Obstacle_reflection_ = NULL;
+const ::google::protobuf::Descriptor* ObstacleList_descriptor_ = NULL;
+const ::google::protobuf::internal::GeneratedMessageReflection*
+  ObstacleList_reflection_ = NULL;
+const ::google::protobuf::Descriptor* ObstacleMap_descriptor_ = NULL;
+const ::google::protobuf::internal::GeneratedMessageReflection*
+  ObstacleMap_reflection_ = NULL;
+const ::google::protobuf::Descriptor* Path_descriptor_ = NULL;
+const ::google::protobuf::internal::GeneratedMessageReflection*
+  Path_reflection_ = NULL;
 const ::google::protobuf::Descriptor* PointCloudXYZI_descriptor_ = NULL;
 const ::google::protobuf::internal::GeneratedMessageReflection*
   PointCloudXYZI_reflection_ = NULL;
@@ -35,21 +53,9 @@ const ::google::protobuf::internal::GeneratedMessageReflection*
 const ::google::protobuf::Descriptor* RGBDImage_descriptor_ = NULL;
 const ::google::protobuf::internal::GeneratedMessageReflection*
   RGBDImage_reflection_ = NULL;
-const ::google::protobuf::Descriptor* Obstacle_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  Obstacle_reflection_ = NULL;
-const ::google::protobuf::Descriptor* ObstacleList_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  ObstacleList_reflection_ = NULL;
-const ::google::protobuf::Descriptor* ObstacleMap_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  ObstacleMap_reflection_ = NULL;
 const ::google::protobuf::Descriptor* Waypoint_descriptor_ = NULL;
 const ::google::protobuf::internal::GeneratedMessageReflection*
   Waypoint_reflection_ = NULL;
-const ::google::protobuf::Descriptor* Path_descriptor_ = NULL;
-const ::google::protobuf::internal::GeneratedMessageReflection*
-  Path_reflection_ = NULL;
 
 }  // namespace
 
@@ -77,7 +83,107 @@ void protobuf_AssignDesc_pixhawk_2eproto() {
       ::google::protobuf::DescriptorPool::generated_pool(),
       ::google::protobuf::MessageFactory::generated_factory(),
       sizeof(HeaderInfo));
-  PointCloudXYZI_descriptor_ = file->message_type(1);
+  GLOverlay_descriptor_ = file->message_type(1);
+  static const int GLOverlay_offsets_[7] = {
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, header_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, name_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, coordinateframetype_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, origin_x_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, origin_y_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, origin_z_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, data_),
+  };
+  GLOverlay_reflection_ =
+    new ::google::protobuf::internal::GeneratedMessageReflection(
+      GLOverlay_descriptor_,
+      GLOverlay::default_instance_,
+      GLOverlay_offsets_,
+      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, _has_bits_[0]),
+      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(GLOverlay, _unknown_fields_),
+      -1,
+      ::google::protobuf::DescriptorPool::generated_pool(),
+      ::google::protobuf::MessageFactory::generated_factory(),
+      sizeof(GLOverlay));
+  GLOverlay_CoordinateFrameType_descriptor_ = GLOverlay_descriptor_->enum_type(0);
+  GLOverlay_Mode_descriptor_ = GLOverlay_descriptor_->enum_type(1);
+  GLOverlay_Identifier_descriptor_ = GLOverlay_descriptor_->enum_type(2);
+  Obstacle_descriptor_ = file->message_type(2);
+  static const int Obstacle_offsets_[6] = {
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, x_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, y_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, z_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, length_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, width_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, height_),
+  };
+  Obstacle_reflection_ =
+    new ::google::protobuf::internal::GeneratedMessageReflection(
+      Obstacle_descriptor_,
+      Obstacle::default_instance_,
+      Obstacle_offsets_,
+      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, _has_bits_[0]),
+      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, _unknown_fields_),
+      -1,
+      ::google::protobuf::DescriptorPool::generated_pool(),
+      ::google::protobuf::MessageFactory::generated_factory(),
+      sizeof(Obstacle));
+  ObstacleList_descriptor_ = file->message_type(3);
+  static const int ObstacleList_offsets_[2] = {
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, header_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, obstacles_),
+  };
+  ObstacleList_reflection_ =
+    new ::google::protobuf::internal::GeneratedMessageReflection(
+      ObstacleList_descriptor_,
+      ObstacleList::default_instance_,
+      ObstacleList_offsets_,
+      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, _has_bits_[0]),
+      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, _unknown_fields_),
+      -1,
+      ::google::protobuf::DescriptorPool::generated_pool(),
+      ::google::protobuf::MessageFactory::generated_factory(),
+      sizeof(ObstacleList));
+  ObstacleMap_descriptor_ = file->message_type(4);
+  static const int ObstacleMap_offsets_[10] = {
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, header_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, type_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, resolution_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, rows_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, cols_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, mapr0_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, mapc0_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, arrayr0_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, arrayc0_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, data_),
+  };
+  ObstacleMap_reflection_ =
+    new ::google::protobuf::internal::GeneratedMessageReflection(
+      ObstacleMap_descriptor_,
+      ObstacleMap::default_instance_,
+      ObstacleMap_offsets_,
+      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, _has_bits_[0]),
+      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, _unknown_fields_),
+      -1,
+      ::google::protobuf::DescriptorPool::generated_pool(),
+      ::google::protobuf::MessageFactory::generated_factory(),
+      sizeof(ObstacleMap));
+  Path_descriptor_ = file->message_type(5);
+  static const int Path_offsets_[2] = {
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, header_),
+    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, waypoints_),
+  };
+  Path_reflection_ =
+    new ::google::protobuf::internal::GeneratedMessageReflection(
+      Path_descriptor_,
+      Path::default_instance_,
+      Path_offsets_,
+      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, _has_bits_[0]),
+      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, _unknown_fields_),
+      -1,
+      ::google::protobuf::DescriptorPool::generated_pool(),
+      ::google::protobuf::MessageFactory::generated_factory(),
+      sizeof(Path));
+  PointCloudXYZI_descriptor_ = file->message_type(6);
   static const int PointCloudXYZI_offsets_[2] = {
     GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI, header_),
     GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZI, points_),
@@ -111,7 +217,7 @@ void protobuf_AssignDesc_pixhawk_2eproto() {
       ::google::protobuf::DescriptorPool::generated_pool(),
       ::google::protobuf::MessageFactory::generated_factory(),
       sizeof(PointCloudXYZI_PointXYZI));
-  PointCloudXYZRGB_descriptor_ = file->message_type(2);
+  PointCloudXYZRGB_descriptor_ = file->message_type(7);
   static const int PointCloudXYZRGB_offsets_[2] = {
     GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB, header_),
     GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(PointCloudXYZRGB, points_),
@@ -145,7 +251,7 @@ void protobuf_AssignDesc_pixhawk_2eproto() {
       ::google::protobuf::DescriptorPool::generated_pool(),
       ::google::protobuf::MessageFactory::generated_factory(),
       sizeof(PointCloudXYZRGB_PointXYZRGB));
-  RGBDImage_descriptor_ = file->message_type(3);
+  RGBDImage_descriptor_ = file->message_type(8);
   static const int RGBDImage_offsets_[21] = {
     GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, header_),
     GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(RGBDImage, cols_),
@@ -180,67 +286,7 @@ void protobuf_AssignDesc_pixhawk_2eproto() {
       ::google::protobuf::DescriptorPool::generated_pool(),
       ::google::protobuf::MessageFactory::generated_factory(),
       sizeof(RGBDImage));
-  Obstacle_descriptor_ = file->message_type(4);
-  static const int Obstacle_offsets_[6] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, x_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, y_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, z_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, length_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, width_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, height_),
-  };
-  Obstacle_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      Obstacle_descriptor_,
-      Obstacle::default_instance_,
-      Obstacle_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Obstacle, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(Obstacle));
-  ObstacleList_descriptor_ = file->message_type(5);
-  static const int ObstacleList_offsets_[2] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, header_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, obstacles_),
-  };
-  ObstacleList_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      ObstacleList_descriptor_,
-      ObstacleList::default_instance_,
-      ObstacleList_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleList, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(ObstacleList));
-  ObstacleMap_descriptor_ = file->message_type(6);
-  static const int ObstacleMap_offsets_[10] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, header_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, type_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, resolution_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, rows_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, cols_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, mapr0_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, mapc0_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, arrayr0_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, arrayc0_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, data_),
-  };
-  ObstacleMap_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      ObstacleMap_descriptor_,
-      ObstacleMap::default_instance_,
-      ObstacleMap_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(ObstacleMap, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(ObstacleMap));
-  Waypoint_descriptor_ = file->message_type(7);
+  Waypoint_descriptor_ = file->message_type(9);
   static const int Waypoint_offsets_[6] = {
     GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, x_),
     GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Waypoint, y_),
@@ -260,22 +306,6 @@ void protobuf_AssignDesc_pixhawk_2eproto() {
       ::google::protobuf::DescriptorPool::generated_pool(),
       ::google::protobuf::MessageFactory::generated_factory(),
       sizeof(Waypoint));
-  Path_descriptor_ = file->message_type(8);
-  static const int Path_offsets_[2] = {
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, header_),
-    GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, waypoints_),
-  };
-  Path_reflection_ =
-    new ::google::protobuf::internal::GeneratedMessageReflection(
-      Path_descriptor_,
-      Path::default_instance_,
-      Path_offsets_,
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, _has_bits_[0]),
-      GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(Path, _unknown_fields_),
-      -1,
-      ::google::protobuf::DescriptorPool::generated_pool(),
-      ::google::protobuf::MessageFactory::generated_factory(),
-      sizeof(Path));
 }
 
 namespace {
@@ -290,6 +320,16 @@ void protobuf_RegisterTypes(const ::std::string&) {
   protobuf_AssignDescriptorsOnce();
   ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
     HeaderInfo_descriptor_, &HeaderInfo::default_instance());
+  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
+    GLOverlay_descriptor_, &GLOverlay::default_instance());
+  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
+    Obstacle_descriptor_, &Obstacle::default_instance());
+  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
+    ObstacleList_descriptor_, &ObstacleList::default_instance());
+  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
+    ObstacleMap_descriptor_, &ObstacleMap::default_instance());
+  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
+    Path_descriptor_, &Path::default_instance());
   ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
     PointCloudXYZI_descriptor_, &PointCloudXYZI::default_instance());
   ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
@@ -300,16 +340,8 @@ void protobuf_RegisterTypes(const ::std::string&) {
     PointCloudXYZRGB_PointXYZRGB_descriptor_, &PointCloudXYZRGB_PointXYZRGB::default_instance());
   ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
     RGBDImage_descriptor_, &RGBDImage::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    Obstacle_descriptor_, &Obstacle::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    ObstacleList_descriptor_, &ObstacleList::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    ObstacleMap_descriptor_, &ObstacleMap::default_instance());
   ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
     Waypoint_descriptor_, &Waypoint::default_instance());
-  ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
-    Path_descriptor_, &Path::default_instance());
 }
 
 }  // namespace
@@ -317,6 +349,16 @@ void protobuf_RegisterTypes(const ::std::string&) {
 void protobuf_ShutdownFile_pixhawk_2eproto() {
   delete HeaderInfo::default_instance_;
   delete HeaderInfo_reflection_;
+  delete GLOverlay::default_instance_;
+  delete GLOverlay_reflection_;
+  delete Obstacle::default_instance_;
+  delete Obstacle_reflection_;
+  delete ObstacleList::default_instance_;
+  delete ObstacleList_reflection_;
+  delete ObstacleMap::default_instance_;
+  delete ObstacleMap_reflection_;
+  delete Path::default_instance_;
+  delete Path_reflection_;
   delete PointCloudXYZI::default_instance_;
   delete PointCloudXYZI_reflection_;
   delete PointCloudXYZI_PointXYZI::default_instance_;
@@ -327,16 +369,8 @@ void protobuf_ShutdownFile_pixhawk_2eproto() {
   delete PointCloudXYZRGB_PointXYZRGB_reflection_;
   delete RGBDImage::default_instance_;
   delete RGBDImage_reflection_;
-  delete Obstacle::default_instance_;
-  delete Obstacle_reflection_;
-  delete ObstacleList::default_instance_;
-  delete ObstacleList_reflection_;
-  delete ObstacleMap::default_instance_;
-  delete ObstacleMap_reflection_;
   delete Waypoint::default_instance_;
   delete Waypoint_reflection_;
-  delete Path::default_instance_;
-  delete Path_reflection_;
 }
 
 void protobuf_AddDesc_pixhawk_2eproto() {
@@ -348,61 +382,80 @@ void protobuf_AddDesc_pixhawk_2eproto() {
   ::google::protobuf::DescriptorPool::InternalAddGeneratedFile(
     "\n\rpixhawk.proto\022\002px\"L\n\nHeaderInfo\022\024\n\014sou"
     "rce_sysid\030\001 \002(\005\022\025\n\rsource_compid\030\002 \002(\005\022\021"
-    "\n\ttimestamp\030\003 \002(\001\"\237\001\n\016PointCloudXYZI\022\036\n\006"
-    "header\030\001 \002(\0132\016.px.HeaderInfo\022,\n\006points\030\002"
-    " \003(\0132\034.px.PointCloudXYZI.PointXYZI\032\?\n\tPo"
-    "intXYZI\022\t\n\001x\030\001 \002(\002\022\t\n\001y\030\002 \002(\002\022\t\n\001z\030\003 \002(\002"
-    "\022\021\n\tintensity\030\004 \002(\002\"\241\001\n\020PointCloudXYZRGB"
-    "\022\036\n\006header\030\001 \002(\0132\016.px.HeaderInfo\0220\n\006poin"
-    "ts\030\002 \003(\0132 .px.PointCloudXYZRGB.PointXYZR"
-    "GB\032;\n\013PointXYZRGB\022\t\n\001x\030\001 \002(\002\022\t\n\001y\030\002 \002(\002\022"
-    "\t\n\001z\030\003 \002(\002\022\013\n\003rgb\030\004 \002(\002\"\365\002\n\tRGBDImage\022\036\n"
-    "\006header\030\001 \002(\0132\016.px.HeaderInfo\022\014\n\004cols\030\002 "
-    "\002(\r\022\014\n\004rows\030\003 \002(\r\022\r\n\005step1\030\004 \002(\r\022\r\n\005type"
-    "1\030\005 \002(\r\022\022\n\nimageData1\030\006 \002(\014\022\r\n\005step2\030\007 \002"
-    "(\r\022\r\n\005type2\030\010 \002(\r\022\022\n\nimageData2\030\t \002(\014\022\025\n"
-    "\rcamera_config\030\n \001(\r\022\023\n\013camera_type\030\013 \001("
-    "\r\022\014\n\004roll\030\014 \001(\002\022\r\n\005pitch\030\r \001(\002\022\013\n\003yaw\030\016 "
-    "\001(\002\022\013\n\003lon\030\017 \001(\002\022\013\n\003lat\030\020 \001(\002\022\013\n\003alt\030\021 \001"
-    "(\002\022\020\n\010ground_x\030\022 \001(\002\022\020\n\010ground_y\030\023 \001(\002\022\020"
-    "\n\010ground_z\030\024 \001(\002\022\025\n\rcamera_matrix\030\025 \003(\002\""
-    "Z\n\010Obstacle\022\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\t\n\001z\030\003"
-    " \001(\002\022\016\n\006length\030\004 \001(\002\022\r\n\005width\030\005 \001(\002\022\016\n\006h"
-    "eight\030\006 \001(\002\"O\n\014ObstacleList\022\036\n\006header\030\001 "
-    "\002(\0132\016.px.HeaderInfo\022\037\n\tobstacles\030\002 \003(\0132\014"
-    ".px.Obstacle\"\271\001\n\013ObstacleMap\022\036\n\006header\030\001"
-    " \002(\0132\016.px.HeaderInfo\022\014\n\004type\030\002 \002(\005\022\022\n\nre"
-    "solution\030\003 \001(\002\022\014\n\004rows\030\004 \001(\005\022\014\n\004cols\030\005 \001"
-    "(\005\022\r\n\005mapR0\030\006 \001(\005\022\r\n\005mapC0\030\007 \001(\005\022\017\n\007arra"
-    "yR0\030\010 \001(\005\022\017\n\007arrayC0\030\t \001(\005\022\014\n\004data\030\n \001(\014"
-    "\"U\n\010Waypoint\022\t\n\001x\030\001 \002(\001\022\t\n\001y\030\002 \002(\001\022\t\n\001z\030"
-    "\003 \001(\001\022\014\n\004roll\030\004 \001(\001\022\r\n\005pitch\030\005 \001(\001\022\013\n\003ya"
-    "w\030\006 \001(\001\"G\n\004Path\022\036\n\006header\030\001 \002(\0132\016.px.Hea"
-    "derInfo\022\037\n\twaypoints\030\002 \003(\0132\014.px.Waypoint", 1320);
+    "\n\ttimestamp\030\003 \002(\001\"\377\004\n\tGLOverlay\022\036\n\006heade"
+    "r\030\001 \002(\0132\016.px.HeaderInfo\022\014\n\004name\030\002 \001(\t\022>\n"
+    "\023coordinateFrameType\030\003 \001(\0162!.px.GLOverla"
+    "y.CoordinateFrameType\022\020\n\010origin_x\030\004 \001(\001\022"
+    "\020\n\010origin_y\030\005 \001(\001\022\020\n\010origin_z\030\006 \001(\001\022\014\n\004d"
+    "ata\030\007 \001(\014\",\n\023CoordinateFrameType\022\n\n\006GLOB"
+    "AL\020\000\022\t\n\005LOCAL\020\001\"\333\001\n\004Mode\022\n\n\006POINTS\020\000\022\t\n\005"
+    "LINES\020\001\022\016\n\nLINE_STRIP\020\002\022\r\n\tLINE_LOOP\020\003\022\r"
+    "\n\tTRIANGLES\020\004\022\022\n\016TRIANGLE_STRIP\020\005\022\020\n\014TRI"
+    "ANGLE_FAN\020\006\022\t\n\005QUADS\020\007\022\016\n\nQUAD_STRIP\020\010\022\013"
+    "\n\007POLYGON\020\t\022\020\n\014SOLID_CIRCLE\020\n\022\017\n\013WIRE_CI"
+    "RCLE\020\013\022\016\n\nSOLID_CUBE\020\014\022\r\n\tWIRE_CUBE\020\r\"\263\001"
+    "\n\nIdentifier\022\007\n\003END\020\016\022\014\n\010VERTEX2F\020\017\022\014\n\010V"
+    "ERTEX3F\020\020\022\013\n\007ROTATEF\020\021\022\016\n\nTRANSLATEF\020\022\022\n"
+    "\n\006SCALEF\020\023\022\017\n\013PUSH_MATRIX\020\024\022\016\n\nPOP_MATRI"
+    "X\020\025\022\013\n\007COLOR3F\020\026\022\013\n\007COLOR4F\020\027\022\r\n\tPOINTSI"
+    "ZE\020\030\022\r\n\tLINEWIDTH\020\031\"Z\n\010Obstacle\022\t\n\001x\030\001 \001"
+    "(\002\022\t\n\001y\030\002 \001(\002\022\t\n\001z\030\003 \001(\002\022\016\n\006length\030\004 \001(\002"
+    "\022\r\n\005width\030\005 \001(\002\022\016\n\006height\030\006 \001(\002\"O\n\014Obsta"
+    "cleList\022\036\n\006header\030\001 \002(\0132\016.px.HeaderInfo\022"
+    "\037\n\tobstacles\030\002 \003(\0132\014.px.Obstacle\"\271\001\n\013Obs"
+    "tacleMap\022\036\n\006header\030\001 \002(\0132\016.px.HeaderInfo"
+    "\022\014\n\004type\030\002 \002(\005\022\022\n\nresolution\030\003 \001(\002\022\014\n\004ro"
+    "ws\030\004 \001(\005\022\014\n\004cols\030\005 \001(\005\022\r\n\005mapR0\030\006 \001(\005\022\r\n"
+    "\005mapC0\030\007 \001(\005\022\017\n\007arrayR0\030\010 \001(\005\022\017\n\007arrayC0"
+    "\030\t \001(\005\022\014\n\004data\030\n \001(\014\"G\n\004Path\022\036\n\006header\030\001"
+    " \002(\0132\016.px.HeaderInfo\022\037\n\twaypoints\030\002 \003(\0132"
+    "\014.px.Waypoint\"\237\001\n\016PointCloudXYZI\022\036\n\006head"
+    "er\030\001 \002(\0132\016.px.HeaderInfo\022,\n\006points\030\002 \003(\013"
+    "2\034.px.PointCloudXYZI.PointXYZI\032\?\n\tPointX"
+    "YZI\022\t\n\001x\030\001 \002(\002\022\t\n\001y\030\002 \002(\002\022\t\n\001z\030\003 \002(\002\022\021\n\t"
+    "intensity\030\004 \002(\002\"\241\001\n\020PointCloudXYZRGB\022\036\n\006"
+    "header\030\001 \002(\0132\016.px.HeaderInfo\0220\n\006points\030\002"
+    " \003(\0132 .px.PointCloudXYZRGB.PointXYZRGB\032;"
+    "\n\013PointXYZRGB\022\t\n\001x\030\001 \002(\002\022\t\n\001y\030\002 \002(\002\022\t\n\001z"
+    "\030\003 \002(\002\022\013\n\003rgb\030\004 \002(\002\"\365\002\n\tRGBDImage\022\036\n\006hea"
+    "der\030\001 \002(\0132\016.px.HeaderInfo\022\014\n\004cols\030\002 \002(\r\022"
+    "\014\n\004rows\030\003 \002(\r\022\r\n\005step1\030\004 \002(\r\022\r\n\005type1\030\005 "
+    "\002(\r\022\022\n\nimageData1\030\006 \002(\014\022\r\n\005step2\030\007 \002(\r\022\r"
+    "\n\005type2\030\010 \002(\r\022\022\n\nimageData2\030\t \002(\014\022\025\n\rcam"
+    "era_config\030\n \001(\r\022\023\n\013camera_type\030\013 \001(\r\022\014\n"
+    "\004roll\030\014 \001(\002\022\r\n\005pitch\030\r \001(\002\022\013\n\003yaw\030\016 \001(\002\022"
+    "\013\n\003lon\030\017 \001(\002\022\013\n\003lat\030\020 \001(\002\022\013\n\003alt\030\021 \001(\002\022\020"
+    "\n\010ground_x\030\022 \001(\002\022\020\n\010ground_y\030\023 \001(\002\022\020\n\010gr"
+    "ound_z\030\024 \001(\002\022\025\n\rcamera_matrix\030\025 \003(\002\"U\n\010W"
+    "aypoint\022\t\n\001x\030\001 \002(\001\022\t\n\001y\030\002 \002(\001\022\t\n\001z\030\003 \001(\001"
+    "\022\014\n\004roll\030\004 \001(\001\022\r\n\005pitch\030\005 \001(\001\022\013\n\003yaw\030\006 \001"
+    "(\001", 1962);
   ::google::protobuf::MessageFactory::InternalRegisterGeneratedFile(
     "pixhawk.proto", &protobuf_RegisterTypes);
   HeaderInfo::default_instance_ = new HeaderInfo();
+  GLOverlay::default_instance_ = new GLOverlay();
+  Obstacle::default_instance_ = new Obstacle();
+  ObstacleList::default_instance_ = new ObstacleList();
+  ObstacleMap::default_instance_ = new ObstacleMap();
+  Path::default_instance_ = new Path();
   PointCloudXYZI::default_instance_ = new PointCloudXYZI();
   PointCloudXYZI_PointXYZI::default_instance_ = new PointCloudXYZI_PointXYZI();
   PointCloudXYZRGB::default_instance_ = new PointCloudXYZRGB();
   PointCloudXYZRGB_PointXYZRGB::default_instance_ = new PointCloudXYZRGB_PointXYZRGB();
   RGBDImage::default_instance_ = new RGBDImage();
-  Obstacle::default_instance_ = new Obstacle();
-  ObstacleList::default_instance_ = new ObstacleList();
-  ObstacleMap::default_instance_ = new ObstacleMap();
   Waypoint::default_instance_ = new Waypoint();
-  Path::default_instance_ = new Path();
   HeaderInfo::default_instance_->InitAsDefaultInstance();
+  GLOverlay::default_instance_->InitAsDefaultInstance();
+  Obstacle::default_instance_->InitAsDefaultInstance();
+  ObstacleList::default_instance_->InitAsDefaultInstance();
+  ObstacleMap::default_instance_->InitAsDefaultInstance();
+  Path::default_instance_->InitAsDefaultInstance();
   PointCloudXYZI::default_instance_->InitAsDefaultInstance();
   PointCloudXYZI_PointXYZI::default_instance_->InitAsDefaultInstance();
   PointCloudXYZRGB::default_instance_->InitAsDefaultInstance();
   PointCloudXYZRGB_PointXYZRGB::default_instance_->InitAsDefaultInstance();
   RGBDImage::default_instance_->InitAsDefaultInstance();
-  Obstacle::default_instance_->InitAsDefaultInstance();
-  ObstacleList::default_instance_->InitAsDefaultInstance();
-  ObstacleMap::default_instance_->InitAsDefaultInstance();
   Waypoint::default_instance_->InitAsDefaultInstance();
-  Path::default_instance_->InitAsDefaultInstance();
   ::google::protobuf::internal::OnShutdown(&protobuf_ShutdownFile_pixhawk_2eproto);
 }
 
@@ -702,138 +755,322 @@ void HeaderInfo::Swap(HeaderInfo* other) {
 
 // ===================================================================
 
+const ::google::protobuf::EnumDescriptor* GLOverlay_CoordinateFrameType_descriptor() {
+  protobuf_AssignDescriptorsOnce();
+  return GLOverlay_CoordinateFrameType_descriptor_;
+}
+bool GLOverlay_CoordinateFrameType_IsValid(int value) {
+  switch(value) {
+    case 0:
+    case 1:
+      return true;
+    default:
+      return false;
+  }
+}
+
 #ifndef _MSC_VER
-const int PointCloudXYZI_PointXYZI::kXFieldNumber;
-const int PointCloudXYZI_PointXYZI::kYFieldNumber;
-const int PointCloudXYZI_PointXYZI::kZFieldNumber;
-const int PointCloudXYZI_PointXYZI::kIntensityFieldNumber;
+const GLOverlay_CoordinateFrameType GLOverlay::GLOBAL;
+const GLOverlay_CoordinateFrameType GLOverlay::LOCAL;
+const GLOverlay_CoordinateFrameType GLOverlay::CoordinateFrameType_MIN;
+const GLOverlay_CoordinateFrameType GLOverlay::CoordinateFrameType_MAX;
+const int GLOverlay::CoordinateFrameType_ARRAYSIZE;
+#endif  // _MSC_VER
+const ::google::protobuf::EnumDescriptor* GLOverlay_Mode_descriptor() {
+  protobuf_AssignDescriptorsOnce();
+  return GLOverlay_Mode_descriptor_;
+}
+bool GLOverlay_Mode_IsValid(int value) {
+  switch(value) {
+    case 0:
+    case 1:
+    case 2:
+    case 3:
+    case 4:
+    case 5:
+    case 6:
+    case 7:
+    case 8:
+    case 9:
+    case 10:
+    case 11:
+    case 12:
+    case 13:
+      return true;
+    default:
+      return false;
+  }
+}
+
+#ifndef _MSC_VER
+const GLOverlay_Mode GLOverlay::POINTS;
+const GLOverlay_Mode GLOverlay::LINES;
+const GLOverlay_Mode GLOverlay::LINE_STRIP;
+const GLOverlay_Mode GLOverlay::LINE_LOOP;
+const GLOverlay_Mode GLOverlay::TRIANGLES;
+const GLOverlay_Mode GLOverlay::TRIANGLE_STRIP;
+const GLOverlay_Mode GLOverlay::TRIANGLE_FAN;
+const GLOverlay_Mode GLOverlay::QUADS;
+const GLOverlay_Mode GLOverlay::QUAD_STRIP;
+const GLOverlay_Mode GLOverlay::POLYGON;
+const GLOverlay_Mode GLOverlay::SOLID_CIRCLE;
+const GLOverlay_Mode GLOverlay::WIRE_CIRCLE;
+const GLOverlay_Mode GLOverlay::SOLID_CUBE;
+const GLOverlay_Mode GLOverlay::WIRE_CUBE;
+const GLOverlay_Mode GLOverlay::Mode_MIN;
+const GLOverlay_Mode GLOverlay::Mode_MAX;
+const int GLOverlay::Mode_ARRAYSIZE;
+#endif  // _MSC_VER
+const ::google::protobuf::EnumDescriptor* GLOverlay_Identifier_descriptor() {
+  protobuf_AssignDescriptorsOnce();
+  return GLOverlay_Identifier_descriptor_;
+}
+bool GLOverlay_Identifier_IsValid(int value) {
+  switch(value) {
+    case 14:
+    case 15:
+    case 16:
+    case 17:
+    case 18:
+    case 19:
+    case 20:
+    case 21:
+    case 22:
+    case 23:
+    case 24:
+    case 25:
+      return true;
+    default:
+      return false;
+  }
+}
+
+#ifndef _MSC_VER
+const GLOverlay_Identifier GLOverlay::END;
+const GLOverlay_Identifier GLOverlay::VERTEX2F;
+const GLOverlay_Identifier GLOverlay::VERTEX3F;
+const GLOverlay_Identifier GLOverlay::ROTATEF;
+const GLOverlay_Identifier GLOverlay::TRANSLATEF;
+const GLOverlay_Identifier GLOverlay::SCALEF;
+const GLOverlay_Identifier GLOverlay::PUSH_MATRIX;
+const GLOverlay_Identifier GLOverlay::POP_MATRIX;
+const GLOverlay_Identifier GLOverlay::COLOR3F;
+const GLOverlay_Identifier GLOverlay::COLOR4F;
+const GLOverlay_Identifier GLOverlay::POINTSIZE;
+const GLOverlay_Identifier GLOverlay::LINEWIDTH;
+const GLOverlay_Identifier GLOverlay::Identifier_MIN;
+const GLOverlay_Identifier GLOverlay::Identifier_MAX;
+const int GLOverlay::Identifier_ARRAYSIZE;
+#endif  // _MSC_VER
+#ifndef _MSC_VER
+const int GLOverlay::kHeaderFieldNumber;
+const int GLOverlay::kNameFieldNumber;
+const int GLOverlay::kCoordinateFrameTypeFieldNumber;
+const int GLOverlay::kOriginXFieldNumber;
+const int GLOverlay::kOriginYFieldNumber;
+const int GLOverlay::kOriginZFieldNumber;
+const int GLOverlay::kDataFieldNumber;
 #endif  // !_MSC_VER
 
-PointCloudXYZI_PointXYZI::PointCloudXYZI_PointXYZI()
+GLOverlay::GLOverlay()
   : ::google::protobuf::Message() {
   SharedCtor();
 }
 
-void PointCloudXYZI_PointXYZI::InitAsDefaultInstance() {
+void GLOverlay::InitAsDefaultInstance() {
+  header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
 }
 
-PointCloudXYZI_PointXYZI::PointCloudXYZI_PointXYZI(const PointCloudXYZI_PointXYZI& from)
+GLOverlay::GLOverlay(const GLOverlay& from)
   : ::google::protobuf::Message() {
   SharedCtor();
   MergeFrom(from);
 }
 
-void PointCloudXYZI_PointXYZI::SharedCtor() {
+void GLOverlay::SharedCtor() {
   _cached_size_ = 0;
-  x_ = 0;
-  y_ = 0;
-  z_ = 0;
-  intensity_ = 0;
+  header_ = NULL;
+  name_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
+  coordinateframetype_ = 0;
+  origin_x_ = 0;
+  origin_y_ = 0;
+  origin_z_ = 0;
+  data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
   ::memset(_has_bits_, 0, sizeof(_has_bits_));
 }
 
-PointCloudXYZI_PointXYZI::~PointCloudXYZI_PointXYZI() {
+GLOverlay::~GLOverlay() {
   SharedDtor();
 }
 
-void PointCloudXYZI_PointXYZI::SharedDtor() {
+void GLOverlay::SharedDtor() {
+  if (name_ != &::google::protobuf::internal::kEmptyString) {
+    delete name_;
+  }
+  if (data_ != &::google::protobuf::internal::kEmptyString) {
+    delete data_;
+  }
   if (this != default_instance_) {
+    delete header_;
   }
 }
 
-void PointCloudXYZI_PointXYZI::SetCachedSize(int size) const {
+void GLOverlay::SetCachedSize(int size) const {
   GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
   _cached_size_ = size;
   GOOGLE_SAFE_CONCURRENT_WRITES_END();
 }
-const ::google::protobuf::Descriptor* PointCloudXYZI_PointXYZI::descriptor() {
+const ::google::protobuf::Descriptor* GLOverlay::descriptor() {
   protobuf_AssignDescriptorsOnce();
-  return PointCloudXYZI_PointXYZI_descriptor_;
+  return GLOverlay_descriptor_;
 }
 
-const PointCloudXYZI_PointXYZI& PointCloudXYZI_PointXYZI::default_instance() {
+const GLOverlay& GLOverlay::default_instance() {
   if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
 }
 
-PointCloudXYZI_PointXYZI* PointCloudXYZI_PointXYZI::default_instance_ = NULL;
+GLOverlay* GLOverlay::default_instance_ = NULL;
 
-PointCloudXYZI_PointXYZI* PointCloudXYZI_PointXYZI::New() const {
-  return new PointCloudXYZI_PointXYZI;
+GLOverlay* GLOverlay::New() const {
+  return new GLOverlay;
 }
 
-void PointCloudXYZI_PointXYZI::Clear() {
+void GLOverlay::Clear() {
   if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    x_ = 0;
-    y_ = 0;
-    z_ = 0;
-    intensity_ = 0;
+    if (has_header()) {
+      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
+    }
+    if (has_name()) {
+      if (name_ != &::google::protobuf::internal::kEmptyString) {
+        name_->clear();
+      }
+    }
+    coordinateframetype_ = 0;
+    origin_x_ = 0;
+    origin_y_ = 0;
+    origin_z_ = 0;
+    if (has_data()) {
+      if (data_ != &::google::protobuf::internal::kEmptyString) {
+        data_->clear();
+      }
+    }
   }
   ::memset(_has_bits_, 0, sizeof(_has_bits_));
   mutable_unknown_fields()->Clear();
 }
 
-bool PointCloudXYZI_PointXYZI::MergePartialFromCodedStream(
+bool GLOverlay::MergePartialFromCodedStream(
     ::google::protobuf::io::CodedInputStream* input) {
 #define DO_(EXPRESSION) if (!(EXPRESSION)) return false
   ::google::protobuf::uint32 tag;
   while ((tag = input->ReadTag()) != 0) {
     switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required float x = 1;
+      // required .px.HeaderInfo header = 1;
       case 1: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &x_)));
-          set_has_x();
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
+          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
+               input, mutable_header()));
         } else {
           goto handle_uninterpreted;
         }
-        if (input->ExpectTag(21)) goto parse_y;
+        if (input->ExpectTag(18)) goto parse_name;
         break;
       }
       
-      // required float y = 2;
+      // optional string name = 2;
       case 2: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_y:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &y_)));
-          set_has_y();
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
+         parse_name:
+          DO_(::google::protobuf::internal::WireFormatLite::ReadString(
+                input, this->mutable_name()));
+          ::google::protobuf::internal::WireFormat::VerifyUTF8String(
+            this->name().data(), this->name().length(),
+            ::google::protobuf::internal::WireFormat::PARSE);
         } else {
           goto handle_uninterpreted;
         }
-        if (input->ExpectTag(29)) goto parse_z;
+        if (input->ExpectTag(24)) goto parse_coordinateFrameType;
         break;
       }
       
-      // required float z = 3;
+      // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3;
       case 3: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_z:
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
+         parse_coordinateFrameType:
+          int value;
           DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &z_)));
-          set_has_z();
+                   int, ::google::protobuf::internal::WireFormatLite::TYPE_ENUM>(
+                 input, &value)));
+          if (::px::GLOverlay_CoordinateFrameType_IsValid(value)) {
+            set_coordinateframetype(static_cast< ::px::GLOverlay_CoordinateFrameType >(value));
+          } else {
+            mutable_unknown_fields()->AddVarint(3, value);
+          }
         } else {
           goto handle_uninterpreted;
         }
-        if (input->ExpectTag(37)) goto parse_intensity;
+        if (input->ExpectTag(33)) goto parse_origin_x;
         break;
       }
       
-      // required float intensity = 4;
+      // optional double origin_x = 4;
       case 4: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_intensity:
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
+         parse_origin_x:
           DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &intensity_)));
-          set_has_intensity();
+                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
+                 input, &origin_x_)));
+          set_has_origin_x();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(41)) goto parse_origin_y;
+        break;
+      }
+      
+      // optional double origin_y = 5;
+      case 5: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
+         parse_origin_y:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
+                 input, &origin_y_)));
+          set_has_origin_y();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(49)) goto parse_origin_z;
+        break;
+      }
+      
+      // optional double origin_z = 6;
+      case 6: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
+         parse_origin_z:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
+                 input, &origin_z_)));
+          set_has_origin_z();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(58)) goto parse_data;
+        break;
+      }
+      
+      // optional bytes data = 7;
+      case 7: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
+         parse_data:
+          DO_(::google::protobuf::internal::WireFormatLite::ReadBytes(
+                input, this->mutable_data()));
         } else {
           goto handle_uninterpreted;
         }
@@ -857,26 +1094,48 @@ bool PointCloudXYZI_PointXYZI::MergePartialFromCodedStream(
 #undef DO_
 }
 
-void PointCloudXYZI_PointXYZI::SerializeWithCachedSizes(
+void GLOverlay::SerializeWithCachedSizes(
     ::google::protobuf::io::CodedOutputStream* output) const {
-  // required float x = 1;
-  if (has_x()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(1, this->x(), output);
+  // required .px.HeaderInfo header = 1;
+  if (has_header()) {
+    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
+      1, this->header(), output);
   }
   
-  // required float y = 2;
-  if (has_y()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(2, this->y(), output);
+  // optional string name = 2;
+  if (has_name()) {
+    ::google::protobuf::internal::WireFormat::VerifyUTF8String(
+      this->name().data(), this->name().length(),
+      ::google::protobuf::internal::WireFormat::SERIALIZE);
+    ::google::protobuf::internal::WireFormatLite::WriteString(
+      2, this->name(), output);
   }
   
-  // required float z = 3;
-  if (has_z()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->z(), output);
+  // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3;
+  if (has_coordinateframetype()) {
+    ::google::protobuf::internal::WireFormatLite::WriteEnum(
+      3, this->coordinateframetype(), output);
   }
   
-  // required float intensity = 4;
-  if (has_intensity()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(4, this->intensity(), output);
+  // optional double origin_x = 4;
+  if (has_origin_x()) {
+    ::google::protobuf::internal::WireFormatLite::WriteDouble(4, this->origin_x(), output);
+  }
+  
+  // optional double origin_y = 5;
+  if (has_origin_y()) {
+    ::google::protobuf::internal::WireFormatLite::WriteDouble(5, this->origin_y(), output);
+  }
+  
+  // optional double origin_z = 6;
+  if (has_origin_z()) {
+    ::google::protobuf::internal::WireFormatLite::WriteDouble(6, this->origin_z(), output);
+  }
+  
+  // optional bytes data = 7;
+  if (has_data()) {
+    ::google::protobuf::internal::WireFormatLite::WriteBytes(
+      7, this->data(), output);
   }
   
   if (!unknown_fields().empty()) {
@@ -885,26 +1144,51 @@ void PointCloudXYZI_PointXYZI::SerializeWithCachedSizes(
   }
 }
 
-::google::protobuf::uint8* PointCloudXYZI_PointXYZI::SerializeWithCachedSizesToArray(
+::google::protobuf::uint8* GLOverlay::SerializeWithCachedSizesToArray(
     ::google::protobuf::uint8* target) const {
-  // required float x = 1;
-  if (has_x()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(1, this->x(), target);
+  // required .px.HeaderInfo header = 1;
+  if (has_header()) {
+    target = ::google::protobuf::internal::WireFormatLite::
+      WriteMessageNoVirtualToArray(
+        1, this->header(), target);
   }
   
-  // required float y = 2;
-  if (has_y()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(2, this->y(), target);
+  // optional string name = 2;
+  if (has_name()) {
+    ::google::protobuf::internal::WireFormat::VerifyUTF8String(
+      this->name().data(), this->name().length(),
+      ::google::protobuf::internal::WireFormat::SERIALIZE);
+    target =
+      ::google::protobuf::internal::WireFormatLite::WriteStringToArray(
+        2, this->name(), target);
   }
   
-  // required float z = 3;
-  if (has_z()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->z(), target);
+  // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3;
+  if (has_coordinateframetype()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteEnumToArray(
+      3, this->coordinateframetype(), target);
   }
   
-  // required float intensity = 4;
-  if (has_intensity()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(4, this->intensity(), target);
+  // optional double origin_x = 4;
+  if (has_origin_x()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(4, this->origin_x(), target);
+  }
+  
+  // optional double origin_y = 5;
+  if (has_origin_y()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(5, this->origin_y(), target);
+  }
+  
+  // optional double origin_z = 6;
+  if (has_origin_z()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(6, this->origin_z(), target);
+  }
+  
+  // optional bytes data = 7;
+  if (has_data()) {
+    target =
+      ::google::protobuf::internal::WireFormatLite::WriteBytesToArray(
+        7, this->data(), target);
   }
   
   if (!unknown_fields().empty()) {
@@ -914,28 +1198,50 @@ void PointCloudXYZI_PointXYZI::SerializeWithCachedSizes(
   return target;
 }
 
-int PointCloudXYZI_PointXYZI::ByteSize() const {
+int GLOverlay::ByteSize() const {
   int total_size = 0;
   
   if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required float x = 1;
-    if (has_x()) {
-      total_size += 1 + 4;
+    // required .px.HeaderInfo header = 1;
+    if (has_header()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
+          this->header());
     }
     
-    // required float y = 2;
-    if (has_y()) {
-      total_size += 1 + 4;
+    // optional string name = 2;
+    if (has_name()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::StringSize(
+          this->name());
     }
     
-    // required float z = 3;
-    if (has_z()) {
-      total_size += 1 + 4;
+    // optional .px.GLOverlay.CoordinateFrameType coordinateFrameType = 3;
+    if (has_coordinateframetype()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::EnumSize(this->coordinateframetype());
     }
     
-    // required float intensity = 4;
-    if (has_intensity()) {
-      total_size += 1 + 4;
+    // optional double origin_x = 4;
+    if (has_origin_x()) {
+      total_size += 1 + 8;
+    }
+    
+    // optional double origin_y = 5;
+    if (has_origin_y()) {
+      total_size += 1 + 8;
+    }
+    
+    // optional double origin_z = 6;
+    if (has_origin_z()) {
+      total_size += 1 + 8;
+    }
+    
+    // optional bytes data = 7;
+    if (has_data()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::BytesSize(
+          this->data());
     }
     
   }
@@ -950,10 +1256,10 @@ int PointCloudXYZI_PointXYZI::ByteSize() const {
   return total_size;
 }
 
-void PointCloudXYZI_PointXYZI::MergeFrom(const ::google::protobuf::Message& from) {
+void GLOverlay::MergeFrom(const ::google::protobuf::Message& from) {
   GOOGLE_CHECK_NE(&from, this);
-  const PointCloudXYZI_PointXYZI* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const PointCloudXYZI_PointXYZI*>(
+  const GLOverlay* source =
+    ::google::protobuf::internal::dynamic_cast_if_available<const GLOverlay*>(
       &from);
   if (source == NULL) {
     ::google::protobuf::internal::ReflectionOps::Merge(from, this);
@@ -962,163 +1268,254 @@ void PointCloudXYZI_PointXYZI::MergeFrom(const ::google::protobuf::Message& from
   }
 }
 
-void PointCloudXYZI_PointXYZI::MergeFrom(const PointCloudXYZI_PointXYZI& from) {
+void GLOverlay::MergeFrom(const GLOverlay& from) {
   GOOGLE_CHECK_NE(&from, this);
   if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_x()) {
-      set_x(from.x());
+    if (from.has_header()) {
+      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
     }
-    if (from.has_y()) {
-      set_y(from.y());
+    if (from.has_name()) {
+      set_name(from.name());
     }
-    if (from.has_z()) {
-      set_z(from.z());
+    if (from.has_coordinateframetype()) {
+      set_coordinateframetype(from.coordinateframetype());
     }
-    if (from.has_intensity()) {
-      set_intensity(from.intensity());
+    if (from.has_origin_x()) {
+      set_origin_x(from.origin_x());
+    }
+    if (from.has_origin_y()) {
+      set_origin_y(from.origin_y());
+    }
+    if (from.has_origin_z()) {
+      set_origin_z(from.origin_z());
+    }
+    if (from.has_data()) {
+      set_data(from.data());
     }
   }
   mutable_unknown_fields()->MergeFrom(from.unknown_fields());
 }
 
-void PointCloudXYZI_PointXYZI::CopyFrom(const ::google::protobuf::Message& from) {
+void GLOverlay::CopyFrom(const ::google::protobuf::Message& from) {
   if (&from == this) return;
   Clear();
   MergeFrom(from);
 }
 
-void PointCloudXYZI_PointXYZI::CopyFrom(const PointCloudXYZI_PointXYZI& from) {
+void GLOverlay::CopyFrom(const GLOverlay& from) {
   if (&from == this) return;
   Clear();
   MergeFrom(from);
 }
 
-bool PointCloudXYZI_PointXYZI::IsInitialized() const {
-  if ((_has_bits_[0] & 0x0000000f) != 0x0000000f) return false;
+bool GLOverlay::IsInitialized() const {
+  if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false;
   
+  if (has_header()) {
+    if (!this->header().IsInitialized()) return false;
+  }
   return true;
 }
 
-void PointCloudXYZI_PointXYZI::Swap(PointCloudXYZI_PointXYZI* other) {
+void GLOverlay::Swap(GLOverlay* other) {
   if (other != this) {
-    std::swap(x_, other->x_);
-    std::swap(y_, other->y_);
-    std::swap(z_, other->z_);
-    std::swap(intensity_, other->intensity_);
+    std::swap(header_, other->header_);
+    std::swap(name_, other->name_);
+    std::swap(coordinateframetype_, other->coordinateframetype_);
+    std::swap(origin_x_, other->origin_x_);
+    std::swap(origin_y_, other->origin_y_);
+    std::swap(origin_z_, other->origin_z_);
+    std::swap(data_, other->data_);
     std::swap(_has_bits_[0], other->_has_bits_[0]);
     _unknown_fields_.Swap(&other->_unknown_fields_);
     std::swap(_cached_size_, other->_cached_size_);
   }
 }
 
-::google::protobuf::Metadata PointCloudXYZI_PointXYZI::GetMetadata() const {
+::google::protobuf::Metadata GLOverlay::GetMetadata() const {
   protobuf_AssignDescriptorsOnce();
   ::google::protobuf::Metadata metadata;
-  metadata.descriptor = PointCloudXYZI_PointXYZI_descriptor_;
-  metadata.reflection = PointCloudXYZI_PointXYZI_reflection_;
+  metadata.descriptor = GLOverlay_descriptor_;
+  metadata.reflection = GLOverlay_reflection_;
   return metadata;
 }
 
 
-// -------------------------------------------------------------------
+// ===================================================================
 
 #ifndef _MSC_VER
-const int PointCloudXYZI::kHeaderFieldNumber;
-const int PointCloudXYZI::kPointsFieldNumber;
+const int Obstacle::kXFieldNumber;
+const int Obstacle::kYFieldNumber;
+const int Obstacle::kZFieldNumber;
+const int Obstacle::kLengthFieldNumber;
+const int Obstacle::kWidthFieldNumber;
+const int Obstacle::kHeightFieldNumber;
 #endif  // !_MSC_VER
 
-PointCloudXYZI::PointCloudXYZI()
+Obstacle::Obstacle()
   : ::google::protobuf::Message() {
   SharedCtor();
 }
 
-void PointCloudXYZI::InitAsDefaultInstance() {
-  header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
+void Obstacle::InitAsDefaultInstance() {
 }
 
-PointCloudXYZI::PointCloudXYZI(const PointCloudXYZI& from)
+Obstacle::Obstacle(const Obstacle& from)
   : ::google::protobuf::Message() {
   SharedCtor();
   MergeFrom(from);
 }
 
-void PointCloudXYZI::SharedCtor() {
+void Obstacle::SharedCtor() {
   _cached_size_ = 0;
-  header_ = NULL;
+  x_ = 0;
+  y_ = 0;
+  z_ = 0;
+  length_ = 0;
+  width_ = 0;
+  height_ = 0;
   ::memset(_has_bits_, 0, sizeof(_has_bits_));
 }
 
-PointCloudXYZI::~PointCloudXYZI() {
+Obstacle::~Obstacle() {
   SharedDtor();
 }
 
-void PointCloudXYZI::SharedDtor() {
+void Obstacle::SharedDtor() {
   if (this != default_instance_) {
-    delete header_;
   }
 }
 
-void PointCloudXYZI::SetCachedSize(int size) const {
+void Obstacle::SetCachedSize(int size) const {
   GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
   _cached_size_ = size;
   GOOGLE_SAFE_CONCURRENT_WRITES_END();
 }
-const ::google::protobuf::Descriptor* PointCloudXYZI::descriptor() {
+const ::google::protobuf::Descriptor* Obstacle::descriptor() {
   protobuf_AssignDescriptorsOnce();
-  return PointCloudXYZI_descriptor_;
+  return Obstacle_descriptor_;
 }
 
-const PointCloudXYZI& PointCloudXYZI::default_instance() {
+const Obstacle& Obstacle::default_instance() {
   if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
 }
 
-PointCloudXYZI* PointCloudXYZI::default_instance_ = NULL;
+Obstacle* Obstacle::default_instance_ = NULL;
 
-PointCloudXYZI* PointCloudXYZI::New() const {
-  return new PointCloudXYZI;
+Obstacle* Obstacle::New() const {
+  return new Obstacle;
 }
 
-void PointCloudXYZI::Clear() {
+void Obstacle::Clear() {
   if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (has_header()) {
-      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-    }
+    x_ = 0;
+    y_ = 0;
+    z_ = 0;
+    length_ = 0;
+    width_ = 0;
+    height_ = 0;
   }
-  points_.Clear();
   ::memset(_has_bits_, 0, sizeof(_has_bits_));
   mutable_unknown_fields()->Clear();
 }
 
-bool PointCloudXYZI::MergePartialFromCodedStream(
+bool Obstacle::MergePartialFromCodedStream(
     ::google::protobuf::io::CodedInputStream* input) {
 #define DO_(EXPRESSION) if (!(EXPRESSION)) return false
   ::google::protobuf::uint32 tag;
   while ((tag = input->ReadTag()) != 0) {
     switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required .px.HeaderInfo header = 1;
+      // optional float x = 1;
       case 1: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-               input, mutable_header()));
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &x_)));
+          set_has_x();
         } else {
           goto handle_uninterpreted;
         }
-        if (input->ExpectTag(18)) goto parse_points;
+        if (input->ExpectTag(21)) goto parse_y;
         break;
       }
       
-      // repeated .px.PointCloudXYZI.PointXYZI points = 2;
+      // optional float y = 2;
       case 2: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_points:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-                input, add_points()));
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
+         parse_y:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &y_)));
+          set_has_y();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(29)) goto parse_z;
+        break;
+      }
+      
+      // optional float z = 3;
+      case 3: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
+         parse_z:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &z_)));
+          set_has_z();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(37)) goto parse_length;
+        break;
+      }
+      
+      // optional float length = 4;
+      case 4: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
+         parse_length:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &length_)));
+          set_has_length();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(45)) goto parse_width;
+        break;
+      }
+      
+      // optional float width = 5;
+      case 5: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
+         parse_width:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &width_)));
+          set_has_width();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(53)) goto parse_height;
+        break;
+      }
+      
+      // optional float height = 6;
+      case 6: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
+         parse_height:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &height_)));
+          set_has_height();
         } else {
           goto handle_uninterpreted;
         }
-        if (input->ExpectTag(18)) goto parse_points;
         if (input->ExpectAtEnd()) return true;
         break;
       }
@@ -1139,18 +1536,36 @@ bool PointCloudXYZI::MergePartialFromCodedStream(
 #undef DO_
 }
 
-void PointCloudXYZI::SerializeWithCachedSizes(
+void Obstacle::SerializeWithCachedSizes(
     ::google::protobuf::io::CodedOutputStream* output) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      1, this->header(), output);
+  // optional float x = 1;
+  if (has_x()) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(1, this->x(), output);
   }
   
-  // repeated .px.PointCloudXYZI.PointXYZI points = 2;
-  for (int i = 0; i < this->points_size(); i++) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      2, this->points(i), output);
+  // optional float y = 2;
+  if (has_y()) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(2, this->y(), output);
+  }
+  
+  // optional float z = 3;
+  if (has_z()) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->z(), output);
+  }
+  
+  // optional float length = 4;
+  if (has_length()) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(4, this->length(), output);
+  }
+  
+  // optional float width = 5;
+  if (has_width()) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(5, this->width(), output);
+  }
+  
+  // optional float height = 6;
+  if (has_height()) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(6, this->height(), output);
   }
   
   if (!unknown_fields().empty()) {
@@ -1159,20 +1574,36 @@ void PointCloudXYZI::SerializeWithCachedSizes(
   }
 }
 
-::google::protobuf::uint8* PointCloudXYZI::SerializeWithCachedSizesToArray(
+::google::protobuf::uint8* Obstacle::SerializeWithCachedSizesToArray(
     ::google::protobuf::uint8* target) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        1, this->header(), target);
+  // optional float x = 1;
+  if (has_x()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(1, this->x(), target);
   }
   
-  // repeated .px.PointCloudXYZI.PointXYZI points = 2;
-  for (int i = 0; i < this->points_size(); i++) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        2, this->points(i), target);
+  // optional float y = 2;
+  if (has_y()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(2, this->y(), target);
+  }
+  
+  // optional float z = 3;
+  if (has_z()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->z(), target);
+  }
+  
+  // optional float length = 4;
+  if (has_length()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(4, this->length(), target);
+  }
+  
+  // optional float width = 5;
+  if (has_width()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(5, this->width(), target);
+  }
+  
+  // optional float height = 6;
+  if (has_height()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(6, this->height(), target);
   }
   
   if (!unknown_fields().empty()) {
@@ -1182,26 +1613,41 @@ void PointCloudXYZI::SerializeWithCachedSizes(
   return target;
 }
 
-int PointCloudXYZI::ByteSize() const {
+int Obstacle::ByteSize() const {
   int total_size = 0;
   
   if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required .px.HeaderInfo header = 1;
-    if (has_header()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-          this->header());
+    // optional float x = 1;
+    if (has_x()) {
+      total_size += 1 + 4;
+    }
+    
+    // optional float y = 2;
+    if (has_y()) {
+      total_size += 1 + 4;
+    }
+    
+    // optional float z = 3;
+    if (has_z()) {
+      total_size += 1 + 4;
+    }
+    
+    // optional float length = 4;
+    if (has_length()) {
+      total_size += 1 + 4;
+    }
+    
+    // optional float width = 5;
+    if (has_width()) {
+      total_size += 1 + 4;
+    }
+    
+    // optional float height = 6;
+    if (has_height()) {
+      total_size += 1 + 4;
     }
     
   }
-  // repeated .px.PointCloudXYZI.PointXYZI points = 2;
-  total_size += 1 * this->points_size();
-  for (int i = 0; i < this->points_size(); i++) {
-    total_size +=
-      ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-        this->points(i));
-  }
-  
   if (!unknown_fields().empty()) {
     total_size +=
       ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
@@ -1213,10 +1659,10 @@ int PointCloudXYZI::ByteSize() const {
   return total_size;
 }
 
-void PointCloudXYZI::MergeFrom(const ::google::protobuf::Message& from) {
+void Obstacle::MergeFrom(const ::google::protobuf::Message& from) {
   GOOGLE_CHECK_NE(&from, this);
-  const PointCloudXYZI* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const PointCloudXYZI*>(
+  const Obstacle* source =
+    ::google::protobuf::internal::dynamic_cast_if_available<const Obstacle*>(
       &from);
   if (source == NULL) {
     ::google::protobuf::internal::ReflectionOps::Merge(from, this);
@@ -1225,56 +1671,67 @@ void PointCloudXYZI::MergeFrom(const ::google::protobuf::Message& from) {
   }
 }
 
-void PointCloudXYZI::MergeFrom(const PointCloudXYZI& from) {
+void Obstacle::MergeFrom(const Obstacle& from) {
   GOOGLE_CHECK_NE(&from, this);
-  points_.MergeFrom(from.points_);
   if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_header()) {
-      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
+    if (from.has_x()) {
+      set_x(from.x());
     }
-  }
-  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
-}
-
-void PointCloudXYZI::CopyFrom(const ::google::protobuf::Message& from) {
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void PointCloudXYZI::CopyFrom(const PointCloudXYZI& from) {
-  if (&from == this) return;
-  Clear();
+    if (from.has_y()) {
+      set_y(from.y());
+    }
+    if (from.has_z()) {
+      set_z(from.z());
+    }
+    if (from.has_length()) {
+      set_length(from.length());
+    }
+    if (from.has_width()) {
+      set_width(from.width());
+    }
+    if (from.has_height()) {
+      set_height(from.height());
+    }
+  }
+  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
+}
+
+void Obstacle::CopyFrom(const ::google::protobuf::Message& from) {
+  if (&from == this) return;
+  Clear();
   MergeFrom(from);
 }
 
-bool PointCloudXYZI::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false;
+void Obstacle::CopyFrom(const Obstacle& from) {
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+bool Obstacle::IsInitialized() const {
   
-  if (has_header()) {
-    if (!this->header().IsInitialized()) return false;
-  }
-  for (int i = 0; i < points_size(); i++) {
-    if (!this->points(i).IsInitialized()) return false;
-  }
   return true;
 }
 
-void PointCloudXYZI::Swap(PointCloudXYZI* other) {
+void Obstacle::Swap(Obstacle* other) {
   if (other != this) {
-    std::swap(header_, other->header_);
-    points_.Swap(&other->points_);
+    std::swap(x_, other->x_);
+    std::swap(y_, other->y_);
+    std::swap(z_, other->z_);
+    std::swap(length_, other->length_);
+    std::swap(width_, other->width_);
+    std::swap(height_, other->height_);
     std::swap(_has_bits_[0], other->_has_bits_[0]);
     _unknown_fields_.Swap(&other->_unknown_fields_);
     std::swap(_cached_size_, other->_cached_size_);
   }
 }
 
-::google::protobuf::Metadata PointCloudXYZI::GetMetadata() const {
+::google::protobuf::Metadata Obstacle::GetMetadata() const {
   protobuf_AssignDescriptorsOnce();
   ::google::protobuf::Metadata metadata;
-  metadata.descriptor = PointCloudXYZI_descriptor_;
-  metadata.reflection = PointCloudXYZI_reflection_;
+  metadata.descriptor = Obstacle_descriptor_;
+  metadata.reflection = Obstacle_reflection_;
   return metadata;
 }
 
@@ -1282,140 +1739,102 @@ void PointCloudXYZI::Swap(PointCloudXYZI* other) {
 // ===================================================================
 
 #ifndef _MSC_VER
-const int PointCloudXYZRGB_PointXYZRGB::kXFieldNumber;
-const int PointCloudXYZRGB_PointXYZRGB::kYFieldNumber;
-const int PointCloudXYZRGB_PointXYZRGB::kZFieldNumber;
-const int PointCloudXYZRGB_PointXYZRGB::kRgbFieldNumber;
+const int ObstacleList::kHeaderFieldNumber;
+const int ObstacleList::kObstaclesFieldNumber;
 #endif  // !_MSC_VER
 
-PointCloudXYZRGB_PointXYZRGB::PointCloudXYZRGB_PointXYZRGB()
+ObstacleList::ObstacleList()
   : ::google::protobuf::Message() {
   SharedCtor();
 }
 
-void PointCloudXYZRGB_PointXYZRGB::InitAsDefaultInstance() {
+void ObstacleList::InitAsDefaultInstance() {
+  header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
 }
 
-PointCloudXYZRGB_PointXYZRGB::PointCloudXYZRGB_PointXYZRGB(const PointCloudXYZRGB_PointXYZRGB& from)
+ObstacleList::ObstacleList(const ObstacleList& from)
   : ::google::protobuf::Message() {
   SharedCtor();
   MergeFrom(from);
 }
 
-void PointCloudXYZRGB_PointXYZRGB::SharedCtor() {
+void ObstacleList::SharedCtor() {
   _cached_size_ = 0;
-  x_ = 0;
-  y_ = 0;
-  z_ = 0;
-  rgb_ = 0;
+  header_ = NULL;
   ::memset(_has_bits_, 0, sizeof(_has_bits_));
 }
 
-PointCloudXYZRGB_PointXYZRGB::~PointCloudXYZRGB_PointXYZRGB() {
+ObstacleList::~ObstacleList() {
   SharedDtor();
 }
 
-void PointCloudXYZRGB_PointXYZRGB::SharedDtor() {
+void ObstacleList::SharedDtor() {
   if (this != default_instance_) {
+    delete header_;
   }
 }
 
-void PointCloudXYZRGB_PointXYZRGB::SetCachedSize(int size) const {
+void ObstacleList::SetCachedSize(int size) const {
   GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
   _cached_size_ = size;
   GOOGLE_SAFE_CONCURRENT_WRITES_END();
 }
-const ::google::protobuf::Descriptor* PointCloudXYZRGB_PointXYZRGB::descriptor() {
+const ::google::protobuf::Descriptor* ObstacleList::descriptor() {
   protobuf_AssignDescriptorsOnce();
-  return PointCloudXYZRGB_PointXYZRGB_descriptor_;
+  return ObstacleList_descriptor_;
 }
 
-const PointCloudXYZRGB_PointXYZRGB& PointCloudXYZRGB_PointXYZRGB::default_instance() {
+const ObstacleList& ObstacleList::default_instance() {
   if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
 }
 
-PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB_PointXYZRGB::default_instance_ = NULL;
+ObstacleList* ObstacleList::default_instance_ = NULL;
 
-PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB_PointXYZRGB::New() const {
-  return new PointCloudXYZRGB_PointXYZRGB;
+ObstacleList* ObstacleList::New() const {
+  return new ObstacleList;
 }
 
-void PointCloudXYZRGB_PointXYZRGB::Clear() {
+void ObstacleList::Clear() {
   if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    x_ = 0;
-    y_ = 0;
-    z_ = 0;
-    rgb_ = 0;
+    if (has_header()) {
+      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
+    }
   }
+  obstacles_.Clear();
   ::memset(_has_bits_, 0, sizeof(_has_bits_));
   mutable_unknown_fields()->Clear();
 }
 
-bool PointCloudXYZRGB_PointXYZRGB::MergePartialFromCodedStream(
+bool ObstacleList::MergePartialFromCodedStream(
     ::google::protobuf::io::CodedInputStream* input) {
 #define DO_(EXPRESSION) if (!(EXPRESSION)) return false
   ::google::protobuf::uint32 tag;
   while ((tag = input->ReadTag()) != 0) {
     switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required float x = 1;
+      // required .px.HeaderInfo header = 1;
       case 1: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &x_)));
-          set_has_x();
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
+          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
+               input, mutable_header()));
         } else {
           goto handle_uninterpreted;
         }
-        if (input->ExpectTag(21)) goto parse_y;
+        if (input->ExpectTag(18)) goto parse_obstacles;
         break;
       }
       
-      // required float y = 2;
+      // repeated .px.Obstacle obstacles = 2;
       case 2: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_y:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &y_)));
-          set_has_y();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(29)) goto parse_z;
-        break;
-      }
-      
-      // required float z = 3;
-      case 3: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_z:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &z_)));
-          set_has_z();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(37)) goto parse_rgb;
-        break;
-      }
-      
-      // required float rgb = 4;
-      case 4: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_rgb:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &rgb_)));
-          set_has_rgb();
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
+         parse_obstacles:
+          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
+                input, add_obstacles()));
         } else {
           goto handle_uninterpreted;
         }
+        if (input->ExpectTag(18)) goto parse_obstacles;
         if (input->ExpectAtEnd()) return true;
         break;
       }
@@ -1436,26 +1855,18 @@ bool PointCloudXYZRGB_PointXYZRGB::MergePartialFromCodedStream(
 #undef DO_
 }
 
-void PointCloudXYZRGB_PointXYZRGB::SerializeWithCachedSizes(
+void ObstacleList::SerializeWithCachedSizes(
     ::google::protobuf::io::CodedOutputStream* output) const {
-  // required float x = 1;
-  if (has_x()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(1, this->x(), output);
-  }
-  
-  // required float y = 2;
-  if (has_y()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(2, this->y(), output);
-  }
-  
-  // required float z = 3;
-  if (has_z()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->z(), output);
+  // required .px.HeaderInfo header = 1;
+  if (has_header()) {
+    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
+      1, this->header(), output);
   }
   
-  // required float rgb = 4;
-  if (has_rgb()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(4, this->rgb(), output);
+  // repeated .px.Obstacle obstacles = 2;
+  for (int i = 0; i < this->obstacles_size(); i++) {
+    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
+      2, this->obstacles(i), output);
   }
   
   if (!unknown_fields().empty()) {
@@ -1464,26 +1875,20 @@ void PointCloudXYZRGB_PointXYZRGB::SerializeWithCachedSizes(
   }
 }
 
-::google::protobuf::uint8* PointCloudXYZRGB_PointXYZRGB::SerializeWithCachedSizesToArray(
+::google::protobuf::uint8* ObstacleList::SerializeWithCachedSizesToArray(
     ::google::protobuf::uint8* target) const {
-  // required float x = 1;
-  if (has_x()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(1, this->x(), target);
-  }
-  
-  // required float y = 2;
-  if (has_y()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(2, this->y(), target);
-  }
-  
-  // required float z = 3;
-  if (has_z()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->z(), target);
+  // required .px.HeaderInfo header = 1;
+  if (has_header()) {
+    target = ::google::protobuf::internal::WireFormatLite::
+      WriteMessageNoVirtualToArray(
+        1, this->header(), target);
   }
   
-  // required float rgb = 4;
-  if (has_rgb()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(4, this->rgb(), target);
+  // repeated .px.Obstacle obstacles = 2;
+  for (int i = 0; i < this->obstacles_size(); i++) {
+    target = ::google::protobuf::internal::WireFormatLite::
+      WriteMessageNoVirtualToArray(
+        2, this->obstacles(i), target);
   }
   
   if (!unknown_fields().empty()) {
@@ -1493,31 +1898,26 @@ void PointCloudXYZRGB_PointXYZRGB::SerializeWithCachedSizes(
   return target;
 }
 
-int PointCloudXYZRGB_PointXYZRGB::ByteSize() const {
+int ObstacleList::ByteSize() const {
   int total_size = 0;
   
   if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required float x = 1;
-    if (has_x()) {
-      total_size += 1 + 4;
-    }
-    
-    // required float y = 2;
-    if (has_y()) {
-      total_size += 1 + 4;
-    }
-    
-    // required float z = 3;
-    if (has_z()) {
-      total_size += 1 + 4;
-    }
-    
-    // required float rgb = 4;
-    if (has_rgb()) {
-      total_size += 1 + 4;
+    // required .px.HeaderInfo header = 1;
+    if (has_header()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
+          this->header());
     }
     
   }
+  // repeated .px.Obstacle obstacles = 2;
+  total_size += 1 * this->obstacles_size();
+  for (int i = 0; i < this->obstacles_size(); i++) {
+    total_size +=
+      ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
+        this->obstacles(i));
+  }
+  
   if (!unknown_fields().empty()) {
     total_size +=
       ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
@@ -1529,10 +1929,10 @@ int PointCloudXYZRGB_PointXYZRGB::ByteSize() const {
   return total_size;
 }
 
-void PointCloudXYZRGB_PointXYZRGB::MergeFrom(const ::google::protobuf::Message& from) {
+void ObstacleList::MergeFrom(const ::google::protobuf::Message& from) {
   GOOGLE_CHECK_NE(&from, this);
-  const PointCloudXYZRGB_PointXYZRGB* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const PointCloudXYZRGB_PointXYZRGB*>(
+  const ObstacleList* source =
+    ::google::protobuf::internal::dynamic_cast_if_available<const ObstacleList*>(
       &from);
   if (source == NULL) {
     ::google::protobuf::internal::ReflectionOps::Merge(from, this);
@@ -1541,134 +1941,161 @@ void PointCloudXYZRGB_PointXYZRGB::MergeFrom(const ::google::protobuf::Message&
   }
 }
 
-void PointCloudXYZRGB_PointXYZRGB::MergeFrom(const PointCloudXYZRGB_PointXYZRGB& from) {
+void ObstacleList::MergeFrom(const ObstacleList& from) {
   GOOGLE_CHECK_NE(&from, this);
+  obstacles_.MergeFrom(from.obstacles_);
   if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_x()) {
-      set_x(from.x());
-    }
-    if (from.has_y()) {
-      set_y(from.y());
-    }
-    if (from.has_z()) {
-      set_z(from.z());
-    }
-    if (from.has_rgb()) {
-      set_rgb(from.rgb());
+    if (from.has_header()) {
+      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
     }
   }
   mutable_unknown_fields()->MergeFrom(from.unknown_fields());
 }
 
-void PointCloudXYZRGB_PointXYZRGB::CopyFrom(const ::google::protobuf::Message& from) {
+void ObstacleList::CopyFrom(const ::google::protobuf::Message& from) {
   if (&from == this) return;
   Clear();
   MergeFrom(from);
 }
 
-void PointCloudXYZRGB_PointXYZRGB::CopyFrom(const PointCloudXYZRGB_PointXYZRGB& from) {
+void ObstacleList::CopyFrom(const ObstacleList& from) {
   if (&from == this) return;
   Clear();
   MergeFrom(from);
 }
 
-bool PointCloudXYZRGB_PointXYZRGB::IsInitialized() const {
-  if ((_has_bits_[0] & 0x0000000f) != 0x0000000f) return false;
+bool ObstacleList::IsInitialized() const {
+  if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false;
   
+  if (has_header()) {
+    if (!this->header().IsInitialized()) return false;
+  }
   return true;
 }
 
-void PointCloudXYZRGB_PointXYZRGB::Swap(PointCloudXYZRGB_PointXYZRGB* other) {
+void ObstacleList::Swap(ObstacleList* other) {
   if (other != this) {
-    std::swap(x_, other->x_);
-    std::swap(y_, other->y_);
-    std::swap(z_, other->z_);
-    std::swap(rgb_, other->rgb_);
+    std::swap(header_, other->header_);
+    obstacles_.Swap(&other->obstacles_);
     std::swap(_has_bits_[0], other->_has_bits_[0]);
     _unknown_fields_.Swap(&other->_unknown_fields_);
     std::swap(_cached_size_, other->_cached_size_);
   }
 }
 
-::google::protobuf::Metadata PointCloudXYZRGB_PointXYZRGB::GetMetadata() const {
+::google::protobuf::Metadata ObstacleList::GetMetadata() const {
   protobuf_AssignDescriptorsOnce();
   ::google::protobuf::Metadata metadata;
-  metadata.descriptor = PointCloudXYZRGB_PointXYZRGB_descriptor_;
-  metadata.reflection = PointCloudXYZRGB_PointXYZRGB_reflection_;
+  metadata.descriptor = ObstacleList_descriptor_;
+  metadata.reflection = ObstacleList_reflection_;
   return metadata;
 }
 
 
-// -------------------------------------------------------------------
+// ===================================================================
 
 #ifndef _MSC_VER
-const int PointCloudXYZRGB::kHeaderFieldNumber;
-const int PointCloudXYZRGB::kPointsFieldNumber;
+const int ObstacleMap::kHeaderFieldNumber;
+const int ObstacleMap::kTypeFieldNumber;
+const int ObstacleMap::kResolutionFieldNumber;
+const int ObstacleMap::kRowsFieldNumber;
+const int ObstacleMap::kColsFieldNumber;
+const int ObstacleMap::kMapR0FieldNumber;
+const int ObstacleMap::kMapC0FieldNumber;
+const int ObstacleMap::kArrayR0FieldNumber;
+const int ObstacleMap::kArrayC0FieldNumber;
+const int ObstacleMap::kDataFieldNumber;
 #endif  // !_MSC_VER
 
-PointCloudXYZRGB::PointCloudXYZRGB()
+ObstacleMap::ObstacleMap()
   : ::google::protobuf::Message() {
   SharedCtor();
 }
 
-void PointCloudXYZRGB::InitAsDefaultInstance() {
+void ObstacleMap::InitAsDefaultInstance() {
   header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
 }
 
-PointCloudXYZRGB::PointCloudXYZRGB(const PointCloudXYZRGB& from)
+ObstacleMap::ObstacleMap(const ObstacleMap& from)
   : ::google::protobuf::Message() {
   SharedCtor();
   MergeFrom(from);
 }
 
-void PointCloudXYZRGB::SharedCtor() {
+void ObstacleMap::SharedCtor() {
   _cached_size_ = 0;
   header_ = NULL;
+  type_ = 0;
+  resolution_ = 0;
+  rows_ = 0;
+  cols_ = 0;
+  mapr0_ = 0;
+  mapc0_ = 0;
+  arrayr0_ = 0;
+  arrayc0_ = 0;
+  data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
   ::memset(_has_bits_, 0, sizeof(_has_bits_));
 }
 
-PointCloudXYZRGB::~PointCloudXYZRGB() {
+ObstacleMap::~ObstacleMap() {
   SharedDtor();
 }
 
-void PointCloudXYZRGB::SharedDtor() {
+void ObstacleMap::SharedDtor() {
+  if (data_ != &::google::protobuf::internal::kEmptyString) {
+    delete data_;
+  }
   if (this != default_instance_) {
     delete header_;
   }
 }
 
-void PointCloudXYZRGB::SetCachedSize(int size) const {
+void ObstacleMap::SetCachedSize(int size) const {
   GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
   _cached_size_ = size;
   GOOGLE_SAFE_CONCURRENT_WRITES_END();
 }
-const ::google::protobuf::Descriptor* PointCloudXYZRGB::descriptor() {
+const ::google::protobuf::Descriptor* ObstacleMap::descriptor() {
   protobuf_AssignDescriptorsOnce();
-  return PointCloudXYZRGB_descriptor_;
+  return ObstacleMap_descriptor_;
 }
 
-const PointCloudXYZRGB& PointCloudXYZRGB::default_instance() {
+const ObstacleMap& ObstacleMap::default_instance() {
   if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
 }
 
-PointCloudXYZRGB* PointCloudXYZRGB::default_instance_ = NULL;
+ObstacleMap* ObstacleMap::default_instance_ = NULL;
 
-PointCloudXYZRGB* PointCloudXYZRGB::New() const {
-  return new PointCloudXYZRGB;
+ObstacleMap* ObstacleMap::New() const {
+  return new ObstacleMap;
 }
 
-void PointCloudXYZRGB::Clear() {
+void ObstacleMap::Clear() {
   if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
     if (has_header()) {
       if (header_ != NULL) header_->::px::HeaderInfo::Clear();
     }
+    type_ = 0;
+    resolution_ = 0;
+    rows_ = 0;
+    cols_ = 0;
+    mapr0_ = 0;
+    mapc0_ = 0;
+    arrayr0_ = 0;
+  }
+  if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) {
+    arrayc0_ = 0;
+    if (has_data()) {
+      if (data_ != &::google::protobuf::internal::kEmptyString) {
+        data_->clear();
+      }
+    }
   }
-  points_.Clear();
   ::memset(_has_bits_, 0, sizeof(_has_bits_));
   mutable_unknown_fields()->Clear();
 }
 
-bool PointCloudXYZRGB::MergePartialFromCodedStream(
+bool ObstacleMap::MergePartialFromCodedStream(
     ::google::protobuf::io::CodedInputStream* input) {
 #define DO_(EXPRESSION) if (!(EXPRESSION)) return false
   ::google::protobuf::uint32 tag;
@@ -1683,21 +2110,148 @@ bool PointCloudXYZRGB::MergePartialFromCodedStream(
         } else {
           goto handle_uninterpreted;
         }
-        if (input->ExpectTag(18)) goto parse_points;
+        if (input->ExpectTag(16)) goto parse_type;
         break;
       }
       
-      // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
+      // required int32 type = 2;
       case 2: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
+         parse_type:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
+                 input, &type_)));
+          set_has_type();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(29)) goto parse_resolution;
+        break;
+      }
+      
+      // optional float resolution = 3;
+      case 3: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
+         parse_resolution:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &resolution_)));
+          set_has_resolution();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(32)) goto parse_rows;
+        break;
+      }
+      
+      // optional int32 rows = 4;
+      case 4: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
+         parse_rows:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
+                 input, &rows_)));
+          set_has_rows();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(40)) goto parse_cols;
+        break;
+      }
+      
+      // optional int32 cols = 5;
+      case 5: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
+         parse_cols:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
+                 input, &cols_)));
+          set_has_cols();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(48)) goto parse_mapR0;
+        break;
+      }
+      
+      // optional int32 mapR0 = 6;
+      case 6: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
+         parse_mapR0:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
+                 input, &mapr0_)));
+          set_has_mapr0();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(56)) goto parse_mapC0;
+        break;
+      }
+      
+      // optional int32 mapC0 = 7;
+      case 7: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
+         parse_mapC0:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
+                 input, &mapc0_)));
+          set_has_mapc0();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(64)) goto parse_arrayR0;
+        break;
+      }
+      
+      // optional int32 arrayR0 = 8;
+      case 8: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
+         parse_arrayR0:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
+                 input, &arrayr0_)));
+          set_has_arrayr0();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(72)) goto parse_arrayC0;
+        break;
+      }
+      
+      // optional int32 arrayC0 = 9;
+      case 9: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
+         parse_arrayC0:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
+                 input, &arrayc0_)));
+          set_has_arrayc0();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(82)) goto parse_data;
+        break;
+      }
+      
+      // optional bytes data = 10;
+      case 10: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
             ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_points:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-                input, add_points()));
+         parse_data:
+          DO_(::google::protobuf::internal::WireFormatLite::ReadBytes(
+                input, this->mutable_data()));
         } else {
           goto handle_uninterpreted;
         }
-        if (input->ExpectTag(18)) goto parse_points;
         if (input->ExpectAtEnd()) return true;
         break;
       }
@@ -1718,7 +2272,7 @@ bool PointCloudXYZRGB::MergePartialFromCodedStream(
 #undef DO_
 }
 
-void PointCloudXYZRGB::SerializeWithCachedSizes(
+void ObstacleMap::SerializeWithCachedSizes(
     ::google::protobuf::io::CodedOutputStream* output) const {
   // required .px.HeaderInfo header = 1;
   if (has_header()) {
@@ -1726,10 +2280,50 @@ void PointCloudXYZRGB::SerializeWithCachedSizes(
       1, this->header(), output);
   }
   
-  // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
-  for (int i = 0; i < this->points_size(); i++) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      2, this->points(i), output);
+  // required int32 type = 2;
+  if (has_type()) {
+    ::google::protobuf::internal::WireFormatLite::WriteInt32(2, this->type(), output);
+  }
+  
+  // optional float resolution = 3;
+  if (has_resolution()) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->resolution(), output);
+  }
+  
+  // optional int32 rows = 4;
+  if (has_rows()) {
+    ::google::protobuf::internal::WireFormatLite::WriteInt32(4, this->rows(), output);
+  }
+  
+  // optional int32 cols = 5;
+  if (has_cols()) {
+    ::google::protobuf::internal::WireFormatLite::WriteInt32(5, this->cols(), output);
+  }
+  
+  // optional int32 mapR0 = 6;
+  if (has_mapr0()) {
+    ::google::protobuf::internal::WireFormatLite::WriteInt32(6, this->mapr0(), output);
+  }
+  
+  // optional int32 mapC0 = 7;
+  if (has_mapc0()) {
+    ::google::protobuf::internal::WireFormatLite::WriteInt32(7, this->mapc0(), output);
+  }
+  
+  // optional int32 arrayR0 = 8;
+  if (has_arrayr0()) {
+    ::google::protobuf::internal::WireFormatLite::WriteInt32(8, this->arrayr0(), output);
+  }
+  
+  // optional int32 arrayC0 = 9;
+  if (has_arrayc0()) {
+    ::google::protobuf::internal::WireFormatLite::WriteInt32(9, this->arrayc0(), output);
+  }
+  
+  // optional bytes data = 10;
+  if (has_data()) {
+    ::google::protobuf::internal::WireFormatLite::WriteBytes(
+      10, this->data(), output);
   }
   
   if (!unknown_fields().empty()) {
@@ -1738,7 +2332,7 @@ void PointCloudXYZRGB::SerializeWithCachedSizes(
   }
 }
 
-::google::protobuf::uint8* PointCloudXYZRGB::SerializeWithCachedSizesToArray(
+::google::protobuf::uint8* ObstacleMap::SerializeWithCachedSizesToArray(
     ::google::protobuf::uint8* target) const {
   // required .px.HeaderInfo header = 1;
   if (has_header()) {
@@ -1747,11 +2341,51 @@ void PointCloudXYZRGB::SerializeWithCachedSizes(
         1, this->header(), target);
   }
   
-  // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
-  for (int i = 0; i < this->points_size(); i++) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        2, this->points(i), target);
+  // required int32 type = 2;
+  if (has_type()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(2, this->type(), target);
+  }
+  
+  // optional float resolution = 3;
+  if (has_resolution()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->resolution(), target);
+  }
+  
+  // optional int32 rows = 4;
+  if (has_rows()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(4, this->rows(), target);
+  }
+  
+  // optional int32 cols = 5;
+  if (has_cols()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(5, this->cols(), target);
+  }
+  
+  // optional int32 mapR0 = 6;
+  if (has_mapr0()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(6, this->mapr0(), target);
+  }
+  
+  // optional int32 mapC0 = 7;
+  if (has_mapc0()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(7, this->mapc0(), target);
+  }
+  
+  // optional int32 arrayR0 = 8;
+  if (has_arrayr0()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(8, this->arrayr0(), target);
+  }
+  
+  // optional int32 arrayC0 = 9;
+  if (has_arrayc0()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(9, this->arrayc0(), target);
+  }
+  
+  // optional bytes data = 10;
+  if (has_data()) {
+    target =
+      ::google::protobuf::internal::WireFormatLite::WriteBytesToArray(
+        10, this->data(), target);
   }
   
   if (!unknown_fields().empty()) {
@@ -1761,7 +2395,7 @@ void PointCloudXYZRGB::SerializeWithCachedSizes(
   return target;
 }
 
-int PointCloudXYZRGB::ByteSize() const {
+int ObstacleMap::ByteSize() const {
   int total_size = 0;
   
   if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
@@ -1772,15 +2406,70 @@ int PointCloudXYZRGB::ByteSize() const {
           this->header());
     }
     
+    // required int32 type = 2;
+    if (has_type()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::Int32Size(
+          this->type());
+    }
+    
+    // optional float resolution = 3;
+    if (has_resolution()) {
+      total_size += 1 + 4;
+    }
+    
+    // optional int32 rows = 4;
+    if (has_rows()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::Int32Size(
+          this->rows());
+    }
+    
+    // optional int32 cols = 5;
+    if (has_cols()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::Int32Size(
+          this->cols());
+    }
+    
+    // optional int32 mapR0 = 6;
+    if (has_mapr0()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::Int32Size(
+          this->mapr0());
+    }
+    
+    // optional int32 mapC0 = 7;
+    if (has_mapc0()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::Int32Size(
+          this->mapc0());
+    }
+    
+    // optional int32 arrayR0 = 8;
+    if (has_arrayr0()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::Int32Size(
+          this->arrayr0());
+    }
+    
   }
-  // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
-  total_size += 1 * this->points_size();
-  for (int i = 0; i < this->points_size(); i++) {
-    total_size +=
-      ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-        this->points(i));
+  if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) {
+    // optional int32 arrayC0 = 9;
+    if (has_arrayc0()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::Int32Size(
+          this->arrayc0());
+    }
+    
+    // optional bytes data = 10;
+    if (has_data()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::BytesSize(
+          this->data());
+    }
+    
   }
-  
   if (!unknown_fields().empty()) {
     total_size +=
       ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
@@ -1792,10 +2481,10 @@ int PointCloudXYZRGB::ByteSize() const {
   return total_size;
 }
 
-void PointCloudXYZRGB::MergeFrom(const ::google::protobuf::Message& from) {
+void ObstacleMap::MergeFrom(const ::google::protobuf::Message& from) {
   GOOGLE_CHECK_NE(&from, this);
-  const PointCloudXYZRGB* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const PointCloudXYZRGB*>(
+  const ObstacleMap* source =
+    ::google::protobuf::internal::dynamic_cast_if_available<const ObstacleMap*>(
       &from);
   if (source == NULL) {
     ::google::protobuf::internal::ReflectionOps::Merge(from, this);
@@ -1804,56 +2493,89 @@ void PointCloudXYZRGB::MergeFrom(const ::google::protobuf::Message& from) {
   }
 }
 
-void PointCloudXYZRGB::MergeFrom(const PointCloudXYZRGB& from) {
+void ObstacleMap::MergeFrom(const ObstacleMap& from) {
   GOOGLE_CHECK_NE(&from, this);
-  points_.MergeFrom(from.points_);
   if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
     if (from.has_header()) {
       mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
     }
+    if (from.has_type()) {
+      set_type(from.type());
+    }
+    if (from.has_resolution()) {
+      set_resolution(from.resolution());
+    }
+    if (from.has_rows()) {
+      set_rows(from.rows());
+    }
+    if (from.has_cols()) {
+      set_cols(from.cols());
+    }
+    if (from.has_mapr0()) {
+      set_mapr0(from.mapr0());
+    }
+    if (from.has_mapc0()) {
+      set_mapc0(from.mapc0());
+    }
+    if (from.has_arrayr0()) {
+      set_arrayr0(from.arrayr0());
+    }
+  }
+  if (from._has_bits_[8 / 32] & (0xffu << (8 % 32))) {
+    if (from.has_arrayc0()) {
+      set_arrayc0(from.arrayc0());
+    }
+    if (from.has_data()) {
+      set_data(from.data());
+    }
   }
   mutable_unknown_fields()->MergeFrom(from.unknown_fields());
 }
 
-void PointCloudXYZRGB::CopyFrom(const ::google::protobuf::Message& from) {
+void ObstacleMap::CopyFrom(const ::google::protobuf::Message& from) {
   if (&from == this) return;
   Clear();
   MergeFrom(from);
 }
 
-void PointCloudXYZRGB::CopyFrom(const PointCloudXYZRGB& from) {
+void ObstacleMap::CopyFrom(const ObstacleMap& from) {
   if (&from == this) return;
   Clear();
   MergeFrom(from);
 }
 
-bool PointCloudXYZRGB::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false;
+bool ObstacleMap::IsInitialized() const {
+  if ((_has_bits_[0] & 0x00000003) != 0x00000003) return false;
   
   if (has_header()) {
     if (!this->header().IsInitialized()) return false;
   }
-  for (int i = 0; i < points_size(); i++) {
-    if (!this->points(i).IsInitialized()) return false;
-  }
   return true;
 }
 
-void PointCloudXYZRGB::Swap(PointCloudXYZRGB* other) {
+void ObstacleMap::Swap(ObstacleMap* other) {
   if (other != this) {
     std::swap(header_, other->header_);
-    points_.Swap(&other->points_);
+    std::swap(type_, other->type_);
+    std::swap(resolution_, other->resolution_);
+    std::swap(rows_, other->rows_);
+    std::swap(cols_, other->cols_);
+    std::swap(mapr0_, other->mapr0_);
+    std::swap(mapc0_, other->mapc0_);
+    std::swap(arrayr0_, other->arrayr0_);
+    std::swap(arrayc0_, other->arrayc0_);
+    std::swap(data_, other->data_);
     std::swap(_has_bits_[0], other->_has_bits_[0]);
     _unknown_fields_.Swap(&other->_unknown_fields_);
     std::swap(_cached_size_, other->_cached_size_);
   }
 }
 
-::google::protobuf::Metadata PointCloudXYZRGB::GetMetadata() const {
+::google::protobuf::Metadata ObstacleMap::GetMetadata() const {
   protobuf_AssignDescriptorsOnce();
   ::google::protobuf::Metadata metadata;
-  metadata.descriptor = PointCloudXYZRGB_descriptor_;
-  metadata.reflection = PointCloudXYZRGB_reflection_;
+  metadata.descriptor = ObstacleMap_descriptor_;
+  metadata.reflection = ObstacleMap_reflection_;
   return metadata;
 }
 
@@ -1861,148 +2583,73 @@ void PointCloudXYZRGB::Swap(PointCloudXYZRGB* other) {
 // ===================================================================
 
 #ifndef _MSC_VER
-const int RGBDImage::kHeaderFieldNumber;
-const int RGBDImage::kColsFieldNumber;
-const int RGBDImage::kRowsFieldNumber;
-const int RGBDImage::kStep1FieldNumber;
-const int RGBDImage::kType1FieldNumber;
-const int RGBDImage::kImageData1FieldNumber;
-const int RGBDImage::kStep2FieldNumber;
-const int RGBDImage::kType2FieldNumber;
-const int RGBDImage::kImageData2FieldNumber;
-const int RGBDImage::kCameraConfigFieldNumber;
-const int RGBDImage::kCameraTypeFieldNumber;
-const int RGBDImage::kRollFieldNumber;
-const int RGBDImage::kPitchFieldNumber;
-const int RGBDImage::kYawFieldNumber;
-const int RGBDImage::kLonFieldNumber;
-const int RGBDImage::kLatFieldNumber;
-const int RGBDImage::kAltFieldNumber;
-const int RGBDImage::kGroundXFieldNumber;
-const int RGBDImage::kGroundYFieldNumber;
-const int RGBDImage::kGroundZFieldNumber;
-const int RGBDImage::kCameraMatrixFieldNumber;
-#endif  // !_MSC_VER
+const int Path::kHeaderFieldNumber;
+const int Path::kWaypointsFieldNumber;
+#endif  // !_MSC_VER
 
-RGBDImage::RGBDImage()
+Path::Path()
   : ::google::protobuf::Message() {
   SharedCtor();
 }
 
-void RGBDImage::InitAsDefaultInstance() {
+void Path::InitAsDefaultInstance() {
   header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
 }
 
-RGBDImage::RGBDImage(const RGBDImage& from)
+Path::Path(const Path& from)
   : ::google::protobuf::Message() {
   SharedCtor();
   MergeFrom(from);
 }
 
-void RGBDImage::SharedCtor() {
+void Path::SharedCtor() {
   _cached_size_ = 0;
   header_ = NULL;
-  cols_ = 0u;
-  rows_ = 0u;
-  step1_ = 0u;
-  type1_ = 0u;
-  imagedata1_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-  step2_ = 0u;
-  type2_ = 0u;
-  imagedata2_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
-  camera_config_ = 0u;
-  camera_type_ = 0u;
-  roll_ = 0;
-  pitch_ = 0;
-  yaw_ = 0;
-  lon_ = 0;
-  lat_ = 0;
-  alt_ = 0;
-  ground_x_ = 0;
-  ground_y_ = 0;
-  ground_z_ = 0;
   ::memset(_has_bits_, 0, sizeof(_has_bits_));
 }
 
-RGBDImage::~RGBDImage() {
+Path::~Path() {
   SharedDtor();
 }
 
-void RGBDImage::SharedDtor() {
-  if (imagedata1_ != &::google::protobuf::internal::kEmptyString) {
-    delete imagedata1_;
-  }
-  if (imagedata2_ != &::google::protobuf::internal::kEmptyString) {
-    delete imagedata2_;
-  }
+void Path::SharedDtor() {
   if (this != default_instance_) {
     delete header_;
   }
 }
 
-void RGBDImage::SetCachedSize(int size) const {
+void Path::SetCachedSize(int size) const {
   GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
   _cached_size_ = size;
   GOOGLE_SAFE_CONCURRENT_WRITES_END();
 }
-const ::google::protobuf::Descriptor* RGBDImage::descriptor() {
+const ::google::protobuf::Descriptor* Path::descriptor() {
   protobuf_AssignDescriptorsOnce();
-  return RGBDImage_descriptor_;
+  return Path_descriptor_;
 }
 
-const RGBDImage& RGBDImage::default_instance() {
+const Path& Path::default_instance() {
   if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
 }
 
-RGBDImage* RGBDImage::default_instance_ = NULL;
+Path* Path::default_instance_ = NULL;
 
-RGBDImage* RGBDImage::New() const {
-  return new RGBDImage;
+Path* Path::New() const {
+  return new Path;
 }
 
-void RGBDImage::Clear() {
+void Path::Clear() {
   if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
     if (has_header()) {
       if (header_ != NULL) header_->::px::HeaderInfo::Clear();
     }
-    cols_ = 0u;
-    rows_ = 0u;
-    step1_ = 0u;
-    type1_ = 0u;
-    if (has_imagedata1()) {
-      if (imagedata1_ != &::google::protobuf::internal::kEmptyString) {
-        imagedata1_->clear();
-      }
-    }
-    step2_ = 0u;
-    type2_ = 0u;
-  }
-  if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) {
-    if (has_imagedata2()) {
-      if (imagedata2_ != &::google::protobuf::internal::kEmptyString) {
-        imagedata2_->clear();
-      }
-    }
-    camera_config_ = 0u;
-    camera_type_ = 0u;
-    roll_ = 0;
-    pitch_ = 0;
-    yaw_ = 0;
-    lon_ = 0;
-    lat_ = 0;
-  }
-  if (_has_bits_[16 / 32] & (0xffu << (16 % 32))) {
-    alt_ = 0;
-    ground_x_ = 0;
-    ground_y_ = 0;
-    ground_z_ = 0;
   }
-  camera_matrix_.Clear();
+  waypoints_.Clear();
   ::memset(_has_bits_, 0, sizeof(_has_bits_));
   mutable_unknown_fields()->Clear();
 }
 
-bool RGBDImage::MergePartialFromCodedStream(
+bool Path::MergePartialFromCodedStream(
     ::google::protobuf::io::CodedInputStream* input) {
 #define DO_(EXPRESSION) if (!(EXPRESSION)) return false
   ::google::protobuf::uint32 tag;
@@ -2017,340 +2664,33 @@ bool RGBDImage::MergePartialFromCodedStream(
         } else {
           goto handle_uninterpreted;
         }
-        if (input->ExpectTag(16)) goto parse_cols;
+        if (input->ExpectTag(18)) goto parse_waypoints;
         break;
       }
       
-      // required uint32 cols = 2;
+      // repeated .px.Waypoint waypoints = 2;
       case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_cols:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &cols_)));
-          set_has_cols();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(24)) goto parse_rows;
-        break;
-      }
-      
-      // required uint32 rows = 3;
-      case 3: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_rows:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &rows_)));
-          set_has_rows();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(32)) goto parse_step1;
-        break;
-      }
-      
-      // required uint32 step1 = 4;
-      case 4: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_step1:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &step1_)));
-          set_has_step1();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(40)) goto parse_type1;
-        break;
-      }
-      
-      // required uint32 type1 = 5;
-      case 5: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_type1:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &type1_)));
-          set_has_type1();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(50)) goto parse_imageData1;
-        break;
-      }
-      
-      // required bytes imageData1 = 6;
-      case 6: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_imageData1:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadBytes(
-                input, this->mutable_imagedata1()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(56)) goto parse_step2;
-        break;
-      }
-      
-      // required uint32 step2 = 7;
-      case 7: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_step2:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &step2_)));
-          set_has_step2();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(64)) goto parse_type2;
-        break;
-      }
-      
-      // required uint32 type2 = 8;
-      case 8: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_type2:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &type2_)));
-          set_has_type2();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(74)) goto parse_imageData2;
-        break;
-      }
-      
-      // required bytes imageData2 = 9;
-      case 9: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
             ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_imageData2:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadBytes(
-                input, this->mutable_imagedata2()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(80)) goto parse_camera_config;
-        break;
-      }
-      
-      // optional uint32 camera_config = 10;
-      case 10: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_camera_config:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &camera_config_)));
-          set_has_camera_config();
+         parse_waypoints:
+          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
+                input, add_waypoints()));
         } else {
           goto handle_uninterpreted;
         }
-        if (input->ExpectTag(88)) goto parse_camera_type;
+        if (input->ExpectTag(18)) goto parse_waypoints;
+        if (input->ExpectAtEnd()) return true;
         break;
       }
       
-      // optional uint32 camera_type = 11;
-      case 11: {
+      default: {
+      handle_uninterpreted:
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_camera_type:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
-                 input, &camera_type_)));
-          set_has_camera_type();
-        } else {
-          goto handle_uninterpreted;
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
+          return true;
         }
-        if (input->ExpectTag(101)) goto parse_roll;
-        break;
-      }
-      
-      // optional float roll = 12;
-      case 12: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_roll:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &roll_)));
-          set_has_roll();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(109)) goto parse_pitch;
-        break;
-      }
-      
-      // optional float pitch = 13;
-      case 13: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_pitch:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &pitch_)));
-          set_has_pitch();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(117)) goto parse_yaw;
-        break;
-      }
-      
-      // optional float yaw = 14;
-      case 14: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_yaw:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &yaw_)));
-          set_has_yaw();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(125)) goto parse_lon;
-        break;
-      }
-      
-      // optional float lon = 15;
-      case 15: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_lon:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &lon_)));
-          set_has_lon();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(133)) goto parse_lat;
-        break;
-      }
-      
-      // optional float lat = 16;
-      case 16: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_lat:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &lat_)));
-          set_has_lat();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(141)) goto parse_alt;
-        break;
-      }
-      
-      // optional float alt = 17;
-      case 17: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_alt:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &alt_)));
-          set_has_alt();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(149)) goto parse_ground_x;
-        break;
-      }
-      
-      // optional float ground_x = 18;
-      case 18: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_ground_x:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &ground_x_)));
-          set_has_ground_x();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(157)) goto parse_ground_y;
-        break;
-      }
-      
-      // optional float ground_y = 19;
-      case 19: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_ground_y:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &ground_y_)));
-          set_has_ground_y();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(165)) goto parse_ground_z;
-        break;
-      }
-      
-      // optional float ground_z = 20;
-      case 20: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_ground_z:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &ground_z_)));
-          set_has_ground_z();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(173)) goto parse_camera_matrix;
-        break;
-      }
-      
-      // repeated float camera_matrix = 21;
-      case 21: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_camera_matrix:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadRepeatedPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 2, 173, input, this->mutable_camera_matrix())));
-        } else if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag)
-                   == ::google::protobuf::internal::WireFormatLite::
-                      WIRETYPE_LENGTH_DELIMITED) {
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPackedPrimitiveNoInline<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, this->mutable_camera_matrix())));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(173)) goto parse_camera_matrix;
-        if (input->ExpectAtEnd()) return true;
-        break;
-      }
-      
-      default: {
-      handle_uninterpreted:
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
-          return true;
-        }
-        DO_(::google::protobuf::internal::WireFormat::SkipField(
-              input, tag, mutable_unknown_fields()));
+        DO_(::google::protobuf::internal::WireFormat::SkipField(
+              input, tag, mutable_unknown_fields()));
         break;
       }
     }
@@ -2359,7 +2699,7 @@ bool RGBDImage::MergePartialFromCodedStream(
 #undef DO_
 }
 
-void RGBDImage::SerializeWithCachedSizes(
+void Path::SerializeWithCachedSizes(
     ::google::protobuf::io::CodedOutputStream* output) const {
   // required .px.HeaderInfo header = 1;
   if (has_header()) {
@@ -2367,374 +2707,377 @@ void RGBDImage::SerializeWithCachedSizes(
       1, this->header(), output);
   }
   
-  // required uint32 cols = 2;
-  if (has_cols()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(2, this->cols(), output);
-  }
-  
-  // required uint32 rows = 3;
-  if (has_rows()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(3, this->rows(), output);
-  }
-  
-  // required uint32 step1 = 4;
-  if (has_step1()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(4, this->step1(), output);
+  // repeated .px.Waypoint waypoints = 2;
+  for (int i = 0; i < this->waypoints_size(); i++) {
+    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
+      2, this->waypoints(i), output);
   }
   
-  // required uint32 type1 = 5;
-  if (has_type1()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(5, this->type1(), output);
+  if (!unknown_fields().empty()) {
+    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
+        unknown_fields(), output);
   }
-  
-  // required bytes imageData1 = 6;
-  if (has_imagedata1()) {
-    ::google::protobuf::internal::WireFormatLite::WriteBytes(
-      6, this->imagedata1(), output);
+}
+
+::google::protobuf::uint8* Path::SerializeWithCachedSizesToArray(
+    ::google::protobuf::uint8* target) const {
+  // required .px.HeaderInfo header = 1;
+  if (has_header()) {
+    target = ::google::protobuf::internal::WireFormatLite::
+      WriteMessageNoVirtualToArray(
+        1, this->header(), target);
   }
   
-  // required uint32 step2 = 7;
-  if (has_step2()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(7, this->step2(), output);
+  // repeated .px.Waypoint waypoints = 2;
+  for (int i = 0; i < this->waypoints_size(); i++) {
+    target = ::google::protobuf::internal::WireFormatLite::
+      WriteMessageNoVirtualToArray(
+        2, this->waypoints(i), target);
   }
   
-  // required uint32 type2 = 8;
-  if (has_type2()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(8, this->type2(), output);
+  if (!unknown_fields().empty()) {
+    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
+        unknown_fields(), target);
   }
+  return target;
+}
+
+int Path::ByteSize() const {
+  int total_size = 0;
   
-  // required bytes imageData2 = 9;
-  if (has_imagedata2()) {
-    ::google::protobuf::internal::WireFormatLite::WriteBytes(
-      9, this->imagedata2(), output);
+  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
+    // required .px.HeaderInfo header = 1;
+    if (has_header()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
+          this->header());
+    }
+    
   }
-  
-  // optional uint32 camera_config = 10;
-  if (has_camera_config()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(10, this->camera_config(), output);
+  // repeated .px.Waypoint waypoints = 2;
+  total_size += 1 * this->waypoints_size();
+  for (int i = 0; i < this->waypoints_size(); i++) {
+    total_size +=
+      ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
+        this->waypoints(i));
   }
   
-  // optional uint32 camera_type = 11;
-  if (has_camera_type()) {
-    ::google::protobuf::internal::WireFormatLite::WriteUInt32(11, this->camera_type(), output);
-  }
-  
-  // optional float roll = 12;
-  if (has_roll()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(12, this->roll(), output);
-  }
-  
-  // optional float pitch = 13;
-  if (has_pitch()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(13, this->pitch(), output);
-  }
-  
-  // optional float yaw = 14;
-  if (has_yaw()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(14, this->yaw(), output);
-  }
-  
-  // optional float lon = 15;
-  if (has_lon()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(15, this->lon(), output);
-  }
-  
-  // optional float lat = 16;
-  if (has_lat()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(16, this->lat(), output);
-  }
-  
-  // optional float alt = 17;
-  if (has_alt()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(17, this->alt(), output);
-  }
-  
-  // optional float ground_x = 18;
-  if (has_ground_x()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(18, this->ground_x(), output);
-  }
-  
-  // optional float ground_y = 19;
-  if (has_ground_y()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(19, this->ground_y(), output);
-  }
-  
-  // optional float ground_z = 20;
-  if (has_ground_z()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(20, this->ground_z(), output);
-  }
-  
-  // repeated float camera_matrix = 21;
-  for (int i = 0; i < this->camera_matrix_size(); i++) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(
-      21, this->camera_matrix(i), output);
-  }
-  
-  if (!unknown_fields().empty()) {
-    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
-        unknown_fields(), output);
+  if (!unknown_fields().empty()) {
+    total_size +=
+      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
+        unknown_fields());
   }
+  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
+  _cached_size_ = total_size;
+  GOOGLE_SAFE_CONCURRENT_WRITES_END();
+  return total_size;
 }
 
-::google::protobuf::uint8* RGBDImage::SerializeWithCachedSizesToArray(
-    ::google::protobuf::uint8* target) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        1, this->header(), target);
-  }
-  
-  // required uint32 cols = 2;
-  if (has_cols()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(2, this->cols(), target);
-  }
-  
-  // required uint32 rows = 3;
-  if (has_rows()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(3, this->rows(), target);
-  }
-  
-  // required uint32 step1 = 4;
-  if (has_step1()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(4, this->step1(), target);
-  }
-  
-  // required uint32 type1 = 5;
-  if (has_type1()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(5, this->type1(), target);
-  }
-  
-  // required bytes imageData1 = 6;
-  if (has_imagedata1()) {
-    target =
-      ::google::protobuf::internal::WireFormatLite::WriteBytesToArray(
-        6, this->imagedata1(), target);
-  }
-  
-  // required uint32 step2 = 7;
-  if (has_step2()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(7, this->step2(), target);
-  }
-  
-  // required uint32 type2 = 8;
-  if (has_type2()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(8, this->type2(), target);
-  }
-  
-  // required bytes imageData2 = 9;
-  if (has_imagedata2()) {
-    target =
-      ::google::protobuf::internal::WireFormatLite::WriteBytesToArray(
-        9, this->imagedata2(), target);
-  }
-  
-  // optional uint32 camera_config = 10;
-  if (has_camera_config()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(10, this->camera_config(), target);
-  }
-  
-  // optional uint32 camera_type = 11;
-  if (has_camera_type()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(11, this->camera_type(), target);
-  }
-  
-  // optional float roll = 12;
-  if (has_roll()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(12, this->roll(), target);
-  }
-  
-  // optional float pitch = 13;
-  if (has_pitch()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(13, this->pitch(), target);
-  }
-  
-  // optional float yaw = 14;
-  if (has_yaw()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(14, this->yaw(), target);
-  }
-  
-  // optional float lon = 15;
-  if (has_lon()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(15, this->lon(), target);
-  }
-  
-  // optional float lat = 16;
-  if (has_lat()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(16, this->lat(), target);
-  }
-  
-  // optional float alt = 17;
-  if (has_alt()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(17, this->alt(), target);
+void Path::MergeFrom(const ::google::protobuf::Message& from) {
+  GOOGLE_CHECK_NE(&from, this);
+  const Path* source =
+    ::google::protobuf::internal::dynamic_cast_if_available<const Path*>(
+      &from);
+  if (source == NULL) {
+    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
+  } else {
+    MergeFrom(*source);
   }
-  
-  // optional float ground_x = 18;
-  if (has_ground_x()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(18, this->ground_x(), target);
+}
+
+void Path::MergeFrom(const Path& from) {
+  GOOGLE_CHECK_NE(&from, this);
+  waypoints_.MergeFrom(from.waypoints_);
+  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
+    if (from.has_header()) {
+      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
+    }
   }
+  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
+}
+
+void Path::CopyFrom(const ::google::protobuf::Message& from) {
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+void Path::CopyFrom(const Path& from) {
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+bool Path::IsInitialized() const {
+  if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false;
   
-  // optional float ground_y = 19;
-  if (has_ground_y()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(19, this->ground_y(), target);
+  if (has_header()) {
+    if (!this->header().IsInitialized()) return false;
   }
-  
-  // optional float ground_z = 20;
-  if (has_ground_z()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(20, this->ground_z(), target);
+  for (int i = 0; i < waypoints_size(); i++) {
+    if (!this->waypoints(i).IsInitialized()) return false;
   }
-  
-  // repeated float camera_matrix = 21;
-  for (int i = 0; i < this->camera_matrix_size(); i++) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteFloatToArray(21, this->camera_matrix(i), target);
+  return true;
+}
+
+void Path::Swap(Path* other) {
+  if (other != this) {
+    std::swap(header_, other->header_);
+    waypoints_.Swap(&other->waypoints_);
+    std::swap(_has_bits_[0], other->_has_bits_[0]);
+    _unknown_fields_.Swap(&other->_unknown_fields_);
+    std::swap(_cached_size_, other->_cached_size_);
   }
-  
-  if (!unknown_fields().empty()) {
-    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
-        unknown_fields(), target);
+}
+
+::google::protobuf::Metadata Path::GetMetadata() const {
+  protobuf_AssignDescriptorsOnce();
+  ::google::protobuf::Metadata metadata;
+  metadata.descriptor = Path_descriptor_;
+  metadata.reflection = Path_reflection_;
+  return metadata;
+}
+
+
+// ===================================================================
+
+#ifndef _MSC_VER
+const int PointCloudXYZI_PointXYZI::kXFieldNumber;
+const int PointCloudXYZI_PointXYZI::kYFieldNumber;
+const int PointCloudXYZI_PointXYZI::kZFieldNumber;
+const int PointCloudXYZI_PointXYZI::kIntensityFieldNumber;
+#endif  // !_MSC_VER
+
+PointCloudXYZI_PointXYZI::PointCloudXYZI_PointXYZI()
+  : ::google::protobuf::Message() {
+  SharedCtor();
+}
+
+void PointCloudXYZI_PointXYZI::InitAsDefaultInstance() {
+}
+
+PointCloudXYZI_PointXYZI::PointCloudXYZI_PointXYZI(const PointCloudXYZI_PointXYZI& from)
+  : ::google::protobuf::Message() {
+  SharedCtor();
+  MergeFrom(from);
+}
+
+void PointCloudXYZI_PointXYZI::SharedCtor() {
+  _cached_size_ = 0;
+  x_ = 0;
+  y_ = 0;
+  z_ = 0;
+  intensity_ = 0;
+  ::memset(_has_bits_, 0, sizeof(_has_bits_));
+}
+
+PointCloudXYZI_PointXYZI::~PointCloudXYZI_PointXYZI() {
+  SharedDtor();
+}
+
+void PointCloudXYZI_PointXYZI::SharedDtor() {
+  if (this != default_instance_) {
   }
-  return target;
 }
 
-int RGBDImage::ByteSize() const {
-  int total_size = 0;
-  
+void PointCloudXYZI_PointXYZI::SetCachedSize(int size) const {
+  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
+  _cached_size_ = size;
+  GOOGLE_SAFE_CONCURRENT_WRITES_END();
+}
+const ::google::protobuf::Descriptor* PointCloudXYZI_PointXYZI::descriptor() {
+  protobuf_AssignDescriptorsOnce();
+  return PointCloudXYZI_PointXYZI_descriptor_;
+}
+
+const PointCloudXYZI_PointXYZI& PointCloudXYZI_PointXYZI::default_instance() {
+  if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
+}
+
+PointCloudXYZI_PointXYZI* PointCloudXYZI_PointXYZI::default_instance_ = NULL;
+
+PointCloudXYZI_PointXYZI* PointCloudXYZI_PointXYZI::New() const {
+  return new PointCloudXYZI_PointXYZI;
+}
+
+void PointCloudXYZI_PointXYZI::Clear() {
   if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required .px.HeaderInfo header = 1;
-    if (has_header()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-          this->header());
-    }
-    
-    // required uint32 cols = 2;
-    if (has_cols()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->cols());
-    }
-    
-    // required uint32 rows = 3;
-    if (has_rows()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->rows());
-    }
-    
-    // required uint32 step1 = 4;
-    if (has_step1()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->step1());
-    }
-    
-    // required uint32 type1 = 5;
-    if (has_type1()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->type1());
-    }
-    
-    // required bytes imageData1 = 6;
-    if (has_imagedata1()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::BytesSize(
-          this->imagedata1());
-    }
-    
-    // required uint32 step2 = 7;
-    if (has_step2()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->step2());
-    }
-    
-    // required uint32 type2 = 8;
-    if (has_type2()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->type2());
-    }
-    
+    x_ = 0;
+    y_ = 0;
+    z_ = 0;
+    intensity_ = 0;
   }
-  if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) {
-    // required bytes imageData2 = 9;
-    if (has_imagedata2()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::BytesSize(
-          this->imagedata2());
-    }
-    
-    // optional uint32 camera_config = 10;
-    if (has_camera_config()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->camera_config());
-    }
-    
-    // optional uint32 camera_type = 11;
-    if (has_camera_type()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::UInt32Size(
-          this->camera_type());
+  ::memset(_has_bits_, 0, sizeof(_has_bits_));
+  mutable_unknown_fields()->Clear();
+}
+
+bool PointCloudXYZI_PointXYZI::MergePartialFromCodedStream(
+    ::google::protobuf::io::CodedInputStream* input) {
+#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
+  ::google::protobuf::uint32 tag;
+  while ((tag = input->ReadTag()) != 0) {
+    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
+      // required float x = 1;
+      case 1: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &x_)));
+          set_has_x();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(21)) goto parse_y;
+        break;
+      }
+      
+      // required float y = 2;
+      case 2: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
+         parse_y:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &y_)));
+          set_has_y();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(29)) goto parse_z;
+        break;
+      }
+      
+      // required float z = 3;
+      case 3: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
+         parse_z:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &z_)));
+          set_has_z();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(37)) goto parse_intensity;
+        break;
+      }
+      
+      // required float intensity = 4;
+      case 4: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
+         parse_intensity:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &intensity_)));
+          set_has_intensity();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectAtEnd()) return true;
+        break;
+      }
+      
+      default: {
+      handle_uninterpreted:
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
+          return true;
+        }
+        DO_(::google::protobuf::internal::WireFormat::SkipField(
+              input, tag, mutable_unknown_fields()));
+        break;
+      }
     }
-    
-    // optional float roll = 12;
-    if (has_roll()) {
+  }
+  return true;
+#undef DO_
+}
+
+void PointCloudXYZI_PointXYZI::SerializeWithCachedSizes(
+    ::google::protobuf::io::CodedOutputStream* output) const {
+  // required float x = 1;
+  if (has_x()) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(1, this->x(), output);
+  }
+  
+  // required float y = 2;
+  if (has_y()) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(2, this->y(), output);
+  }
+  
+  // required float z = 3;
+  if (has_z()) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->z(), output);
+  }
+  
+  // required float intensity = 4;
+  if (has_intensity()) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(4, this->intensity(), output);
+  }
+  
+  if (!unknown_fields().empty()) {
+    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
+        unknown_fields(), output);
+  }
+}
+
+::google::protobuf::uint8* PointCloudXYZI_PointXYZI::SerializeWithCachedSizesToArray(
+    ::google::protobuf::uint8* target) const {
+  // required float x = 1;
+  if (has_x()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(1, this->x(), target);
+  }
+  
+  // required float y = 2;
+  if (has_y()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(2, this->y(), target);
+  }
+  
+  // required float z = 3;
+  if (has_z()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->z(), target);
+  }
+  
+  // required float intensity = 4;
+  if (has_intensity()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(4, this->intensity(), target);
+  }
+  
+  if (!unknown_fields().empty()) {
+    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
+        unknown_fields(), target);
+  }
+  return target;
+}
+
+int PointCloudXYZI_PointXYZI::ByteSize() const {
+  int total_size = 0;
+  
+  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
+    // required float x = 1;
+    if (has_x()) {
       total_size += 1 + 4;
     }
     
-    // optional float pitch = 13;
-    if (has_pitch()) {
+    // required float y = 2;
+    if (has_y()) {
       total_size += 1 + 4;
     }
     
-    // optional float yaw = 14;
-    if (has_yaw()) {
+    // required float z = 3;
+    if (has_z()) {
       total_size += 1 + 4;
     }
     
-    // optional float lon = 15;
-    if (has_lon()) {
+    // required float intensity = 4;
+    if (has_intensity()) {
       total_size += 1 + 4;
     }
     
-    // optional float lat = 16;
-    if (has_lat()) {
-      total_size += 2 + 4;
-    }
-    
-  }
-  if (_has_bits_[16 / 32] & (0xffu << (16 % 32))) {
-    // optional float alt = 17;
-    if (has_alt()) {
-      total_size += 2 + 4;
-    }
-    
-    // optional float ground_x = 18;
-    if (has_ground_x()) {
-      total_size += 2 + 4;
-    }
-    
-    // optional float ground_y = 19;
-    if (has_ground_y()) {
-      total_size += 2 + 4;
-    }
-    
-    // optional float ground_z = 20;
-    if (has_ground_z()) {
-      total_size += 2 + 4;
-    }
-    
-  }
-  // repeated float camera_matrix = 21;
-  {
-    int data_size = 0;
-    data_size = 4 * this->camera_matrix_size();
-    total_size += 2 * this->camera_matrix_size() + data_size;
   }
-  
   if (!unknown_fields().empty()) {
     total_size +=
       ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
@@ -2746,10 +3089,10 @@ int RGBDImage::ByteSize() const {
   return total_size;
 }
 
-void RGBDImage::MergeFrom(const ::google::protobuf::Message& from) {
+void PointCloudXYZI_PointXYZI::MergeFrom(const ::google::protobuf::Message& from) {
   GOOGLE_CHECK_NE(&from, this);
-  const RGBDImage* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const RGBDImage*>(
+  const PointCloudXYZI_PointXYZI* source =
+    ::google::protobuf::internal::dynamic_cast_if_available<const PointCloudXYZI_PointXYZI*>(
       &from);
   if (source == NULL) {
     ::google::protobuf::internal::ReflectionOps::Merge(from, this);
@@ -2758,312 +3101,163 @@ void RGBDImage::MergeFrom(const ::google::protobuf::Message& from) {
   }
 }
 
-void RGBDImage::MergeFrom(const RGBDImage& from) {
+void PointCloudXYZI_PointXYZI::MergeFrom(const PointCloudXYZI_PointXYZI& from) {
   GOOGLE_CHECK_NE(&from, this);
-  camera_matrix_.MergeFrom(from.camera_matrix_);
   if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_header()) {
-      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
-    }
-    if (from.has_cols()) {
-      set_cols(from.cols());
-    }
-    if (from.has_rows()) {
-      set_rows(from.rows());
-    }
-    if (from.has_step1()) {
-      set_step1(from.step1());
-    }
-    if (from.has_type1()) {
-      set_type1(from.type1());
-    }
-    if (from.has_imagedata1()) {
-      set_imagedata1(from.imagedata1());
-    }
-    if (from.has_step2()) {
-      set_step2(from.step2());
-    }
-    if (from.has_type2()) {
-      set_type2(from.type2());
-    }
-  }
-  if (from._has_bits_[8 / 32] & (0xffu << (8 % 32))) {
-    if (from.has_imagedata2()) {
-      set_imagedata2(from.imagedata2());
-    }
-    if (from.has_camera_config()) {
-      set_camera_config(from.camera_config());
-    }
-    if (from.has_camera_type()) {
-      set_camera_type(from.camera_type());
-    }
-    if (from.has_roll()) {
-      set_roll(from.roll());
-    }
-    if (from.has_pitch()) {
-      set_pitch(from.pitch());
-    }
-    if (from.has_yaw()) {
-      set_yaw(from.yaw());
-    }
-    if (from.has_lon()) {
-      set_lon(from.lon());
-    }
-    if (from.has_lat()) {
-      set_lat(from.lat());
-    }
-  }
-  if (from._has_bits_[16 / 32] & (0xffu << (16 % 32))) {
-    if (from.has_alt()) {
-      set_alt(from.alt());
+    if (from.has_x()) {
+      set_x(from.x());
     }
-    if (from.has_ground_x()) {
-      set_ground_x(from.ground_x());
+    if (from.has_y()) {
+      set_y(from.y());
     }
-    if (from.has_ground_y()) {
-      set_ground_y(from.ground_y());
+    if (from.has_z()) {
+      set_z(from.z());
     }
-    if (from.has_ground_z()) {
-      set_ground_z(from.ground_z());
+    if (from.has_intensity()) {
+      set_intensity(from.intensity());
     }
   }
   mutable_unknown_fields()->MergeFrom(from.unknown_fields());
 }
 
-void RGBDImage::CopyFrom(const ::google::protobuf::Message& from) {
+void PointCloudXYZI_PointXYZI::CopyFrom(const ::google::protobuf::Message& from) {
   if (&from == this) return;
   Clear();
   MergeFrom(from);
 }
 
-void RGBDImage::CopyFrom(const RGBDImage& from) {
+void PointCloudXYZI_PointXYZI::CopyFrom(const PointCloudXYZI_PointXYZI& from) {
   if (&from == this) return;
   Clear();
   MergeFrom(from);
 }
 
-bool RGBDImage::IsInitialized() const {
-  if ((_has_bits_[0] & 0x000001ff) != 0x000001ff) return false;
+bool PointCloudXYZI_PointXYZI::IsInitialized() const {
+  if ((_has_bits_[0] & 0x0000000f) != 0x0000000f) return false;
   
-  if (has_header()) {
-    if (!this->header().IsInitialized()) return false;
-  }
   return true;
 }
 
-void RGBDImage::Swap(RGBDImage* other) {
+void PointCloudXYZI_PointXYZI::Swap(PointCloudXYZI_PointXYZI* other) {
   if (other != this) {
-    std::swap(header_, other->header_);
-    std::swap(cols_, other->cols_);
-    std::swap(rows_, other->rows_);
-    std::swap(step1_, other->step1_);
-    std::swap(type1_, other->type1_);
-    std::swap(imagedata1_, other->imagedata1_);
-    std::swap(step2_, other->step2_);
-    std::swap(type2_, other->type2_);
-    std::swap(imagedata2_, other->imagedata2_);
-    std::swap(camera_config_, other->camera_config_);
-    std::swap(camera_type_, other->camera_type_);
-    std::swap(roll_, other->roll_);
-    std::swap(pitch_, other->pitch_);
-    std::swap(yaw_, other->yaw_);
-    std::swap(lon_, other->lon_);
-    std::swap(lat_, other->lat_);
-    std::swap(alt_, other->alt_);
-    std::swap(ground_x_, other->ground_x_);
-    std::swap(ground_y_, other->ground_y_);
-    std::swap(ground_z_, other->ground_z_);
-    camera_matrix_.Swap(&other->camera_matrix_);
+    std::swap(x_, other->x_);
+    std::swap(y_, other->y_);
+    std::swap(z_, other->z_);
+    std::swap(intensity_, other->intensity_);
     std::swap(_has_bits_[0], other->_has_bits_[0]);
     _unknown_fields_.Swap(&other->_unknown_fields_);
     std::swap(_cached_size_, other->_cached_size_);
   }
 }
 
-::google::protobuf::Metadata RGBDImage::GetMetadata() const {
+::google::protobuf::Metadata PointCloudXYZI_PointXYZI::GetMetadata() const {
   protobuf_AssignDescriptorsOnce();
   ::google::protobuf::Metadata metadata;
-  metadata.descriptor = RGBDImage_descriptor_;
-  metadata.reflection = RGBDImage_reflection_;
+  metadata.descriptor = PointCloudXYZI_PointXYZI_descriptor_;
+  metadata.reflection = PointCloudXYZI_PointXYZI_reflection_;
   return metadata;
 }
 
 
-// ===================================================================
+// -------------------------------------------------------------------
 
-#ifndef _MSC_VER
-const int Obstacle::kXFieldNumber;
-const int Obstacle::kYFieldNumber;
-const int Obstacle::kZFieldNumber;
-const int Obstacle::kLengthFieldNumber;
-const int Obstacle::kWidthFieldNumber;
-const int Obstacle::kHeightFieldNumber;
+#ifndef _MSC_VER
+const int PointCloudXYZI::kHeaderFieldNumber;
+const int PointCloudXYZI::kPointsFieldNumber;
 #endif  // !_MSC_VER
 
-Obstacle::Obstacle()
+PointCloudXYZI::PointCloudXYZI()
   : ::google::protobuf::Message() {
   SharedCtor();
 }
 
-void Obstacle::InitAsDefaultInstance() {
+void PointCloudXYZI::InitAsDefaultInstance() {
+  header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
 }
 
-Obstacle::Obstacle(const Obstacle& from)
+PointCloudXYZI::PointCloudXYZI(const PointCloudXYZI& from)
   : ::google::protobuf::Message() {
   SharedCtor();
   MergeFrom(from);
 }
 
-void Obstacle::SharedCtor() {
+void PointCloudXYZI::SharedCtor() {
   _cached_size_ = 0;
-  x_ = 0;
-  y_ = 0;
-  z_ = 0;
-  length_ = 0;
-  width_ = 0;
-  height_ = 0;
+  header_ = NULL;
   ::memset(_has_bits_, 0, sizeof(_has_bits_));
 }
 
-Obstacle::~Obstacle() {
+PointCloudXYZI::~PointCloudXYZI() {
   SharedDtor();
 }
 
-void Obstacle::SharedDtor() {
+void PointCloudXYZI::SharedDtor() {
   if (this != default_instance_) {
+    delete header_;
   }
 }
 
-void Obstacle::SetCachedSize(int size) const {
+void PointCloudXYZI::SetCachedSize(int size) const {
   GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
   _cached_size_ = size;
   GOOGLE_SAFE_CONCURRENT_WRITES_END();
 }
-const ::google::protobuf::Descriptor* Obstacle::descriptor() {
+const ::google::protobuf::Descriptor* PointCloudXYZI::descriptor() {
   protobuf_AssignDescriptorsOnce();
-  return Obstacle_descriptor_;
+  return PointCloudXYZI_descriptor_;
 }
 
-const Obstacle& Obstacle::default_instance() {
+const PointCloudXYZI& PointCloudXYZI::default_instance() {
   if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
 }
 
-Obstacle* Obstacle::default_instance_ = NULL;
+PointCloudXYZI* PointCloudXYZI::default_instance_ = NULL;
 
-Obstacle* Obstacle::New() const {
-  return new Obstacle;
+PointCloudXYZI* PointCloudXYZI::New() const {
+  return new PointCloudXYZI;
 }
 
-void Obstacle::Clear() {
+void PointCloudXYZI::Clear() {
   if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    x_ = 0;
-    y_ = 0;
-    z_ = 0;
-    length_ = 0;
-    width_ = 0;
-    height_ = 0;
+    if (has_header()) {
+      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
+    }
   }
+  points_.Clear();
   ::memset(_has_bits_, 0, sizeof(_has_bits_));
   mutable_unknown_fields()->Clear();
 }
 
-bool Obstacle::MergePartialFromCodedStream(
+bool PointCloudXYZI::MergePartialFromCodedStream(
     ::google::protobuf::io::CodedInputStream* input) {
 #define DO_(EXPRESSION) if (!(EXPRESSION)) return false
   ::google::protobuf::uint32 tag;
   while ((tag = input->ReadTag()) != 0) {
     switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // optional float x = 1;
+      // required .px.HeaderInfo header = 1;
       case 1: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &x_)));
-          set_has_x();
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
+          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
+               input, mutable_header()));
         } else {
           goto handle_uninterpreted;
         }
-        if (input->ExpectTag(21)) goto parse_y;
+        if (input->ExpectTag(18)) goto parse_points;
         break;
       }
       
-      // optional float y = 2;
+      // repeated .px.PointCloudXYZI.PointXYZI points = 2;
       case 2: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_y:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &y_)));
-          set_has_y();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(29)) goto parse_z;
-        break;
-      }
-      
-      // optional float z = 3;
-      case 3: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_z:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &z_)));
-          set_has_z();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(37)) goto parse_length;
-        break;
-      }
-      
-      // optional float length = 4;
-      case 4: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_length:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &length_)));
-          set_has_length();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(45)) goto parse_width;
-        break;
-      }
-      
-      // optional float width = 5;
-      case 5: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_width:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &width_)));
-          set_has_width();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(53)) goto parse_height;
-        break;
-      }
-      
-      // optional float height = 6;
-      case 6: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_height:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &height_)));
-          set_has_height();
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
+         parse_points:
+          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
+                input, add_points()));
         } else {
           goto handle_uninterpreted;
         }
+        if (input->ExpectTag(18)) goto parse_points;
         if (input->ExpectAtEnd()) return true;
         break;
       }
@@ -3084,36 +3278,18 @@ bool Obstacle::MergePartialFromCodedStream(
 #undef DO_
 }
 
-void Obstacle::SerializeWithCachedSizes(
+void PointCloudXYZI::SerializeWithCachedSizes(
     ::google::protobuf::io::CodedOutputStream* output) const {
-  // optional float x = 1;
-  if (has_x()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(1, this->x(), output);
-  }
-  
-  // optional float y = 2;
-  if (has_y()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(2, this->y(), output);
-  }
-  
-  // optional float z = 3;
-  if (has_z()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->z(), output);
-  }
-  
-  // optional float length = 4;
-  if (has_length()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(4, this->length(), output);
-  }
-  
-  // optional float width = 5;
-  if (has_width()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(5, this->width(), output);
+  // required .px.HeaderInfo header = 1;
+  if (has_header()) {
+    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
+      1, this->header(), output);
   }
   
-  // optional float height = 6;
-  if (has_height()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(6, this->height(), output);
+  // repeated .px.PointCloudXYZI.PointXYZI points = 2;
+  for (int i = 0; i < this->points_size(); i++) {
+    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
+      2, this->points(i), output);
   }
   
   if (!unknown_fields().empty()) {
@@ -3122,36 +3298,20 @@ void Obstacle::SerializeWithCachedSizes(
   }
 }
 
-::google::protobuf::uint8* Obstacle::SerializeWithCachedSizesToArray(
+::google::protobuf::uint8* PointCloudXYZI::SerializeWithCachedSizesToArray(
     ::google::protobuf::uint8* target) const {
-  // optional float x = 1;
-  if (has_x()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(1, this->x(), target);
-  }
-  
-  // optional float y = 2;
-  if (has_y()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(2, this->y(), target);
-  }
-  
-  // optional float z = 3;
-  if (has_z()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->z(), target);
-  }
-  
-  // optional float length = 4;
-  if (has_length()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(4, this->length(), target);
-  }
-  
-  // optional float width = 5;
-  if (has_width()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(5, this->width(), target);
+  // required .px.HeaderInfo header = 1;
+  if (has_header()) {
+    target = ::google::protobuf::internal::WireFormatLite::
+      WriteMessageNoVirtualToArray(
+        1, this->header(), target);
   }
   
-  // optional float height = 6;
-  if (has_height()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(6, this->height(), target);
+  // repeated .px.PointCloudXYZI.PointXYZI points = 2;
+  for (int i = 0; i < this->points_size(); i++) {
+    target = ::google::protobuf::internal::WireFormatLite::
+      WriteMessageNoVirtualToArray(
+        2, this->points(i), target);
   }
   
   if (!unknown_fields().empty()) {
@@ -3161,41 +3321,26 @@ void Obstacle::SerializeWithCachedSizes(
   return target;
 }
 
-int Obstacle::ByteSize() const {
+int PointCloudXYZI::ByteSize() const {
   int total_size = 0;
   
   if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // optional float x = 1;
-    if (has_x()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float y = 2;
-    if (has_y()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float z = 3;
-    if (has_z()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float length = 4;
-    if (has_length()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float width = 5;
-    if (has_width()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional float height = 6;
-    if (has_height()) {
-      total_size += 1 + 4;
+    // required .px.HeaderInfo header = 1;
+    if (has_header()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
+          this->header());
     }
     
   }
+  // repeated .px.PointCloudXYZI.PointXYZI points = 2;
+  total_size += 1 * this->points_size();
+  for (int i = 0; i < this->points_size(); i++) {
+    total_size +=
+      ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
+        this->points(i));
+  }
+  
   if (!unknown_fields().empty()) {
     total_size +=
       ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
@@ -3207,10 +3352,10 @@ int Obstacle::ByteSize() const {
   return total_size;
 }
 
-void Obstacle::MergeFrom(const ::google::protobuf::Message& from) {
+void PointCloudXYZI::MergeFrom(const ::google::protobuf::Message& from) {
   GOOGLE_CHECK_NE(&from, this);
-  const Obstacle* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const Obstacle*>(
+  const PointCloudXYZI* source =
+    ::google::protobuf::internal::dynamic_cast_if_available<const PointCloudXYZI*>(
       &from);
   if (source == NULL) {
     ::google::protobuf::internal::ReflectionOps::Merge(from, this);
@@ -3219,67 +3364,56 @@ void Obstacle::MergeFrom(const ::google::protobuf::Message& from) {
   }
 }
 
-void Obstacle::MergeFrom(const Obstacle& from) {
+void PointCloudXYZI::MergeFrom(const PointCloudXYZI& from) {
   GOOGLE_CHECK_NE(&from, this);
+  points_.MergeFrom(from.points_);
   if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_x()) {
-      set_x(from.x());
-    }
-    if (from.has_y()) {
-      set_y(from.y());
-    }
-    if (from.has_z()) {
-      set_z(from.z());
-    }
-    if (from.has_length()) {
-      set_length(from.length());
-    }
-    if (from.has_width()) {
-      set_width(from.width());
-    }
-    if (from.has_height()) {
-      set_height(from.height());
+    if (from.has_header()) {
+      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
     }
   }
   mutable_unknown_fields()->MergeFrom(from.unknown_fields());
 }
 
-void Obstacle::CopyFrom(const ::google::protobuf::Message& from) {
+void PointCloudXYZI::CopyFrom(const ::google::protobuf::Message& from) {
   if (&from == this) return;
   Clear();
   MergeFrom(from);
 }
 
-void Obstacle::CopyFrom(const Obstacle& from) {
+void PointCloudXYZI::CopyFrom(const PointCloudXYZI& from) {
   if (&from == this) return;
   Clear();
   MergeFrom(from);
 }
 
-bool Obstacle::IsInitialized() const {
+bool PointCloudXYZI::IsInitialized() const {
+  if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false;
   
+  if (has_header()) {
+    if (!this->header().IsInitialized()) return false;
+  }
+  for (int i = 0; i < points_size(); i++) {
+    if (!this->points(i).IsInitialized()) return false;
+  }
   return true;
 }
 
-void Obstacle::Swap(Obstacle* other) {
+void PointCloudXYZI::Swap(PointCloudXYZI* other) {
   if (other != this) {
-    std::swap(x_, other->x_);
-    std::swap(y_, other->y_);
-    std::swap(z_, other->z_);
-    std::swap(length_, other->length_);
-    std::swap(width_, other->width_);
-    std::swap(height_, other->height_);
+    std::swap(header_, other->header_);
+    points_.Swap(&other->points_);
     std::swap(_has_bits_[0], other->_has_bits_[0]);
     _unknown_fields_.Swap(&other->_unknown_fields_);
     std::swap(_cached_size_, other->_cached_size_);
   }
 }
 
-::google::protobuf::Metadata Obstacle::GetMetadata() const {
+::google::protobuf::Metadata PointCloudXYZI::GetMetadata() const {
   protobuf_AssignDescriptorsOnce();
   ::google::protobuf::Metadata metadata;
-  metadata.descriptor = Obstacle_descriptor_;
-  metadata.reflection = Obstacle_reflection_;
+  metadata.descriptor = PointCloudXYZI_descriptor_;
+  metadata.reflection = PointCloudXYZI_reflection_;
   return metadata;
 }
 
@@ -3287,102 +3421,140 @@ void Obstacle::Swap(Obstacle* other) {
 // ===================================================================
 
 #ifndef _MSC_VER
-const int ObstacleList::kHeaderFieldNumber;
-const int ObstacleList::kObstaclesFieldNumber;
+const int PointCloudXYZRGB_PointXYZRGB::kXFieldNumber;
+const int PointCloudXYZRGB_PointXYZRGB::kYFieldNumber;
+const int PointCloudXYZRGB_PointXYZRGB::kZFieldNumber;
+const int PointCloudXYZRGB_PointXYZRGB::kRgbFieldNumber;
 #endif  // !_MSC_VER
 
-ObstacleList::ObstacleList()
+PointCloudXYZRGB_PointXYZRGB::PointCloudXYZRGB_PointXYZRGB()
   : ::google::protobuf::Message() {
   SharedCtor();
 }
 
-void ObstacleList::InitAsDefaultInstance() {
-  header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
+void PointCloudXYZRGB_PointXYZRGB::InitAsDefaultInstance() {
 }
 
-ObstacleList::ObstacleList(const ObstacleList& from)
+PointCloudXYZRGB_PointXYZRGB::PointCloudXYZRGB_PointXYZRGB(const PointCloudXYZRGB_PointXYZRGB& from)
   : ::google::protobuf::Message() {
   SharedCtor();
   MergeFrom(from);
 }
 
-void ObstacleList::SharedCtor() {
+void PointCloudXYZRGB_PointXYZRGB::SharedCtor() {
   _cached_size_ = 0;
-  header_ = NULL;
+  x_ = 0;
+  y_ = 0;
+  z_ = 0;
+  rgb_ = 0;
   ::memset(_has_bits_, 0, sizeof(_has_bits_));
 }
 
-ObstacleList::~ObstacleList() {
+PointCloudXYZRGB_PointXYZRGB::~PointCloudXYZRGB_PointXYZRGB() {
   SharedDtor();
 }
 
-void ObstacleList::SharedDtor() {
+void PointCloudXYZRGB_PointXYZRGB::SharedDtor() {
   if (this != default_instance_) {
-    delete header_;
   }
 }
 
-void ObstacleList::SetCachedSize(int size) const {
+void PointCloudXYZRGB_PointXYZRGB::SetCachedSize(int size) const {
   GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
   _cached_size_ = size;
   GOOGLE_SAFE_CONCURRENT_WRITES_END();
 }
-const ::google::protobuf::Descriptor* ObstacleList::descriptor() {
+const ::google::protobuf::Descriptor* PointCloudXYZRGB_PointXYZRGB::descriptor() {
   protobuf_AssignDescriptorsOnce();
-  return ObstacleList_descriptor_;
+  return PointCloudXYZRGB_PointXYZRGB_descriptor_;
 }
 
-const ObstacleList& ObstacleList::default_instance() {
+const PointCloudXYZRGB_PointXYZRGB& PointCloudXYZRGB_PointXYZRGB::default_instance() {
   if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
 }
 
-ObstacleList* ObstacleList::default_instance_ = NULL;
+PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB_PointXYZRGB::default_instance_ = NULL;
 
-ObstacleList* ObstacleList::New() const {
-  return new ObstacleList;
+PointCloudXYZRGB_PointXYZRGB* PointCloudXYZRGB_PointXYZRGB::New() const {
+  return new PointCloudXYZRGB_PointXYZRGB;
 }
 
-void ObstacleList::Clear() {
+void PointCloudXYZRGB_PointXYZRGB::Clear() {
   if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (has_header()) {
-      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-    }
+    x_ = 0;
+    y_ = 0;
+    z_ = 0;
+    rgb_ = 0;
   }
-  obstacles_.Clear();
   ::memset(_has_bits_, 0, sizeof(_has_bits_));
   mutable_unknown_fields()->Clear();
 }
 
-bool ObstacleList::MergePartialFromCodedStream(
+bool PointCloudXYZRGB_PointXYZRGB::MergePartialFromCodedStream(
     ::google::protobuf::io::CodedInputStream* input) {
 #define DO_(EXPRESSION) if (!(EXPRESSION)) return false
   ::google::protobuf::uint32 tag;
   while ((tag = input->ReadTag()) != 0) {
     switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required .px.HeaderInfo header = 1;
+      // required float x = 1;
       case 1: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-               input, mutable_header()));
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &x_)));
+          set_has_x();
         } else {
           goto handle_uninterpreted;
         }
-        if (input->ExpectTag(18)) goto parse_obstacles;
+        if (input->ExpectTag(21)) goto parse_y;
         break;
       }
       
-      // repeated .px.Obstacle obstacles = 2;
+      // required float y = 2;
       case 2: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_obstacles:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-                input, add_obstacles()));
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
+         parse_y:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &y_)));
+          set_has_y();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(29)) goto parse_z;
+        break;
+      }
+      
+      // required float z = 3;
+      case 3: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
+         parse_z:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &z_)));
+          set_has_z();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(37)) goto parse_rgb;
+        break;
+      }
+      
+      // required float rgb = 4;
+      case 4: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
+         parse_rgb:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &rgb_)));
+          set_has_rgb();
         } else {
           goto handle_uninterpreted;
         }
-        if (input->ExpectTag(18)) goto parse_obstacles;
         if (input->ExpectAtEnd()) return true;
         break;
       }
@@ -3403,18 +3575,26 @@ bool ObstacleList::MergePartialFromCodedStream(
 #undef DO_
 }
 
-void ObstacleList::SerializeWithCachedSizes(
+void PointCloudXYZRGB_PointXYZRGB::SerializeWithCachedSizes(
     ::google::protobuf::io::CodedOutputStream* output) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      1, this->header(), output);
+  // required float x = 1;
+  if (has_x()) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(1, this->x(), output);
   }
   
-  // repeated .px.Obstacle obstacles = 2;
-  for (int i = 0; i < this->obstacles_size(); i++) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      2, this->obstacles(i), output);
+  // required float y = 2;
+  if (has_y()) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(2, this->y(), output);
+  }
+  
+  // required float z = 3;
+  if (has_z()) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->z(), output);
+  }
+  
+  // required float rgb = 4;
+  if (has_rgb()) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(4, this->rgb(), output);
   }
   
   if (!unknown_fields().empty()) {
@@ -3423,20 +3603,26 @@ void ObstacleList::SerializeWithCachedSizes(
   }
 }
 
-::google::protobuf::uint8* ObstacleList::SerializeWithCachedSizesToArray(
+::google::protobuf::uint8* PointCloudXYZRGB_PointXYZRGB::SerializeWithCachedSizesToArray(
     ::google::protobuf::uint8* target) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        1, this->header(), target);
+  // required float x = 1;
+  if (has_x()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(1, this->x(), target);
   }
   
-  // repeated .px.Obstacle obstacles = 2;
-  for (int i = 0; i < this->obstacles_size(); i++) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        2, this->obstacles(i), target);
+  // required float y = 2;
+  if (has_y()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(2, this->y(), target);
+  }
+  
+  // required float z = 3;
+  if (has_z()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->z(), target);
+  }
+  
+  // required float rgb = 4;
+  if (has_rgb()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(4, this->rgb(), target);
   }
   
   if (!unknown_fields().empty()) {
@@ -3446,26 +3632,31 @@ void ObstacleList::SerializeWithCachedSizes(
   return target;
 }
 
-int ObstacleList::ByteSize() const {
+int PointCloudXYZRGB_PointXYZRGB::ByteSize() const {
   int total_size = 0;
   
   if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required .px.HeaderInfo header = 1;
-    if (has_header()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-          this->header());
+    // required float x = 1;
+    if (has_x()) {
+      total_size += 1 + 4;
+    }
+    
+    // required float y = 2;
+    if (has_y()) {
+      total_size += 1 + 4;
+    }
+    
+    // required float z = 3;
+    if (has_z()) {
+      total_size += 1 + 4;
+    }
+    
+    // required float rgb = 4;
+    if (has_rgb()) {
+      total_size += 1 + 4;
     }
     
   }
-  // repeated .px.Obstacle obstacles = 2;
-  total_size += 1 * this->obstacles_size();
-  for (int i = 0; i < this->obstacles_size(); i++) {
-    total_size +=
-      ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-        this->obstacles(i));
-  }
-  
   if (!unknown_fields().empty()) {
     total_size +=
       ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
@@ -3477,10 +3668,10 @@ int ObstacleList::ByteSize() const {
   return total_size;
 }
 
-void ObstacleList::MergeFrom(const ::google::protobuf::Message& from) {
+void PointCloudXYZRGB_PointXYZRGB::MergeFrom(const ::google::protobuf::Message& from) {
   GOOGLE_CHECK_NE(&from, this);
-  const ObstacleList* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const ObstacleList*>(
+  const PointCloudXYZRGB_PointXYZRGB* source =
+    ::google::protobuf::internal::dynamic_cast_if_available<const PointCloudXYZRGB_PointXYZRGB*>(
       &from);
   if (source == NULL) {
     ::google::protobuf::internal::ReflectionOps::Merge(from, this);
@@ -3489,317 +3680,163 @@ void ObstacleList::MergeFrom(const ::google::protobuf::Message& from) {
   }
 }
 
-void ObstacleList::MergeFrom(const ObstacleList& from) {
+void PointCloudXYZRGB_PointXYZRGB::MergeFrom(const PointCloudXYZRGB_PointXYZRGB& from) {
   GOOGLE_CHECK_NE(&from, this);
-  obstacles_.MergeFrom(from.obstacles_);
   if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_header()) {
-      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
+    if (from.has_x()) {
+      set_x(from.x());
+    }
+    if (from.has_y()) {
+      set_y(from.y());
+    }
+    if (from.has_z()) {
+      set_z(from.z());
+    }
+    if (from.has_rgb()) {
+      set_rgb(from.rgb());
     }
   }
   mutable_unknown_fields()->MergeFrom(from.unknown_fields());
 }
 
-void ObstacleList::CopyFrom(const ::google::protobuf::Message& from) {
+void PointCloudXYZRGB_PointXYZRGB::CopyFrom(const ::google::protobuf::Message& from) {
   if (&from == this) return;
   Clear();
   MergeFrom(from);
 }
 
-void ObstacleList::CopyFrom(const ObstacleList& from) {
+void PointCloudXYZRGB_PointXYZRGB::CopyFrom(const PointCloudXYZRGB_PointXYZRGB& from) {
   if (&from == this) return;
   Clear();
   MergeFrom(from);
 }
 
-bool ObstacleList::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false;
+bool PointCloudXYZRGB_PointXYZRGB::IsInitialized() const {
+  if ((_has_bits_[0] & 0x0000000f) != 0x0000000f) return false;
   
-  if (has_header()) {
-    if (!this->header().IsInitialized()) return false;
-  }
   return true;
 }
 
-void ObstacleList::Swap(ObstacleList* other) {
+void PointCloudXYZRGB_PointXYZRGB::Swap(PointCloudXYZRGB_PointXYZRGB* other) {
   if (other != this) {
-    std::swap(header_, other->header_);
-    obstacles_.Swap(&other->obstacles_);
+    std::swap(x_, other->x_);
+    std::swap(y_, other->y_);
+    std::swap(z_, other->z_);
+    std::swap(rgb_, other->rgb_);
     std::swap(_has_bits_[0], other->_has_bits_[0]);
     _unknown_fields_.Swap(&other->_unknown_fields_);
     std::swap(_cached_size_, other->_cached_size_);
   }
 }
 
-::google::protobuf::Metadata ObstacleList::GetMetadata() const {
+::google::protobuf::Metadata PointCloudXYZRGB_PointXYZRGB::GetMetadata() const {
   protobuf_AssignDescriptorsOnce();
   ::google::protobuf::Metadata metadata;
-  metadata.descriptor = ObstacleList_descriptor_;
-  metadata.reflection = ObstacleList_reflection_;
+  metadata.descriptor = PointCloudXYZRGB_PointXYZRGB_descriptor_;
+  metadata.reflection = PointCloudXYZRGB_PointXYZRGB_reflection_;
   return metadata;
 }
 
 
-// ===================================================================
+// -------------------------------------------------------------------
 
 #ifndef _MSC_VER
-const int ObstacleMap::kHeaderFieldNumber;
-const int ObstacleMap::kTypeFieldNumber;
-const int ObstacleMap::kResolutionFieldNumber;
-const int ObstacleMap::kRowsFieldNumber;
-const int ObstacleMap::kColsFieldNumber;
-const int ObstacleMap::kMapR0FieldNumber;
-const int ObstacleMap::kMapC0FieldNumber;
-const int ObstacleMap::kArrayR0FieldNumber;
-const int ObstacleMap::kArrayC0FieldNumber;
-const int ObstacleMap::kDataFieldNumber;
+const int PointCloudXYZRGB::kHeaderFieldNumber;
+const int PointCloudXYZRGB::kPointsFieldNumber;
 #endif  // !_MSC_VER
 
-ObstacleMap::ObstacleMap()
+PointCloudXYZRGB::PointCloudXYZRGB()
   : ::google::protobuf::Message() {
   SharedCtor();
 }
 
-void ObstacleMap::InitAsDefaultInstance() {
+void PointCloudXYZRGB::InitAsDefaultInstance() {
   header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
 }
 
-ObstacleMap::ObstacleMap(const ObstacleMap& from)
+PointCloudXYZRGB::PointCloudXYZRGB(const PointCloudXYZRGB& from)
   : ::google::protobuf::Message() {
   SharedCtor();
   MergeFrom(from);
 }
 
-void ObstacleMap::SharedCtor() {
+void PointCloudXYZRGB::SharedCtor() {
   _cached_size_ = 0;
   header_ = NULL;
-  type_ = 0;
-  resolution_ = 0;
-  rows_ = 0;
-  cols_ = 0;
-  mapr0_ = 0;
-  mapc0_ = 0;
-  arrayr0_ = 0;
-  arrayc0_ = 0;
-  data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
   ::memset(_has_bits_, 0, sizeof(_has_bits_));
 }
 
-ObstacleMap::~ObstacleMap() {
+PointCloudXYZRGB::~PointCloudXYZRGB() {
   SharedDtor();
 }
 
-void ObstacleMap::SharedDtor() {
-  if (data_ != &::google::protobuf::internal::kEmptyString) {
-    delete data_;
-  }
+void PointCloudXYZRGB::SharedDtor() {
   if (this != default_instance_) {
     delete header_;
   }
 }
 
-void ObstacleMap::SetCachedSize(int size) const {
+void PointCloudXYZRGB::SetCachedSize(int size) const {
   GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
   _cached_size_ = size;
   GOOGLE_SAFE_CONCURRENT_WRITES_END();
 }
-const ::google::protobuf::Descriptor* ObstacleMap::descriptor() {
+const ::google::protobuf::Descriptor* PointCloudXYZRGB::descriptor() {
   protobuf_AssignDescriptorsOnce();
-  return ObstacleMap_descriptor_;
+  return PointCloudXYZRGB_descriptor_;
 }
 
-const ObstacleMap& ObstacleMap::default_instance() {
+const PointCloudXYZRGB& PointCloudXYZRGB::default_instance() {
   if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
 }
 
-ObstacleMap* ObstacleMap::default_instance_ = NULL;
-
-ObstacleMap* ObstacleMap::New() const {
-  return new ObstacleMap;
-}
+PointCloudXYZRGB* PointCloudXYZRGB::default_instance_ = NULL;
 
-void ObstacleMap::Clear() {
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (has_header()) {
-      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-    }
-    type_ = 0;
-    resolution_ = 0;
-    rows_ = 0;
-    cols_ = 0;
-    mapr0_ = 0;
-    mapc0_ = 0;
-    arrayr0_ = 0;
-  }
-  if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) {
-    arrayc0_ = 0;
-    if (has_data()) {
-      if (data_ != &::google::protobuf::internal::kEmptyString) {
-        data_->clear();
-      }
-    }
-  }
-  ::memset(_has_bits_, 0, sizeof(_has_bits_));
-  mutable_unknown_fields()->Clear();
+PointCloudXYZRGB* PointCloudXYZRGB::New() const {
+  return new PointCloudXYZRGB;
 }
 
-bool ObstacleMap::MergePartialFromCodedStream(
-    ::google::protobuf::io::CodedInputStream* input) {
-#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
-  ::google::protobuf::uint32 tag;
-  while ((tag = input->ReadTag()) != 0) {
-    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required .px.HeaderInfo header = 1;
-      case 1: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-               input, mutable_header()));
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(16)) goto parse_type;
-        break;
-      }
-      
-      // required int32 type = 2;
-      case 2: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_type:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &type_)));
-          set_has_type();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(29)) goto parse_resolution;
-        break;
-      }
-      
-      // optional float resolution = 3;
-      case 3: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
-         parse_resolution:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &resolution_)));
-          set_has_resolution();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(32)) goto parse_rows;
-        break;
-      }
-      
-      // optional int32 rows = 4;
-      case 4: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_rows:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &rows_)));
-          set_has_rows();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(40)) goto parse_cols;
-        break;
-      }
-      
-      // optional int32 cols = 5;
-      case 5: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_cols:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &cols_)));
-          set_has_cols();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(48)) goto parse_mapR0;
-        break;
-      }
-      
-      // optional int32 mapR0 = 6;
-      case 6: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_mapR0:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &mapr0_)));
-          set_has_mapr0();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(56)) goto parse_mapC0;
-        break;
-      }
-      
-      // optional int32 mapC0 = 7;
-      case 7: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_mapC0:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &mapc0_)));
-          set_has_mapc0();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(64)) goto parse_arrayR0;
-        break;
-      }
-      
-      // optional int32 arrayR0 = 8;
-      case 8: {
-        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_arrayR0:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &arrayr0_)));
-          set_has_arrayr0();
-        } else {
-          goto handle_uninterpreted;
-        }
-        if (input->ExpectTag(72)) goto parse_arrayC0;
-        break;
-      }
-      
-      // optional int32 arrayC0 = 9;
-      case 9: {
+void PointCloudXYZRGB::Clear() {
+  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
+    if (has_header()) {
+      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
+    }
+  }
+  points_.Clear();
+  ::memset(_has_bits_, 0, sizeof(_has_bits_));
+  mutable_unknown_fields()->Clear();
+}
+
+bool PointCloudXYZRGB::MergePartialFromCodedStream(
+    ::google::protobuf::io::CodedInputStream* input) {
+#define DO_(EXPRESSION) if (!(EXPRESSION)) return false
+  ::google::protobuf::uint32 tag;
+  while ((tag = input->ReadTag()) != 0) {
+    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
+      // required .px.HeaderInfo header = 1;
+      case 1: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
-         parse_arrayC0:
-          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
-                 input, &arrayc0_)));
-          set_has_arrayc0();
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
+          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
+               input, mutable_header()));
         } else {
           goto handle_uninterpreted;
         }
-        if (input->ExpectTag(82)) goto parse_data;
+        if (input->ExpectTag(18)) goto parse_points;
         break;
       }
       
-      // optional bytes data = 10;
-      case 10: {
+      // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
+      case 2: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
             ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_data:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadBytes(
-                input, this->mutable_data()));
+         parse_points:
+          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
+                input, add_points()));
         } else {
           goto handle_uninterpreted;
         }
+        if (input->ExpectTag(18)) goto parse_points;
         if (input->ExpectAtEnd()) return true;
         break;
       }
@@ -3820,7 +3857,7 @@ bool ObstacleMap::MergePartialFromCodedStream(
 #undef DO_
 }
 
-void ObstacleMap::SerializeWithCachedSizes(
+void PointCloudXYZRGB::SerializeWithCachedSizes(
     ::google::protobuf::io::CodedOutputStream* output) const {
   // required .px.HeaderInfo header = 1;
   if (has_header()) {
@@ -3828,50 +3865,10 @@ void ObstacleMap::SerializeWithCachedSizes(
       1, this->header(), output);
   }
   
-  // required int32 type = 2;
-  if (has_type()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(2, this->type(), output);
-  }
-  
-  // optional float resolution = 3;
-  if (has_resolution()) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(3, this->resolution(), output);
-  }
-  
-  // optional int32 rows = 4;
-  if (has_rows()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(4, this->rows(), output);
-  }
-  
-  // optional int32 cols = 5;
-  if (has_cols()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(5, this->cols(), output);
-  }
-  
-  // optional int32 mapR0 = 6;
-  if (has_mapr0()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(6, this->mapr0(), output);
-  }
-  
-  // optional int32 mapC0 = 7;
-  if (has_mapc0()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(7, this->mapc0(), output);
-  }
-  
-  // optional int32 arrayR0 = 8;
-  if (has_arrayr0()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(8, this->arrayr0(), output);
-  }
-  
-  // optional int32 arrayC0 = 9;
-  if (has_arrayc0()) {
-    ::google::protobuf::internal::WireFormatLite::WriteInt32(9, this->arrayc0(), output);
-  }
-  
-  // optional bytes data = 10;
-  if (has_data()) {
-    ::google::protobuf::internal::WireFormatLite::WriteBytes(
-      10, this->data(), output);
+  // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
+  for (int i = 0; i < this->points_size(); i++) {
+    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
+      2, this->points(i), output);
   }
   
   if (!unknown_fields().empty()) {
@@ -3880,7 +3877,7 @@ void ObstacleMap::SerializeWithCachedSizes(
   }
 }
 
-::google::protobuf::uint8* ObstacleMap::SerializeWithCachedSizesToArray(
+::google::protobuf::uint8* PointCloudXYZRGB::SerializeWithCachedSizesToArray(
     ::google::protobuf::uint8* target) const {
   // required .px.HeaderInfo header = 1;
   if (has_header()) {
@@ -3889,51 +3886,11 @@ void ObstacleMap::SerializeWithCachedSizes(
         1, this->header(), target);
   }
   
-  // required int32 type = 2;
-  if (has_type()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(2, this->type(), target);
-  }
-  
-  // optional float resolution = 3;
-  if (has_resolution()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(3, this->resolution(), target);
-  }
-  
-  // optional int32 rows = 4;
-  if (has_rows()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(4, this->rows(), target);
-  }
-  
-  // optional int32 cols = 5;
-  if (has_cols()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(5, this->cols(), target);
-  }
-  
-  // optional int32 mapR0 = 6;
-  if (has_mapr0()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(6, this->mapr0(), target);
-  }
-  
-  // optional int32 mapC0 = 7;
-  if (has_mapc0()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(7, this->mapc0(), target);
-  }
-  
-  // optional int32 arrayR0 = 8;
-  if (has_arrayr0()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(8, this->arrayr0(), target);
-  }
-  
-  // optional int32 arrayC0 = 9;
-  if (has_arrayc0()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(9, this->arrayc0(), target);
-  }
-  
-  // optional bytes data = 10;
-  if (has_data()) {
-    target =
-      ::google::protobuf::internal::WireFormatLite::WriteBytesToArray(
-        10, this->data(), target);
+  // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
+  for (int i = 0; i < this->points_size(); i++) {
+    target = ::google::protobuf::internal::WireFormatLite::
+      WriteMessageNoVirtualToArray(
+        2, this->points(i), target);
   }
   
   if (!unknown_fields().empty()) {
@@ -3943,7 +3900,7 @@ void ObstacleMap::SerializeWithCachedSizes(
   return target;
 }
 
-int ObstacleMap::ByteSize() const {
+int PointCloudXYZRGB::ByteSize() const {
   int total_size = 0;
   
   if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
@@ -3954,70 +3911,15 @@ int ObstacleMap::ByteSize() const {
           this->header());
     }
     
-    // required int32 type = 2;
-    if (has_type()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->type());
-    }
-    
-    // optional float resolution = 3;
-    if (has_resolution()) {
-      total_size += 1 + 4;
-    }
-    
-    // optional int32 rows = 4;
-    if (has_rows()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->rows());
-    }
-    
-    // optional int32 cols = 5;
-    if (has_cols()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->cols());
-    }
-    
-    // optional int32 mapR0 = 6;
-    if (has_mapr0()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->mapr0());
-    }
-    
-    // optional int32 mapC0 = 7;
-    if (has_mapc0()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->mapc0());
-    }
-    
-    // optional int32 arrayR0 = 8;
-    if (has_arrayr0()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->arrayr0());
-    }
-    
   }
-  if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) {
-    // optional int32 arrayC0 = 9;
-    if (has_arrayc0()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::Int32Size(
-          this->arrayc0());
-    }
-    
-    // optional bytes data = 10;
-    if (has_data()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::BytesSize(
-          this->data());
-    }
-    
+  // repeated .px.PointCloudXYZRGB.PointXYZRGB points = 2;
+  total_size += 1 * this->points_size();
+  for (int i = 0; i < this->points_size(); i++) {
+    total_size +=
+      ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
+        this->points(i));
   }
+  
   if (!unknown_fields().empty()) {
     total_size +=
       ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
@@ -4029,101 +3931,68 @@ int ObstacleMap::ByteSize() const {
   return total_size;
 }
 
-void ObstacleMap::MergeFrom(const ::google::protobuf::Message& from) {
+void PointCloudXYZRGB::MergeFrom(const ::google::protobuf::Message& from) {
   GOOGLE_CHECK_NE(&from, this);
-  const ObstacleMap* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const ObstacleMap*>(
+  const PointCloudXYZRGB* source =
+    ::google::protobuf::internal::dynamic_cast_if_available<const PointCloudXYZRGB*>(
       &from);
   if (source == NULL) {
     ::google::protobuf::internal::ReflectionOps::Merge(from, this);
   } else {
     MergeFrom(*source);
-  }
-}
-
-void ObstacleMap::MergeFrom(const ObstacleMap& from) {
-  GOOGLE_CHECK_NE(&from, this);
-  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_header()) {
-      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
-    }
-    if (from.has_type()) {
-      set_type(from.type());
-    }
-    if (from.has_resolution()) {
-      set_resolution(from.resolution());
-    }
-    if (from.has_rows()) {
-      set_rows(from.rows());
-    }
-    if (from.has_cols()) {
-      set_cols(from.cols());
-    }
-    if (from.has_mapr0()) {
-      set_mapr0(from.mapr0());
-    }
-    if (from.has_mapc0()) {
-      set_mapc0(from.mapc0());
-    }
-    if (from.has_arrayr0()) {
-      set_arrayr0(from.arrayr0());
-    }
-  }
-  if (from._has_bits_[8 / 32] & (0xffu << (8 % 32))) {
-    if (from.has_arrayc0()) {
-      set_arrayc0(from.arrayc0());
-    }
-    if (from.has_data()) {
-      set_data(from.data());
+  }
+}
+
+void PointCloudXYZRGB::MergeFrom(const PointCloudXYZRGB& from) {
+  GOOGLE_CHECK_NE(&from, this);
+  points_.MergeFrom(from.points_);
+  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
+    if (from.has_header()) {
+      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
     }
   }
   mutable_unknown_fields()->MergeFrom(from.unknown_fields());
 }
 
-void ObstacleMap::CopyFrom(const ::google::protobuf::Message& from) {
+void PointCloudXYZRGB::CopyFrom(const ::google::protobuf::Message& from) {
   if (&from == this) return;
   Clear();
   MergeFrom(from);
 }
 
-void ObstacleMap::CopyFrom(const ObstacleMap& from) {
+void PointCloudXYZRGB::CopyFrom(const PointCloudXYZRGB& from) {
   if (&from == this) return;
   Clear();
   MergeFrom(from);
 }
 
-bool ObstacleMap::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000003) != 0x00000003) return false;
+bool PointCloudXYZRGB::IsInitialized() const {
+  if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false;
   
   if (has_header()) {
     if (!this->header().IsInitialized()) return false;
   }
+  for (int i = 0; i < points_size(); i++) {
+    if (!this->points(i).IsInitialized()) return false;
+  }
   return true;
 }
 
-void ObstacleMap::Swap(ObstacleMap* other) {
+void PointCloudXYZRGB::Swap(PointCloudXYZRGB* other) {
   if (other != this) {
     std::swap(header_, other->header_);
-    std::swap(type_, other->type_);
-    std::swap(resolution_, other->resolution_);
-    std::swap(rows_, other->rows_);
-    std::swap(cols_, other->cols_);
-    std::swap(mapr0_, other->mapr0_);
-    std::swap(mapc0_, other->mapc0_);
-    std::swap(arrayr0_, other->arrayr0_);
-    std::swap(arrayc0_, other->arrayc0_);
-    std::swap(data_, other->data_);
+    points_.Swap(&other->points_);
     std::swap(_has_bits_[0], other->_has_bits_[0]);
     _unknown_fields_.Swap(&other->_unknown_fields_);
     std::swap(_cached_size_, other->_cached_size_);
   }
 }
 
-::google::protobuf::Metadata ObstacleMap::GetMetadata() const {
+::google::protobuf::Metadata PointCloudXYZRGB::GetMetadata() const {
   protobuf_AssignDescriptorsOnce();
   ::google::protobuf::Metadata metadata;
-  metadata.descriptor = ObstacleMap_descriptor_;
-  metadata.reflection = ObstacleMap_reflection_;
+  metadata.descriptor = PointCloudXYZRGB_descriptor_;
+  metadata.reflection = PointCloudXYZRGB_reflection_;
   return metadata;
 }
 
@@ -4131,178 +4000,484 @@ void ObstacleMap::Swap(ObstacleMap* other) {
 // ===================================================================
 
 #ifndef _MSC_VER
-const int Waypoint::kXFieldNumber;
-const int Waypoint::kYFieldNumber;
-const int Waypoint::kZFieldNumber;
-const int Waypoint::kRollFieldNumber;
-const int Waypoint::kPitchFieldNumber;
-const int Waypoint::kYawFieldNumber;
+const int RGBDImage::kHeaderFieldNumber;
+const int RGBDImage::kColsFieldNumber;
+const int RGBDImage::kRowsFieldNumber;
+const int RGBDImage::kStep1FieldNumber;
+const int RGBDImage::kType1FieldNumber;
+const int RGBDImage::kImageData1FieldNumber;
+const int RGBDImage::kStep2FieldNumber;
+const int RGBDImage::kType2FieldNumber;
+const int RGBDImage::kImageData2FieldNumber;
+const int RGBDImage::kCameraConfigFieldNumber;
+const int RGBDImage::kCameraTypeFieldNumber;
+const int RGBDImage::kRollFieldNumber;
+const int RGBDImage::kPitchFieldNumber;
+const int RGBDImage::kYawFieldNumber;
+const int RGBDImage::kLonFieldNumber;
+const int RGBDImage::kLatFieldNumber;
+const int RGBDImage::kAltFieldNumber;
+const int RGBDImage::kGroundXFieldNumber;
+const int RGBDImage::kGroundYFieldNumber;
+const int RGBDImage::kGroundZFieldNumber;
+const int RGBDImage::kCameraMatrixFieldNumber;
 #endif  // !_MSC_VER
 
-Waypoint::Waypoint()
+RGBDImage::RGBDImage()
   : ::google::protobuf::Message() {
   SharedCtor();
 }
 
-void Waypoint::InitAsDefaultInstance() {
+void RGBDImage::InitAsDefaultInstance() {
+  header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
 }
 
-Waypoint::Waypoint(const Waypoint& from)
+RGBDImage::RGBDImage(const RGBDImage& from)
   : ::google::protobuf::Message() {
   SharedCtor();
   MergeFrom(from);
 }
 
-void Waypoint::SharedCtor() {
+void RGBDImage::SharedCtor() {
   _cached_size_ = 0;
-  x_ = 0;
-  y_ = 0;
-  z_ = 0;
+  header_ = NULL;
+  cols_ = 0u;
+  rows_ = 0u;
+  step1_ = 0u;
+  type1_ = 0u;
+  imagedata1_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
+  step2_ = 0u;
+  type2_ = 0u;
+  imagedata2_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
+  camera_config_ = 0u;
+  camera_type_ = 0u;
   roll_ = 0;
   pitch_ = 0;
   yaw_ = 0;
+  lon_ = 0;
+  lat_ = 0;
+  alt_ = 0;
+  ground_x_ = 0;
+  ground_y_ = 0;
+  ground_z_ = 0;
   ::memset(_has_bits_, 0, sizeof(_has_bits_));
 }
 
-Waypoint::~Waypoint() {
+RGBDImage::~RGBDImage() {
   SharedDtor();
 }
 
-void Waypoint::SharedDtor() {
+void RGBDImage::SharedDtor() {
+  if (imagedata1_ != &::google::protobuf::internal::kEmptyString) {
+    delete imagedata1_;
+  }
+  if (imagedata2_ != &::google::protobuf::internal::kEmptyString) {
+    delete imagedata2_;
+  }
   if (this != default_instance_) {
+    delete header_;
   }
 }
 
-void Waypoint::SetCachedSize(int size) const {
+void RGBDImage::SetCachedSize(int size) const {
   GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
   _cached_size_ = size;
   GOOGLE_SAFE_CONCURRENT_WRITES_END();
 }
-const ::google::protobuf::Descriptor* Waypoint::descriptor() {
+const ::google::protobuf::Descriptor* RGBDImage::descriptor() {
   protobuf_AssignDescriptorsOnce();
-  return Waypoint_descriptor_;
+  return RGBDImage_descriptor_;
 }
 
-const Waypoint& Waypoint::default_instance() {
+const RGBDImage& RGBDImage::default_instance() {
   if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
 }
 
-Waypoint* Waypoint::default_instance_ = NULL;
+RGBDImage* RGBDImage::default_instance_ = NULL;
 
-Waypoint* Waypoint::New() const {
-  return new Waypoint;
+RGBDImage* RGBDImage::New() const {
+  return new RGBDImage;
 }
 
-void Waypoint::Clear() {
+void RGBDImage::Clear() {
   if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    x_ = 0;
-    y_ = 0;
-    z_ = 0;
+    if (has_header()) {
+      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
+    }
+    cols_ = 0u;
+    rows_ = 0u;
+    step1_ = 0u;
+    type1_ = 0u;
+    if (has_imagedata1()) {
+      if (imagedata1_ != &::google::protobuf::internal::kEmptyString) {
+        imagedata1_->clear();
+      }
+    }
+    step2_ = 0u;
+    type2_ = 0u;
+  }
+  if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) {
+    if (has_imagedata2()) {
+      if (imagedata2_ != &::google::protobuf::internal::kEmptyString) {
+        imagedata2_->clear();
+      }
+    }
+    camera_config_ = 0u;
+    camera_type_ = 0u;
     roll_ = 0;
     pitch_ = 0;
     yaw_ = 0;
+    lon_ = 0;
+    lat_ = 0;
+  }
+  if (_has_bits_[16 / 32] & (0xffu << (16 % 32))) {
+    alt_ = 0;
+    ground_x_ = 0;
+    ground_y_ = 0;
+    ground_z_ = 0;
   }
+  camera_matrix_.Clear();
   ::memset(_has_bits_, 0, sizeof(_has_bits_));
   mutable_unknown_fields()->Clear();
 }
 
-bool Waypoint::MergePartialFromCodedStream(
+bool RGBDImage::MergePartialFromCodedStream(
     ::google::protobuf::io::CodedInputStream* input) {
 #define DO_(EXPRESSION) if (!(EXPRESSION)) return false
   ::google::protobuf::uint32 tag;
   while ((tag = input->ReadTag()) != 0) {
     switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required double x = 1;
+      // required .px.HeaderInfo header = 1;
       case 1: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
+          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
+               input, mutable_header()));
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(16)) goto parse_cols;
+        break;
+      }
+      
+      // required uint32 cols = 2;
+      case 2: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
+         parse_cols:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
+                 input, &cols_)));
+          set_has_cols();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(24)) goto parse_rows;
+        break;
+      }
+      
+      // required uint32 rows = 3;
+      case 3: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
+         parse_rows:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
+                 input, &rows_)));
+          set_has_rows();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(32)) goto parse_step1;
+        break;
+      }
+      
+      // required uint32 step1 = 4;
+      case 4: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
+         parse_step1:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
+                 input, &step1_)));
+          set_has_step1();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(40)) goto parse_type1;
+        break;
+      }
+      
+      // required uint32 type1 = 5;
+      case 5: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
+         parse_type1:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
+                 input, &type1_)));
+          set_has_type1();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(50)) goto parse_imageData1;
+        break;
+      }
+      
+      // required bytes imageData1 = 6;
+      case 6: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
+         parse_imageData1:
+          DO_(::google::protobuf::internal::WireFormatLite::ReadBytes(
+                input, this->mutable_imagedata1()));
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(56)) goto parse_step2;
+        break;
+      }
+      
+      // required uint32 step2 = 7;
+      case 7: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
+         parse_step2:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
+                 input, &step2_)));
+          set_has_step2();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(64)) goto parse_type2;
+        break;
+      }
+      
+      // required uint32 type2 = 8;
+      case 8: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
+         parse_type2:
           DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &x_)));
-          set_has_x();
+                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
+                 input, &type2_)));
+          set_has_type2();
         } else {
           goto handle_uninterpreted;
         }
-        if (input->ExpectTag(17)) goto parse_y;
+        if (input->ExpectTag(74)) goto parse_imageData2;
         break;
       }
       
-      // required double y = 2;
-      case 2: {
+      // required bytes imageData2 = 9;
+      case 9: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
-         parse_y:
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
+         parse_imageData2:
+          DO_(::google::protobuf::internal::WireFormatLite::ReadBytes(
+                input, this->mutable_imagedata2()));
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(80)) goto parse_camera_config;
+        break;
+      }
+      
+      // optional uint32 camera_config = 10;
+      case 10: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
+         parse_camera_config:
           DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &y_)));
-          set_has_y();
+                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
+                 input, &camera_config_)));
+          set_has_camera_config();
         } else {
           goto handle_uninterpreted;
         }
-        if (input->ExpectTag(25)) goto parse_z;
+        if (input->ExpectTag(88)) goto parse_camera_type;
         break;
       }
       
-      // optional double z = 3;
-      case 3: {
+      // optional uint32 camera_type = 11;
+      case 11: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
-         parse_z:
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_VARINT) {
+         parse_camera_type:
           DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
-                 input, &z_)));
-          set_has_z();
+                   ::google::protobuf::uint32, ::google::protobuf::internal::WireFormatLite::TYPE_UINT32>(
+                 input, &camera_type_)));
+          set_has_camera_type();
         } else {
           goto handle_uninterpreted;
         }
-        if (input->ExpectTag(33)) goto parse_roll;
+        if (input->ExpectTag(101)) goto parse_roll;
         break;
       }
       
-      // optional double roll = 4;
-      case 4: {
+      // optional float roll = 12;
+      case 12: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
          parse_roll:
           DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
                  input, &roll_)));
           set_has_roll();
         } else {
           goto handle_uninterpreted;
         }
-        if (input->ExpectTag(41)) goto parse_pitch;
+        if (input->ExpectTag(109)) goto parse_pitch;
         break;
       }
       
-      // optional double pitch = 5;
-      case 5: {
+      // optional float pitch = 13;
+      case 13: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
          parse_pitch:
           DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
                  input, &pitch_)));
           set_has_pitch();
         } else {
           goto handle_uninterpreted;
         }
-        if (input->ExpectTag(49)) goto parse_yaw;
+        if (input->ExpectTag(117)) goto parse_yaw;
         break;
       }
       
-      // optional double yaw = 6;
-      case 6: {
+      // optional float yaw = 14;
+      case 14: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
          parse_yaw:
           DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
-                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
                  input, &yaw_)));
           set_has_yaw();
         } else {
           goto handle_uninterpreted;
         }
+        if (input->ExpectTag(125)) goto parse_lon;
+        break;
+      }
+      
+      // optional float lon = 15;
+      case 15: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
+         parse_lon:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &lon_)));
+          set_has_lon();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(133)) goto parse_lat;
+        break;
+      }
+      
+      // optional float lat = 16;
+      case 16: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
+         parse_lat:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &lat_)));
+          set_has_lat();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(141)) goto parse_alt;
+        break;
+      }
+      
+      // optional float alt = 17;
+      case 17: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
+         parse_alt:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &alt_)));
+          set_has_alt();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(149)) goto parse_ground_x;
+        break;
+      }
+      
+      // optional float ground_x = 18;
+      case 18: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
+         parse_ground_x:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &ground_x_)));
+          set_has_ground_x();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(157)) goto parse_ground_y;
+        break;
+      }
+      
+      // optional float ground_y = 19;
+      case 19: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
+         parse_ground_y:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &ground_y_)));
+          set_has_ground_y();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(165)) goto parse_ground_z;
+        break;
+      }
+      
+      // optional float ground_z = 20;
+      case 20: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
+         parse_ground_z:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &ground_z_)));
+          set_has_ground_z();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(173)) goto parse_camera_matrix;
+        break;
+      }
+      
+      // repeated float camera_matrix = 21;
+      case 21: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED32) {
+         parse_camera_matrix:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadRepeatedPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 2, 173, input, this->mutable_camera_matrix())));
+        } else if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag)
+                   == ::google::protobuf::internal::WireFormatLite::
+                      WIRETYPE_LENGTH_DELIMITED) {
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPackedPrimitiveNoInline<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, this->mutable_camera_matrix())));
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(173)) goto parse_camera_matrix;
         if (input->ExpectAtEnd()) return true;
         break;
       }
@@ -4319,40 +4494,119 @@ bool Waypoint::MergePartialFromCodedStream(
       }
     }
   }
-  return true;
-#undef DO_
-}
-
-void Waypoint::SerializeWithCachedSizes(
-    ::google::protobuf::io::CodedOutputStream* output) const {
-  // required double x = 1;
-  if (has_x()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(1, this->x(), output);
+  return true;
+#undef DO_
+}
+
+void RGBDImage::SerializeWithCachedSizes(
+    ::google::protobuf::io::CodedOutputStream* output) const {
+  // required .px.HeaderInfo header = 1;
+  if (has_header()) {
+    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
+      1, this->header(), output);
+  }
+  
+  // required uint32 cols = 2;
+  if (has_cols()) {
+    ::google::protobuf::internal::WireFormatLite::WriteUInt32(2, this->cols(), output);
+  }
+  
+  // required uint32 rows = 3;
+  if (has_rows()) {
+    ::google::protobuf::internal::WireFormatLite::WriteUInt32(3, this->rows(), output);
+  }
+  
+  // required uint32 step1 = 4;
+  if (has_step1()) {
+    ::google::protobuf::internal::WireFormatLite::WriteUInt32(4, this->step1(), output);
+  }
+  
+  // required uint32 type1 = 5;
+  if (has_type1()) {
+    ::google::protobuf::internal::WireFormatLite::WriteUInt32(5, this->type1(), output);
+  }
+  
+  // required bytes imageData1 = 6;
+  if (has_imagedata1()) {
+    ::google::protobuf::internal::WireFormatLite::WriteBytes(
+      6, this->imagedata1(), output);
+  }
+  
+  // required uint32 step2 = 7;
+  if (has_step2()) {
+    ::google::protobuf::internal::WireFormatLite::WriteUInt32(7, this->step2(), output);
+  }
+  
+  // required uint32 type2 = 8;
+  if (has_type2()) {
+    ::google::protobuf::internal::WireFormatLite::WriteUInt32(8, this->type2(), output);
   }
   
-  // required double y = 2;
-  if (has_y()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(2, this->y(), output);
+  // required bytes imageData2 = 9;
+  if (has_imagedata2()) {
+    ::google::protobuf::internal::WireFormatLite::WriteBytes(
+      9, this->imagedata2(), output);
   }
   
-  // optional double z = 3;
-  if (has_z()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(3, this->z(), output);
+  // optional uint32 camera_config = 10;
+  if (has_camera_config()) {
+    ::google::protobuf::internal::WireFormatLite::WriteUInt32(10, this->camera_config(), output);
   }
   
-  // optional double roll = 4;
+  // optional uint32 camera_type = 11;
+  if (has_camera_type()) {
+    ::google::protobuf::internal::WireFormatLite::WriteUInt32(11, this->camera_type(), output);
+  }
+  
+  // optional float roll = 12;
   if (has_roll()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(4, this->roll(), output);
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(12, this->roll(), output);
   }
   
-  // optional double pitch = 5;
+  // optional float pitch = 13;
   if (has_pitch()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(5, this->pitch(), output);
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(13, this->pitch(), output);
   }
   
-  // optional double yaw = 6;
+  // optional float yaw = 14;
   if (has_yaw()) {
-    ::google::protobuf::internal::WireFormatLite::WriteDouble(6, this->yaw(), output);
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(14, this->yaw(), output);
+  }
+  
+  // optional float lon = 15;
+  if (has_lon()) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(15, this->lon(), output);
+  }
+  
+  // optional float lat = 16;
+  if (has_lat()) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(16, this->lat(), output);
+  }
+  
+  // optional float alt = 17;
+  if (has_alt()) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(17, this->alt(), output);
+  }
+  
+  // optional float ground_x = 18;
+  if (has_ground_x()) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(18, this->ground_x(), output);
+  }
+  
+  // optional float ground_y = 19;
+  if (has_ground_y()) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(19, this->ground_y(), output);
+  }
+  
+  // optional float ground_z = 20;
+  if (has_ground_z()) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(20, this->ground_z(), output);
+  }
+  
+  // repeated float camera_matrix = 21;
+  for (int i = 0; i < this->camera_matrix_size(); i++) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(
+      21, this->camera_matrix(i), output);
   }
   
   if (!unknown_fields().empty()) {
@@ -4361,80 +4615,265 @@ void Waypoint::SerializeWithCachedSizes(
   }
 }
 
-::google::protobuf::uint8* Waypoint::SerializeWithCachedSizesToArray(
+::google::protobuf::uint8* RGBDImage::SerializeWithCachedSizesToArray(
     ::google::protobuf::uint8* target) const {
-  // required double x = 1;
-  if (has_x()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(1, this->x(), target);
+  // required .px.HeaderInfo header = 1;
+  if (has_header()) {
+    target = ::google::protobuf::internal::WireFormatLite::
+      WriteMessageNoVirtualToArray(
+        1, this->header(), target);
   }
   
-  // required double y = 2;
-  if (has_y()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(2, this->y(), target);
+  // required uint32 cols = 2;
+  if (has_cols()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(2, this->cols(), target);
   }
   
-  // optional double z = 3;
-  if (has_z()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(3, this->z(), target);
+  // required uint32 rows = 3;
+  if (has_rows()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(3, this->rows(), target);
   }
   
-  // optional double roll = 4;
+  // required uint32 step1 = 4;
+  if (has_step1()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(4, this->step1(), target);
+  }
+  
+  // required uint32 type1 = 5;
+  if (has_type1()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(5, this->type1(), target);
+  }
+  
+  // required bytes imageData1 = 6;
+  if (has_imagedata1()) {
+    target =
+      ::google::protobuf::internal::WireFormatLite::WriteBytesToArray(
+        6, this->imagedata1(), target);
+  }
+  
+  // required uint32 step2 = 7;
+  if (has_step2()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(7, this->step2(), target);
+  }
+  
+  // required uint32 type2 = 8;
+  if (has_type2()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(8, this->type2(), target);
+  }
+  
+  // required bytes imageData2 = 9;
+  if (has_imagedata2()) {
+    target =
+      ::google::protobuf::internal::WireFormatLite::WriteBytesToArray(
+        9, this->imagedata2(), target);
+  }
+  
+  // optional uint32 camera_config = 10;
+  if (has_camera_config()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(10, this->camera_config(), target);
+  }
+  
+  // optional uint32 camera_type = 11;
+  if (has_camera_type()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteUInt32ToArray(11, this->camera_type(), target);
+  }
+  
+  // optional float roll = 12;
   if (has_roll()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(4, this->roll(), target);
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(12, this->roll(), target);
   }
   
-  // optional double pitch = 5;
+  // optional float pitch = 13;
   if (has_pitch()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(5, this->pitch(), target);
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(13, this->pitch(), target);
   }
   
-  // optional double yaw = 6;
+  // optional float yaw = 14;
   if (has_yaw()) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(6, this->yaw(), target);
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(14, this->yaw(), target);
+  }
+  
+  // optional float lon = 15;
+  if (has_lon()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(15, this->lon(), target);
+  }
+  
+  // optional float lat = 16;
+  if (has_lat()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(16, this->lat(), target);
+  }
+  
+  // optional float alt = 17;
+  if (has_alt()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(17, this->alt(), target);
+  }
+  
+  // optional float ground_x = 18;
+  if (has_ground_x()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(18, this->ground_x(), target);
+  }
+  
+  // optional float ground_y = 19;
+  if (has_ground_y()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(19, this->ground_y(), target);
+  }
+  
+  // optional float ground_z = 20;
+  if (has_ground_z()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(20, this->ground_z(), target);
+  }
+  
+  // repeated float camera_matrix = 21;
+  for (int i = 0; i < this->camera_matrix_size(); i++) {
+    target = ::google::protobuf::internal::WireFormatLite::
+      WriteFloatToArray(21, this->camera_matrix(i), target);
   }
   
   if (!unknown_fields().empty()) {
     target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
         unknown_fields(), target);
   }
-  return target;
-}
-
-int Waypoint::ByteSize() const {
-  int total_size = 0;
-  
-  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required double x = 1;
-    if (has_x()) {
-      total_size += 1 + 8;
+  return target;
+}
+
+int RGBDImage::ByteSize() const {
+  int total_size = 0;
+  
+  if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
+    // required .px.HeaderInfo header = 1;
+    if (has_header()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
+          this->header());
+    }
+    
+    // required uint32 cols = 2;
+    if (has_cols()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::UInt32Size(
+          this->cols());
+    }
+    
+    // required uint32 rows = 3;
+    if (has_rows()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::UInt32Size(
+          this->rows());
+    }
+    
+    // required uint32 step1 = 4;
+    if (has_step1()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::UInt32Size(
+          this->step1());
+    }
+    
+    // required uint32 type1 = 5;
+    if (has_type1()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::UInt32Size(
+          this->type1());
+    }
+    
+    // required bytes imageData1 = 6;
+    if (has_imagedata1()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::BytesSize(
+          this->imagedata1());
+    }
+    
+    // required uint32 step2 = 7;
+    if (has_step2()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::UInt32Size(
+          this->step2());
+    }
+    
+    // required uint32 type2 = 8;
+    if (has_type2()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::UInt32Size(
+          this->type2());
+    }
+    
+  }
+  if (_has_bits_[8 / 32] & (0xffu << (8 % 32))) {
+    // required bytes imageData2 = 9;
+    if (has_imagedata2()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::BytesSize(
+          this->imagedata2());
     }
     
-    // required double y = 2;
-    if (has_y()) {
-      total_size += 1 + 8;
+    // optional uint32 camera_config = 10;
+    if (has_camera_config()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::UInt32Size(
+          this->camera_config());
     }
     
-    // optional double z = 3;
-    if (has_z()) {
-      total_size += 1 + 8;
+    // optional uint32 camera_type = 11;
+    if (has_camera_type()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::UInt32Size(
+          this->camera_type());
     }
     
-    // optional double roll = 4;
+    // optional float roll = 12;
     if (has_roll()) {
-      total_size += 1 + 8;
+      total_size += 1 + 4;
     }
     
-    // optional double pitch = 5;
+    // optional float pitch = 13;
     if (has_pitch()) {
-      total_size += 1 + 8;
+      total_size += 1 + 4;
     }
     
-    // optional double yaw = 6;
+    // optional float yaw = 14;
     if (has_yaw()) {
-      total_size += 1 + 8;
+      total_size += 1 + 4;
+    }
+    
+    // optional float lon = 15;
+    if (has_lon()) {
+      total_size += 1 + 4;
+    }
+    
+    // optional float lat = 16;
+    if (has_lat()) {
+      total_size += 2 + 4;
+    }
+    
+  }
+  if (_has_bits_[16 / 32] & (0xffu << (16 % 32))) {
+    // optional float alt = 17;
+    if (has_alt()) {
+      total_size += 2 + 4;
+    }
+    
+    // optional float ground_x = 18;
+    if (has_ground_x()) {
+      total_size += 2 + 4;
+    }
+    
+    // optional float ground_y = 19;
+    if (has_ground_y()) {
+      total_size += 2 + 4;
     }
     
+    // optional float ground_z = 20;
+    if (has_ground_z()) {
+      total_size += 2 + 4;
+    }
+    
+  }
+  // repeated float camera_matrix = 21;
+  {
+    int data_size = 0;
+    data_size = 4 * this->camera_matrix_size();
+    total_size += 2 * this->camera_matrix_size() + data_size;
   }
+  
   if (!unknown_fields().empty()) {
     total_size +=
       ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
@@ -4446,10 +4885,10 @@ int Waypoint::ByteSize() const {
   return total_size;
 }
 
-void Waypoint::MergeFrom(const ::google::protobuf::Message& from) {
+void RGBDImage::MergeFrom(const ::google::protobuf::Message& from) {
   GOOGLE_CHECK_NE(&from, this);
-  const Waypoint* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const Waypoint*>(
+  const RGBDImage* source =
+    ::google::protobuf::internal::dynamic_cast_if_available<const RGBDImage*>(
       &from);
   if (source == NULL) {
     ::google::protobuf::internal::ReflectionOps::Merge(from, this);
@@ -4458,17 +4897,44 @@ void Waypoint::MergeFrom(const ::google::protobuf::Message& from) {
   }
 }
 
-void Waypoint::MergeFrom(const Waypoint& from) {
+void RGBDImage::MergeFrom(const RGBDImage& from) {
   GOOGLE_CHECK_NE(&from, this);
+  camera_matrix_.MergeFrom(from.camera_matrix_);
   if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_x()) {
-      set_x(from.x());
+    if (from.has_header()) {
+      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
     }
-    if (from.has_y()) {
-      set_y(from.y());
+    if (from.has_cols()) {
+      set_cols(from.cols());
     }
-    if (from.has_z()) {
-      set_z(from.z());
+    if (from.has_rows()) {
+      set_rows(from.rows());
+    }
+    if (from.has_step1()) {
+      set_step1(from.step1());
+    }
+    if (from.has_type1()) {
+      set_type1(from.type1());
+    }
+    if (from.has_imagedata1()) {
+      set_imagedata1(from.imagedata1());
+    }
+    if (from.has_step2()) {
+      set_step2(from.step2());
+    }
+    if (from.has_type2()) {
+      set_type2(from.type2());
+    }
+  }
+  if (from._has_bits_[8 / 32] & (0xffu << (8 % 32))) {
+    if (from.has_imagedata2()) {
+      set_imagedata2(from.imagedata2());
+    }
+    if (from.has_camera_config()) {
+      set_camera_config(from.camera_config());
+    }
+    if (from.has_camera_type()) {
+      set_camera_type(from.camera_type());
     }
     if (from.has_roll()) {
       set_roll(from.roll());
@@ -4479,47 +4945,85 @@ void Waypoint::MergeFrom(const Waypoint& from) {
     if (from.has_yaw()) {
       set_yaw(from.yaw());
     }
+    if (from.has_lon()) {
+      set_lon(from.lon());
+    }
+    if (from.has_lat()) {
+      set_lat(from.lat());
+    }
+  }
+  if (from._has_bits_[16 / 32] & (0xffu << (16 % 32))) {
+    if (from.has_alt()) {
+      set_alt(from.alt());
+    }
+    if (from.has_ground_x()) {
+      set_ground_x(from.ground_x());
+    }
+    if (from.has_ground_y()) {
+      set_ground_y(from.ground_y());
+    }
+    if (from.has_ground_z()) {
+      set_ground_z(from.ground_z());
+    }
   }
   mutable_unknown_fields()->MergeFrom(from.unknown_fields());
 }
 
-void Waypoint::CopyFrom(const ::google::protobuf::Message& from) {
+void RGBDImage::CopyFrom(const ::google::protobuf::Message& from) {
   if (&from == this) return;
   Clear();
   MergeFrom(from);
 }
 
-void Waypoint::CopyFrom(const Waypoint& from) {
+void RGBDImage::CopyFrom(const RGBDImage& from) {
   if (&from == this) return;
   Clear();
   MergeFrom(from);
 }
 
-bool Waypoint::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000003) != 0x00000003) return false;
+bool RGBDImage::IsInitialized() const {
+  if ((_has_bits_[0] & 0x000001ff) != 0x000001ff) return false;
   
+  if (has_header()) {
+    if (!this->header().IsInitialized()) return false;
+  }
   return true;
 }
 
-void Waypoint::Swap(Waypoint* other) {
+void RGBDImage::Swap(RGBDImage* other) {
   if (other != this) {
-    std::swap(x_, other->x_);
-    std::swap(y_, other->y_);
-    std::swap(z_, other->z_);
+    std::swap(header_, other->header_);
+    std::swap(cols_, other->cols_);
+    std::swap(rows_, other->rows_);
+    std::swap(step1_, other->step1_);
+    std::swap(type1_, other->type1_);
+    std::swap(imagedata1_, other->imagedata1_);
+    std::swap(step2_, other->step2_);
+    std::swap(type2_, other->type2_);
+    std::swap(imagedata2_, other->imagedata2_);
+    std::swap(camera_config_, other->camera_config_);
+    std::swap(camera_type_, other->camera_type_);
     std::swap(roll_, other->roll_);
     std::swap(pitch_, other->pitch_);
     std::swap(yaw_, other->yaw_);
+    std::swap(lon_, other->lon_);
+    std::swap(lat_, other->lat_);
+    std::swap(alt_, other->alt_);
+    std::swap(ground_x_, other->ground_x_);
+    std::swap(ground_y_, other->ground_y_);
+    std::swap(ground_z_, other->ground_z_);
+    camera_matrix_.Swap(&other->camera_matrix_);
     std::swap(_has_bits_[0], other->_has_bits_[0]);
     _unknown_fields_.Swap(&other->_unknown_fields_);
     std::swap(_cached_size_, other->_cached_size_);
   }
 }
 
-::google::protobuf::Metadata Waypoint::GetMetadata() const {
+::google::protobuf::Metadata RGBDImage::GetMetadata() const {
   protobuf_AssignDescriptorsOnce();
   ::google::protobuf::Metadata metadata;
-  metadata.descriptor = Waypoint_descriptor_;
-  metadata.reflection = Waypoint_reflection_;
+  metadata.descriptor = RGBDImage_descriptor_;
+  metadata.reflection = RGBDImage_reflection_;
   return metadata;
 }
 
@@ -4527,102 +5031,178 @@ void Waypoint::Swap(Waypoint* other) {
 // ===================================================================
 
 #ifndef _MSC_VER
-const int Path::kHeaderFieldNumber;
-const int Path::kWaypointsFieldNumber;
+const int Waypoint::kXFieldNumber;
+const int Waypoint::kYFieldNumber;
+const int Waypoint::kZFieldNumber;
+const int Waypoint::kRollFieldNumber;
+const int Waypoint::kPitchFieldNumber;
+const int Waypoint::kYawFieldNumber;
 #endif  // !_MSC_VER
 
-Path::Path()
+Waypoint::Waypoint()
   : ::google::protobuf::Message() {
   SharedCtor();
 }
 
-void Path::InitAsDefaultInstance() {
-  header_ = const_cast< ::px::HeaderInfo*>(&::px::HeaderInfo::default_instance());
+void Waypoint::InitAsDefaultInstance() {
 }
 
-Path::Path(const Path& from)
+Waypoint::Waypoint(const Waypoint& from)
   : ::google::protobuf::Message() {
   SharedCtor();
   MergeFrom(from);
 }
 
-void Path::SharedCtor() {
+void Waypoint::SharedCtor() {
   _cached_size_ = 0;
-  header_ = NULL;
+  x_ = 0;
+  y_ = 0;
+  z_ = 0;
+  roll_ = 0;
+  pitch_ = 0;
+  yaw_ = 0;
   ::memset(_has_bits_, 0, sizeof(_has_bits_));
 }
 
-Path::~Path() {
+Waypoint::~Waypoint() {
   SharedDtor();
 }
 
-void Path::SharedDtor() {
+void Waypoint::SharedDtor() {
   if (this != default_instance_) {
-    delete header_;
   }
 }
 
-void Path::SetCachedSize(int size) const {
+void Waypoint::SetCachedSize(int size) const {
   GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
   _cached_size_ = size;
   GOOGLE_SAFE_CONCURRENT_WRITES_END();
 }
-const ::google::protobuf::Descriptor* Path::descriptor() {
+const ::google::protobuf::Descriptor* Waypoint::descriptor() {
   protobuf_AssignDescriptorsOnce();
-  return Path_descriptor_;
+  return Waypoint_descriptor_;
 }
 
-const Path& Path::default_instance() {
+const Waypoint& Waypoint::default_instance() {
   if (default_instance_ == NULL) protobuf_AddDesc_pixhawk_2eproto();  return *default_instance_;
 }
 
-Path* Path::default_instance_ = NULL;
+Waypoint* Waypoint::default_instance_ = NULL;
 
-Path* Path::New() const {
-  return new Path;
+Waypoint* Waypoint::New() const {
+  return new Waypoint;
 }
 
-void Path::Clear() {
+void Waypoint::Clear() {
   if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (has_header()) {
-      if (header_ != NULL) header_->::px::HeaderInfo::Clear();
-    }
+    x_ = 0;
+    y_ = 0;
+    z_ = 0;
+    roll_ = 0;
+    pitch_ = 0;
+    yaw_ = 0;
   }
-  waypoints_.Clear();
   ::memset(_has_bits_, 0, sizeof(_has_bits_));
   mutable_unknown_fields()->Clear();
 }
 
-bool Path::MergePartialFromCodedStream(
+bool Waypoint::MergePartialFromCodedStream(
     ::google::protobuf::io::CodedInputStream* input) {
 #define DO_(EXPRESSION) if (!(EXPRESSION)) return false
   ::google::protobuf::uint32 tag;
   while ((tag = input->ReadTag()) != 0) {
     switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
-      // required .px.HeaderInfo header = 1;
+      // required double x = 1;
       case 1: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-               input, mutable_header()));
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
+                 input, &x_)));
+          set_has_x();
         } else {
           goto handle_uninterpreted;
         }
-        if (input->ExpectTag(18)) goto parse_waypoints;
+        if (input->ExpectTag(17)) goto parse_y;
         break;
       }
       
-      // repeated .px.Waypoint waypoints = 2;
+      // required double y = 2;
       case 2: {
         if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
-            ::google::protobuf::internal::WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
-         parse_waypoints:
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-                input, add_waypoints()));
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
+         parse_y:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
+                 input, &y_)));
+          set_has_y();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(25)) goto parse_z;
+        break;
+      }
+      
+      // optional double z = 3;
+      case 3: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
+         parse_z:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
+                 input, &z_)));
+          set_has_z();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(33)) goto parse_roll;
+        break;
+      }
+      
+      // optional double roll = 4;
+      case 4: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
+         parse_roll:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
+                 input, &roll_)));
+          set_has_roll();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(41)) goto parse_pitch;
+        break;
+      }
+      
+      // optional double pitch = 5;
+      case 5: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
+         parse_pitch:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
+                 input, &pitch_)));
+          set_has_pitch();
+        } else {
+          goto handle_uninterpreted;
+        }
+        if (input->ExpectTag(49)) goto parse_yaw;
+        break;
+      }
+      
+      // optional double yaw = 6;
+      case 6: {
+        if (::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
+            ::google::protobuf::internal::WireFormatLite::WIRETYPE_FIXED64) {
+         parse_yaw:
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
+                 input, &yaw_)));
+          set_has_yaw();
         } else {
           goto handle_uninterpreted;
         }
-        if (input->ExpectTag(18)) goto parse_waypoints;
         if (input->ExpectAtEnd()) return true;
         break;
       }
@@ -4643,18 +5223,36 @@ bool Path::MergePartialFromCodedStream(
 #undef DO_
 }
 
-void Path::SerializeWithCachedSizes(
+void Waypoint::SerializeWithCachedSizes(
     ::google::protobuf::io::CodedOutputStream* output) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      1, this->header(), output);
+  // required double x = 1;
+  if (has_x()) {
+    ::google::protobuf::internal::WireFormatLite::WriteDouble(1, this->x(), output);
   }
   
-  // repeated .px.Waypoint waypoints = 2;
-  for (int i = 0; i < this->waypoints_size(); i++) {
-    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
-      2, this->waypoints(i), output);
+  // required double y = 2;
+  if (has_y()) {
+    ::google::protobuf::internal::WireFormatLite::WriteDouble(2, this->y(), output);
+  }
+  
+  // optional double z = 3;
+  if (has_z()) {
+    ::google::protobuf::internal::WireFormatLite::WriteDouble(3, this->z(), output);
+  }
+  
+  // optional double roll = 4;
+  if (has_roll()) {
+    ::google::protobuf::internal::WireFormatLite::WriteDouble(4, this->roll(), output);
+  }
+  
+  // optional double pitch = 5;
+  if (has_pitch()) {
+    ::google::protobuf::internal::WireFormatLite::WriteDouble(5, this->pitch(), output);
+  }
+  
+  // optional double yaw = 6;
+  if (has_yaw()) {
+    ::google::protobuf::internal::WireFormatLite::WriteDouble(6, this->yaw(), output);
   }
   
   if (!unknown_fields().empty()) {
@@ -4663,20 +5261,36 @@ void Path::SerializeWithCachedSizes(
   }
 }
 
-::google::protobuf::uint8* Path::SerializeWithCachedSizesToArray(
+::google::protobuf::uint8* Waypoint::SerializeWithCachedSizesToArray(
     ::google::protobuf::uint8* target) const {
-  // required .px.HeaderInfo header = 1;
-  if (has_header()) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        1, this->header(), target);
+  // required double x = 1;
+  if (has_x()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(1, this->x(), target);
   }
   
-  // repeated .px.Waypoint waypoints = 2;
-  for (int i = 0; i < this->waypoints_size(); i++) {
-    target = ::google::protobuf::internal::WireFormatLite::
-      WriteMessageNoVirtualToArray(
-        2, this->waypoints(i), target);
+  // required double y = 2;
+  if (has_y()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(2, this->y(), target);
+  }
+  
+  // optional double z = 3;
+  if (has_z()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(3, this->z(), target);
+  }
+  
+  // optional double roll = 4;
+  if (has_roll()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(4, this->roll(), target);
+  }
+  
+  // optional double pitch = 5;
+  if (has_pitch()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(5, this->pitch(), target);
+  }
+  
+  // optional double yaw = 6;
+  if (has_yaw()) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(6, this->yaw(), target);
   }
   
   if (!unknown_fields().empty()) {
@@ -4686,26 +5300,41 @@ void Path::SerializeWithCachedSizes(
   return target;
 }
 
-int Path::ByteSize() const {
+int Waypoint::ByteSize() const {
   int total_size = 0;
   
   if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    // required .px.HeaderInfo header = 1;
-    if (has_header()) {
-      total_size += 1 +
-        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-          this->header());
+    // required double x = 1;
+    if (has_x()) {
+      total_size += 1 + 8;
+    }
+    
+    // required double y = 2;
+    if (has_y()) {
+      total_size += 1 + 8;
+    }
+    
+    // optional double z = 3;
+    if (has_z()) {
+      total_size += 1 + 8;
+    }
+    
+    // optional double roll = 4;
+    if (has_roll()) {
+      total_size += 1 + 8;
+    }
+    
+    // optional double pitch = 5;
+    if (has_pitch()) {
+      total_size += 1 + 8;
+    }
+    
+    // optional double yaw = 6;
+    if (has_yaw()) {
+      total_size += 1 + 8;
     }
     
   }
-  // repeated .px.Waypoint waypoints = 2;
-  total_size += 1 * this->waypoints_size();
-  for (int i = 0; i < this->waypoints_size(); i++) {
-    total_size +=
-      ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
-        this->waypoints(i));
-  }
-  
   if (!unknown_fields().empty()) {
     total_size +=
       ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
@@ -4717,10 +5346,10 @@ int Path::ByteSize() const {
   return total_size;
 }
 
-void Path::MergeFrom(const ::google::protobuf::Message& from) {
+void Waypoint::MergeFrom(const ::google::protobuf::Message& from) {
   GOOGLE_CHECK_NE(&from, this);
-  const Path* source =
-    ::google::protobuf::internal::dynamic_cast_if_available<const Path*>(
+  const Waypoint* source =
+    ::google::protobuf::internal::dynamic_cast_if_available<const Waypoint*>(
       &from);
   if (source == NULL) {
     ::google::protobuf::internal::ReflectionOps::Merge(from, this);
@@ -4729,56 +5358,68 @@ void Path::MergeFrom(const ::google::protobuf::Message& from) {
   }
 }
 
-void Path::MergeFrom(const Path& from) {
+void Waypoint::MergeFrom(const Waypoint& from) {
   GOOGLE_CHECK_NE(&from, this);
-  waypoints_.MergeFrom(from.waypoints_);
   if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
-    if (from.has_header()) {
-      mutable_header()->::px::HeaderInfo::MergeFrom(from.header());
+    if (from.has_x()) {
+      set_x(from.x());
+    }
+    if (from.has_y()) {
+      set_y(from.y());
+    }
+    if (from.has_z()) {
+      set_z(from.z());
+    }
+    if (from.has_roll()) {
+      set_roll(from.roll());
+    }
+    if (from.has_pitch()) {
+      set_pitch(from.pitch());
+    }
+    if (from.has_yaw()) {
+      set_yaw(from.yaw());
     }
   }
   mutable_unknown_fields()->MergeFrom(from.unknown_fields());
 }
 
-void Path::CopyFrom(const ::google::protobuf::Message& from) {
+void Waypoint::CopyFrom(const ::google::protobuf::Message& from) {
   if (&from == this) return;
   Clear();
   MergeFrom(from);
 }
 
-void Path::CopyFrom(const Path& from) {
+void Waypoint::CopyFrom(const Waypoint& from) {
   if (&from == this) return;
   Clear();
   MergeFrom(from);
 }
 
-bool Path::IsInitialized() const {
-  if ((_has_bits_[0] & 0x00000001) != 0x00000001) return false;
+bool Waypoint::IsInitialized() const {
+  if ((_has_bits_[0] & 0x00000003) != 0x00000003) return false;
   
-  if (has_header()) {
-    if (!this->header().IsInitialized()) return false;
-  }
-  for (int i = 0; i < waypoints_size(); i++) {
-    if (!this->waypoints(i).IsInitialized()) return false;
-  }
   return true;
 }
 
-void Path::Swap(Path* other) {
+void Waypoint::Swap(Waypoint* other) {
   if (other != this) {
-    std::swap(header_, other->header_);
-    waypoints_.Swap(&other->waypoints_);
+    std::swap(x_, other->x_);
+    std::swap(y_, other->y_);
+    std::swap(z_, other->z_);
+    std::swap(roll_, other->roll_);
+    std::swap(pitch_, other->pitch_);
+    std::swap(yaw_, other->yaw_);
     std::swap(_has_bits_[0], other->_has_bits_[0]);
     _unknown_fields_.Swap(&other->_unknown_fields_);
     std::swap(_cached_size_, other->_cached_size_);
   }
 }
 
-::google::protobuf::Metadata Path::GetMetadata() const {
+::google::protobuf::Metadata Waypoint::GetMetadata() const {
   protobuf_AssignDescriptorsOnce();
   ::google::protobuf::Metadata metadata;
-  metadata.descriptor = Path_descriptor_;
-  metadata.reflection = Path_reflection_;
+  metadata.descriptor = Waypoint_descriptor_;
+  metadata.reflection = Waypoint_reflection_;
   return metadata;
 }