Visualize orientation for each pose source in local 3D view.

src/uas/UAS.cc

@@ -462,9 +462,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
 //                    compass = 0.0f;
 //                }
 
-
                 attitudeKnown = true;
                 emit attitudeChanged(this, roll, pitch, yaw, time);
+                emit attitudeChanged(this, message.compid, roll, pitch, yaw, time);
                 emit attitudeSpeedChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time);
             }
             break;
@@ -532,6 +532,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 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);
+                emit attitudeChanged(this, message.compid, pos.roll, pos.pitch, pos.yaw, time);
             }
             break;
         case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:

src/uas/UASInterface.h

@@ -447,6 +447,7 @@ signals:
     void thrustChanged(UASInterface*, double thrust);
     void heartbeat(UASInterface* uas);
     void attitudeChanged(UASInterface*, double roll, double pitch, double yaw, quint64 usec);
+    void attitudeChanged(UASInterface*, int component, 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 */

src/ui/map3D/Pixhawk3DWidget.cc

@@ -91,6 +91,9 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     trailNode = new osg::Geode;
     rollingMap->addChild(trailNode);
 
+    orientationNode = new osg::Group;
+    rollingMap->addChild(orientationNode);
+
     // generate map model
     mapNode = createMap();
     rollingMap->addChild(mapNode);
@@ -153,10 +156,15 @@ Pixhawk3DWidget::setActiveUAS(UASInterface* uas)
     {
         // Disconnect any previously connected active MAV
         disconnect(this->uas, SIGNAL(localPositionChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(addToTrails(UASInterface*,int,double,double,double,quint64)));
+        disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(updateAttitude(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)));
+    connect(uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,int,double,double,double,quint64)));
+
     trails.clear();
+    trailNode->removeDrawables(0, trailNode->getNumDrawables());
+    orientationNode->removeChildren(0, orientationNode->getNumChildren());
 
     this->uas = uas;
 }
@@ -173,10 +181,56 @@ Pixhawk3DWidget::addToTrails(UASInterface* uas, int component, double x, double
     {
         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)));
+
+        osg::Vec4 color((float)qrand() / RAND_MAX,
+                        (float)qrand() / RAND_MAX,
+                        (float)qrand() / RAND_MAX,
+                        0.5);
+        trailNode->addDrawable(createTrail(color));
+
+        double radius = 0.5;
+
+        osg::ref_ptr<osg::Group> group = new osg::Group;
+
+        // cone indicates waypoint orientation
+        osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
+        double coneRadius = radius / 2.0;
+        osg::ref_ptr<osg::Cone> cone =
+            new osg::Cone(osg::Vec3d(0.0, 0.0, 0.0),
+                          coneRadius, radius * 2.0);
+
+        sd->setShape(cone);
+        sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
+        sd->setColor(color);
+
+        osg::ref_ptr<osg::Geode> geode = new osg::Geode;
+        geode->addDrawable(sd);
+
+        osg::ref_ptr<osg::PositionAttitudeTransform> pat =
+            new osg::PositionAttitudeTransform;
+        pat->addChild(geode);
+        pat->setAttitude(osg::Quat(- M_PI_2, osg::Vec3d(1.0f, 0.0f, 0.0f),
+                                   M_PI_2, osg::Vec3d(0.0f, 1.0f, 0.0f),
+                                   0.0, osg::Vec3d(0.0f, 0.0f, 1.0f)));
+        group->addChild(pat);
+
+        // cylinder indicates waypoint position
+        sd = new osg::ShapeDrawable;
+        osg::ref_ptr<osg::Cylinder> cylinder =
+            new osg::Cylinder(osg::Vec3d(0.0, 0.0, 0.0),
+                              radius, 0);
+
+        sd->setShape(cylinder);
+        sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
+        sd->setColor(color);
+
+        geode = new osg::Geode;
+        geode->addDrawable(sd);
+        group->addChild(geode);
+
+        pat = new osg::PositionAttitudeTransform;
+        orientationNode->addChild(pat);
+        pat->addChild(group);
     }
 
     QVarLengthArray<osg::Vec3d, 10000>& trail = trails[component];
@@ -213,6 +267,29 @@ Pixhawk3DWidget::addToTrails(UASInterface* uas, int component, double x, double
 }
 
 void
+Pixhawk3DWidget::updateAttitude(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 time)
+{
+    if (this->uas->getUASID() != uas->getUASID())
+    {
+        return;
+    }
+
+    if (!trails.contains(component))
+    {
+        return;
+    }
+
+    int idx = trailDrawableIdxs[component];
+
+    osg::PositionAttitudeTransform* pat =
+        dynamic_cast<osg::PositionAttitudeTransform*>(orientationNode->getChild(idx));
+
+    pat->setAttitude(osg::Quat(-yaw, osg::Vec3d(0.0f, 0.0f, 1.0f),
+                               0.0, osg::Vec3d(1.0f, 0.0f, 0.0f),
+                               0.0, osg::Vec3d(0.0f, 1.0f, 0.0f)));
+}
+
+void
 Pixhawk3DWidget::selectFrame(QString text)
 {
     if (text.compare("Global") == 0)
@@ -780,6 +857,7 @@ Pixhawk3DWidget::display(void)
     allocentricMap->setChildValue(worldGridNode, displayWorldGrid);
     rollingMap->setChildValue(localGridNode, displayLocalGrid);
     rollingMap->setChildValue(trailNode, displayTrails);
+    rollingMap->setChildValue(orientationNode, displayTrails);
     rollingMap->setChildValue(mapNode, displayImagery);
     rollingMap->setChildValue(waypointGroupNode, displayWaypoints);
     rollingMap->setChildValue(targetNode, enableTarget);
@@ -1458,6 +1536,12 @@ Pixhawk3DWidget::updateTrails(double robotX, double robotY, double robotZ)
         drawArrays->setFirst(0);
         drawArrays->setCount(vertices->size());
         geometry->dirtyBound();
+
+        osg::PositionAttitudeTransform* pat =
+            dynamic_cast<osg::PositionAttitudeTransform*>(orientationNode->getChild(it.value()));
+        pat->setPosition(osg::Vec3(trail[trail.size() - 1].y() - robotY,
+                                   trail[trail.size() - 1].x() - robotX,
+                                   -(trail[trail.size() - 1].z() - robotZ)));
     }
 }
 

src/ui/map3D/Pixhawk3DWidget.h

@@ -60,6 +60,7 @@ public:
 public slots:
     void setActiveUAS(UASInterface* uas);
     void addToTrails(UASInterface* uas, int component, double x, double y, double z, quint64 time);
+    void updateAttitude(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 time);
 
 private slots:
     void selectFrame(QString text);
@@ -176,6 +177,7 @@ private:
     osg::ref_ptr<osg::Geode> localGridNode;
     osg::ref_ptr<osg::Geode> worldGridNode;
     osg::ref_ptr<osg::Geode> trailNode;
+    osg::ref_ptr<osg::Group> orientationNode;
     osg::ref_ptr<Imagery> mapNode;
     osg::ref_ptr<WaypointGroupNode> waypointGroupNode;
     osg::ref_ptr<osg::Node> targetNode;