Working on multi-component, multi-path visualization

src/uas/UAS.cc

@@ -240,9 +240,21 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
         bool multiComponentSourceDetected = false;
         bool wrongComponent = false;
 
+        switch (message.compid)
+        {
+        case MAV_COMP_ID_IMU_2:
+            // Prefer IMU 2 over IMU 1 (FIXME)
+            componentID[message.msgid] = MAV_COMP_ID_IMU_2;
+            break;
+        default:
+            // Do nothing
+            break;
+        }
+
         // Store component ID
         if (componentID[message.msgid] == -1)
         {
+            // Prefer the first component
             componentID[message.msgid] = message.compid;
         }
         else
@@ -488,19 +500,38 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 mavlink_local_position_ned_t pos;
                 mavlink_msg_local_position_ned_decode(&message, &pos);
                 quint64 time = getUnixTime(pos.time_boot_ms);
-                localX = pos.x;
-                localY = pos.y;
-                localZ = pos.z;
-                emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
-                emit speedChanged(this, pos.vx, pos.vy, pos.vz, time);
 
-                // Set internal state
-                if (!positionLock) {
-                    // If position was not locked before, notify positive
-                    GAudioOutput::instance()->notifyPositive();
+                // Emit position always with component ID
+                emit localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time);
+
+                if (!wrongComponent)
+                {
+
+                    localX = pos.x;
+                    localY = pos.y;
+                    localZ = pos.z;
+
+                    // Emit
+
+                    emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
+                    emit speedChanged(this, pos.vx, pos.vy, pos.vz, time);
+
+                    // Set internal state
+                    if (!positionLock) {
+                        // If position was not locked before, notify positive
+                        GAudioOutput::instance()->notifyPositive();
+                    }
+                    positionLock = true;
+                    isLocalPositionKnown = true;
                 }
-                positionLock = true;
-                isLocalPositionKnown = true;
+            }
+            break;
+        case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE:
+            {
+                mavlink_global_vision_position_estimate_t pos;
+                mavlink_msg_global_vision_position_estimate_decode(&message, &pos);
+                quint64 time = getUnixTime(pos.usec);
+                emit localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time);
             }
             break;
         case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
@@ -785,6 +816,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit positionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw, QGC::groundTimeUsecs());
             }
             break;
+        case MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT:
+            {
+                mavlink_set_local_position_setpoint_t p;
+                mavlink_msg_set_local_position_setpoint_decode(&message, &p);
+                emit userPositionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw);
+            }
+            break;
         case MAVLINK_MSG_ID_STATUSTEXT:
             {
                 QByteArray b;
@@ -954,7 +992,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
 
 #endif
             // Messages to ignore
-        case MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT:
         case MAVLINK_MSG_ID_RAW_IMU:
         case MAVLINK_MSG_ID_SCALED_IMU:
         case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:

src/uas/UASInterface.h

@@ -449,8 +449,12 @@ signals:
     void attitudeChanged(UASInterface*, double roll, double pitch, double yaw, quint64 usec);
     void attitudeSpeedChanged(int uas, double rollspeed, double pitchspeed, double yawspeed, quint64 usec);
     void attitudeThrustSetPointChanged(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec);
+    /** @brief The MAV set a new setpoint in the local (not body) NED X, Y, Z frame */
     void positionSetPointsChanged(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec);
+    /** @brief A user (or an autonomous mission or obstacle avoidance planner) requested to set a new setpoint */
+    void userPositionSetPointsChanged(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired);
     void localPositionChanged(UASInterface*, double x, double y, double z, quint64 usec);
+    void localPositionChanged(UASInterface*, int component, double x, double y, double z, quint64 usec);
     void globalPositionChanged(UASInterface*, double lat, double lon, double alt, quint64 usec);
     void altitudeChanged(int uasid, double altitude);
     /** @brief Update the status of one satellite used for localization */

src/ui/HSIDisplay.cc

@@ -668,6 +668,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
         disconnect(this->uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
         disconnect(this->uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64)));
         disconnect(this->uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64)));
+        disconnect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), this, SLOT(updateUserPositionSetpoints(int,float,float,float,float)));
         disconnect(this->uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
         disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
 
@@ -688,6 +689,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
     connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
     connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64)));
     connect(uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64)));
+    connect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), this, SLOT(updateUserPositionSetpoints(int,float,float,float,float)));
     connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
     connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
 
@@ -811,6 +813,16 @@ void HSIDisplay::updateAttitude(UASInterface* uas, double roll, double pitch, do
     this->yaw = yaw;
 }
 
+void HSIDisplay::updateUserPositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired)
+{
+    uiXSetCoordinate = xDesired;
+    uiYSetCoordinate = yDesired;
+    uiZSetCoordinate = zDesired;
+    uiYawSet = yawDesired;
+    userXYSetPointSet = true;
+    userSetPointSet = true;
+}
+
 void HSIDisplay::updatePositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec)
 {
     Q_UNUSED(usec);

src/ui/HSIDisplay.h

@@ -57,6 +57,7 @@ public slots:
     void updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used);
     void updateAttitudeSetpoints(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec);
     void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 time);
+    void updateUserPositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired);
     void updatePositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec);
     void updateLocalPosition(UASInterface*, double x, double y, double z, quint64 usec);
     void updateGlobalPosition(UASInterface*, double lat, double lon, double alt, quint64 usec);

src/ui/map3D/Pixhawk3DWidget.cc

@@ -58,7 +58,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     , selectedWpIndex(-1)
     , displayLocalGrid(false)
     , displayWorldGrid(true)
-    , displayTrail(true)
+    , displayTrails(true)
     , displayImagery(true)
     , displayWaypoints(true)
     , displayRGBD2D(false)
@@ -88,7 +88,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     allocentricMap->addChild(worldGridNode);
 
     // generate empty trail model
-    trailNode = createTrail(osg::Vec4(1.0f, 0.0f, 0.0f, 1.0f));
+    trailNode = new osg::Geode;
     rollingMap->addChild(trailNode);
 
     // generate map model
@@ -114,7 +114,8 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     rollingMap->addChild(obstacleGroupNode);
 
     // generate path model
-    pathNode = createTrail(osg::Vec4(1.0f, 0.8f, 0.0f, 1.0f));
+    pathNode = new osg::Geode;
+    pathNode->addDrawable(createTrail(osg::Vec4(1.0f, 0.8f, 0.0f, 1.0f)));
     rollingMap->addChild(pathNode);
 #endif
 
@@ -143,16 +144,75 @@ Pixhawk3DWidget::~Pixhawk3DWidget()
 void
 Pixhawk3DWidget::setActiveUAS(UASInterface* uas)
 {
-    if (this->uas != NULL && this->uas != uas)
+    if (this->uas == uas)
+    {
+        return;
+    }
+
+    if (this->uas != NULL)
     {
         // Disconnect any previously connected active MAV
-        //disconnect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64)));
+        disconnect(this->uas, SIGNAL(localPositionChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(addToTrails(UASInterface*,int,double,double,double,quint64)));
     }
 
+    connect(uas, SIGNAL(localPositionChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(addToTrails(UASInterface*,int,double,double,double,quint64)));
+    trails.clear();
+
     this->uas = uas;
 }
 
 void
+Pixhawk3DWidget::addToTrails(UASInterface* uas, int component, double x, double y, double z, quint64 time)
+{
+    if (this->uas->getUASID() != uas->getUASID())
+    {
+        return;
+    }
+
+    if (!trails.contains(component))
+    {
+        trails[component] = QVarLengthArray<osg::Vec3d, 10000>();
+        trailDrawableIdxs[component] = trails.size() - 1;
+        trailNode->addDrawable(createTrail(osg::Vec4((float)qrand() / RAND_MAX,
+                                                     (float)qrand() / RAND_MAX,
+                                                     (float)qrand() / RAND_MAX,
+                                                     1.0)));
+    }
+
+    QVarLengthArray<osg::Vec3d, 10000>& trail = trails[component];
+
+    bool addToTrail = false;
+    if (trail.size() > 0)
+    {
+        if (fabs(x - trail[trail.size() - 1].x()) > 0.01f ||
+                fabs(y - trail[trail.size() - 1].y()) > 0.01f ||
+                fabs(z - trail[trail.size() - 1].z()) > 0.01f)
+        {
+            addToTrail = true;
+        }
+    }
+    else
+    {
+        addToTrail = true;
+    }
+
+    if (addToTrail)
+    {
+        osg::Vec3d p(x, y, z);
+        if (trail.size() == trail.capacity())
+        {
+            memcpy(trail.data(), trail.data() + 1,
+                   (trail.size() - 1) * sizeof(osg::Vec3d));
+            trail[trail.size() - 1] = p;
+        }
+        else
+        {
+            trail.append(p);
+        }
+    }
+}
+
+void
 Pixhawk3DWidget::selectFrame(QString text)
 {
     if (text.compare("Global") == 0)
@@ -196,20 +256,20 @@ Pixhawk3DWidget::showWorldGrid(int32_t state)
 }
 
 void
-Pixhawk3DWidget::showTrail(int32_t state)
+Pixhawk3DWidget::showTrails(int32_t state)
 {
     if (state == Qt::Checked)
     {
-        if (!displayTrail)
+        if (!displayTrails)
         {
-            trail.clear();
+            trails.clear();
         }
 
-        displayTrail = true;
+        displayTrails = true;
     }
     else
     {
-        displayTrail = false;
+        displayTrails = false;
     }
 }
 
@@ -638,8 +698,8 @@ Pixhawk3DWidget::buildLayout(void)
     worldGridCheckBox->setChecked(displayWorldGrid);
 
     QCheckBox* trailCheckBox = new QCheckBox(this);
-    trailCheckBox->setText("Trail");
-    trailCheckBox->setChecked(displayTrail);
+    trailCheckBox->setText("Trails");
+    trailCheckBox->setChecked(displayTrails);
 
     QCheckBox* waypointsCheckBox = new QCheckBox(this);
     waypointsCheckBox->setText("Waypoints");
@@ -693,7 +753,7 @@ Pixhawk3DWidget::buildLayout(void)
     connect(worldGridCheckBox, SIGNAL(stateChanged(int)),
             this, SLOT(showWorldGrid(int)));
     connect(trailCheckBox, SIGNAL(stateChanged(int)),
-            this, SLOT(showTrail(int)));
+            this, SLOT(showTrails(int)));
     connect(waypointsCheckBox, SIGNAL(stateChanged(int)),
             this, SLOT(showWaypoints(int)));
     connect(mapComboBox, SIGNAL(currentIndexChanged(int)),
@@ -719,7 +779,7 @@ Pixhawk3DWidget::display(void)
     // set node visibility
     allocentricMap->setChildValue(worldGridNode, displayWorldGrid);
     rollingMap->setChildValue(localGridNode, displayLocalGrid);
-    rollingMap->setChildValue(trailNode, displayTrail);
+    rollingMap->setChildValue(trailNode, displayTrails);
     rollingMap->setChildValue(mapNode, displayImagery);
     rollingMap->setChildValue(waypointGroupNode, displayWaypoints);
     rollingMap->setChildValue(targetNode, enableTarget);
@@ -763,9 +823,9 @@ Pixhawk3DWidget::display(void)
                                          robotPitch, osg::Vec3d(1.0f, 0.0f, 0.0f),
                                          robotRoll, osg::Vec3d(0.0f, 1.0f, 0.0f)));
 
-    if (displayTrail)
+    if (displayTrails)
     {
-        updateTrail(robotX, robotY, robotZ);
+        updateTrails(robotX, robotY, robotZ);
     }
 
     if (frame == MAV_FRAME_GLOBAL && displayImagery)
@@ -1157,13 +1217,11 @@ Pixhawk3DWidget::createWorldGrid(void)
     return geode;
 }
 
-osg::ref_ptr<osg::Geode>
+osg::ref_ptr<osg::Geometry>
 Pixhawk3DWidget::createTrail(const osg::Vec4& color)
 {
-    osg::ref_ptr<osg::Geode> geode(new osg::Geode());
     osg::ref_ptr<osg::Geometry> geometry(new osg::Geometry());
     geometry->setUseDisplayList(false);
-    geode->addDrawable(geometry.get());
 
     osg::ref_ptr<osg::Vec3dArray> vertices(new osg::Vec3dArray());
     geometry->setVertexArray(vertices);
@@ -1183,7 +1241,7 @@ Pixhawk3DWidget::createTrail(const osg::Vec4& color)
     stateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
     geometry->setStateSet(stateset);
 
-    return geode;
+    return geometry;
 }
 
 osg::ref_ptr<Imagery>
@@ -1374,58 +1432,33 @@ Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ,
 }
 
 void
-Pixhawk3DWidget::updateTrail(double robotX, double robotY, double robotZ)
+Pixhawk3DWidget::updateTrails(double robotX, double robotY, double robotZ)
 {
-    if (robotX == 0.0f || robotY == 0.0f || robotZ == 0.0f)
-    {
-        return;
-    }
+    QMapIterator<int,int> it(trailDrawableIdxs);
 
-    bool addToTrail = false;
-    if (trail.size() > 0)
-    {
-        if (fabs(robotX - trail[trail.size() - 1].x()) > 0.01f ||
-                fabs(robotY - trail[trail.size() - 1].y()) > 0.01f ||
-                fabs(robotZ - trail[trail.size() - 1].z()) > 0.01f)
-        {
-            addToTrail = true;
-        }
-    }
-    else
+    while (it.hasNext())
     {
-        addToTrail = true;
-    }
+        it.next();
 
-    if (addToTrail)
-    {
-        osg::Vec3d p(robotX, robotY, robotZ);
-        if (trail.size() == trail.capacity())
-        {
-            memcpy(trail.data(), trail.data() + 1,
-                   (trail.size() - 1) * sizeof(osg::Vec3d));
-            trail[trail.size() - 1] = p;
-        }
-        else
+        osg::Geometry* geometry = trailNode->getDrawable(it.value())->asGeometry();
+        osg::DrawArrays* drawArrays = reinterpret_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
+
+        osg::ref_ptr<osg::Vec3Array> vertices(new osg::Vec3Array);
+
+        const QVarLengthArray<osg::Vec3d, 10000>& trail = trails.value(it.key());
+
+        for (int i = 0; i < trail.size(); ++i)
         {
-            trail.append(p);
+            vertices->push_back(osg::Vec3d(trail[i].y() - robotY,
+                                           trail[i].x() - robotX,
+                                           -(trail[i].z() - robotZ)));
         }
-    }
-
-    osg::Geometry* geometry = trailNode->getDrawable(0)->asGeometry();
-    osg::DrawArrays* drawArrays = reinterpret_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
 
-    osg::ref_ptr<osg::Vec3Array> vertices(new osg::Vec3Array);
-    for (int i = 0; i < trail.size(); ++i)
-    {
-        vertices->push_back(osg::Vec3d(trail[i].y() - robotY,
-                                       trail[i].x() - robotX,
-                                       -(trail[i].z() - robotZ)));
+        geometry->setVertexArray(vertices);
+        drawArrays->setFirst(0);
+        drawArrays->setCount(vertices->size());
+        geometry->dirtyBound();
     }
-
-    geometry->setVertexArray(vertices);
-    drawArrays->setFirst(0);
-    drawArrays->setCount(vertices->size());
-    geometry->dirtyBound();
 }
 
 void

src/ui/map3D/Pixhawk3DWidget.h

@@ -59,12 +59,13 @@ public:
 
 public slots:
     void setActiveUAS(UASInterface* uas);
+    void addToTrails(UASInterface* uas, int component, double x, double y, double z, quint64 time);
 
 private slots:
     void selectFrame(QString text);
     void showLocalGrid(int state);
     void showWorldGrid(int state);
-    void showTrail(int state);
+    void showTrails(int state);
     void showWaypoints(int state);
     void selectMapSource(int index);
     void selectVehicleModel(int index);
@@ -109,7 +110,7 @@ private:
 
     osg::ref_ptr<osg::Geode> createLocalGrid(void);
     osg::ref_ptr<osg::Geode> createWorldGrid(void);
-    osg::ref_ptr<osg::Geode> createTrail(const osg::Vec4& color);
+    osg::ref_ptr<osg::Geometry> createTrail(const osg::Vec4& color);
     osg::ref_ptr<Imagery> createMap(void);
     osg::ref_ptr<osg::Geode> createRGBD3D(void);
     osg::ref_ptr<osg::Node> createTarget(void);
@@ -120,7 +121,7 @@ private:
     void updateHUD(double robotX, double robotY, double robotZ,
                    double robotRoll, double robotPitch, double robotYaw,
                    const QString& utmZone);
-    void updateTrail(double robotX, double robotY, double robotZ);
+    void updateTrails(double robotX, double robotY, double robotZ);
     void updateImagery(double originX, double originY, double originZ,
                        const QString& zone);
     void updateWaypoints(void);
@@ -149,7 +150,7 @@ private:
 
     bool displayLocalGrid;
     bool displayWorldGrid;
-    bool displayTrail;
+    bool displayTrails;
     bool displayImagery;
     bool displayWaypoints;
     bool displayRGBD2D;
@@ -161,7 +162,8 @@ private:
 
     bool followCamera;
 
-    QVarLengthArray<osg::Vec3d, 10000> trail;
+    QMap<int, QVarLengthArray<osg::Vec3d, 10000> > trails;
+    QMap<int, int> trailDrawableIdxs;
 
     osg::ref_ptr<osg::Node> vehicleModel;
     osg::ref_ptr<osg::Geometry> hudBackgroundGeometry;