Enabled visualization of point cloud data via Protocol Buffers serialization...

Enabled visualization of point cloud data via Protocol Buffers serialization and extended_mavlink_message.

src/comm/MAVLinkProtocol.cc

@@ -31,10 +31,6 @@
 #include "QGCMAVLinkUASFactory.h"
 #include "QGC.h"
 
-#ifdef QGC_PROTOBUF_ENABLED
-#include "mavlink_protobuf_manager.hpp"
-#endif
-
 /**
  * The default constructor will create a new MAVLink object sending heartbeats at
  * the MAVLINK_HEARTBEAT_DEFAULT_RATE to all connected links.
@@ -189,7 +185,28 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
 #ifdef QGC_PROTOBUF_ENABLED
             if (message.msgid == MAVLINK_MSG_ID_EXTENDED_MESSAGE)
             {
-                mavlink::ProtobufManager::instance();
+                mavlink_extended_message_t extended_message;
+
+                extended_message.base_msg = message;
+
+                // read extended header
+                uint8_t* payload = reinterpret_cast<uint8_t*>(message.payload64);
+                memcpy(&extended_message.extended_payload_len, payload + 3, 4);
+
+                const uint8_t* extended_payload = reinterpret_cast<const uint8_t*>(b.constData()) + MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_EXTENDED_HEADER_LEN;
+
+                // copy extended payload data
+                memcpy(extended_message.extended_payload, extended_payload, extended_message.extended_payload_len);
+
+                if (protobufManager.cacheFragment(extended_message))
+                {
+                    std::tr1::shared_ptr<google::protobuf::Message> protobuf_msg;
+
+                    if (protobufManager.getMessage(protobuf_msg))
+                    {
+                        emit extendedMessageReceived(link, protobuf_msg);
+                    }
+                }
             }
 #endif
 

src/comm/MAVLinkProtocol.h

@@ -42,6 +42,11 @@ This file is part of the QGROUNDCONTROL project
 #include "QGCMAVLink.h"
 #include "QGC.h"
 
+#ifdef QGC_PROTOBUF_ENABLED
+#include <mavlink_protobuf_manager.hpp>
+#endif
+
+
 /**
  * @brief MAVLink micro air vehicle protocol reference implementation.
  *
@@ -197,10 +202,17 @@ protected:
     int currLossCounter;
     bool versionMismatchIgnore;
     int systemId;
+#ifdef QGC_PROTOBUF_ENABLED
+    mavlink::ProtobufManager protobufManager;
+#endif
 
 signals:
     /** @brief Message received and directly copied via signal */
     void messageReceived(LinkInterface* link, mavlink_message_t message);
+#ifdef QGC_PROTOBUF_ENABLED
+    /** @brief Message received via signal */
+    void extendedMessageReceived(LinkInterface *link, std::tr1::shared_ptr<google::protobuf::Message> message);
+#endif
     /** @brief Emitted if heartbeat emission mode is changed */
     void heartbeatChanged(bool heartbeats);
     /** @brief Emitted if logging is started / stopped */

src/uas/PxQuadMAV.cc

@@ -206,6 +206,14 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
 #endif
 }
 
+#ifdef QGC_PROTOBUF_ENABLED
+void PxQuadMAV::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<google::protobuf::Message> message)
+{
+    UAS::receiveExtendedMessage(link, message);
+}
+
+#endif
+
 void PxQuadMAV::sendProcessCommand(int watchdogId, int processId, unsigned int command)
 {
 #ifdef MAVLINK_ENABLED_PIXHAWK

src/uas/PxQuadMAV.h

@@ -35,6 +35,10 @@ public:
 public slots:
     /** @brief Receive a MAVLink message from this MAV */
     void receiveMessage(LinkInterface* link, mavlink_message_t message);
+#ifdef QGC_PROTOBUF_ENABLED
+    /** @brief Receive a Protobuf message from this MAV */
+    void receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<google::protobuf::Message> message);
+#endif
     /** @brief Send a command to an onboard process */
     void sendProcessCommand(int watchdogId, int processId, unsigned int command);
 signals:

src/uas/QGCMAVLinkUASFactory.cc

@@ -30,6 +30,9 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
         mav->setSystemType((int)heartbeat->type);
         // Connect this robot to the UAS object
         connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
+#ifdef QGC_PROTOBUF_ENABLED
+        connect(mavlink, SIGNAL(extendedMessageReceived(LinkInterface*, std::tr1::shared_ptr<google::protobuf::Message>)), mav, SLOT(receiveExtendedMessage(LinkInterface*, std::tr1::shared_ptr<google::protobuf::Message>)));
+#endif
         uas = mav;
     }
     break;
@@ -43,6 +46,9 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
         // else the slot of the parent object is called (and thus the special
         // packets never reach their goal)
         connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
+#ifdef QGC_PROTOBUF_ENABLED
+        connect(mavlink, SIGNAL(extendedMessageReceived(LinkInterface*, std::tr1::shared_ptr<google::protobuf::Message>)), mav, SLOT(receiveExtendedMessage(LinkInterface*, std::tr1::shared_ptr<google::protobuf::Message>)));
+#endif
         uas = mav;
     }
     break;

src/uas/UAS.cc

@@ -970,6 +970,23 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
     }
 }
 
+#ifdef QGC_PROTOBUF_ENABLED
+void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<google::protobuf::Message> message)
+{
+    if (!link) return;
+    if (!links->contains(link))
+    {
+        addLink(link);
+    }
+
+    if (message->GetTypeName() == pointCloud.GetTypeName())
+    {
+        pointCloud.CopyFrom(*message);
+    }
+}
+
+#endif
+
 void UAS::setHomePosition(double lat, double lon, double alt)
 {
     QMessageBox msgBox;

src/uas/UAS.h

@@ -128,6 +128,12 @@ public:
     }
     bool getSelected() const;
 
+#ifdef QGC_PROTOBUF_ENABLED
+    px::PointCloudXYZRGB getPointCloud() const {
+        return pointCloud;
+    }
+#endif
+
     friend class UASWaypointManager;
 
 protected: //COMMENTS FOR TEST UNIT
@@ -208,6 +214,10 @@ protected: //COMMENTS FOR TEST UNIT
     QImage image;               ///< Image data of last completely transmitted image
     quint64 imageStart;
 
+#ifdef QGC_PROTOBUF_ENABLED
+    px::PointCloudXYZRGB pointCloud;
+#endif
+
     QMap<int, QMap<QString, QVariant>* > parameters; ///< All parameters
     bool paramsOnceRequested;       ///< If the parameter list has been read at least once
     int airframe;                   ///< The airframe type
@@ -451,6 +461,11 @@ public slots:
     /** @brief Receive a message from one of the communication links. */
     virtual void receiveMessage(LinkInterface* link, mavlink_message_t message);
 
+#ifdef QGC_PROTOBUF_ENABLED
+    /** @brief Receive a message from one of the communication links. */
+    virtual void receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<google::protobuf::Message> message);
+#endif
+
     /** @brief Send a message over this link (to this or to all UAS on this link) */
     void sendMessage(LinkInterface* link, mavlink_message_t message);
     /** @brief Send a message over all links this UAS can be reached with (!= all links) */

src/uas/UASInterface.h

@@ -44,6 +44,11 @@ This file is part of the QGROUNDCONTROL project
 #include "QGCUASParamManager.h"
 #include "RadioCalibration/RadioCalibrationData.h"
 
+#ifdef QGC_PROTOBUF_ENABLED
+#include <tr1/memory>
+#include <pixhawk.pb.h>
+#endif
+
 /**
  * @brief Interface for all robots.
  *
@@ -89,6 +94,10 @@ public:
 
     virtual bool getSelected() const = 0;
 
+#ifdef QGC_PROTOBUF_ENABLED
+    virtual px::PointCloudXYZRGB getPointCloud() const = 0;
+#endif
+
     virtual bool isArmed() const = 0;
 
     /** @brief Set the airframe of this MAV */

src/ui/map3D/Pixhawk3DWidget.cc

@@ -44,6 +44,11 @@
 
 #include "QGC.h"
 
+#ifdef QGC_PROTOBUF_ENABLED
+#include <tr1/memory>
+#include <pixhawk.pb.h>
+#endif
+
 Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     : Q3DWidget(parent)
     , uas(NULL)
@@ -58,7 +63,6 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     , enableRGBDColor(true)
     , enableTarget(false)
     , followCamera(true)
-    , enableFreenect(false)
     , frame(MAV_FRAME_GLOBAL)
     , lastRobotX(0.0f)
     , lastRobotY(0.0f)
@@ -92,16 +96,9 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     targetNode = createTarget();
     rollingMap->addChild(targetNode);
 
-#ifdef QGC_LIBFREENECT_ENABLED
-    freenect.reset(new Freenect());
-    enableFreenect = freenect->init();
-#endif
-
     // generate RGBD model
-    if (enableFreenect) {
-        rgbd3DNode = createRGBD3D();
-        egocentricMap->addChild(rgbd3DNode);
-    }
+    rgbd3DNode = createRGBD3D();
+    egocentricMap->addChild(rgbd3DNode);
 
     setupHUD();
 
@@ -123,7 +120,8 @@ Pixhawk3DWidget::~Pixhawk3DWidget()
  *
  * @param uas the UAS/MAV to monitor/display with the HUD
  */
-void Pixhawk3DWidget::setActiveUAS(UASInterface* uas)
+void
+Pixhawk3DWidget::setActiveUAS(UASInterface* uas)
 {
     if (this->uas != NULL && this->uas != uas) {
         // Disconnect any previously connected active MAV
@@ -555,11 +553,12 @@ Pixhawk3DWidget::display(void)
         updateTarget(robotX, robotY);
     }
 
-#ifdef QGC_LIBFREENECT_ENABLED
-    if (enableFreenect && (displayRGBD2D || displayRGBD3D)) {
-        updateRGBD();
+#ifdef QGC_PROTOBUF_ENABLED
+    if (displayRGBD2D || displayRGBD3D) {
+        updateRGBD(robotX, robotY, robotZ);
     }
 #endif
+
     updateHUD(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw, utmZone);
 
     // set node visibility
@@ -569,9 +568,7 @@ Pixhawk3DWidget::display(void)
     rollingMap->setChildValue(mapNode, displayImagery);
     rollingMap->setChildValue(waypointGroupNode, displayWaypoints);
     rollingMap->setChildValue(targetNode, enableTarget);
-    if (enableFreenect) {
-        egocentricMap->setChildValue(rgbd3DNode, displayRGBD3D);
-    }
+    egocentricMap->setChildValue(rgbd3DNode, displayRGBD3D);
     hudGroup->setChildValue(rgb2DGeode, displayRGBD2D);
     hudGroup->setChildValue(depth2DGeode, displayRGBD2D);
 
@@ -788,7 +785,7 @@ Pixhawk3DWidget::createMap(void)
 osg::ref_ptr<osg::Geode>
 Pixhawk3DWidget::createRGBD3D(void)
 {
-    int frameSize = 640 * 480;
+    int frameSize = 752 * 480;
 
     osg::ref_ptr<osg::Geode> geode(new osg::Geode);
     osg::ref_ptr<osg::Geometry> geometry(new osg::Geometry);
@@ -1218,46 +1215,51 @@ float colormap_jet[128][3] = {
     {0.5f,0.0f,0.0f}
 };
 
-#ifdef QGC_LIBFREENECT_ENABLED
+#ifdef QGC_PROTOBUF_ENABLED
 void
-Pixhawk3DWidget::updateRGBD(void)
+Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
 {
-    QSharedPointer<QByteArray> rgb = freenect->getRgbData();
-    if (!rgb.isNull()) {
-        rgbImage->setImage(640, 480, 1,
-                           GL_RGB, GL_RGB, GL_UNSIGNED_BYTE,
-                           reinterpret_cast<unsigned char *>(rgb->data()),
-                           osg::Image::NO_DELETE);
-        rgbImage->dirty();
-    }
-
-    QSharedPointer<QByteArray> coloredDepth = freenect->getColoredDepthData();
-    if (!coloredDepth.isNull()) {
-        depthImage->setImage(640, 480, 1,
-                             GL_RGB, GL_RGB, GL_UNSIGNED_BYTE,
-                             reinterpret_cast<unsigned char *>(coloredDepth->data()),
-                             osg::Image::NO_DELETE);
-        depthImage->dirty();
-    }
+    px::PointCloudXYZRGB pointCloud = uas->getPointCloud();
+
+//    QSharedPointer<QByteArray> rgb = freenect->getRgbData();
+//    if (!rgb.isNull()) {
+//        rgbImage->setImage(640, 480, 1,
+//                           GL_RGB, GL_RGB, GL_UNSIGNED_BYTE,
+//                           reinterpret_cast<unsigned char *>(rgb->data()),
+//                           osg::Image::NO_DELETE);
+//        rgbImage->dirty();
+//    }
 
-    QSharedPointer< QVector<Freenect::Vector6D> > pointCloud =
-        freenect->get6DPointCloudData();
+//    QSharedPointer<QByteArray> coloredDepth = freenect->getColoredDepthData();
+//    if (!coloredDepth.isNull()) {
+//        depthImage->setImage(640, 480, 1,
+//                             GL_RGB, GL_RGB, GL_UNSIGNED_BYTE,
+//                             reinterpret_cast<unsigned char *>(coloredDepth->data()),
+//                             osg::Image::NO_DELETE);
+//        depthImage->dirty();
+//    }
 
     osg::Geometry* geometry = rgbd3DNode->getDrawable(0)->asGeometry();
 
     osg::Vec3Array* vertices = static_cast<osg::Vec3Array*>(geometry->getVertexArray());
     osg::Vec4Array* colors = static_cast<osg::Vec4Array*>(geometry->getColorArray());
-    for (int i = 0; i < pointCloud->size(); ++i) {
-        double x = pointCloud->at(i).x;
-        double y = pointCloud->at(i).y;
-        double z = pointCloud->at(i).z;
-        (*vertices)[i].set(x, z, -y);
+    for (int i = 0; i < pointCloud.points_size(); ++i) {
+        const px::PointCloudXYZRGB_PointXYZRGB& p = pointCloud.points(i);
+
+        double x = p.x() - robotX;
+        double y = p.y() - robotY;
+        double z = p.z() - robotZ;
+
+        (*vertices)[i].set(y, x, -z);
 
         if (enableRGBDColor) {
-            (*colors)[i].set(pointCloud->at(i).r / 255.0f,
-                             pointCloud->at(i).g / 255.0f,
-                             pointCloud->at(i).b / 255.0f,
-                             1.0f);
+            float rgb = p.rgb();
+
+            float b = *(reinterpret_cast<unsigned char*>(&rgb)) / 255.0f;
+            float g = *(1 + reinterpret_cast<unsigned char*>(&rgb)) / 255.0f;
+            float r = *(2 + reinterpret_cast<unsigned char*>(&rgb)) / 255.0f;
+
+            (*colors)[i].set(r, g, b, 1.0f);
         } else {
             double dist = sqrt(x * x + y * y + z * z);
             int colorIndex = static_cast<int>(fmin(dist / 7.0 * 127.0, 127.0));
@@ -1270,10 +1272,10 @@ Pixhawk3DWidget::updateRGBD(void)
 
     if (geometry->getNumPrimitiveSets() == 0) {
         geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS,
-                                  0, pointCloud->size()));
+                                  0, pointCloud.points_size()));
     } else {
         osg::DrawArrays* drawarrays = static_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
-        drawarrays->setCount(pointCloud->size());
+        drawarrays->setCount(pointCloud.points_size());
     }
 }
 #endif

src/ui/map3D/Pixhawk3DWidget.h

@@ -39,10 +39,6 @@
 #include "ImageWindowGeode.h"
 #include "WaypointGroupNode.h"
 
-#ifdef QGC_LIBFREENECT_ENABLED
-#include "Freenect.h"
-#endif
-
 #include "Q3DWidget.h"
 
 class UASInterface;
@@ -115,8 +111,8 @@ private:
                        const QString& zone);
     void updateWaypoints(void);
     void updateTarget(double robotX, double robotY);
-#ifdef QGC_LIBFREENECT_ENABLED
-    void updateRGBD(void);
+#ifdef QGC_PROTOBUF_ENABLED
+    void updateRGBD(double robotX, double robotY, double robotZ);
 #endif
 
     int findWaypoint(int mouseX, int mouseY);
@@ -161,10 +157,6 @@ private:
     osg::ref_ptr<WaypointGroupNode> waypointGroupNode;
     osg::ref_ptr<osg::Node> targetNode;
     osg::ref_ptr<osg::Geode> rgbd3DNode;
-#ifdef QGC_LIBFREENECT_ENABLED
-    QScopedPointer<Freenect> freenect;
-#endif
-    bool enableFreenect;
 
     QVector< osg::ref_ptr<osg::Node> > vehicleModels;