Commit 4560ef33 authored by hengli's avatar hengli

Added setpoint visualization to 3D view.

parent e9dfef53
...@@ -127,8 +127,12 @@ Pixhawk3DWidget::systemCreated(UASInterface *uas) ...@@ -127,8 +127,12 @@ Pixhawk3DWidget::systemCreated(UASInterface *uas)
connect(uas, SIGNAL(localPositionChanged(UASInterface*,int,double,double,double,quint64)), connect(uas, SIGNAL(localPositionChanged(UASInterface*,int,double,double,double,quint64)),
this, SLOT(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)), connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)),
this, SLOT(attitudeChanged(UASInterface*,int,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()); initializeSystem(systemId, uas->getColor());
...@@ -166,10 +170,6 @@ Pixhawk3DWidget::localPositionChanged(UASInterface* uas, int component, ...@@ -166,10 +170,6 @@ Pixhawk3DWidget::localPositionChanged(UASInterface* uas, int component,
systemData.trailNode()->addDrawable(createLink(uas->getColor())); 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]; QVector<osg::Vec3d>& trail = systemData.trailMap()[component];
bool addToTrail = false; bool addToTrail = false;
...@@ -204,7 +204,23 @@ Pixhawk3DWidget::localPositionChanged(UASInterface* uas, int component, ...@@ -204,7 +204,23 @@ Pixhawk3DWidget::localPositionChanged(UASInterface* uas, int component,
} }
void 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, double roll, double pitch, double yaw,
quint64 time) quint64 time)
{ {
...@@ -222,6 +238,66 @@ Pixhawk3DWidget::attitudeChanged(UASInterface* uas, int component, ...@@ -222,6 +238,66 @@ Pixhawk3DWidget::attitudeChanged(UASInterface* uas, int component,
m3DWidget->systemGroup(systemId)->attitude()->setAttitude(q); 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 void
Pixhawk3DWidget::showViewParamWindow(void) Pixhawk3DWidget::showViewParamWindow(void)
{ {
...@@ -642,6 +718,10 @@ Pixhawk3DWidget::update(void) ...@@ -642,6 +718,10 @@ Pixhawk3DWidget::update(void)
SystemContainer& systemData = mSystemContainerMap[it.key()]; SystemContainer& systemData = mSystemContainerMap[it.key()];
const SystemViewParamsPtr& systemViewParams = it.value(); 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(); osg::ref_ptr<osg::Switch>& rollingMap = systemNode->rollingMap();
rollingMap->setChildValue(systemData.localGridNode(), rollingMap->setChildValue(systemData.localGridNode(),
systemViewParams->displayLocalGrid()); systemViewParams->displayLocalGrid());
...@@ -724,6 +804,14 @@ Pixhawk3DWidget::update(void) ...@@ -724,6 +804,14 @@ Pixhawk3DWidget::update(void)
getPose(uas, frame, x, y, z, roll, pitch, yaw); getPose(uas, frame, x, y, z, roll, pitch, yaw);
if (systemViewParams->displaySetpoints())
{
}
else
{
systemData.setpointGroupNode()->removeChildren(0, systemData.setpointGroupNode()->getNumChildren());
}
if (systemViewParams->displayTarget()) if (systemViewParams->displayTarget())
{ {
if (systemData.target().isNull()) if (systemData.target().isNull())
...@@ -1010,6 +1098,10 @@ Pixhawk3DWidget::initializeSystem(int systemId, const QColor& systemColor) ...@@ -1010,6 +1098,10 @@ Pixhawk3DWidget::initializeSystem(int systemId, const QColor& systemColor)
systemData.pointCloudNode() = createPointCloud(); systemData.pointCloudNode() = createPointCloud();
systemNode->rollingMap()->addChild(systemData.pointCloudNode(), false); systemNode->rollingMap()->addChild(systemData.pointCloudNode(), false);
// generate setpoint model
systemData.setpointGroupNode() = new osg::Group;
systemNode->allocentricMap()->addChild(systemData.setpointGroupNode(), false);
// generate target model // generate target model
systemData.targetNode() = createTarget(systemColor); systemData.targetNode() = createTarget(systemColor);
systemNode->rollingMap()->addChild(systemData.targetNode(), false); systemNode->rollingMap()->addChild(systemData.targetNode(), false);
......
...@@ -57,7 +57,9 @@ public slots: ...@@ -57,7 +57,9 @@ public slots:
void activeSystemChanged(UASInterface* uas); void activeSystemChanged(UASInterface* uas);
void systemCreated(UASInterface* uas); void systemCreated(UASInterface* uas);
void localPositionChanged(UASInterface* uas, int component, double x, double y, double z, quint64 time); 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: signals:
void systemCreatedSignal(UASInterface* uas); void systemCreatedSignal(UASInterface* uas);
......
...@@ -61,6 +61,12 @@ SystemContainer::rgbImageNode(void) ...@@ -61,6 +61,12 @@ SystemContainer::rgbImageNode(void)
return mRGBImageNode; return mRGBImageNode;
} }
osg::ref_ptr<osg::Group>&
SystemContainer::setpointGroupNode(void)
{
return mSetpointGroupNode;
}
osg::ref_ptr<osg::Node>& osg::ref_ptr<osg::Node>&
SystemContainer::targetNode(void) SystemContainer::targetNode(void)
{ {
......
...@@ -30,6 +30,7 @@ public: ...@@ -30,6 +30,7 @@ public:
osg::ref_ptr<osg::Node>& modelNode(void); osg::ref_ptr<osg::Node>& modelNode(void);
osg::ref_ptr<osg::Geode>& pointCloudNode(void); osg::ref_ptr<osg::Geode>& pointCloudNode(void);
osg::ref_ptr<ImageWindowGeode>& rgbImageNode(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::Node>& targetNode(void);
osg::ref_ptr<osg::Geode>& trailNode(void); osg::ref_ptr<osg::Geode>& trailNode(void);
osg::ref_ptr<WaypointGroupNode>& waypointGroupNode(void); osg::ref_ptr<WaypointGroupNode>& waypointGroupNode(void);
...@@ -53,6 +54,7 @@ private: ...@@ -53,6 +54,7 @@ private:
osg::ref_ptr<osg::Node> mModelNode; osg::ref_ptr<osg::Node> mModelNode;
osg::ref_ptr<osg::Geode> mPointCloudNode; osg::ref_ptr<osg::Geode> mPointCloudNode;
osg::ref_ptr<ImageWindowGeode> mRGBImageNode; osg::ref_ptr<ImageWindowGeode> mRGBImageNode;
osg::ref_ptr<osg::Group> mSetpointGroupNode;
osg::ref_ptr<osg::Node> mTargetNode; osg::ref_ptr<osg::Node> mTargetNode;
osg::ref_ptr<osg::Geode> mTrailNode; osg::ref_ptr<osg::Geode> mTrailNode;
osg::ref_ptr<WaypointGroupNode> mWaypointGroupNode; osg::ref_ptr<WaypointGroupNode> mWaypointGroupNode;
......
...@@ -8,10 +8,12 @@ SystemViewParams::SystemViewParams(int systemId) ...@@ -8,10 +8,12 @@ SystemViewParams::SystemViewParams(int systemId)
, mDisplayPlannedPath(true) , mDisplayPlannedPath(true)
, mDisplayPointCloud(true) , mDisplayPointCloud(true)
, mDisplayRGBD(false) , mDisplayRGBD(false)
, mDisplaySetpoints(true)
, mDisplayTarget(true) , mDisplayTarget(true)
, mDisplayTrails(true) , mDisplayTrails(true)
, mDisplayWaypoints(true) , mDisplayWaypoints(true)
, mModelIndex(-1) , mModelIndex(-1)
, mSetpointHistoryLength(100)
{ {
} }
...@@ -88,6 +90,18 @@ SystemViewParams::displayRGBD(void) const ...@@ -88,6 +90,18 @@ SystemViewParams::displayRGBD(void) const
return mDisplayRGBD; return mDisplayRGBD;
} }
bool&
SystemViewParams::displaySetpoints(void)
{
return mDisplaySetpoints;
}
bool
SystemViewParams::displaySetpoints(void) const
{
return mDisplaySetpoints;
}
bool& bool&
SystemViewParams::displayTarget(void) SystemViewParams::displayTarget(void)
{ {
...@@ -148,6 +162,18 @@ SystemViewParams::modelNames(void) const ...@@ -148,6 +162,18 @@ SystemViewParams::modelNames(void) const
return mModelNames; return mModelNames;
} }
int&
SystemViewParams::setpointHistoryLength(void)
{
return mSetpointHistoryLength;
}
int
SystemViewParams::setpointHistoryLength(void) const
{
return mSetpointHistoryLength;
}
void void
SystemViewParams::modelChanged(int index) SystemViewParams::modelChanged(int index)
{ {
...@@ -156,6 +182,12 @@ SystemViewParams::modelChanged(int index) ...@@ -156,6 +182,12 @@ SystemViewParams::modelChanged(int index)
emit modelChangedSignal(mSystemId, index); emit modelChangedSignal(mSystemId, index);
} }
void
SystemViewParams::setSetpointHistoryLength(int length)
{
mSetpointHistoryLength = length;
}
void void
SystemViewParams::toggleColorPointCloud(int state) SystemViewParams::toggleColorPointCloud(int state)
{ {
...@@ -234,6 +266,19 @@ SystemViewParams::toggleRGBD(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 void
SystemViewParams::toggleTarget(int state) SystemViewParams::toggleTarget(int state)
{ {
......
...@@ -30,6 +30,9 @@ public: ...@@ -30,6 +30,9 @@ public:
bool& displayRGBD(void); bool& displayRGBD(void);
bool displayRGBD(void) const; bool displayRGBD(void) const;
bool& displaySetpoints(void);
bool displaySetpoints(void) const;
bool& displayTarget(void); bool& displayTarget(void);
bool displayTarget(void) const; bool displayTarget(void) const;
...@@ -45,14 +48,19 @@ public: ...@@ -45,14 +48,19 @@ public:
QVector<QString>& modelNames(void); QVector<QString>& modelNames(void);
const QVector<QString>& modelNames(void) const; const QVector<QString>& modelNames(void) const;
int& setpointHistoryLength(void);
int setpointHistoryLength(void) const;
public slots: public slots:
void modelChanged(int index); void modelChanged(int index);
void setSetpointHistoryLength(int length);
void toggleColorPointCloud(int state); void toggleColorPointCloud(int state);
void toggleLocalGrid(int state); void toggleLocalGrid(int state);
void toggleObstacleList(int state); void toggleObstacleList(int state);
void togglePlannedPath(int state); void togglePlannedPath(int state);
void togglePointCloud(int state); void togglePointCloud(int state);
void toggleRGBD(int state); void toggleRGBD(int state);
void toggleSetpoints(int state);
void toggleTarget(int state); void toggleTarget(int state);
void toggleTrails(int state); void toggleTrails(int state);
void toggleWaypoints(int state); void toggleWaypoints(int state);
...@@ -69,11 +77,13 @@ private: ...@@ -69,11 +77,13 @@ private:
bool mDisplayPlannedPath; bool mDisplayPlannedPath;
bool mDisplayPointCloud; bool mDisplayPointCloud;
bool mDisplayRGBD; bool mDisplayRGBD;
bool mDisplaySetpoints;
bool mDisplayTarget; bool mDisplayTarget;
bool mDisplayTrails; bool mDisplayTrails;
bool mDisplayWaypoints; bool mDisplayWaypoints;
int mModelIndex; int mModelIndex;
QVector<QString> mModelNames; QVector<QString> mModelNames;
int mSetpointHistoryLength;
}; };
typedef QSharedPointer<SystemViewParams> SystemViewParamsPtr; typedef QSharedPointer<SystemViewParams> SystemViewParamsPtr;
......
...@@ -57,6 +57,19 @@ ViewParamWidget::systemCreated(UASInterface *uas) ...@@ -57,6 +57,19 @@ ViewParamWidget::systemCreated(UASInterface *uas)
mFollowCameraComboBox->addItem(text); mFollowCameraComboBox->addItem(text);
} }
void
ViewParamWidget::setpointsCheckBoxToggled(int state)
{
if (state == Qt::Checked)
{
mSetpointHistoryLengthSpinBox->setEnabled(true);
}
else
{
mSetpointHistoryLengthSpinBox->setEnabled(false);
}
}
void void
ViewParamWidget::buildLayout(QVBoxLayout* layout) ViewParamWidget::buildLayout(QVBoxLayout* layout)
{ {
...@@ -136,6 +149,15 @@ ViewParamWidget::addTab(int systemId) ...@@ -136,6 +149,15 @@ ViewParamWidget::addTab(int systemId)
QCheckBox* rgbdCheckBox = new QCheckBox(this); QCheckBox* rgbdCheckBox = new QCheckBox(this);
rgbdCheckBox->setChecked(systemViewParams->displayRGBD()); 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); QCheckBox* targetCheckBox = new QCheckBox(this);
targetCheckBox->setChecked(systemViewParams->displayTarget()); targetCheckBox->setChecked(systemViewParams->displayTarget());
...@@ -155,6 +177,8 @@ ViewParamWidget::addTab(int systemId) ...@@ -155,6 +177,8 @@ ViewParamWidget::addTab(int systemId)
formLayout->addRow(tr("Planned Path"), plannedPathCheckBox); formLayout->addRow(tr("Planned Path"), plannedPathCheckBox);
formLayout->addRow(tr("Point Cloud"), pointCloudCheckBox); formLayout->addRow(tr("Point Cloud"), pointCloudCheckBox);
formLayout->addRow(tr("RGBD"), rgbdCheckBox); 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("Target"), targetCheckBox);
formLayout->addRow(tr("Trails"), trailsCheckBox); formLayout->addRow(tr("Trails"), trailsCheckBox);
formLayout->addRow(tr("Waypoints"), waypointsCheckBox); formLayout->addRow(tr("Waypoints"), waypointsCheckBox);
...@@ -179,6 +203,12 @@ ViewParamWidget::addTab(int systemId) ...@@ -179,6 +203,12 @@ ViewParamWidget::addTab(int systemId)
systemViewParams.data(), SLOT(togglePointCloud(int))); systemViewParams.data(), SLOT(togglePointCloud(int)));
connect(rgbdCheckBox, SIGNAL(stateChanged(int)), connect(rgbdCheckBox, SIGNAL(stateChanged(int)),
systemViewParams.data(), SLOT(toggleRGBD(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)), connect(targetCheckBox, SIGNAL(stateChanged(int)),
systemViewParams.data(), SLOT(toggleTarget(int))); systemViewParams.data(), SLOT(toggleTarget(int)));
connect(trailsCheckBox, SIGNAL(stateChanged(int)), connect(trailsCheckBox, SIGNAL(stateChanged(int)),
......
...@@ -3,6 +3,7 @@ ...@@ -3,6 +3,7 @@
#include <QComboBox> #include <QComboBox>
#include <QDockWidget> #include <QDockWidget>
#include <QSpinBox>
#include <QTabWidget> #include <QTabWidget>
#include <QVBoxLayout> #include <QVBoxLayout>
...@@ -26,6 +27,7 @@ signals: ...@@ -26,6 +27,7 @@ signals:
private slots: private slots:
void systemCreated(UASInterface* uas); void systemCreated(UASInterface* uas);
void setpointsCheckBoxToggled(int state);
private: private:
void buildLayout(QVBoxLayout* layout); void buildLayout(QVBoxLayout* layout);
...@@ -40,6 +42,7 @@ private: ...@@ -40,6 +42,7 @@ private:
// child widgets // child widgets
QComboBox* mFollowCameraComboBox; QComboBox* mFollowCameraComboBox;
QSpinBox* mSetpointHistoryLengthSpinBox;
QTabWidget* mTabWidget; QTabWidget* mTabWidget;
}; };
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment