Added setpoint visualization to 3D view.

src/ui/map3D/Pixhawk3DWidget.cc

@@ -127,8 +127,12 @@ Pixhawk3DWidget::systemCreated(UASInterface *uas)
 
     connect(uas, SIGNAL(localPositionChanged(UASInterface*,int,double,double,double,quint64)),
             this, SLOT(localPositionChanged(UASInterface*,int,double,double,double,quint64)));
-    connect(uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)),
-            this, SLOT(attitudeChanged(UASInterface*,int,double,double,double,quint64)));
+    connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)),
+            this, SLOT(localPositionChanged(UASInterface*,double,double,double,quint64)));
+    connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)),
+            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)));
 
     initializeSystem(systemId, uas->getColor());
 
@@ -166,10 +170,6 @@ Pixhawk3DWidget::localPositionChanged(UASInterface* uas, int component,
         systemData.trailNode()->addDrawable(createLink(uas->getColor()));
     }
 
-    // update system position
-    // FIXME
-    if (component == 201) m3DWidget->systemGroup(systemId)->position()->setPosition(osg::Vec3d(y, x, -z));
-
     QVector<osg::Vec3d>& trail = systemData.trailMap()[component];
 
     bool addToTrail = false;
@@ -204,7 +204,23 @@ Pixhawk3DWidget::localPositionChanged(UASInterface* uas, int component,
 }
 
 void
-Pixhawk3DWidget::attitudeChanged(UASInterface* uas, int component,
+Pixhawk3DWidget::localPositionChanged(UASInterface* uas,
+                                      double x, double y, double z,
+                                      quint64 time)
+{
+    int systemId = uas->getUASID();
+
+    if (!mSystemContainerMap.contains(systemId))
+    {
+        return;
+    }
+
+    // update system position
+    m3DWidget->systemGroup(systemId)->position()->setPosition(osg::Vec3d(y, x, -z));
+}
+
+void
+Pixhawk3DWidget::attitudeChanged(UASInterface* uas,
                                  double roll, double pitch, double yaw,
                                  quint64 time)
 {
@@ -222,6 +238,66 @@ Pixhawk3DWidget::attitudeChanged(UASInterface* uas, int component,
     m3DWidget->systemGroup(systemId)->attitude()->setAttitude(q);
 }
 
+void
+Pixhawk3DWidget::setpointChanged(int uasId, float x, float y, float z,
+                                 float yaw)
+{
+    if (!mSystemContainerMap.contains(uasId))
+    {
+        return;
+    }
+
+    UASInterface* uas = UASManager::instance()->getUASForId(uasId);
+    if (!uas)
+    {
+        return;
+    }
+
+    QColor color = uas->getColor();
+    const SystemViewParamsPtr& systemViewParams = mSystemViewParamMap.value(uasId);
+
+    osg::ref_ptr<osg::PositionAttitudeTransform> pat =
+        new osg::PositionAttitudeTransform;
+
+    pat->setPosition(osg::Vec3d(y, x, -z));
+    pat->setAttitude(osg::Quat(osg::DegreesToRadians(yaw) - 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)));
+
+    osg::ref_ptr<osg::Cone> cone = new osg::Cone(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.1f, 0.3f);
+    osg::ref_ptr<osg::ShapeDrawable> coneDrawable = new osg::ShapeDrawable(cone);
+    coneDrawable->setColor(osg::Vec4f(color.redF(), color.greenF(), color.blueF(), 1.0f));
+    coneDrawable->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
+    osg::ref_ptr<osg::Geode> coneGeode = new osg::Geode;
+    coneGeode->addDrawable(coneDrawable);
+
+    pat->addChild(coneGeode);
+
+    osg::ref_ptr<osg::Group>& setpointGroupNode = mSystemContainerMap[uasId].setpointGroupNode();
+
+    setpointGroupNode->addChild(pat);
+    if (setpointGroupNode->getNumChildren() > systemViewParams->setpointHistoryLength())
+    {
+        setpointGroupNode->removeChildren(0, setpointGroupNode->getNumChildren() - systemViewParams->setpointHistoryLength());
+    }
+
+    osg::Vec4f setpointColor(color.redF(), color.greenF(), color.blueF(), 1.0f);
+    int setpointCount = setpointGroupNode->getNumChildren();
+
+    // update colors
+    for (int i = 0; i < setpointCount; ++i)
+    {
+        osg::PositionAttitudeTransform* pat =
+            dynamic_cast<osg::PositionAttitudeTransform*>(setpointGroupNode->getChild(i));
+
+        osg::Geode* geode = dynamic_cast<osg::Geode*>(pat->getChild(0));
+        osg::ShapeDrawable* sd = dynamic_cast<osg::ShapeDrawable*>(geode->getDrawable(0));
+
+        setpointColor.a() = static_cast<float>(i + 1) / setpointCount;
+        sd->setColor(setpointColor);
+    }
+}
+
 void
 Pixhawk3DWidget::showViewParamWindow(void)
 {
@@ -642,6 +718,10 @@ Pixhawk3DWidget::update(void)
         SystemContainer& systemData = mSystemContainerMap[it.key()];
         const SystemViewParamsPtr& systemViewParams = it.value();
 
+        osg::ref_ptr<osg::Switch>& allocentricMap = systemNode->allocentricMap();
+        allocentricMap->setChildValue(systemData.setpointGroupNode(),
+                                      systemViewParams->displaySetpoints());
+
         osg::ref_ptr<osg::Switch>& rollingMap = systemNode->rollingMap();
         rollingMap->setChildValue(systemData.localGridNode(),
                                   systemViewParams->displayLocalGrid());
@@ -724,6 +804,14 @@ Pixhawk3DWidget::update(void)
 
         getPose(uas, frame, x, y, z, roll, pitch, yaw);
 
+        if (systemViewParams->displaySetpoints())
+        {
+
+        }
+        else
+        {
+            systemData.setpointGroupNode()->removeChildren(0, systemData.setpointGroupNode()->getNumChildren());
+        }
         if (systemViewParams->displayTarget())
         {
             if (systemData.target().isNull())
@@ -1010,6 +1098,10 @@ Pixhawk3DWidget::initializeSystem(int systemId, const QColor& systemColor)
     systemData.pointCloudNode() = createPointCloud();
     systemNode->rollingMap()->addChild(systemData.pointCloudNode(), false);
 
+    // generate setpoint model
+    systemData.setpointGroupNode() = new osg::Group;
+    systemNode->allocentricMap()->addChild(systemData.setpointGroupNode(), false);
+
     // generate target model
     systemData.targetNode() = createTarget(systemColor);
     systemNode->rollingMap()->addChild(systemData.targetNode(), false);

src/ui/map3D/Pixhawk3DWidget.h

@@ -57,7 +57,9 @@ public slots:
     void activeSystemChanged(UASInterface* uas);
     void systemCreated(UASInterface* uas);
     void localPositionChanged(UASInterface* uas, int component, double x, double y, double z, quint64 time);
-    void attitudeChanged(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 time);
+    void localPositionChanged(UASInterface* uas, double x, double y, double z, quint64 time);
+    void attitudeChanged(UASInterface* uas, double roll, double pitch, double yaw, quint64 time);
+    void setpointChanged(int uasId, float x, float y, float z, float yaw);
 
 signals:
     void systemCreatedSignal(UASInterface* uas);

src/ui/map3D/SystemContainer.cc

@@ -61,6 +61,12 @@ SystemContainer::rgbImageNode(void)
     return mRGBImageNode;
 }
 
+osg::ref_ptr<osg::Group>&
+SystemContainer::setpointGroupNode(void)
+{
+    return mSetpointGroupNode;
+}
+
 osg::ref_ptr<osg::Node>&
 SystemContainer::targetNode(void)
 {

src/ui/map3D/SystemContainer.h

@@ -30,6 +30,7 @@ public:
     osg::ref_ptr<osg::Node>& modelNode(void);
     osg::ref_ptr<osg::Geode>& pointCloudNode(void);
     osg::ref_ptr<ImageWindowGeode>& rgbImageNode(void);
+    osg::ref_ptr<osg::Group>& setpointGroupNode(void);
     osg::ref_ptr<osg::Node>& targetNode(void);
     osg::ref_ptr<osg::Geode>& trailNode(void);
     osg::ref_ptr<WaypointGroupNode>& waypointGroupNode(void);
@@ -53,6 +54,7 @@ private:
     osg::ref_ptr<osg::Node> mModelNode;
     osg::ref_ptr<osg::Geode> mPointCloudNode;
     osg::ref_ptr<ImageWindowGeode> mRGBImageNode;
+    osg::ref_ptr<osg::Group> mSetpointGroupNode;
     osg::ref_ptr<osg::Node> mTargetNode;
     osg::ref_ptr<osg::Geode> mTrailNode;
     osg::ref_ptr<WaypointGroupNode> mWaypointGroupNode;

src/ui/map3D/SystemViewParams.cc

@@ -8,10 +8,12 @@ SystemViewParams::SystemViewParams(int systemId)
  , mDisplayPlannedPath(true)
  , mDisplayPointCloud(true)
  , mDisplayRGBD(false)
+ , mDisplaySetpoints(true)
  , mDisplayTarget(true)
  , mDisplayTrails(true)
  , mDisplayWaypoints(true)
  , mModelIndex(-1)
+ , mSetpointHistoryLength(100)
 {
 
 }
@@ -88,6 +90,18 @@ SystemViewParams::displayRGBD(void) const
     return mDisplayRGBD;
 }
 
+bool&
+SystemViewParams::displaySetpoints(void)
+{
+    return mDisplaySetpoints;
+}
+
+bool
+SystemViewParams::displaySetpoints(void) const
+{
+    return mDisplaySetpoints;
+}
+
 bool&
 SystemViewParams::displayTarget(void)
 {
@@ -148,6 +162,18 @@ SystemViewParams::modelNames(void) const
     return mModelNames;
 }
 
+int&
+SystemViewParams::setpointHistoryLength(void)
+{
+    return mSetpointHistoryLength;
+}
+
+int
+SystemViewParams::setpointHistoryLength(void) const
+{
+    return mSetpointHistoryLength;
+}
+
 void
 SystemViewParams::modelChanged(int index)
 {
@@ -156,6 +182,12 @@ SystemViewParams::modelChanged(int index)
     emit modelChangedSignal(mSystemId, index);
 }
 
+void
+SystemViewParams::setSetpointHistoryLength(int length)
+{
+    mSetpointHistoryLength = length;
+}
+
 void
 SystemViewParams::toggleColorPointCloud(int state)
 {
@@ -234,6 +266,19 @@ SystemViewParams::toggleRGBD(int state)
     }
 }
 
+void
+SystemViewParams::toggleSetpoints(int state)
+{
+    if (state == Qt::Checked)
+    {
+        mDisplaySetpoints = true;
+    }
+    else
+    {
+        mDisplaySetpoints = false;
+    }
+}
+
 void
 SystemViewParams::toggleTarget(int state)
 {

src/ui/map3D/SystemViewParams.h

@@ -30,6 +30,9 @@ public:
     bool& displayRGBD(void);
     bool displayRGBD(void) const;
 
+    bool& displaySetpoints(void);
+    bool displaySetpoints(void) const;
+
     bool& displayTarget(void);
     bool displayTarget(void) const;
 
@@ -45,14 +48,19 @@ public:
     QVector<QString>& modelNames(void);
     const QVector<QString>& modelNames(void) const;
 
+    int& setpointHistoryLength(void);
+    int setpointHistoryLength(void) const;
+
 public slots:
     void modelChanged(int index);
+    void setSetpointHistoryLength(int length);
     void toggleColorPointCloud(int state);
     void toggleLocalGrid(int state);
     void toggleObstacleList(int state);
     void togglePlannedPath(int state);
     void togglePointCloud(int state);
     void toggleRGBD(int state);
+    void toggleSetpoints(int state);
     void toggleTarget(int state);
     void toggleTrails(int state);
     void toggleWaypoints(int state);
@@ -69,11 +77,13 @@ private:
     bool mDisplayPlannedPath;
     bool mDisplayPointCloud;
     bool mDisplayRGBD;
+    bool mDisplaySetpoints;
     bool mDisplayTarget;
     bool mDisplayTrails;
     bool mDisplayWaypoints;
     int mModelIndex;
     QVector<QString> mModelNames;
+    int mSetpointHistoryLength;
 };
 
 typedef QSharedPointer<SystemViewParams> SystemViewParamsPtr;

src/ui/map3D/ViewParamWidget.cc

@@ -57,6 +57,19 @@ ViewParamWidget::systemCreated(UASInterface *uas)
     mFollowCameraComboBox->addItem(text);
 }
 
+void
+ViewParamWidget::setpointsCheckBoxToggled(int state)
+{
+    if (state == Qt::Checked)
+    {
+        mSetpointHistoryLengthSpinBox->setEnabled(true);
+    }
+    else
+    {
+        mSetpointHistoryLengthSpinBox->setEnabled(false);
+    }
+}
+
 void
 ViewParamWidget::buildLayout(QVBoxLayout* layout)
 {
@@ -136,6 +149,15 @@ ViewParamWidget::addTab(int systemId)
     QCheckBox* rgbdCheckBox = new QCheckBox(this);
     rgbdCheckBox->setChecked(systemViewParams->displayRGBD());
 
+    QCheckBox* setpointsCheckBox = new QCheckBox(this);
+    setpointsCheckBox->setChecked(systemViewParams->displaySetpoints());
+
+    mSetpointHistoryLengthSpinBox = new QSpinBox(this);
+    mSetpointHistoryLengthSpinBox->setRange(1, 10000);
+    mSetpointHistoryLengthSpinBox->setSingleStep(10);
+    mSetpointHistoryLengthSpinBox->setValue(systemViewParams->setpointHistoryLength());
+    mSetpointHistoryLengthSpinBox->setEnabled(systemViewParams->displaySetpoints());
+
     QCheckBox* targetCheckBox = new QCheckBox(this);
     targetCheckBox->setChecked(systemViewParams->displayTarget());
 
@@ -155,6 +177,8 @@ ViewParamWidget::addTab(int systemId)
     formLayout->addRow(tr("Planned Path"), plannedPathCheckBox);
     formLayout->addRow(tr("Point Cloud"), pointCloudCheckBox);
     formLayout->addRow(tr("RGBD"), rgbdCheckBox);
+    formLayout->addRow(tr("Setpoints"), setpointsCheckBox);
+    formLayout->addRow(tr("Setpoint History Length"), mSetpointHistoryLengthSpinBox);
     formLayout->addRow(tr("Target"), targetCheckBox);
     formLayout->addRow(tr("Trails"), trailsCheckBox);
     formLayout->addRow(tr("Waypoints"), waypointsCheckBox);
@@ -179,6 +203,12 @@ ViewParamWidget::addTab(int systemId)
             systemViewParams.data(), SLOT(togglePointCloud(int)));
     connect(rgbdCheckBox, SIGNAL(stateChanged(int)),
             systemViewParams.data(), SLOT(toggleRGBD(int)));
+    connect(setpointsCheckBox, SIGNAL(stateChanged(int)),
+            systemViewParams.data(), SLOT(toggleSetpoints(int)));
+    connect(setpointsCheckBox, SIGNAL(stateChanged(int)),
+            this, SLOT(setpointsCheckBoxToggled(int)));
+    connect(mSetpointHistoryLengthSpinBox, SIGNAL(valueChanged(int)),
+            systemViewParams.data(), SLOT(setSetpointHistoryLength(int)));
     connect(targetCheckBox, SIGNAL(stateChanged(int)),
             systemViewParams.data(), SLOT(toggleTarget(int)));
     connect(trailsCheckBox, SIGNAL(stateChanged(int)),

src/ui/map3D/ViewParamWidget.h

@@ -3,6 +3,7 @@
 
 #include <QComboBox>
 #include <QDockWidget>
+#include <QSpinBox>
 #include <QTabWidget>
 #include <QVBoxLayout>
 
@@ -26,6 +27,7 @@ signals:
 
 private slots:
     void systemCreated(UASInterface* uas);
+    void setpointsCheckBoxToggled(int state);
 
 private:
     void buildLayout(QVBoxLayout* layout);
@@ -40,6 +42,7 @@ private:
 
     // child widgets
     QComboBox* mFollowCameraComboBox;
+    QSpinBox* mSetpointHistoryLengthSpinBox;
     QTabWidget* mTabWidget;
 };