Commit 73be7dcf authored by hengli's avatar hengli

Added 2D visualization of Kinect data. 3D visualization to follow soon.

parent 90c7131c
......@@ -74,16 +74,29 @@ 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")
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
LIBS += -L/opt/local/lib/
INCLUDEPATH += /opt/local/include
# Include OpenSceneGraph and osgEarth libraries
LIBS += -losg \
-losgViewer \
# Include osgEarth libraries
LIBS += -losgViewer \
-losgEarth
DEFINES += QGC_OSG_ENABLED
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
}
}
......@@ -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 \
exists(/usr/include/osgEarth) {
message("Building support for osgEarth")
DEPENDENCIES_PRESENT += osgearth
# Include osgEarth libraries
LIBS += -losgViewer \
-losgEarth
DEFINES += QGC_OSGEARTH_ENABLED
}
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 \
exists(/usr/include/osgEarth) {
message("Building support for osgEarth")
DEPENDENCIES_PRESENT += osgearth
# Include osgEarth libraries
LIBS += -losgViewer \
-losgEarth
DEFINES += QGC_OSGEARTH_ENABLED
}
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
}
}
......
......@@ -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 -----")
......@@ -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
src/comm/QGCMAVLink.h \
src/ui/map3D/ImageWindowGeode.h
contains(DEPENDENCIES_PRESENT, osg) {
message("Including headers for OpenSceneGraph")
contains(DEPENDENCIES_PRESENT, osgearth) {
message("Including headers for OSGEARTH")
# Enable only if OpenSceneGraph is available
HEADERS += src/ui/map3D/Q3DWidget.h \
# 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,18 +314,24 @@ 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 \
src/ui/RadioCalibration/RadioCalibrationData.cc \ # src/ui/WaypointGlobalView.cc \
src/ui/map3D/ImageWindowGeode.cc
contains(DEPENDENCIES_PRESENT, osg) {
message("Including headers for OpenSceneGraph")
contains(DEPENDENCIES_PRESENT, osgearth) {
message("Including sources for OSGEARTH")
# Enable only if OpenSceneGraph is available
SOURCES += src/ui/map3D/Q3DWidget.cc \
# 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
# Include RT-LAB Library
......
#include "Freenect.h"
#include <string.h>
#include <QDebug>
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<QByteArray>
Freenect::getRgbData(void)
{
QMutexLocker locker(&rgbMutex);
return QSharedPointer<QByteArray>(new QByteArray(rgb, FREENECT_RGB_SIZE));
}
QSharedPointer<QByteArray>
Freenect::getDepthData(void)
{
QMutexLocker locker(&depthMutex);
return QSharedPointer<QByteArray>(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 *>(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 *>(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 *>(freenect_get_user(device));
freenect_depth* data = reinterpret_cast<freenect_depth *>(depth);
QMutexLocker locker(&freenect->depthMutex);
memcpy(freenect->depth, data, FREENECT_DEPTH_SIZE);
}
#ifndef FREENECT_H
#define FREENECT_H
#include <libfreenect.h>
#include <QMutex>
#include <QScopedPointer>
#include <QSharedPointer>
#include <QThread>
class Freenect
{
public:
Freenect();
~Freenect();
bool init(int userDeviceNumber = 0);
bool process(void);
QSharedPointer<QByteArray> getRgbData(void);
QSharedPointer<QByteArray> 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<FreenectThread> 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
///*=====================================================================
//
//QGroundControl Open Source Ground Control Station
//
//(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
//
//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 <http://www.gnu.org/licenses/>.
//
//======================================================================*/
/**
* @file
* @brief Definition of the class ImageWindowGeode.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
*/
#include "ImageWindowGeode.h"
ImageWindowGeode::ImageWindowGeode(const QString& caption,
const osg::Vec4& backgroundColor,
osg::ref_ptr<osg::Image>& image)
: border(5)
{
// image
osg::ref_ptr<osg::Geometry> imageGeometry = new osg::Geometry;
imageVertices = new osg::Vec3Array(4);
osg::ref_ptr<osg::Vec2Array> 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<osg::Vec4Array> 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<osg::Texture2D> 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<osg::Geometry> 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<osg::Vec4Array> 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);
}
///*=====================================================================
//
//QGroundControl Open Source Ground Control Station
//
//(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
//
//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 <http://www.gnu.org/licenses/>.
//
//======================================================================*/
/**
* @file
* @brief Definition of the class ImageWindowGeode.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
*/
#ifndef IMAGEWINDOWGEODE_H
#define IMAGEWINDOWGEODE_H
#include <osg/Geode>
#include <osg/Geometry>
#include <osgText/Text>
#include <QString>
class ImageWindowGeode : public osg::Geode
{
public:
ImageWindowGeode(const QString& caption, const osg::Vec4& backgroundColor,
osg::ref_ptr<osg::Image>& image);
void setAttributes(int x, int y, int width, int height);
private:
int border;
osg::ref_ptr<osg::Vec3Array> imageVertices;
osg::ref_ptr<osg::Vec3Array> backgroundVertices;
osg::ref_ptr<osgText::Text> text;
};
#endif // IMAGEWINDOWGEODE_H
This diff is collapsed.
......@@ -33,11 +33,17 @@
#define PIXHAWK3DWIDGET_H
#include <osgText/Text>
#ifdef QGC_OSGEARTH_ENABLED
#include <osgEarth/MapNode>
#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<osg::Node> createTarget(void);
osg::ref_ptr<osg::Group> createWaypoints(void);
#ifdef QGC_LIBFREENECT_ENABLED
osg::ref_ptr<osg::Geode> 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<osg::Geometry> hudBackgroundGeometry;
osg::ref_ptr<osgText::Text> statusText;
osg::ref_ptr<ImageWindowGeode> rgb2DGeode;
osg::ref_ptr<ImageWindowGeode> depth2DGeode;
osg::ref_ptr<osg::Image> rgbImage;
osg::ref_ptr<osg::Image> depthImage;
osg::ref_ptr<osg::Geode> gridNode;
osg::ref_ptr<osg::Geode> trailNode;
osg::ref_ptr<osg::Geometry> trailGeometry;
......@@ -116,6 +137,13 @@ private:
osg::ref_ptr<osg::Geode> targetNode;
osg::ref_ptr<osg::PositionAttitudeTransform> targetPosition;
osg::ref_ptr<osg::Group> waypointsNode;
#ifdef QGC_LIBFREENECT_ENABLED
osg::ref_ptr<osg::Geode> rgbdNode;
QScopedPointer<Freenect> freenect;
#endif
QSharedPointer<QByteArray> rgb;
QSharedPointer<QByteArray> depth;
unsigned short gammaLookup[2048];
QPushButton* targetButton;
......
......@@ -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<osg::StateSet> 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");
......
......@@ -249,7 +249,7 @@ protected:
osg::ref_ptr<osg::PositionAttitudeTransform> robotPosition;
osg::ref_ptr<osg::PositionAttitudeTransform> robotAttitude;
osg::ref_ptr<osg::Geode> hudGeode; /**< A geode which contains renderable HUD objects. */
osg::ref_ptr<osg::Switch> hudGroup; /**< A group which contains renderable HUD objects. */
osg::ref_ptr<osg::Projection> hudProjectionMatrix; /**< An orthographic projection matrix for HUD display. */