Added multi-UAS support to QGCRGBDView. Fixed display timeout issue on...

Added multi-UAS support to QGCRGBDView. Fixed display timeout issue on computer for which data source is not time-synchronized.

src/uas/UAS.cc

@@ -75,6 +75,12 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
     pitch(0.0),
     yaw(0.0),
     statusTimeout(new QTimer(this)),
+#ifdef QGC_PROTOBUF_ENABLED
+    receivedPointCloudTimestamp(0.0),
+    receivedRGBDImageTimestamp(0.0),
+    receivedObstacleListTimestamp(0.0),
+    receivedPathTimestamp(0.0),
+#endif
     paramsOnceRequested(false),
     airframe(QGC_AIRFRAME_EASYSTAR),
     attitudeKnown(false),
@@ -1024,21 +1030,25 @@ void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<googl
 
     if (message->GetTypeName() == pointCloud.GetTypeName())
     {
+        receivedPointCloudTimestamp = QGC::groundTimeSeconds();
         pointCloud.CopyFrom(*message);
         emit pointCloudChanged(this);
     }
     else if (message->GetTypeName() == rgbdImage.GetTypeName())
     {
+        receivedRGBDImageTimestamp = QGC::groundTimeSeconds();
         rgbdImage.CopyFrom(*message);
         emit rgbdImageChanged(this);
     }
     else if (message->GetTypeName() == obstacleList.GetTypeName())
     {
+        receivedObstacleListTimestamp = QGC::groundTimeSeconds();
         obstacleList.CopyFrom(*message);
         emit obstacleListChanged(this);
     }
     else if (message->GetTypeName() == path.GetTypeName())
     {
+        receivedPathTimestamp = QGC::groundTimeSeconds();
         path.CopyFrom(*message);
         emit pathChanged(this);
     }

src/uas/UAS.h

@@ -139,17 +139,37 @@ public:
         return pointCloud;
     }
 
+    px::PointCloudXYZRGB getPointCloud(qreal& receivedTimestamp) const {
+        receivedTimestamp = receivedPointCloudTimestamp;
+        return pointCloud;
+    }
+
     px::RGBDImage getRGBDImage() const {
         return rgbdImage;
     }
 
+    px::RGBDImage getRGBDImage(qreal& receivedTimestamp) const {
+        receivedTimestamp = receivedRGBDImageTimestamp;
+        return rgbdImage;
+    }
+
     px::ObstacleList getObstacleList() const {
         return obstacleList;
     }
 
+    px::ObstacleList getObstacleList(qreal& receivedTimestamp) const {
+        receivedTimestamp = receivedObstacleListTimestamp;
+        return obstacleList;
+    }
+
     px::Path getPath() const {
         return path;
     }
+
+    px::Path getPath(qreal& receivedTimestamp) const {
+        receivedTimestamp = receivedPathTimestamp;
+        return path;
+    }
 #endif
 
     friend class UASWaypointManager;
@@ -237,9 +257,16 @@ protected: //COMMENTS FOR TEST UNIT
 
 #ifdef QGC_PROTOBUF_ENABLED
     px::PointCloudXYZRGB pointCloud;
+    qreal receivedPointCloudTimestamp;
+
     px::RGBDImage rgbdImage;
+    qreal receivedRGBDImageTimestamp;
+
     px::ObstacleList obstacleList;
+    qreal receivedObstacleListTimestamp;
+
     px::Path path;
+    qreal receivedPathTimestamp;
 #endif
 
     QMap<int, QMap<QString, QVariant>* > parameters; ///< All parameters

src/uas/UASInterface.h

@@ -96,9 +96,13 @@ public:
 
 #ifdef QGC_PROTOBUF_ENABLED
     virtual px::PointCloudXYZRGB getPointCloud() const = 0;
+    virtual px::PointCloudXYZRGB getPointCloud(qreal& receivedTimestamp) const = 0;
     virtual px::RGBDImage getRGBDImage() const = 0;
+    virtual px::RGBDImage getRGBDImage(qreal& receivedTimestamp) const = 0;
     virtual px::ObstacleList getObstacleList() const = 0;
+    virtual px::ObstacleList getObstacleList(qreal& receivedTimestamp) const = 0;
     virtual px::Path getPath() const = 0;
+    virtual px::Path getPath(qreal& receivedTimestamp) const = 0;
 #endif
 
     virtual bool isArmed() const = 0;

src/ui/HUD.h

@@ -61,7 +61,7 @@ public slots:
     //void paintGL();
 
     /** @brief Set the currently monitored UAS */
-    void setActiveUAS(UASInterface* uas);
+    virtual void setActiveUAS(UASInterface* uas);
 
     void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp);
 //    void updateAttitudeThrustSetPoint(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec);

src/ui/QGCRGBDView.cc

@@ -21,16 +21,38 @@ QGCRGBDView::QGCRGBDView(int width, int height, QWidget *parent) :
     enableDepthAction->setChecked(depthEnabled);
     connect(enableDepthAction, SIGNAL(triggered(bool)), this, SLOT(enableDepth(bool)));
 
-    connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*)));
+    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
 }
 
-void QGCRGBDView::addUAS(UASInterface *uas)
+void QGCRGBDView::setActiveUAS(UASInterface* uas)
 {
-    // TODO Enable multi-uas support
-    connect(uas, SIGNAL(rgbdImageChanged(UASInterface*)), this, SLOT(updateData(UASInterface*)));
+    if (this->uas != NULL)
+    {
+        // Disconnect any previously connected active MAV
+        disconnect(this->uas, SIGNAL(rgbdImageChanged(UASInterface*)), this, SLOT(updateData(UASInterface*)));
+
+        clearData();
+    }
+
+    if (uas)
+    {
+        // Now connect the new UAS
+        // Setup communication
+        connect(uas, SIGNAL(rgbdImageChanged(UASInterface*)), this, SLOT(updateData(UASInterface*)));
+    }
+
+    HUD::setActiveUAS(uas);
+}
+
+void QGCRGBDView::clearData(void)
+{
+    QImage emptyImg(16, 16, QImage::Format_Mono);
+    emptyImg.fill(0);
+
+    glImage = QGLWidget::convertToGLFormat(emptyImg);
 }
 
-void QGCRGBDView::contextMenuEvent (QContextMenuEvent* event)
+void QGCRGBDView::contextMenuEvent(QContextMenuEvent* event)
 {
     QMenu menu(this);
     // Update actions
@@ -248,7 +270,7 @@ void QGCRGBDView::updateData(UASInterface *uas)
             {
                 if (depth[c] != 0)
                 {
-                    int idx = fminf(depth[c], 7.0f) / 7.0f * 127.0f;
+                    int idx = fminf(depth[c], 10.0f) / 10.0f * 127.0f;
                     idx = 127 - idx;
 
                     pixel[0] = colormapJet[idx][2] * 255.0f;

src/ui/QGCRGBDView.h

@@ -12,7 +12,9 @@ public:
 signals:
 
 public slots:
-    void addUAS(UASInterface* uas);
+    void setActiveUAS(UASInterface* uas);
+
+    void clearData(void);
     void enableRGB(bool enabled);
     void enableDepth(bool enabled);
     void updateData(UASInterface *uas);

src/ui/map3D/ObstacleGroupNode.cc

@@ -57,23 +57,13 @@ ObstacleGroupNode::clear(void)
 }
 
 void
-ObstacleGroupNode::update(MAV_FRAME frame, UASInterface *uas)
+ObstacleGroupNode::update(double robotX, double robotY, double robotZ,
+                          const px::ObstacleList& obstacleList)
 {
-    if (!uas || frame == MAV_FRAME_GLOBAL)
-    {
-        return;
-    }
-
-    double robotX = uas->getLocalX();
-    double robotY = uas->getLocalY();
-    double robotZ = uas->getLocalZ();
-
     clear();
 
     osg::ref_ptr<osg::Geode> geode = new osg::Geode;
 
-    px::ObstacleList obstacleList = uas->getObstacleList();
-
     for (int i = 0; i < obstacleList.obstacles_size(); ++i)
     {
         const px::Obstacle& obs = obstacleList.obstacles(i);

src/ui/map3D/ObstacleGroupNode.h

@@ -43,7 +43,8 @@ public:
 
     void init(void);
     void clear(void);
-    void update(MAV_FRAME frame, UASInterface* uas);
+    void update(double robotX, double robotY, double robotZ,
+                const px::ObstacleList& obstacleList);
 };
 
 #endif // OBSTACLEGROUPNODE_H

src/ui/map3D/Pixhawk3DWidget.cc

@@ -53,7 +53,7 @@
 Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     : Q3DWidget(parent)
     , uas(NULL)
-    , kMessageTimeout(2.0)
+    , kMessageTimeout(4.0)
     , mode(DEFAULT_MODE)
     , selectedWpIndex(-1)
     , displayLocalGrid(false)
@@ -791,7 +791,7 @@ Pixhawk3DWidget::display(void)
 
     if (displayObstacleList)
     {
-        updateObstacles();
+        updateObstacles(robotX, robotY, robotZ);
     }
 
     if (displayPath)
@@ -1534,10 +1534,12 @@ Pixhawk3DWidget::updateTarget(double robotX, double robotY, double robotZ)
 void
 Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
 {
-    px::RGBDImage rgbdImage = uas->getRGBDImage();
-    px::PointCloudXYZRGB pointCloud = uas->getPointCloud();
+    qreal receivedRGBDImageTimestamp, receivedPointCloudTimestamp;
+    px::RGBDImage rgbdImage = uas->getRGBDImage(receivedRGBDImageTimestamp);
+    px::PointCloudXYZRGB pointCloud = uas->getPointCloud(receivedPointCloudTimestamp);
 
-    if (rgbdImage.rows() > 0 && rgbdImage.cols() > 0)
+    if (rgbdImage.rows() > 0 && rgbdImage.cols() > 0 &&
+        QGC::groundTimeSeconds() - receivedRGBDImageTimestamp < kMessageTimeout)
     {
         rgbImage->setImage(rgbdImage.cols(), rgbdImage.rows(), 1,
                            GL_LUMINANCE, GL_LUMINANCE, GL_UNSIGNED_BYTE,
@@ -1576,9 +1578,15 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
     }
 
     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());
+
+    if (QGC::groundTimeSeconds() - receivedPointCloudTimestamp > kMessageTimeout)
+    {
+        geometry->removePrimitiveSet(0, geometry->getNumPrimitiveSets());
+        return;
+    }
+
     for (int i = 0; i < pointCloud.points_size(); ++i)
     {
         const px::PointCloudXYZRGB_PointXYZRGB& p = pointCloud.points(i);
@@ -1625,11 +1633,20 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
 }
 
 void
-Pixhawk3DWidget::updateObstacles(void)
+Pixhawk3DWidget::updateObstacles(double robotX, double robotY, double robotZ)
 {
-    if (QGC::groundTimeSeconds() - uas->getObstacleList().header().timestamp() < kMessageTimeout)
+    if (frame == MAV_FRAME_GLOBAL)
+    {
+        obstacleGroupNode->clear();
+        return;
+    }
+
+    qreal receivedTimestamp;
+    px::ObstacleList obstacleList = uas->getObstacleList(receivedTimestamp);
+
+    if (QGC::groundTimeSeconds() - receivedTimestamp < kMessageTimeout)
     {
-        obstacleGroupNode->update(frame, uas);
+        obstacleGroupNode->update(robotX, robotY, robotZ, obstacleList);
     }
     else
     {
@@ -1640,7 +1657,8 @@ Pixhawk3DWidget::updateObstacles(void)
 void
 Pixhawk3DWidget::updatePath(double robotX, double robotY, double robotZ)
 {
-    px::Path path = uas->getPath();
+    qreal receivedTimestamp;
+    px::Path path = uas->getPath(receivedTimestamp);
 
     osg::Geometry* geometry = pathNode->getDrawable(0)->asGeometry();
     osg::DrawArrays* drawArrays = reinterpret_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
@@ -1655,7 +1673,7 @@ Pixhawk3DWidget::updatePath(double robotX, double robotY, double robotZ)
 
     osg::ref_ptr<osg::Vec3Array> vertices(new osg::Vec3Array);
 
-    if (QGC::groundTimeSeconds() - path.header().timestamp() < kMessageTimeout)
+    if (QGC::groundTimeSeconds() - receivedTimestamp < kMessageTimeout)
     {
         // find path length
         float length = 0.0f;

src/ui/map3D/Pixhawk3DWidget.h

@@ -127,7 +127,7 @@ private:
     void updateTarget(double robotX, double robotY, double robotZ);
 #ifdef QGC_PROTOBUF_ENABLED
     void updateRGBD(double robotX, double robotY, double robotZ);
-    void updateObstacles(void);
+    void updateObstacles(double robotX, double robotY, double robotZ);
     void updatePath(double robotX, double robotY, double robotZ);
 #endif