Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Q
qgroundcontrol
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
880879cb
Commit
880879cb
authored
Dec 12, 2011
by
pixhawk
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Added widgets to display RGBD data.
parent
3122d2ca
Changes
12
Show whitespace changes
Inline
Side-by-side
Showing
12 changed files
with
382 additions
and
40 deletions
+382
-40
qgroundcontrol.pri
qgroundcontrol.pri
+6
-4
qgroundcontrol.pro
qgroundcontrol.pro
+4
-2
MAVLinkProtocol.cc
src/comm/MAVLinkProtocol.cc
+1
-0
UAS.cc
src/uas/UAS.cc
+2
-0
UAS.h
src/uas/UAS.h
+4
-0
HUD.cc
src/ui/HUD.cc
+4
-0
HUD.h
src/ui/HUD.h
+1
-0
MainWindow.cc
src/ui/MainWindow.cc
+27
-4
MainWindow.h
src/ui/MainWindow.h
+2
-0
QGCRGBDView.cc
src/ui/QGCRGBDView.cc
+269
-0
QGCRGBDView.h
src/ui/QGCRGBDView.h
+29
-0
Pixhawk3DWidget.cc
src/ui/map3D/Pixhawk3DWidget.cc
+33
-30
No files found.
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
(
tru
e
);
video1
->
enableVideo
(
fals
e
);
// 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
(
tru
e
);
video2
->
enableVideo
(
fals
e
);
// 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.0
f
,
0.0
f
,
0.53125
f
},
{
0.0
f
,
0.0
f
,
0.5625
f
},
{
0.0
f
,
0.0
f
,
0.59375
f
},
{
0.0
f
,
0.0
f
,
0.625
f
},
{
0.0
f
,
0.0
f
,
0.65625
f
},
{
0.0
f
,
0.0
f
,
0.6875
f
},
{
0.0
f
,
0.0
f
,
0.71875
f
},
{
0.0
f
,
0.0
f
,
0.75
f
},
{
0.0
f
,
0.0
f
,
0.78125
f
},
{
0.0
f
,
0.0
f
,
0.8125
f
},
{
0.0
f
,
0.0
f
,
0.84375
f
},
{
0.0
f
,
0.0
f
,
0.875
f
},
{
0.0
f
,
0.0
f
,
0.90625
f
},
{
0.0
f
,
0.0
f
,
0.9375
f
},
{
0.0
f
,
0.0
f
,
0.96875
f
},
{
0.0
f
,
0.0
f
,
1.0
f
},
{
0.0
f
,
0.03125
f
,
1.0
f
},
{
0.0
f
,
0.0625
f
,
1.0
f
},
{
0.0
f
,
0.09375
f
,
1.0
f
},
{
0.0
f
,
0.125
f
,
1.0
f
},
{
0.0
f
,
0.15625
f
,
1.0
f
},
{
0.0
f
,
0.1875
f
,
1.0
f
},
{
0.0
f
,
0.21875
f
,
1.0
f
},
{
0.0
f
,
0.25
f
,
1.0
f
},
{
0.0
f
,
0.28125
f
,
1.0
f
},
{
0.0
f
,
0.3125
f
,
1.0
f
},
{
0.0
f
,
0.34375
f
,
1.0
f
},
{
0.0
f
,
0.375
f
,
1.0
f
},
{
0.0
f
,
0.40625
f
,
1.0
f
},
{
0.0
f
,
0.4375
f
,
1.0
f
},
{
0.0
f
,
0.46875
f
,
1.0
f
},
{
0.0
f
,
0.5
f
,
1.0
f
},
{
0.0
f
,
0.53125
f
,
1.0
f
},
{
0.0
f
,
0.5625
f
,
1.0
f
},
{
0.0
f
,
0.59375
f
,
1.0
f
},
{
0.0
f
,
0.625
f
,
1.0
f
},
{
0.0
f
,
0.65625
f
,
1.0
f
},
{
0.0
f
,
0.6875
f
,
1.0
f
},
{
0.0
f
,
0.71875
f
,
1.0
f
},
{
0.0
f
,
0.75
f
,
1.0
f
},
{
0.0
f
,
0.78125
f
,
1.0
f
},
{
0.0
f
,
0.8125
f
,
1.0
f
},
{
0.0
f
,
0.84375
f
,
1.0
f
},
{
0.0
f
,
0.875
f
,
1.0
f
},
{
0.0
f
,
0.90625
f
,
1.0
f
},
{
0.0
f
,
0.9375
f
,
1.0
f
},
{
0.0
f
,
0.96875
f
,
1.0
f
},
{
0.0
f
,
1.0
f
,
1.0
f
},
{
0.03125
f
,
1.0
f
,
0.96875
f
},
{
0.0625
f
,
1.0
f
,
0.9375
f
},
{
0.09375
f
,
1.0
f
,
0.90625
f
},
{
0.125
f
,
1.0
f
,
0.875
f
},
{
0.15625
f
,
1.0
f
,
0.84375
f
},
{
0.1875
f
,
1.0
f
,
0.8125
f
},
{
0.21875
f
,
1.0
f
,
0.78125
f
},
{
0.25
f
,
1.0
f
,
0.75
f
},
{
0.28125
f
,
1.0
f
,
0.71875
f
},
{
0.3125
f
,
1.0
f
,
0.6875
f
},
{
0.34375
f
,
1.0
f
,
0.65625
f
},
{
0.375
f
,
1.0
f
,
0.625
f
},
{
0.40625
f
,
1.0
f
,
0.59375
f
},
{
0.4375
f
,
1.0
f
,
0.5625
f
},
{
0.46875
f
,
1.0
f
,
0.53125
f
},
{
0.5
f
,
1.0
f
,
0.5
f
},
{
0.53125
f
,
1.0
f
,
0.46875
f
},
{
0.5625
f
,
1.0
f
,
0.4375
f
},
{
0.59375
f
,
1.0
f
,
0.40625
f
},
{
0.625
f
,
1.0
f
,
0.375
f
},
{
0.65625
f
,
1.0
f
,
0.34375
f
},
{
0.6875
f
,
1.0
f
,
0.3125
f
},
{
0.71875
f
,
1.0
f
,
0.28125
f
},
{
0.75
f
,
1.0
f
,
0.25
f
},
{
0.78125
f
,
1.0
f
,
0.21875
f
},
{
0.8125
f
,
1.0
f
,
0.1875
f
},
{
0.84375
f
,
1.0
f
,
0.15625
f
},
{
0.875
f
,
1.0
f
,
0.125
f
},
{
0.90625
f
,
1.0
f
,
0.09375
f
},
{
0.9375
f
,
1.0
f
,
0.0625
f
},
{
0.96875
f
,
1.0
f
,
0.03125
f
},
{
1.0
f
,
1.0
f
,
0.0
f
},
{
1.0
f
,
0.96875
f
,
0.0
f
},
{
1.0
f
,
0.9375
f
,
0.0
f
},
{
1.0
f
,
0.90625
f
,
0.0
f
},
{
1.0
f
,
0.875
f
,
0.0
f
},
{
1.0
f
,
0.84375
f
,
0.0
f
},
{
1.0
f
,
0.8125
f
,
0.0
f
},
{
1.0
f
,
0.78125
f
,
0.0
f
},
{
1.0
f
,
0.75
f
,
0.0
f
},
{
1.0
f
,
0.71875
f
,
0.0
f
},
{
1.0
f
,
0.6875
f
,
0.0
f
},
{
1.0
f
,
0.65625
f
,
0.0
f
},
{
1.0
f
,
0.625
f
,
0.0
f
},
{
1.0
f
,
0.59375
f
,
0.0
f
},
{
1.0
f
,
0.5625
f
,
0.0
f
},
{
1.0
f
,
0.53125
f
,
0.0
f
},
{
1.0
f
,
0.5
f
,
0.0
f
},
{
1.0
f
,
0.46875
f
,
0.0
f
},
{
1.0
f
,
0.4375
f
,
0.0
f
},
{
1.0
f
,
0.40625
f
,
0.0
f
},
{
1.0
f
,
0.375
f
,
0.0
f
},
{
1.0
f
,
0.34375
f
,
0.0
f
},
{
1.0
f
,
0.3125
f
,
0.0
f
},
{
1.0
f
,
0.28125
f
,
0.0
f
},
{
1.0
f
,
0.25
f
,
0.0
f
},
{
1.0
f
,
0.21875
f
,
0.0
f
},
{
1.0
f
,
0.1875
f
,
0.0
f
},
{
1.0
f
,
0.15625
f
,
0.0
f
},
{
1.0
f
,
0.125
f
,
0.0
f
},
{
1.0
f
,
0.09375
f
,
0.0
f
},
{
1.0
f
,
0.0625
f
,
0.0
f
},
{
1.0
f
,
0.03125
f
,
0.0
f
},
{
1.0
f
,
0.0
f
,
0.0
f
},
{
0.96875
f
,
0.0
f
,
0.0
f
},
{
0.9375
f
,
0.0
f
,
0.0
f
},
{
0.90625
f
,
0.0
f
,
0.0
f
},
{
0.875
f
,
0.0
f
,
0.0
f
},
{
0.84375
f
,
0.0
f
,
0.0
f
},
{
0.8125
f
,
0.0
f
,
0.0
f
},
{
0.78125
f
,
0.0
f
,
0.0
f
},
{
0.75
f
,
0.0
f
,
0.0
f
},
{
0.71875
f
,
0.0
f
,
0.0
f
},
{
0.6875
f
,
0.0
f
,
0.0
f
},
{
0.65625
f
,
0.0
f
,
0.0
f
},
{
0.625
f
,
0.0
f
,
0.0
f
},
{
0.59375
f
,
0.0
f
,
0.0
f
},
{
0.5625
f
,
0.0
f
,
0.0
f
},
{
0.53125
f
,
0.0
f
,
0.0
f
},
{
0.5
f
,
0.0
f
,
0.0
f
}
};
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.0
f
)
/
10.0
f
*
127.0
f
;
idx
=
127
-
idx
;
pixel
[
0
]
=
colormapJet
[
idx
][
2
]
*
255.0
f
;
pixel
[
1
]
=
colormapJet
[
idx
][
1
]
*
255.0
f
;
pixel
[
2
]
=
colormapJet
[
idx
][
0
]
*
255.0
f
;
}
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
(
fals
e
)
,
enableRGBDColor
(
tru
e
)
,
displayRGBD3D
(
tru
e
)
,
enableRGBDColor
(
fals
e
)
,
enableTarget
(
false
)
,
followCamera
(
true
)
,
frame
(
MAV_FRAME_
GLOBAL
)
,
frame
(
MAV_FRAME_
LOCAL_NED
)
,
lastRobotX
(
0.0
f
)
,
lastRobotY
(
0.0
f
)
,
lastRobotZ
(
0.0
f
)
...
...
@@ -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,6 +1221,8 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
px
::
RGBDImage
rgbdImage
=
uas
->
getRGBDImage
();
px
::
PointCloudXYZRGB
pointCloud
=
uas
->
getPointCloud
();
if
(
rgbdImage
.
rows
()
>
0
&&
rgbdImage
.
cols
()
>
0
)
{
rgbImage
->
setImage
(
rgbdImage
.
cols
(),
rgbdImage
.
rows
(),
1
,
GL_LUMINANCE
,
GL_LUMINANCE
,
GL_UNSIGNED_BYTE
,
reinterpret_cast
<
unsigned
char
*>
(
&
(
*
(
rgbdImage
.
mutable_imagedata1
()))[
0
]),
...
...
@@ -1254,6 +1255,7 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
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
)
{
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment