Skip to content

Q
qgroundcontrol
Commit 880879cb authored by pixhawk's avatar pixhawk
Browse files
Options

Added widgets to display RGBD data.

parent 3122d2ca
Hide whitespace changes
Inline Side-by-side
Showing with 382 additions and 40 deletions
qgroundcontrol.pri
View file @ 880879cb
......@@ -142,7 +142,7 @@ linux-g++ {
#CONFIG -= console
}
QMAKE_POST_LINK += cp -rf $$BASEDIR/audio $$DESTDIR/.
#QMAKE_POST_LINK += cp -rf $$BASEDIR/audio $$DESTDIR/.
message("Compiling for linux 32")
......@@ -172,10 +172,11 @@ message("Compiling for linux 32")
-losgViewer \
-losgGA \
-losgDB \
-losgQt \
-losgText \
-lOpenThreads
#-losgQt \
DEFINES += QGC_OSG_ENABLED
}
......@@ -225,7 +226,7 @@ linux-g++-64 {
#CONFIG -= console
}
QMAKE_POST_LINK += cp -rf $$BASEDIR/audio $$DESTDIR/.
#QMAKE_POST_LINK += cp -rf $$BASEDIR/audio $$DESTDIR/.
INCLUDEPATH += /usr/include \
/usr/include/qt4/phonon
......@@ -253,10 +254,11 @@ linux-g++-64 {
-losgViewer \
-losgGA \
-losgDB \
-losgQt \
-losgText \
-lOpenThreads
# -losgQt \
DEFINES += QGC_OSG_ENABLED
}
......
qgroundcontrol.pro
View file @ 880879cb
......@@ -350,7 +350,8 @@ HEADERS += src/MG.h \
src/ui/WaypointViewOnlyView.h \
src/ui/WaypointViewOnlyView.h \
src/ui/WaypointEditableView.h \
src/ui/UnconnectedUASInfoWidget.h
src/ui/UnconnectedUASInfoWidget.h \
src/ui/QGCRGBDView.h
# Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler
macx|win32-msvc2008|win32-msvc2010::HEADERS += src/ui/map3D/QGCGoogleEarthView.h
......@@ -477,7 +478,8 @@ SOURCES += src/main.cc \
src/ui/MAVLinkDecoder.cc \
src/ui/WaypointViewOnlyView.cc \
src/ui/WaypointEditableView.cc \
src/ui/UnconnectedUASInfoWidget.cc
src/ui/UnconnectedUASInfoWidget.cc \
src/ui/QGCRGBDView.cc
# Enable Google Earth only on Mac OS and Windows with Visual Studio compiler
macx|win32-msvc2008|win32-msvc2010::SOURCES += src/ui/map3D/QGCGoogleEarthView.cc
......
src/comm/MAVLinkProtocol.cc
View file @ 880879cb
......@@ -31,6 +31,7 @@
#include "QGCMAVLinkUASFactory.h"
#include "QGC.h"
/**
* The default constructor will create a new MAVLink object sending heartbeats at
* the MAVLINK_HEARTBEAT_DEFAULT_RATE to all connected links.
......
src/uas/UAS.cc
View file @ 880879cb
......@@ -982,10 +982,12 @@ void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<googl
if (message->GetTypeName() == pointCloud.GetTypeName())
{
pointCloud.CopyFrom(*message);
emit pointCloudChanged(this);
}
else if (message->GetTypeName() == rgbdImage.GetTypeName())
{
rgbdImage.CopyFrom(*message);
emit rgbdImageChanged(this);
}
}
......
src/uas/UAS.h
View file @ 880879cb
......@@ -554,6 +554,10 @@ signals:
void imageStarted(quint64 timestamp);
/** @brief A new camera image has arrived */
void imageReady(UASInterface* uas);
/** @brief Point cloud data has been changed */
void pointCloudChanged(UASInterface* uas);
/** @brief RGBD image data has been changed */
void rgbdImageChanged(UASInterface* uas);
/** @brief HIL controls have changed */
void hilControlsChanged(uint64_t time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, uint8_t systemMode, uint8_t navMode);
......
src/ui/HUD.cc
View file @ 880879cb
......@@ -661,6 +661,10 @@ void HUD::paintHUD()
nextOfflineImage = "";
}
}
if (dataStreamEnabled || videoEnabled)
{
glRasterPos2i(0, 0);
xImageFactor = width() / (float)glImage.width();
......
src/ui/HUD.h
View file @ 880879cb
......@@ -214,6 +214,7 @@ protected:
QString nextOfflineImage;
bool hudInstrumentsEnabled;
bool videoEnabled;
bool dataStreamEnabled;
float xImageFactor;
float yImageFactor;
QAction* enableHUDAction;
......
src/ui/MainWindow.cc
View file @ 880879cb
......@@ -51,6 +51,7 @@ This file is part of the QGROUNDCONTROL project
#include "QGCSettingsWidget.h"
#include "QGCMapTool.h"
#include "MAVLinkDecoder.h"
#include "QGCRGBDView.h"
#ifdef QGC_OSG_ENABLED
#include "Q3DWidgetFactory.h"
......@@ -459,9 +460,9 @@ void MainWindow::buildCommonWidgets()
if (!video1DockWidget) {
video1DockWidget = new QDockWidget(tr("Video Stream 1"), this);
HUD* video1 = new HUD(160, 120, this);
QGCRGBDView* video1 = new QGCRGBDView(160, 120, this);
video1->enableHUDInstruments(false);
video1->enableVideo(true);
video1->enableVideo(false);
// FIXME select video stream as well
video1DockWidget->setWidget(video1);
video1DockWidget->setObjectName("VIDEO_STREAM_1_DOCK_WIDGET");
......@@ -470,15 +471,37 @@ void MainWindow::buildCommonWidgets()
if (!video2DockWidget) {
video2DockWidget = new QDockWidget(tr("Video Stream 2"), this);
HUD* video2 = new HUD(160, 120, this);
QGCRGBDView* video2 = new QGCRGBDView(160, 120, this);
video2->enableHUDInstruments(false);
video2->enableVideo(true);
video2->enableVideo(false);
// FIXME select video stream as well
video2DockWidget->setWidget(video2);
video2DockWidget->setObjectName("VIDEO_STREAM_2_DOCK_WIDGET");
addTool(video2DockWidget, tr("Video Stream 2"), Qt::LeftDockWidgetArea);
}
// if (!rgbd1DockWidget) {
// rgbd1DockWidget = new QDockWidget(tr("Video Stream 1"), this);
// HUD* video1 = new HUD(160, 120, this);
// video1->enableHUDInstruments(false);
// video1->enableVideo(true);
// // FIXME select video stream as well
// video1DockWidget->setWidget(video1);
// video1DockWidget->setObjectName("VIDEO_STREAM_1_DOCK_WIDGET");
// addTool(video1DockWidget, tr("Video Stream 1"), Qt::LeftDockWidgetArea);
// }
// if (!rgbd2DockWidget) {
// video2DockWidget = new QDockWidget(tr("Video Stream 2"), this);
// HUD* video2 = new HUD(160, 120, this);
// video2->enableHUDInstruments(false);
// video2->enableVideo(true);
// // FIXME select video stream as well
// video2DockWidget->setWidget(video2);
// video2DockWidget->setObjectName("VIDEO_STREAM_2_DOCK_WIDGET");
// addTool(video2DockWidget, tr("Video Stream 2"), Qt::LeftDockWidgetArea);
// }
// Custom widgets, added last to all menus and layouts
buildCustomWidget();
......
src/ui/MainWindow.h
View file @ 880879cb
......@@ -338,6 +338,8 @@ protected:
QPointer<QDockWidget> headUpDockWidget;
QPointer<QDockWidget> video1DockWidget;
QPointer<QDockWidget> video2DockWidget;
QPointer<QDockWidget> rgbd1DockWidget;
QPointer<QDockWidget> rgbd2DockWidget;
QPointer<QDockWidget> logPlayerDockWidget;
QPointer<QDockWidget> hsiDockWidget;
......
src/ui/QGCRGBDView.cc 0 → 100644
View file @ 880879cb
#include <QMenu>
#include <QContextMenuEvent>
#include "QGCRGBDView.h"
#include "UASManager.h"
QGCRGBDView::QGCRGBDView(int width, int height, QWidget *parent) :
HUD(width, height, parent),
rgbEnabled(false),
depthEnabled(false)
{
enableRGBAction = new QAction(tr("Enable RGB Image"), this);
enableRGBAction->setStatusTip(tr("Show the RGB image live stream in this window"));
enableRGBAction->setCheckable(true);
enableRGBAction->setChecked(rgbEnabled);
connect(enableRGBAction, SIGNAL(triggered(bool)), this, SLOT(enableRGB(bool)));
enableDepthAction = new QAction(tr("Enable Depthmap"), this);
enableDepthAction->setStatusTip(tr("Show the Depthmap in this window"));
enableDepthAction->setCheckable(true);
enableDepthAction->setChecked(depthEnabled);
connect(enableDepthAction, SIGNAL(triggered(bool)), this, SLOT(enableDepth(bool)));
connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*)));
}
void QGCRGBDView::addUAS(UASInterface *uas)
{
// TODO Enable multi-uas support
connect(uas, SIGNAL(rgbdImageChanged(UASInterface*)), this, SLOT(updateData(UASInterface*)));
}
void QGCRGBDView::contextMenuEvent (QContextMenuEvent* event)
{
QMenu menu(this);
// Update actions
enableHUDAction->setChecked(hudInstrumentsEnabled);
//enableVideoAction->setChecked(videoEnabled);
enableRGBAction->setChecked(rgbEnabled);
enableDepthAction->setChecked(depthEnabled);
menu.addAction(enableHUDAction);
menu.addAction(enableRGBAction);
menu.addAction(enableDepthAction);
//menu.addAction(selectHUDColorAction);
//menu.addAction(enableVideoAction);
//menu.addAction(selectOfflineDirectoryAction);
//menu.addAction(selectVideoChannelAction);
menu.exec(event->globalPos());
}
void QGCRGBDView::enableRGB(bool enabled)
{
rgbEnabled = enabled;
dataStreamEnabled = rgbEnabled | depthEnabled;
resize(size());
}
void QGCRGBDView::enableDepth(bool enabled)
{
depthEnabled = enabled;
dataStreamEnabled = rgbEnabled | depthEnabled;
resize(size());
}
float colormapJet[128][3] = {
{0.0f,0.0f,0.53125f},
{0.0f,0.0f,0.5625f},
{0.0f,0.0f,0.59375f},
{0.0f,0.0f,0.625f},
{0.0f,0.0f,0.65625f},
{0.0f,0.0f,0.6875f},
{0.0f,0.0f,0.71875f},
{0.0f,0.0f,0.75f},
{0.0f,0.0f,0.78125f},
{0.0f,0.0f,0.8125f},
{0.0f,0.0f,0.84375f},
{0.0f,0.0f,0.875f},
{0.0f,0.0f,0.90625f},
{0.0f,0.0f,0.9375f},
{0.0f,0.0f,0.96875f},
{0.0f,0.0f,1.0f},
{0.0f,0.03125f,1.0f},
{0.0f,0.0625f,1.0f},
{0.0f,0.09375f,1.0f},
{0.0f,0.125f,1.0f},
{0.0f,0.15625f,1.0f},
{0.0f,0.1875f,1.0f},
{0.0f,0.21875f,1.0f},
{0.0f,0.25f,1.0f},
{0.0f,0.28125f,1.0f},
{0.0f,0.3125f,1.0f},
{0.0f,0.34375f,1.0f},
{0.0f,0.375f,1.0f},
{0.0f,0.40625f,1.0f},
{0.0f,0.4375f,1.0f},
{0.0f,0.46875f,1.0f},
{0.0f,0.5f,1.0f},
{0.0f,0.53125f,1.0f},
{0.0f,0.5625f,1.0f},
{0.0f,0.59375f,1.0f},
{0.0f,0.625f,1.0f},
{0.0f,0.65625f,1.0f},
{0.0f,0.6875f,1.0f},
{0.0f,0.71875f,1.0f},
{0.0f,0.75f,1.0f},
{0.0f,0.78125f,1.0f},
{0.0f,0.8125f,1.0f},
{0.0f,0.84375f,1.0f},
{0.0f,0.875f,1.0f},
{0.0f,0.90625f,1.0f},
{0.0f,0.9375f,1.0f},
{0.0f,0.96875f,1.0f},
{0.0f,1.0f,1.0f},
{0.03125f,1.0f,0.96875f},
{0.0625f,1.0f,0.9375f},
{0.09375f,1.0f,0.90625f},
{0.125f,1.0f,0.875f},
{0.15625f,1.0f,0.84375f},
{0.1875f,1.0f,0.8125f},
{0.21875f,1.0f,0.78125f},
{0.25f,1.0f,0.75f},
{0.28125f,1.0f,0.71875f},
{0.3125f,1.0f,0.6875f},
{0.34375f,1.0f,0.65625f},
{0.375f,1.0f,0.625f},
{0.40625f,1.0f,0.59375f},
{0.4375f,1.0f,0.5625f},
{0.46875f,1.0f,0.53125f},
{0.5f,1.0f,0.5f},
{0.53125f,1.0f,0.46875f},
{0.5625f,1.0f,0.4375f},
{0.59375f,1.0f,0.40625f},
{0.625f,1.0f,0.375f},
{0.65625f,1.0f,0.34375f},
{0.6875f,1.0f,0.3125f},
{0.71875f,1.0f,0.28125f},
{0.75f,1.0f,0.25f},
{0.78125f,1.0f,0.21875f},
{0.8125f,1.0f,0.1875f},
{0.84375f,1.0f,0.15625f},
{0.875f,1.0f,0.125f},
{0.90625f,1.0f,0.09375f},
{0.9375f,1.0f,0.0625f},
{0.96875f,1.0f,0.03125f},
{1.0f,1.0f,0.0f},
{1.0f,0.96875f,0.0f},
{1.0f,0.9375f,0.0f},
{1.0f,0.90625f,0.0f},
{1.0f,0.875f,0.0f},
{1.0f,0.84375f,0.0f},
{1.0f,0.8125f,0.0f},
{1.0f,0.78125f,0.0f},
{1.0f,0.75f,0.0f},
{1.0f,0.71875f,0.0f},
{1.0f,0.6875f,0.0f},
{1.0f,0.65625f,0.0f},
{1.0f,0.625f,0.0f},
{1.0f,0.59375f,0.0f},
{1.0f,0.5625f,0.0f},
{1.0f,0.53125f,0.0f},
{1.0f,0.5f,0.0f},
{1.0f,0.46875f,0.0f},
{1.0f,0.4375f,0.0f},
{1.0f,0.40625f,0.0f},
{1.0f,0.375f,0.0f},
{1.0f,0.34375f,0.0f},
{1.0f,0.3125f,0.0f},
{1.0f,0.28125f,0.0f},
{1.0f,0.25f,0.0f},
{1.0f,0.21875f,0.0f},
{1.0f,0.1875f,0.0f},
{1.0f,0.15625f,0.0f},
{1.0f,0.125f,0.0f},
{1.0f,0.09375f,0.0f},
{1.0f,0.0625f,0.0f},
{1.0f,0.03125f,0.0f},
{1.0f,0.0f,0.0f},
{0.96875f,0.0f,0.0f},
{0.9375f,0.0f,0.0f},
{0.90625f,0.0f,0.0f},
{0.875f,0.0f,0.0f},
{0.84375f,0.0f,0.0f},
{0.8125f,0.0f,0.0f},
{0.78125f,0.0f,0.0f},
{0.75f,0.0f,0.0f},
{0.71875f,0.0f,0.0f},
{0.6875f,0.0f,0.0f},
{0.65625f,0.0f,0.0f},
{0.625f,0.0f,0.0f},
{0.59375f,0.0f,0.0f},
{0.5625f,0.0f,0.0f},
{0.53125f,0.0f,0.0f},
{0.5f,0.0f,0.0f}
};
void QGCRGBDView::updateData(UASInterface *uas)
{
#ifdef QGC_PROTOBUF_ENABLED
px::RGBDImage rgbdImage = uas->getRGBDImage();
if (rgbdImage.rows() == 0 || rgbdImage.cols() == 0 || (!rgbEnabled && !depthEnabled))
{
return;
}
QImage fill;
if (rgbEnabled)
{
// fill = QImage(reinterpret_cast<const unsigned char*>(rgbdImage.imagedata1().c_str()),
// rgbdImage.cols(), rgbdImage.rows(), QImage::Format_Mono);
// Construct PGM header
QString header("P5\n%1 %2\n%3\n");
int imgColors = 255;
header = header.arg(rgbdImage.cols()).arg(rgbdImage.rows()).arg(imgColors);
//QByteArray tmpImage(rgbdImage.imagedata1().c_str(), rgbdImage.cols()*rgbdImage.rows());
QByteArray tmpImage(header.toStdString().c_str(), header.toStdString().size());
tmpImage.append(rgbdImage.imagedata1().c_str(), rgbdImage.cols()*rgbdImage.rows());
//qDebug() << "IMAGE SIZE:" << tmpImage.size() << "HEADER SIZE: (15):" << header.size() << "HEADER: " << header;
// if (imageRecBuffer.isNull())
// {
// qDebug()<< "could not convertToPGM()";
// return QImage();
// }
if (!fill.loadFromData(tmpImage, "PGM"))
{
qDebug()<< "could not create extracted image";
// return QImage();
}
}
if (depthEnabled)
{
QByteArray coloredDepth(rgbdImage.cols() * rgbdImage.rows() * 3, 0);
for (uint32_t r = 0; r < rgbdImage.rows(); ++r)
{
const float* depth = reinterpret_cast<const float*>(rgbdImage.imagedata2().c_str() + r * rgbdImage.step2());
uint8_t* pixel = reinterpret_cast<uint8_t*>(coloredDepth.data()) + r * rgbdImage.cols() * 3;
for (uint32_t c = 0; c < rgbdImage.cols(); ++c)
{
if (depth[c] != 0)
{
int idx = fminf(depth[c], 10.0f) / 10.0f * 127.0f;
idx = 127 - idx;
pixel[0] = colormapJet[idx][2] * 255.0f;
pixel[1] = colormapJet[idx][1] * 255.0f;
pixel[2] = colormapJet[idx][0] * 255.0f;
}
pixel += 3;
}
}
fill = QImage(reinterpret_cast<const uchar*>(coloredDepth.constData()),
rgbdImage.cols(), rgbdImage.rows(), QImage::Format_RGB888);
}
glImage = QGLWidget::convertToGLFormat(fill);
#endif
}
src/ui/QGCRGBDView.h 0 → 100644
View file @ 880879cb
#ifndef QGCRGBDVIEW_H
#define QGCRGBDVIEW_H
#include "HUD.h"
class QGCRGBDView : public HUD
{
Q_OBJECT
public:
explicit QGCRGBDView(int width=640, int height=480, QWidget *parent = 0);
signals:
public slots:
void addUAS(UASInterface* uas);
void enableRGB(bool enabled);
void enableDepth(bool enabled);
void updateData(UASInterface *uas);
protected:
bool rgbEnabled;
bool depthEnabled;
QAction* enableRGBAction;
QAction* enableDepthAction;
void contextMenuEvent (QContextMenuEvent* event);
};
#endif // QGCRGBDVIEW_H
src/ui/map3D/Pixhawk3DWidget.cc
View file @ 880879cb
......@@ -59,11 +59,11 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
, displayImagery(true)
, displayWaypoints(true)
, displayRGBD2D(false)
, displayRGBD3D(false)
, enableRGBDColor(true)
, displayRGBD3D(true)
, enableRGBDColor(false)
, enableTarget(false)
, followCamera(true)
, frame(MAV_FRAME_GLOBAL)
, frame(MAV_FRAME_LOCAL_NED)
, lastRobotX(0.0f)
, lastRobotY(0.0f)
, lastRobotZ(0.0f)
......@@ -433,8 +433,8 @@ void
Pixhawk3DWidget::buildLayout(void)
{
QComboBox* frameComboBox = new QComboBox(this);
frameComboBox->addItem("Global");
frameComboBox->addItem("Local");
frameComboBox->addItem("Global");
frameComboBox->setFixedWidth(70);
QCheckBox* gridCheckBox = new QCheckBox(this);
......@@ -643,7 +643,6 @@ Pixhawk3DWidget::getPose(double& x, double& y, double& z,
z = uas->getLocalZ();
}
roll = uas->getRoll();
pitch = uas->getPitch();
yaw = uas->getYaw();
......@@ -1222,38 +1221,41 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
px::RGBDImage rgbdImage = uas->getRGBDImage();
px::PointCloudXYZRGB pointCloud = uas->getPointCloud();
rgbImage->setImage(rgbdImage.cols(), rgbdImage.rows(), 1,
GL_LUMINANCE, GL_LUMINANCE, GL_UNSIGNED_BYTE,
reinterpret_cast<unsigned char *>(&(*(rgbdImage.mutable_imagedata1()))[0]),
osg::Image::NO_DELETE);
rgbImage->dirty();
QByteArray coloredDepth(rgbdImage.cols() * rgbdImage.rows() * 3, 0);
for (uint32_t r = 0; r < rgbdImage.rows(); ++r)
if (rgbdImage.rows() > 0 && rgbdImage.cols() > 0)
{
const float* depth = reinterpret_cast<const float*>(rgbdImage.imagedata2().c_str() + r * rgbdImage.step2());
uint8_t* pixel = reinterpret_cast<uint8_t*>(coloredDepth.data()) + r * rgbdImage.cols() * 3;
for (uint32_t c = 0; c < rgbdImage.cols(); ++c)
rgbImage->setImage(rgbdImage.cols(), rgbdImage.rows(), 1,
GL_LUMINANCE, GL_LUMINANCE, GL_UNSIGNED_BYTE,
reinterpret_cast<unsigned char *>(&(*(rgbdImage.mutable_imagedata1()))[0]),
osg::Image::NO_DELETE);
rgbImage->dirty();
QByteArray coloredDepth(rgbdImage.cols() * rgbdImage.rows() * 3, 0);
for (uint32_t r = 0; r < rgbdImage.rows(); ++r)
{
if (depth[c] != 0)
const float* depth = reinterpret_cast<const float*>(rgbdImage.imagedata2().c_str() + r * rgbdImage.step2());
uint8_t* pixel = reinterpret_cast<uint8_t*>(coloredDepth.data()) + r * rgbdImage.cols() * 3;
for (uint32_t c = 0; c < rgbdImage.cols(); ++c)
{
int idx = fminf(depth[c], 10.0f) / 10.0f * 127.0f;
idx = 127 - idx;
if (depth[c] != 0)
{
int idx = fminf(depth[c], 10.0f) / 10.0f * 127.0f;
idx = 127 - idx;
pixel[0] = colormap_jet[idx][2] * 255.0f;
pixel[1] = colormap_jet[idx][1] * 255.0f;
pixel[2] = colormap_jet[idx][0] * 255.0f;
}
pixel[0] = colormap_jet[idx][2] * 255.0f;
pixel[1] = colormap_jet[idx][1] * 255.0f;
pixel[2] = colormap_jet[idx][0] * 255.0f;
pixel += 3;
}
pixel += 3;
}
}
depthImage->setImage(rgbdImage.cols(), rgbdImage.rows(), 1,
GL_RGB, GL_RGB, GL_UNSIGNED_BYTE,
reinterpret_cast<unsigned char *>(coloredDepth.data()),
osg::Image::NO_DELETE);
depthImage->dirty();
depthImage->setImage(rgbdImage.cols(), rgbdImage.rows(), 1,
GL_RGB, GL_RGB, GL_UNSIGNED_BYTE,
reinterpret_cast<unsigned char *>(coloredDepth.data()),
osg::Image::NO_DELETE);
depthImage->dirty();
}
osg::Geometry* geometry = rgbd3DNode->getDrawable(0)->asGeometry();
......@@ -1266,6 +1268,7 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
double y = p.y() - robotY;
double z = p.z() - robotZ;
(*vertices)[i].set(y, x, -z);
if (enableRGBDColor) {
......
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