diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri index ea1e7ff..b8de01e 100644 --- a/qgroundcontrol.pri +++ b/qgroundcontrol.pri @@ -74,17 +74,30 @@ macx { # Copy audio files if needed QMAKE_PRE_LINK += cp -r $$BASEDIR/audio $$DESTDIR/qgroundcontrol.app/Contents/MacOs/. - exists(/opt/local/lib/osg):exists("/opt/local/lib/osgEarth") { - message("Building support for OSGEARTH") - DEPENDENCIES_PRESENT += osgearth - LIBS += -L/opt/local/lib/ - INCLUDEPATH += /opt/local/include - # Include OpenSceneGraph and osgEarth libraries - LIBS += -losg \ - -losgViewer \ - -losgEarth + exists(/usr/include/osg) { + message("Building support for OpenSceneGraph") + DEPENDENCIES_PRESENT += osg + # Include OpenSceneGraph libraries + LIBS += -losg DEFINES += QGC_OSG_ENABLED } + + exists(/usr/include/osgEarth) { + message("Building support for osgEarth") + DEPENDENCIES_PRESENT += osgearth + # Include osgEarth libraries + LIBS += -losgViewer \ + -losgEarth + DEFINES += QGC_OSGEARTH_ENABLED + } + + exists(/usr/local/include/libfreenect) { + message("Building support for libfreenect") + DEPENDENCIES_PRESENT += libfreenect + # Include libfreenect libraries + LIBS += -lfreenect + DEFINES += QGC_LIBFREENECT_ENABLED + } } # GNU/Linux @@ -127,24 +140,32 @@ linux-g++ { -lSDL \ -lSDLmain -exists(/usr/include/osg) { -message("Building support for OpenSceneGraph") -DEPENDENCIES_PRESENT += osg -# Include OpenSceneGraph libraries -LIBS += -losg -DEFINES += QGC_OSG_ENABLED -} + exists(/usr/include/osg) { + message("Building support for OpenSceneGraph") + DEPENDENCIES_PRESENT += osg + # Include OpenSceneGraph libraries + LIBS += -losg + DEFINES += QGC_OSG_ENABLED + } -exists(/usr/include/osgEarth) { -message("Building support for osgEarth") -DEPENDENCIES_PRESENT += osgearth -# Include osgEarth libraries -LIBS += -losgViewer \ - -losgEarth -DEFINES += QGC_OSGEARTH_ENABLED -} + exists(/usr/include/osgEarth) { + message("Building support for osgEarth") + DEPENDENCIES_PRESENT += osgearth + # Include osgEarth libraries + LIBS += -losgViewer \ + -losgEarth + DEFINES += QGC_OSGEARTH_ENABLED + } -QMAKE_CXXFLAGS += -Wl,-E + exists(/usr/local/include/libfreenect) { + message("Building support for libfreenect") + DEPENDENCIES_PRESENT += libfreenect + # Include libfreenect libraries + LIBS += -lfreenect + DEFINES += QGC_LIBFREENECT_ENABLED + } + + QMAKE_CXXFLAGS += -Wl,-E #-lflite_cmu_us_rms \ #-lflite_cmu_us_slt \ @@ -188,22 +209,31 @@ linux-g++-64 { -lSDL \ -lSDLmain -exists(/usr/include/osg) { -message("Building support for OpenSceneGraph") -DEPENDENCIES_PRESENT += osg -# Include OpenSceneGraph libraries -LIBS += -losg -DEFINES += QGC_OSG_ENABLED -} + exists(/usr/include/osg) { + message("Building support for OpenSceneGraph") + DEPENDENCIES_PRESENT += osg + # Include OpenSceneGraph libraries + LIBS += -losg + DEFINES += QGC_OSG_ENABLED + } + + exists(/usr/include/osgEarth) { + message("Building support for osgEarth") + DEPENDENCIES_PRESENT += osgearth + # Include osgEarth libraries + LIBS += -losgViewer \ + -losgEarth + DEFINES += QGC_OSGEARTH_ENABLED + } + + exists(/usr/local/include/libfreenect) { + message("Building support for libfreenect") + DEPENDENCIES_PRESENT += libfreenect + # Include libfreenect libraries + LIBS += -lfreenect + DEFINES += QGC_LIBFREENECT_ENABLED + } -exists(/usr/include/osgEarth) { -message("Building support for osgEarth") -DEPENDENCIES_PRESENT += osgearth -# Include osgEarth libraries -LIBS += -losgViewer \ - -losgEarth -DEFINES += QGC_OSGEARTH_ENABLED -} } diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 2353817..b71770e 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -47,7 +47,6 @@ MAVLINK_CONF = "" # if the variable MAVLINK_CONF contains the name of an # additional project, QGroundControl includes the support # of custom MAVLink messages of this project - exists(user_config.pri) { include(user_config.pri) message("----- USING CUSTOM USER QGROUNDCONTROL CONFIG FROM user_config.pri -----") @@ -79,10 +78,10 @@ contains(MAVLINK_CONF, ualberta) { INCLUDEPATH += $$BASEDIR/../mavlink/include/ualberta DEFINES += QGC_USE_UALBERTA_MESSAGES } -contains(MAVLINK_CONF, ardupilotmega) { +contains(MAVLINK_CONF, ardupilotmega) { # Remove the default set - it is included anyway INCLUDEPATH -= $$BASEDIR/../mavlink/include/common - + # UALBERTA SPECIAL MESSAGES INCLUDEPATH += $$BASEDIR/../mavlink/include/ardupilotmega DEFINES += QGC_USE_ARDUPILOTMEGA_MESSAGES @@ -139,7 +138,8 @@ FORMS += src/ui/MainWindow.ui \ src/ui/QGCPxImuFirmwareUpdate.ui \ src/ui/QGCDataPlot2D.ui \ src/ui/QGCRemoteControlView.ui - #src/ui/WaypointGlobalView.ui + +# src/ui/WaypointGlobalView.ui INCLUDEPATH += src \ src/ui \ src/ui/linechart \ @@ -221,26 +221,31 @@ HEADERS += src/MG.h \ src/ui/linechart/IncrementalPlot.h \ src/ui/map/Waypoint2DIcon.h \ src/ui/map/MAV2DIcon.h \ - src/ui/QGCRemoteControlView.h \ - #src/ui/WaypointGlobalView.h \ + src/ui/QGCRemoteControlView.h \ # src/ui/WaypointGlobalView.h \ src/ui/RadioCalibration/RadioCalibrationData.h \ src/ui/RadioCalibration/RadioCalibrationWindow.h \ src/ui/RadioCalibration/AirfoilServoCalibrator.h \ src/ui/RadioCalibration/SwitchCalibrator.h \ src/ui/RadioCalibration/CurveCalibrator.h \ src/ui/RadioCalibration/AbstractCalibrator.h \ - src/comm/QGCMAVLink.h - -contains(DEPENDENCIES_PRESENT, osgearth) { -message("Including headers for OSGEARTH") -# Enable only if OpenSceneGraph is available -HEADERS += src/ui/map3D/Q3DWidget.h \ - src/ui/map3D/PixhawkCheetahGeode.h \ - src/ui/map3D/Pixhawk3DWidget.h \ - src/ui/map3D/Q3DWidgetFactory.h \ - src/ui/map3D/GCManipulator.h + src/comm/QGCMAVLink.h \ + src/ui/map3D/ImageWindowGeode.h +contains(DEPENDENCIES_PRESENT, osg) { + message("Including headers for OpenSceneGraph") + + # Enable only if OpenSceneGraph is available + HEADERS += src/ui/map3D/Q3DWidget.h \ + src/ui/map3D/PixhawkCheetahGeode.h \ + src/ui/map3D/Pixhawk3DWidget.h \ + src/ui/map3D/Q3DWidgetFactory.h \ + src/ui/map3D/GCManipulator.h +} +contains(DEPENDENCIES_PRESENT, libfreenect) { + message("Including headers for libfreenect") + + # Enable only if libfreenect is available + HEADERS += src/input/Freenect.h } - SOURCES += src/main.cc \ src/Core.cc \ src/uas/UASManager.cc \ @@ -309,17 +314,23 @@ SOURCES += src/main.cc \ src/ui/RadioCalibration/SwitchCalibrator.cc \ src/ui/RadioCalibration/CurveCalibrator.cc \ src/ui/RadioCalibration/AbstractCalibrator.cc \ - src/ui/RadioCalibration/RadioCalibrationData.cc \ - #src/ui/WaypointGlobalView.cc \ - -contains(DEPENDENCIES_PRESENT, osgearth) { -message("Including sources for OSGEARTH") -# Enable only if OpenSceneGraph is available -SOURCES += src/ui/map3D/Q3DWidget.cc \ - src/ui/map3D/PixhawkCheetahGeode.cc \ - src/ui/map3D/Pixhawk3DWidget.cc \ - src/ui/map3D/Q3DWidgetFactory.cc \ - src/ui/map3D/GCManipulator.cc + src/ui/RadioCalibration/RadioCalibrationData.cc \ # src/ui/WaypointGlobalView.cc \ + src/ui/map3D/ImageWindowGeode.cc +contains(DEPENDENCIES_PRESENT, osg) { + message("Including headers for OpenSceneGraph") + + # Enable only if OpenSceneGraph is available + SOURCES += src/ui/map3D/Q3DWidget.cc \ + src/ui/map3D/PixhawkCheetahGeode.cc \ + src/ui/map3D/Pixhawk3DWidget.cc \ + src/ui/map3D/Q3DWidgetFactory.cc \ + src/ui/map3D/GCManipulator.cc +} +contains(DEPENDENCIES_PRESENT, libfreenect) { + message("Including headers for libfreenect") + + # Enable only if libfreenect is available + SOURCES += src/input/Freenect.cc } RESOURCES += mavground.qrc diff --git a/src/input/Freenect.cc b/src/input/Freenect.cc new file mode 100644 index 0000000..af6269c --- /dev/null +++ b/src/input/Freenect.cc @@ -0,0 +1,170 @@ +#include "Freenect.h" + +#include +#include + +Freenect::Freenect() + : context(NULL) + , device(NULL) + , tiltAngle(0) +{ + +} + +Freenect::~Freenect() +{ + if (device != NULL) + { + freenect_stop_depth(device); + freenect_stop_rgb(device); + } + + freenect_close_device(device); + + freenect_shutdown(context); +} + +bool +Freenect::init(int userDeviceNumber) +{ + if (freenect_init(&context, NULL) < 0) + { + return false; + } + + freenect_set_log_level(context, FREENECT_LOG_DEBUG); + + if (freenect_num_devices(context) < 1) + { + return false; + } + + if (freenect_open_device(context, &device, userDeviceNumber) < 0) + { + return false; + } + + freenect_set_user(device, this); + + memset(rgb, 0, FREENECT_RGB_SIZE); + memset(depth, 0, FREENECT_DEPTH_SIZE); + + // set Kinect parameters + if (freenect_set_tilt_degs(device, tiltAngle) != 0) + { + return false; + } + if (freenect_set_led(device, LED_RED) != 0) + { + return false; + } + if (freenect_set_rgb_format(device, FREENECT_FORMAT_RGB) != 0) + { + return false; + } + if (freenect_set_depth_format(device, FREENECT_FORMAT_11_BIT) != 0) + { + return false; + } + freenect_set_rgb_callback(device, rgbCallback); + freenect_set_depth_callback(device, depthCallback); + + if (freenect_start_rgb(device) != 0) + { + return false; + } + if (freenect_start_depth(device) != 0) + { + return false; + } + + thread.reset(new FreenectThread(device)); + thread->start(); + + return true; +} + +bool +Freenect::process(void) +{ + if (freenect_process_events(context) < 0) + { + return false; + } + + freenect_get_raw_accel(device, &ax, &ay, &az); + freenect_get_mks_accel(device, &dx, &dy, &dz); + + return true; +} + +QSharedPointer +Freenect::getRgbData(void) +{ + QMutexLocker locker(&rgbMutex); + return QSharedPointer(new QByteArray(rgb, FREENECT_RGB_SIZE)); +} + +QSharedPointer +Freenect::getDepthData(void) +{ + QMutexLocker locker(&depthMutex); + return QSharedPointer(new QByteArray(depth, FREENECT_DEPTH_SIZE)); +} + +int +Freenect::getTiltAngle(void) const +{ + return tiltAngle; +} + +void +Freenect::setTiltAngle(int angle) +{ + if (angle > 30) + { + angle = 30; + } + if (angle < -30) + { + angle = -30; + } + + tiltAngle = angle; +} + +Freenect::FreenectThread::FreenectThread(freenect_device* _device) +{ + device = _device; +} + +void +Freenect::FreenectThread::run(void) +{ + Freenect* freenect = static_cast(freenect_get_user(device)); + while (1) + { + freenect->process(); + } +} + +void +Freenect::rgbCallback(freenect_device* device, freenect_pixel* rgb, + unsigned int timestamp) +{ + Freenect* freenect = static_cast(freenect_get_user(device)); + + QMutexLocker locker(&freenect->rgbMutex); + memcpy(freenect->rgb, rgb, FREENECT_RGB_SIZE); +} + +void +Freenect::depthCallback(freenect_device* device, void* depth, + unsigned int timestamp) +{ + Freenect* freenect = static_cast(freenect_get_user(device)); + freenect_depth* data = reinterpret_cast(depth); + + QMutexLocker locker(&freenect->depthMutex); + memcpy(freenect->depth, data, FREENECT_DEPTH_SIZE); +} diff --git a/src/input/Freenect.h b/src/input/Freenect.h new file mode 100644 index 0000000..91b401c --- /dev/null +++ b/src/input/Freenect.h @@ -0,0 +1,61 @@ +#ifndef FREENECT_H +#define FREENECT_H + +#include +#include +#include +#include +#include + +class Freenect +{ +public: + Freenect(); + ~Freenect(); + + bool init(int userDeviceNumber = 0); + bool process(void); + + QSharedPointer getRgbData(void); + QSharedPointer getDepthData(void); + + int getTiltAngle(void) const; + void setTiltAngle(int angle); + +private: + static void rgbCallback(freenect_device* device, freenect_pixel* rgb, + unsigned int timestamp); + static void depthCallback(freenect_device* device, void* depth, + unsigned int timestamp); + + freenect_context* context; + freenect_device* device; + + class FreenectThread : public QThread + { + public: + explicit FreenectThread(freenect_device* _device); + + protected: + virtual void run(void); + + freenect_device* device; + }; + QScopedPointer thread; + + // tilt angle of Kinect camera + int tiltAngle; + + // rgbd data + char rgb[FREENECT_RGB_SIZE]; + QMutex rgbMutex; + + char depth[FREENECT_DEPTH_SIZE]; + QMutex depthMutex; + + // accelerometer data + short ax, ay, az; + double dx, dy, dz; +}; + +#endif // FREENECT_H diff --git a/src/ui/map3D/ImageWindowGeode.cc b/src/ui/map3D/ImageWindowGeode.cc new file mode 100644 index 0000000..83bdb77 --- /dev/null +++ b/src/ui/map3D/ImageWindowGeode.cc @@ -0,0 +1,115 @@ +///*===================================================================== +// +//QGroundControl Open Source Ground Control Station +// +//(c) 2009, 2010 QGROUNDCONTROL PROJECT +// +//This file is part of the QGROUNDCONTROL project +// +// QGROUNDCONTROL is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// QGROUNDCONTROL is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with QGROUNDCONTROL. If not, see . +// +//======================================================================*/ + +/** + * @file + * @brief Definition of the class ImageWindowGeode. + * + * @author Lionel Heng + * + */ + +#include "ImageWindowGeode.h" + +ImageWindowGeode::ImageWindowGeode(const QString& caption, + const osg::Vec4& backgroundColor, + osg::ref_ptr& image) + : border(5) +{ + // image + osg::ref_ptr imageGeometry = new osg::Geometry; + imageVertices = new osg::Vec3Array(4); + + osg::ref_ptr textureCoords = new osg::Vec2Array; + textureCoords->push_back(osg::Vec2(0.0f, 1.0f)); + textureCoords->push_back(osg::Vec2(1.0f, 1.0f)); + textureCoords->push_back(osg::Vec2(1.0f, 0.0f)); + textureCoords->push_back(osg::Vec2(0.0f, 0.0f)); + + osg::ref_ptr imageColors(new osg::Vec4Array); + imageColors->push_back(osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f)); + imageGeometry->setColorArray(imageColors); + imageGeometry->setColorBinding(osg::Geometry::BIND_OVERALL); + + imageGeometry->setVertexArray(imageVertices); + imageGeometry->setTexCoordArray(0, textureCoords); + + imageGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON, + 0, imageVertices->size())); + + osg::ref_ptr texture = new osg::Texture2D; + texture->setDataVariance(osg::Object::DYNAMIC); + texture->setImage(image); + texture->setResizeNonPowerOfTwoHint(false); + + imageGeometry->getOrCreateStateSet()-> + setTextureAttributeAndModes(0, texture, osg::StateAttribute::ON); + imageGeometry->setUseDisplayList(false); + + // background + osg::ref_ptr backgroundGeometry = new osg::Geometry; + backgroundVertices = new osg::Vec3Array(4); + backgroundGeometry->setVertexArray(backgroundVertices); + backgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON, + 0, backgroundVertices->size())); + osg::ref_ptr backgroundColors(new osg::Vec4Array); + backgroundColors->push_back(backgroundColor); + backgroundGeometry->setColorArray(backgroundColors); + backgroundGeometry->setColorBinding(osg::Geometry::BIND_OVERALL); + backgroundGeometry->setUseDisplayList(false); + + // caption + text = new osgText::Text; + text->setText(caption.toStdString().c_str()); + text->setCharacterSize(11); + text->setFont("images/Vera.ttf"); + text->setAxisAlignment(osgText::Text::SCREEN); + text->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f)); + + addDrawable(imageGeometry); + addDrawable(backgroundGeometry); + addDrawable(text); + + setAttributes(0, 0, 0, 0); +} + +void +ImageWindowGeode::setAttributes(int x, int y, int width, int height) +{ + int imageWidth = width - border * 2; + int imageHeight = height - border * 2 - 15; + int imageXPosition = x + border; + int imageYPosition = y + border; + + imageVertices->at(0) = osg::Vec3(imageXPosition, imageYPosition, 0); + imageVertices->at(1) = osg::Vec3(imageXPosition + imageWidth, imageYPosition, 0); + imageVertices->at(2) = osg::Vec3(imageXPosition + imageWidth, imageYPosition + imageHeight, 0); + imageVertices->at(3) = osg::Vec3(imageXPosition, imageYPosition + imageHeight, 0); + + text->setPosition(osg::Vec3(imageXPosition, imageYPosition + imageHeight + 5, 0)); + + backgroundVertices->at(0) = osg::Vec3(x, y, -1); + backgroundVertices->at(1) = osg::Vec3(x + width, y, -1); + backgroundVertices->at(2) = osg::Vec3(x + width, y + height, -1); + backgroundVertices->at(3) = osg::Vec3(x, y + height, -1); +} diff --git a/src/ui/map3D/ImageWindowGeode.h b/src/ui/map3D/ImageWindowGeode.h new file mode 100644 index 0000000..87281d8 --- /dev/null +++ b/src/ui/map3D/ImageWindowGeode.h @@ -0,0 +1,56 @@ +///*===================================================================== +// +//QGroundControl Open Source Ground Control Station +// +//(c) 2009, 2010 QGROUNDCONTROL PROJECT +// +//This file is part of the QGROUNDCONTROL project +// +// QGROUNDCONTROL is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// QGROUNDCONTROL is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with QGROUNDCONTROL. If not, see . +// +//======================================================================*/ + +/** + * @file + * @brief Definition of the class ImageWindowGeode. + * + * @author Lionel Heng + * + */ + +#ifndef IMAGEWINDOWGEODE_H +#define IMAGEWINDOWGEODE_H + +#include +#include +#include +#include + +class ImageWindowGeode : public osg::Geode +{ +public: + ImageWindowGeode(const QString& caption, const osg::Vec4& backgroundColor, + osg::ref_ptr& image); + + void setAttributes(int x, int y, int width, int height); + +private: + int border; + + osg::ref_ptr imageVertices; + osg::ref_ptr backgroundVertices; + osg::ref_ptr text; +}; + +#endif // IMAGEWINDOWGEODE_H diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index ccf568c..9c677ae 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -34,6 +34,7 @@ #include #include +#include #include #include @@ -49,6 +50,8 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent) , displayTrail(false) , displayTarget(false) , displayWaypoints(true) + , displayRGBD2D(false) + , displayRGBD3D(false) , followCamera(true) , lastRobotX(0.0f) , lastRobotY(0.0f) @@ -81,6 +84,15 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent) waypointsNode = createWaypoints(); rollingMap->addChild(waypointsNode); +#ifdef QGC_LIBFREENECT_ENABLED + // generate RGBD model + freenect.reset(new Freenect()); + freenect->init(); + + rgbdNode = createRGBD(); + egocentricMap->addChild(rgbdNode); +#endif + setupHUD(); buildLayout(); @@ -273,16 +285,21 @@ Pixhawk3DWidget::display(void) robotPitch, osg::Vec3f(1.0f, 0.0f, 0.0f), robotRoll, osg::Vec3f(0.0f, 1.0f, 0.0f))); - updateHUD(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw); updateTrail(robotX, robotY, robotZ); updateTarget(); updateWaypoints(); +#ifdef QGC_LIBFREENECT_ENABLED + updateRGBD(); +#endif + updateHUD(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw); // set node visibility rollingMap->setChildValue(gridNode, displayGrid); rollingMap->setChildValue(trailNode, displayTrail); rollingMap->setChildValue(targetNode, displayTarget); rollingMap->setChildValue(waypointsNode, displayWaypoints); + hudGroup->setChildValue(rgb2DGeode, displayRGBD2D); + hudGroup->setChildValue(depth2DGeode, displayRGBD2D); lastRobotX = robotX; lastRobotY = robotY; @@ -290,6 +307,22 @@ Pixhawk3DWidget::display(void) } void +Pixhawk3DWidget::keyPressEvent(QKeyEvent* event) +{ + if (!event->text().isEmpty()) + { + switch (*(event->text().toAscii().data())) + { + case '1': + displayRGBD2D = !displayRGBD2D; + break; + } + } + + Q3DWidget::keyPressEvent(event); +} + +void Pixhawk3DWidget::mousePressEvent(QMouseEvent* event) { if (event->button() == Qt::LeftButton && targetButton->isChecked()) @@ -425,72 +458,106 @@ Pixhawk3DWidget::createWaypoints(void) return group; } +#ifdef QGC_LIBFREENECT_ENABLED +osg::ref_ptr +Pixhawk3DWidget::createRGBD(void) +{ + return osg::ref_ptr(new osg::Geode); +} +#endif + void Pixhawk3DWidget::setupHUD(void) { - osg::ref_ptr hudBackgroundVertices(new osg::Vec3Array); - hudBackgroundVertices->push_back(osg::Vec3(0, height(), -1)); - hudBackgroundVertices->push_back(osg::Vec3(width(), height(), -1)); - hudBackgroundVertices->push_back(osg::Vec3(width(), height() - 30, -1)); - hudBackgroundVertices->push_back(osg::Vec3(0, height() - 30, -1)); - hudBackgroundVertices->push_back(osg::Vec3(0, 0, -1)); - hudBackgroundVertices->push_back(osg::Vec3(width(), 0, -1)); - hudBackgroundVertices->push_back(osg::Vec3(width(), 25, -1)); - hudBackgroundVertices->push_back(osg::Vec3(0, 25, -1)); - - osg::ref_ptr hudTopBackgroundIndices( - new osg::DrawElementsUInt(osg::PrimitiveSet::POLYGON, 0)); - hudTopBackgroundIndices->push_back(0); - hudTopBackgroundIndices->push_back(1); - hudTopBackgroundIndices->push_back(2); - hudTopBackgroundIndices->push_back(3); - - osg::ref_ptr hudBottomBackgroundIndices( - new osg::DrawElementsUInt(osg::PrimitiveSet::POLYGON, 0)); - hudBottomBackgroundIndices->push_back(4); - hudBottomBackgroundIndices->push_back(5); - hudBottomBackgroundIndices->push_back(6); - hudBottomBackgroundIndices->push_back(7); - osg::ref_ptr hudColors(new osg::Vec4Array); hudColors->push_back(osg::Vec4(0.0f, 0.0f, 0.0f, 0.2f)); hudBackgroundGeometry = new osg::Geometry; - hudBackgroundGeometry->addPrimitiveSet(hudTopBackgroundIndices); - hudBackgroundGeometry->addPrimitiveSet(hudBottomBackgroundIndices); - hudBackgroundGeometry->setVertexArray(hudBackgroundVertices); + hudBackgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON, + 0, 4)); + hudBackgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON, + 4, 4)); hudBackgroundGeometry->setColorArray(hudColors); hudBackgroundGeometry->setColorBinding(osg::Geometry::BIND_OVERALL); - - hudGeode->addDrawable(hudBackgroundGeometry); + hudBackgroundGeometry->setUseDisplayList(false); statusText = new osgText::Text; statusText->setCharacterSize(11); statusText->setFont("images/Vera.ttf"); statusText->setAxisAlignment(osgText::Text::SCREEN); - statusText->setPosition(osg::Vec3(10, height() - 10, -1.5)); statusText->setColor(osg::Vec4(255, 255, 255, 1)); - hudGeode->addDrawable(statusText); + resizeHUD(); + + osg::ref_ptr statusGeode = new osg::Geode; + statusGeode->addDrawable(hudBackgroundGeometry); + statusGeode->addDrawable(statusText); + hudGroup->addChild(statusGeode); + + rgbImage = new osg::Image; + rgb2DGeode = new ImageWindowGeode("RGB Image", + osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f), + rgbImage); + hudGroup->addChild(rgb2DGeode); + + depthImage = new osg::Image; + depthImage->allocateImage(640, 480, 1, GL_RGB, GL_UNSIGNED_BYTE); + depth2DGeode = new ImageWindowGeode("Depth Image", + osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f), + depthImage); + hudGroup->addChild(depth2DGeode); + + for (int i = 0; i < 2048; ++i) + { + float v = static_cast(i) / 2048.0f; + v = powf(v, 3.0f) * 6.0f; + gammaLookup[i] = static_cast(v * 6.0f * 256.0f); + } } void -Pixhawk3DWidget::updateHUD(float robotX, float robotY, float robotZ, - float robotRoll, float robotPitch, float robotYaw) +Pixhawk3DWidget::resizeHUD(void) { - osg::ref_ptr hudBackgroundVertices(new osg::Vec3Array); - hudBackgroundVertices->push_back(osg::Vec3(0, height(), -1)); - hudBackgroundVertices->push_back(osg::Vec3(width(), height(), -1)); - hudBackgroundVertices->push_back(osg::Vec3(width(), height() - 30, -1)); - hudBackgroundVertices->push_back(osg::Vec3(0, height() - 30, -1)); - hudBackgroundVertices->push_back(osg::Vec3(0, 0, -1)); - hudBackgroundVertices->push_back(osg::Vec3(width(), 0, -1)); - hudBackgroundVertices->push_back(osg::Vec3(width(), 25, -1)); - hudBackgroundVertices->push_back(osg::Vec3(0, 25, -1)); - hudBackgroundGeometry->setVertexArray(hudBackgroundVertices); + int topHUDHeight = 30; + int bottomHUDHeight = 25; + + osg::Vec3Array* vertices = static_cast(hudBackgroundGeometry->getVertexArray()); + if (vertices == NULL || vertices->size() != 8) + { + osg::ref_ptr newVertices = new osg::Vec3Array(8); + hudBackgroundGeometry->setVertexArray(newVertices); + + vertices = static_cast(hudBackgroundGeometry->getVertexArray()); + } + + (*vertices)[0] = osg::Vec3(0, height(), -1); + (*vertices)[1] = osg::Vec3(width(), height(), -1); + (*vertices)[2] = osg::Vec3(width(), height() - topHUDHeight, -1); + (*vertices)[3] = osg::Vec3(0, height() - topHUDHeight, -1); + (*vertices)[4] = osg::Vec3(0, 0, -1); + (*vertices)[5] = osg::Vec3(width(), 0, -1); + (*vertices)[6] = osg::Vec3(width(), bottomHUDHeight, -1); + (*vertices)[7] = osg::Vec3(0, bottomHUDHeight, -1); statusText->setPosition(osg::Vec3(10, height() - 20, -1.5)); + if (rgb2DGeode.valid() && depth2DGeode.valid()) + { + int windowWidth = (width() - 20) / 2; + int windowHeight = 3 * windowWidth / 4; + rgb2DGeode->setAttributes(10, (height() - windowHeight) / 2, + windowWidth, windowHeight); + depth2DGeode->setAttributes(width() / 2, (height() - windowHeight) / 2, + windowWidth, windowHeight); + } +} + +void +Pixhawk3DWidget::updateHUD(float robotX, float robotY, float robotZ, + float robotRoll, float robotPitch, float robotYaw) +{ + resizeHUD(); + std::pair cursorPosition = getGlobalCursorPosition(getMouseX(), getMouseY(), -robotZ); @@ -506,6 +573,62 @@ Pixhawk3DWidget::updateHUD(float robotX, float robotY, float robotZ, " Cursor [" << cursorPosition.first << " " << cursorPosition.second << "]"; statusText->setText(oss.str()); + + if (!rgb.isNull()) + { + rgbImage->setImage(640, 480, 1, + GL_RGB, GL_RGB, GL_UNSIGNED_BYTE, + reinterpret_cast(rgb->data()), + osg::Image::NO_DELETE); + rgbImage->dirty(); + + unsigned short* src = reinterpret_cast(depth->data()); + unsigned char* dst = depthImage->data(); + for (int i = 0; i < depthImage->s() * depthImage->t(); ++i) + { + unsigned short pval = gammaLookup[src[i]]; + unsigned short lb = pval & 0xFF; + switch (pval >> 8) + { + case 0: + dst[3 * i] = 255; + dst[3 * i + 1] = 255 - lb; + dst[3 * i + 2] = 255 - lb; + break; + case 1: + dst[3 * i] = 255; + dst[3 * i + 1] = lb; + dst[3 * i + 2] = 0; + break; + case 2: + dst[3 * i] = 255 - lb; + dst[3 * i + 1] = 255; + dst[3 * i + 2] = 0; + break; + case 3: + dst[3 * i] = 0; + dst[3 * i + 1] = 255; + dst[3 * i + 2] = lb; + break; + case 4: + dst[3 * i] = 0; + dst[3 * i + 1] = 255 - lb; + dst[3 * i + 2] = 255; + break; + case 5: + dst[3 * i] = 0; + dst[3 * i + 1] = 0; + dst[3 * i + 2] = 255 - lb; + break; + default: + dst[3 * i] = 0; + dst[3 * i + 1] = 0; + dst[3 * i + 2] = 0; + break; + } + } + depthImage->dirty(); + } } void @@ -641,6 +764,14 @@ Pixhawk3DWidget::updateWaypoints(void) } } +#ifdef QGC_LIBFREENECT_ENABLED +void +Pixhawk3DWidget::updateRGBD(void) +{ + rgb = freenect->getRgbData(); + depth = freenect->getDepthData(); +} +#endif void Pixhawk3DWidget::markTarget(void) diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h index 400a414..47b1bca 100644 --- a/src/ui/map3D/Pixhawk3DWidget.h +++ b/src/ui/map3D/Pixhawk3DWidget.h @@ -33,11 +33,17 @@ #define PIXHAWK3DWIDGET_H #include - #ifdef QGC_OSGEARTH_ENABLED #include #endif +#ifdef QGC_OSG_ENABLED +#include "ImageWindowGeode.h" +#endif +#ifdef QGC_LIBFREENECT_ENABLED +#include "Freenect.h" +#endif + #include "Q3DWidget.h" class UASInterface; @@ -69,6 +75,7 @@ private slots: protected: virtual void display(void); + virtual void keyPressEvent(QKeyEvent* event); virtual void mousePressEvent(QMouseEvent* event); UASInterface* uas; @@ -84,13 +91,21 @@ private: osg::ref_ptr createTarget(void); osg::ref_ptr createWaypoints(void); +#ifdef QGC_LIBFREENECT_ENABLED + osg::ref_ptr createRGBD(void); +#endif + void setupHUD(void); + void resizeHUD(void); void updateHUD(float robotX, float robotY, float robotZ, float robotRoll, float robotPitch, float robotYaw); void updateTrail(float robotX, float robotY, float robotZ); void updateTarget(void); void updateWaypoints(void); +#ifdef QGC_LIBFREENECT_ENABLED + void updateRGBD(void); +#endif void markTarget(void); @@ -98,6 +113,8 @@ private: bool displayTrail; bool displayTarget; bool displayWaypoints; + bool displayRGBD2D; + bool displayRGBD3D; bool followCamera; @@ -106,6 +123,10 @@ private: osg::ref_ptr hudBackgroundGeometry; osg::ref_ptr statusText; + osg::ref_ptr rgb2DGeode; + osg::ref_ptr depth2DGeode; + osg::ref_ptr rgbImage; + osg::ref_ptr depthImage; osg::ref_ptr gridNode; osg::ref_ptr trailNode; osg::ref_ptr trailGeometry; @@ -116,6 +137,13 @@ private: osg::ref_ptr targetNode; osg::ref_ptr targetPosition; osg::ref_ptr waypointsNode; +#ifdef QGC_LIBFREENECT_ENABLED + osg::ref_ptr rgbdNode; + QScopedPointer freenect; +#endif + QSharedPointer rgb; + QSharedPointer depth; + unsigned short gammaLookup[2048]; QPushButton* targetButton; diff --git a/src/ui/map3D/Q3DWidget.cc b/src/ui/map3D/Q3DWidget.cc index 6a89afb..893d5d8 100644 --- a/src/ui/map3D/Q3DWidget.cc +++ b/src/ui/map3D/Q3DWidget.cc @@ -47,7 +47,7 @@ Q3DWidget::Q3DWidget(QWidget* parent) , egocentricMap(new osg::Switch()) , robotPosition(new osg::PositionAttitudeTransform()) , robotAttitude(new osg::PositionAttitudeTransform()) - , hudGeode(new osg::Geode()) + , hudGroup(new osg::Switch()) , hudProjectionMatrix(new osg::Projection) { // set initial camera parameters @@ -159,11 +159,12 @@ Q3DWidget::createHUD(void) hudModelViewMatrix->setReferenceFrame(osg::Transform::ABSOLUTE_RF); hudProjectionMatrix->addChild(hudModelViewMatrix); - hudModelViewMatrix->addChild(hudGeode); + hudModelViewMatrix->addChild(hudGroup); osg::ref_ptr hudStateSet(new osg::StateSet); - hudGeode->setStateSet(hudStateSet); + hudGroup->setStateSet(hudStateSet); hudStateSet->setMode(GL_DEPTH_TEST, osg::StateAttribute::OFF); + hudStateSet->setMode(GL_LIGHTING, osg::StateAttribute::OFF); hudStateSet->setRenderingHint(osg::StateSet::TRANSPARENT_BIN); hudStateSet->setRenderBinDetails(11, "RenderBin"); diff --git a/src/ui/map3D/Q3DWidget.h b/src/ui/map3D/Q3DWidget.h index 40c13ac..0dcd8d9 100644 --- a/src/ui/map3D/Q3DWidget.h +++ b/src/ui/map3D/Q3DWidget.h @@ -249,7 +249,7 @@ protected: osg::ref_ptr robotPosition; osg::ref_ptr robotAttitude; - osg::ref_ptr hudGeode; /**< A geode which contains renderable HUD objects. */ + osg::ref_ptr hudGroup; /**< A group which contains renderable HUD objects. */ osg::ref_ptr hudProjectionMatrix; /**< An orthographic projection matrix for HUD display. */ osg::ref_ptr osgGW; /**< A class which manages OSG graphics windows and events. */