From bfc8f211996f3b3bbcf93f3c5b0b84333c148a4d Mon Sep 17 00:00:00 2001 From: pixhawk Date: Fri, 10 Feb 2012 17:40:46 +0100 Subject: [PATCH] Visualize orientation for each pose source in local 3D view. --- src/uas/UAS.cc | 3 +- src/uas/UASInterface.h | 1 + src/ui/map3D/Pixhawk3DWidget.cc | 92 +++++++++++++++++++++++++++++++-- src/ui/map3D/Pixhawk3DWidget.h | 2 + 4 files changed, 93 insertions(+), 5 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index e1ac0374d..a57e68073 100644 --- a/src/uas/UAS.cc +++ b/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: diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 71dc6d4c9..cec880cb1 100644 --- a/src/uas/UASInterface.h +++ b/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 */ diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index e043af159..22af254d8 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/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(); 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 group = new osg::Group; + + // cone indicates waypoint orientation + osg::ref_ptr sd = new osg::ShapeDrawable; + double coneRadius = radius / 2.0; + osg::ref_ptr 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 geode = new osg::Geode; + geode->addDrawable(sd); + + osg::ref_ptr 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 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& trail = trails[component]; @@ -212,6 +266,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(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) { @@ -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(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))); } } diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h index 117a9a791..60537da14 100644 --- a/src/ui/map3D/Pixhawk3DWidget.h +++ b/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 localGridNode; osg::ref_ptr worldGridNode; osg::ref_ptr trailNode; + osg::ref_ptr orientationNode; osg::ref_ptr mapNode; osg::ref_ptr waypointGroupNode; osg::ref_ptr targetNode; -- 2.22.0