Skip to content

Q
qgroundcontrol
Commit a7e4f7d0 authored by lm's avatar lm
Browse files
Options

Merge branch 'dev' of pixhawk.ethz.ch:qgroundcontrol into dev

parents a9d7bb16 adcd361d
Show whitespace changes
Inline Side-by-side
Showing with 509 additions and 322 deletions
qgroundcontrol.pro
View file @ a7e4f7d0
......@@ -238,7 +238,6 @@ HEADERS += src/MG.h \
src/ui/QGCWebView.h \
src/ui/map3D/QGCGoogleEarthView.h \
src/ui/map3D/QGCWebPage.h
contains(DEPENDENCIES_PRESENT, osg) {
message("Including headers for OpenSceneGraph")
......@@ -255,7 +254,8 @@ contains(DEPENDENCIES_PRESENT, osg) {
src/ui/map3D/TextureCache.h \
src/ui/map3D/Texture.h \
src/ui/map3D/Imagery.h \
src/ui/map3D/HUDScaleGeode.h
src/ui/map3D/HUDScaleGeode.h \
src/ui/map3D/WaypointGroupNode.h
contains(DEPENDENCIES_PRESENT, osgearth) {
message("Including headers for OSGEARTH")
......@@ -357,7 +357,8 @@ contains(DEPENDENCIES_PRESENT, osg) {
src/ui/map3D/TextureCache.cc \
src/ui/map3D/Texture.cc \
src/ui/map3D/Imagery.cc \
src/ui/map3D/HUDScaleGeode.cc
src/ui/map3D/HUDScaleGeode.cc \
src/ui/map3D/WaypointGroupNode.cc
contains(DEPENDENCIES_PRESENT, osgearth) {
message("Including sources for osgEarth")
......
src/input/Freenect.cc
View file @ a7e4f7d0
......@@ -106,7 +106,7 @@ Freenect::~Freenect()
if (device != NULL)
{
freenect_stop_depth(device);
freenect_stop_rgb(device);
freenect_stop_video(device);
}
freenect_close_device(device);
......@@ -136,8 +136,8 @@ Freenect::init(int userDeviceNumber)
freenect_set_user(device, this);
memset(rgb, 0, FREENECT_RGB_SIZE);
memset(depth, 0, FREENECT_DEPTH_SIZE);
memset(rgb, 0, FREENECT_VIDEO_RGB_SIZE);
memset(depth, 0, FREENECT_DEPTH_11BIT_SIZE);
// set Kinect parameters
if (freenect_set_tilt_degs(device, tiltAngle) != 0)
......@@ -148,22 +148,22 @@ Freenect::init(int userDeviceNumber)
{
return false;
}
if (freenect_set_rgb_format(device, FREENECT_FORMAT_RGB) != 0)
if (freenect_set_video_format(device, FREENECT_VIDEO_RGB) != 0)
{
return false;
}
if (freenect_set_depth_format(device, FREENECT_FORMAT_11_BIT) != 0)
if (freenect_set_depth_format(device, FREENECT_DEPTH_11BIT) != 0)
{
return false;
}
freenect_set_rgb_callback(device, rgbCallback);
freenect_set_video_callback(device, videoCallback);
freenect_set_depth_callback(device, depthCallback);
if (freenect_start_rgb(device) != 0)
if (freenect_start_depth(device) != 0)
{
return false;
}
if (freenect_start_depth(device) != 0)
if (freenect_start_video(device) != 0)
{
return false;
}
......@@ -182,14 +182,10 @@ Freenect::process(void)
return false;
}
//libfreenect changed some access functions in one of the new revisions
freenect_raw_device_state state;
freenect_get_mks_accel(&state, &ax, &ay, &az);
//tiltAngle = freenect_get_tilt_degs(&state);
//these are the old access functions
//freenect_get_raw_accel(device, &ax, &ay, &az);
//freenect_get_mks_accel(device, &dx, &dy, &dz);
freenect_raw_tilt_state* state;
freenect_update_tilt_state(device);
state = freenect_get_tilt_state(device);
freenect_get_mks_accel(state, &ax, &ay, &az);
return true;
}
......@@ -199,7 +195,7 @@ Freenect::getRgbData(void)
{
QMutexLocker locker(&rgbMutex);
return QSharedPointer<QByteArray>(
new QByteArray(rgb, FREENECT_RGB_SIZE));
new QByteArray(rgb, FREENECT_VIDEO_RGB_SIZE));
}
QSharedPointer<QByteArray>
......@@ -207,7 +203,7 @@ Freenect::getRawDepthData(void)
{
QMutexLocker locker(&depthMutex);
return QSharedPointer<QByteArray>(
new QByteArray(depth, FREENECT_DEPTH_SIZE));
new QByteArray(depth, FREENECT_DEPTH_11BIT_SIZE));
}
QSharedPointer<QByteArray>
......@@ -215,7 +211,7 @@ Freenect::getColoredDepthData(void)
{
QMutexLocker locker(&coloredDepthMutex);
return QSharedPointer<QByteArray>(
new QByteArray(coloredDepth, FREENECT_RGB_SIZE));
new QByteArray(coloredDepth, FREENECT_VIDEO_RGB_SIZE));
}
QVector<QVector3D>
......@@ -386,22 +382,22 @@ Freenect::projectPixelTo3DRay(const QVector2D& pixel, QVector3D& ray,
}
void
Freenect::rgbCallback(freenect_device* device, freenect_pixel* rgb, uint32_t timestamp)
Freenect::videoCallback(freenect_device* device, void* video, uint32_t timestamp)
{
Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));
QMutexLocker locker(&freenect->rgbMutex);
memcpy(freenect->rgb, rgb, FREENECT_RGB_SIZE);
memcpy(freenect->rgb, video, FREENECT_VIDEO_RGB_SIZE);
}
void
Freenect::depthCallback(freenect_device* device, void* depth, uint32_t timestamp)
{
Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));
freenect_depth* data = reinterpret_cast<freenect_depth *>(depth);
uint16_t* data = reinterpret_cast<uint16_t *>(depth);
QMutexLocker depthLocker(&freenect->depthMutex);
memcpy(freenect->depth, data, FREENECT_DEPTH_SIZE);
memcpy(freenect->depth, data, FREENECT_DEPTH_11BIT_SIZE);
QMutexLocker coloredDepthLocker(&freenect->coloredDepthMutex);
unsigned short* src = reinterpret_cast<unsigned short *>(data);
......
src/input/Freenect.h
View file @ a7e4f7d0
......@@ -95,7 +95,7 @@ private:
void projectPixelTo3DRay(const QVector2D& pixel, QVector3D& ray,
const IntrinsicCameraParameters& params);
static void rgbCallback(freenect_device* device, freenect_pixel* rgb, uint32_t timestamp);
static void videoCallback(freenect_device* device, void* video, uint32_t timestamp);
static void depthCallback(freenect_device* device, void* depth, uint32_t timestamp);
freenect_context* context;
......@@ -122,13 +122,13 @@ private:
int tiltAngle;
// rgbd data
char rgb[FREENECT_RGB_SIZE];
char rgb[FREENECT_VIDEO_RGB_SIZE];
QMutex rgbMutex;
char depth[FREENECT_DEPTH_SIZE];
char depth[FREENECT_DEPTH_11BIT_SIZE];
QMutex depthMutex;
char coloredDepth[FREENECT_RGB_SIZE];
char coloredDepth[FREENECT_VIDEO_RGB_SIZE];
QMutex coloredDepthMutex;
// accelerometer data
......
src/ui/WaypointView.cc
View file @ a7e4f7d0
......@@ -49,7 +49,6 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) :
m_ui->setupUi(this);
this->wp = wp;
wp->setFrame(MAV_FRAME_LOCAL);
// add actions
m_ui->comboBox_action->addItem("Navigate",MAV_ACTION_NAVIGATE);
......@@ -63,7 +62,7 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) :
// defaults
changedAction(0);
changedFrame(0);
changedFrame(wp->getFrame());
// Read values and set user interface
updateValues();
......
src/ui/map3D/Pixhawk3DWidget.cc
View file @ a7e4f7d0
......@@ -41,7 +41,7 @@
#include "PixhawkCheetahGeode.h"
#include "UASManager.h"
#include "UASInterface.h"
#include "QGC.h"
Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
......@@ -52,13 +52,13 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
, displayGrid(true)
, displayTrail(false)
, displayImagery(true)
, displayTarget(false)
, displayWaypoints(true)
, displayRGBD2D(false)
, displayRGBD3D(false)
, enableRGBDColor(true)
, followCamera(true)
, enableFreenect(false)
, frame(MAV_FRAME_GLOBAL)
, lastRobotX(0.0f)
, lastRobotY(0.0f)
, lastRobotZ(0.0f)
......@@ -82,12 +82,10 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
mapNode = createMap();
rollingMap->addChild(mapNode);
// generate target model
allocentricMap->addChild(createTarget());
// generate waypoint model
waypointsNode = createWaypoints();
rollingMap->addChild(waypointsNode);
waypointGroupNode = new WaypointGroupNode;
waypointGroupNode->init();
rollingMap->addChild(waypointGroupNode);
#ifdef QGC_LIBFREENECT_ENABLED
freenect.reset(new Freenect());
......@@ -132,6 +130,23 @@ void Pixhawk3DWidget::setActiveUAS(UASInterface* uas)
this->uas = uas;
}
void
Pixhawk3DWidget::selectFrame(QString text)
{
if (text.compare("Global") == 0)
{
frame = MAV_FRAME_GLOBAL;
}
else if (text.compare("Local") == 0)
{
frame = MAV_FRAME_LOCAL;
}
getPosition(lastRobotX, lastRobotY, lastRobotZ);
recenter();
}
void
Pixhawk3DWidget::showGrid(int32_t state)
{
......@@ -203,16 +218,7 @@ void
Pixhawk3DWidget::recenter(void)
{
double robotX = 0.0f, robotY = 0.0f, robotZ = 0.0f;
if (uas != NULL)
{
double latitude = uas->getLatitude();
double longitude = uas->getLongitude();
double altitude = uas->getAltitude();
QString utmZone;
Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
robotZ = -altitude;
}
getPosition(robotX, robotY, robotZ);
recenterCamera(robotY, robotX, -robotZ);
}
......@@ -234,13 +240,16 @@ void
Pixhawk3DWidget::insertWaypoint(void)
{
if (uas)
{
Waypoint* wp;
if (frame == MAV_FRAME_GLOBAL)
{
double latitude = uas->getLatitude();
double longitude = uas->getLongitude();
double altitude = uas->getAltitude();
double x, y;
QString utmZone;
Imagery::LLtoUTM(uas->getLatitude(), uas->getLongitude(), x, y, utmZone);
Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
std::pair<double,double> cursorWorldCoords =
getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
......@@ -248,12 +257,25 @@ Pixhawk3DWidget::insertWaypoint(void)
Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone,
latitude, longitude);
Waypoint* wp = new Waypoint(0,
longitude,
latitude,
altitude);
wp = new Waypoint(0, longitude, latitude, altitude);
}
else if (frame == MAV_FRAME_LOCAL)
{
double z = uas->getLocalZ();
std::pair<double,double> cursorWorldCoords =
getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
wp = new Waypoint(0, cursorWorldCoords.first,
cursorWorldCoords.second, z);
}
if (wp)
{
wp->setFrame(frame);
uas->getWaypointManager().addWaypoint(wp);
}
}
}
void
......@@ -266,13 +288,19 @@ void
Pixhawk3DWidget::setWaypoint(void)
{
if (uas)
{
const QVector<Waypoint *> waypoints =
uas->getWaypointManager().getWaypointList();
Waypoint* waypoint = waypoints.at(selectedWpIndex);
if (frame == MAV_FRAME_GLOBAL)
{
double latitude = uas->getLatitude();
double longitude = uas->getLongitude();
double altitude = uas->getAltitude();
double x, y;
QString utmZone;
Imagery::LLtoUTM(uas->getLatitude(), uas->getLongitude(), x, y, utmZone);
Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
std::pair<double,double> cursorWorldCoords =
getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
......@@ -280,13 +308,22 @@ Pixhawk3DWidget::setWaypoint(void)
Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone,
latitude, longitude);
const QVector<Waypoint *> waypoints =
uas->getWaypointManager().getWaypointList();
Waypoint* waypoint = waypoints.at(selectedWpIndex);
waypoint->setX(longitude);
waypoint->setY(latitude);
waypoint->setZ(altitude);
}
else if (frame == MAV_FRAME_LOCAL)
{
double z = uas->getLocalZ();
std::pair<double,double> cursorWorldCoords =
getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
waypoint->setX(cursorWorldCoords.first);
waypoint->setY(cursorWorldCoords.second);
waypoint->setZ(z);
}
}
}
void
......@@ -308,13 +345,26 @@ Pixhawk3DWidget::setWaypointAltitude(void)
uas->getWaypointManager().getWaypointList();
Waypoint* waypoint = waypoints.at(selectedWpIndex);
double altitude = waypoint->getZ();
if (frame == MAV_FRAME_LOCAL)
{
altitude = -altitude;
}
double newAltitude =
QInputDialog::getDouble(this, tr("Set altitude of waypoint %1").arg(selectedWpIndex),
tr("Altitude (m):"), waypoint->getZ(), -1000.0, 1000.0, 1, &ok);
if (ok)
{
if (frame == MAV_FRAME_GLOBAL)
{
waypoint->setZ(newAltitude);
}
else if (frame == MAV_FRAME_LOCAL)
{
waypoint->setZ(-newAltitude);
}
}
}
}
......@@ -323,11 +373,6 @@ Pixhawk3DWidget::clearAllWaypoints(void)
{
if (uas)
{
double altitude = uas->getAltitude();
std::pair<double,double> cursorWorldCoords =
getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
const QVector<Waypoint *> waypoints =
uas->getWaypointManager().getWaypointList();
for (int i = waypoints.size() - 1; i >= 0; --i)
......@@ -392,6 +437,11 @@ Pixhawk3DWidget::findVehicleModels(void)
void
Pixhawk3DWidget::buildLayout(void)
{
QComboBox* frameComboBox = new QComboBox(this);
frameComboBox->addItem("Global");
frameComboBox->addItem("Local");
frameComboBox->setFixedWidth(70);
QCheckBox* gridCheckBox = new QCheckBox(this);
gridCheckBox->setText("Grid");
gridCheckBox->setChecked(displayGrid);
......@@ -427,21 +477,25 @@ Pixhawk3DWidget::buildLayout(void)
QGridLayout* layout = new QGridLayout(this);
layout->setMargin(0);
layout->setSpacing(2);
layout->addWidget(gridCheckBox, 1, 0);
layout->addWidget(trailCheckBox, 1, 1);
layout->addWidget(waypointsCheckBox, 1, 2);
layout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 3);
layout->addWidget(mapLabel, 1, 4);
layout->addWidget(mapComboBox, 1, 5);
layout->addWidget(modelLabel, 1, 6);
layout->addWidget(modelComboBox, 1, 7);
layout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 8);
layout->addWidget(recenterButton, 1, 9);
layout->addWidget(followCameraCheckBox, 1, 10);
layout->setRowStretch(0, 100);
layout->setRowStretch(1, 1);
layout->addWidget(frameComboBox, 0, 10);
layout->addWidget(gridCheckBox, 2, 0);
layout->addWidget(trailCheckBox, 2, 1);
layout->addWidget(waypointsCheckBox, 2, 2);
layout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 2, 3);
layout->addWidget(mapLabel, 2, 4);
layout->addWidget(mapComboBox, 2, 5);
layout->addWidget(modelLabel, 2, 6);
layout->addWidget(modelComboBox, 2, 7);
layout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 2, 8);
layout->addWidget(recenterButton, 2, 9);
layout->addWidget(followCameraCheckBox, 2, 10);
layout->setRowStretch(0, 1);
layout->setRowStretch(1, 100);
layout->setRowStretch(2, 1);
setLayout(layout);
connect(frameComboBox, SIGNAL(currentIndexChanged(QString)),
this, SLOT(selectFrame(QString)));
connect(gridCheckBox, SIGNAL(stateChanged(int)),
this, SLOT(showGrid(int)));
connect(trailCheckBox, SIGNAL(stateChanged(int)),
......@@ -465,19 +519,9 @@ Pixhawk3DWidget::display(void)
return;
}
double latitude = uas->getLatitude();
double longitude = uas->getLongitude();
double altitude = uas->getAltitude();
double robotX;
double robotY;
double robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw;
QString utmZone;
Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
double robotZ = -altitude;
double robotRoll = uas->getRoll();
double robotPitch = uas->getPitch();
double robotYaw = uas->getYaw();
getPose(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw, utmZone);
if (lastRobotX == 0.0f && lastRobotY == 0.0f && lastRobotZ == 0.0f)
{
......@@ -509,16 +553,11 @@ Pixhawk3DWidget::display(void)
updateTrail(robotX, robotY, robotZ);
}
if (displayImagery)
if (frame == MAV_FRAME_GLOBAL && displayImagery)
{
updateImagery(robotX, robotY, robotZ, utmZone);
}
if (displayTarget)
{
updateTarget();
}
if (displayWaypoints)
{
updateWaypoints();
......@@ -530,15 +569,14 @@ Pixhawk3DWidget::display(void)
updateRGBD();
}
#endif
updateHUD(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw);
updateHUD(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw, utmZone);
// set node visibility
rollingMap->setChildValue(gridNode, displayGrid);
rollingMap->setChildValue(trailNode, displayTrail);
rollingMap->setChildValue(mapNode, displayImagery);
rollingMap->setChildValue(targetNode, displayTarget);
rollingMap->setChildValue(waypointsNode, displayWaypoints);
rollingMap->setChildValue(waypointGroupNode, displayWaypoints);
if (enableFreenect)
{
egocentricMap->setChildValue(rgbd3DNode, displayRGBD3D);
......@@ -605,6 +643,75 @@ Pixhawk3DWidget::mousePressEvent(QMouseEvent* event)
Q3DWidget::mousePressEvent(event);
}
void
Pixhawk3DWidget::getPose(double& x, double& y, double& z,
double& roll, double& pitch, double& yaw,
QString& utmZone)
{
if (uas)
{
if (frame == MAV_FRAME_GLOBAL)
{
double latitude = uas->getLatitude();
double longitude = uas->getLongitude();
double altitude = uas->getAltitude();
Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
z = -altitude;
}
else if (frame == MAV_FRAME_LOCAL)
{
x = uas->getLocalX();
y = uas->getLocalY();
z = uas->getLocalZ();
}
roll = uas->getRoll();
pitch = uas->getPitch();
yaw = uas->getYaw();
}
}
void
Pixhawk3DWidget::getPose(double& x, double& y, double& z,
double& roll, double& pitch, double& yaw)
{
QString utmZone;
getPose(x, y, z, roll, pitch, yaw);
}
void
Pixhawk3DWidget::getPosition(double& x, double& y, double& z,
QString& utmZone)
{
if (uas)
{
if (frame == MAV_FRAME_GLOBAL)
{
double latitude = uas->getLatitude();
double longitude = uas->getLongitude();
double altitude = uas->getAltitude();
Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
z = -altitude;
}
else if (frame == MAV_FRAME_LOCAL)
{
x = uas->getLocalX();
y = uas->getLocalY();
z = uas->getLocalZ();
}
}
}
void
Pixhawk3DWidget::getPosition(double& x, double& y, double& z)
{
QString utmZone;
getPosition(x, y, z, utmZone);
}
osg::ref_ptr<osg::Geode>
Pixhawk3DWidget::createGrid(void)
{
......@@ -706,25 +813,6 @@ Pixhawk3DWidget::createMap(void)
return osg::ref_ptr<Imagery>(new Imagery());
}
osg::ref_ptr<osg::Node>
Pixhawk3DWidget::createTarget(void)
{
targetPosition = new osg::PositionAttitudeTransform;
targetNode = new osg::Geode;
targetPosition->addChild(targetNode);
return targetPosition;
}
osg::ref_ptr<osg::Group>
Pixhawk3DWidget::createWaypoints(void)
{
osg::ref_ptr<osg::Group> group(new osg::Group());
return group;
}
osg::ref_ptr<osg::Geode>
Pixhawk3DWidget::createRGBD3D(void)
{
......@@ -750,7 +838,8 @@ void
Pixhawk3DWidget::setupHUD(void)
{
osg::ref_ptr<osg::Vec4Array> hudColors(new osg::Vec4Array);
hudColors->push_back(osg::Vec4(0.0f, 0.0f, 0.0f, 0.2f));
hudColors->push_back(osg::Vec4(0.0f, 0.0f, 0.0f, 0.5f));
hudColors->push_back(osg::Vec4(0.0f, 0.0f, 0.0f, 1.0f));
hudBackgroundGeometry = new osg::Geometry;
hudBackgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON,
......@@ -758,7 +847,7 @@ Pixhawk3DWidget::setupHUD(void)
hudBackgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON,
4, 4));
hudBackgroundGeometry->setColorArray(hudColors);
hudBackgroundGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);
hudBackgroundGeometry->setColorBinding(osg::Geometry::BIND_PER_PRIMITIVE_SET);
hudBackgroundGeometry->setUseDisplayList(false);
statusText = new osgText::Text;
......@@ -794,7 +883,7 @@ Pixhawk3DWidget::setupHUD(void)
void
Pixhawk3DWidget::resizeHUD(void)
{
int topHUDHeight = 30;
int topHUDHeight = 25;
int bottomHUDHeight = 25;
osg::Vec3Array* vertices = static_cast<osg::Vec3Array*>(hudBackgroundGeometry->getVertexArray());
......@@ -815,7 +904,7 @@ Pixhawk3DWidget::resizeHUD(void)
(*vertices)[6] = osg::Vec3(width(), bottomHUDHeight, -1);
(*vertices)[7] = osg::Vec3(0, bottomHUDHeight, -1);
statusText->setPosition(osg::Vec3(10, height() - 20, -1.5));
statusText->setPosition(osg::Vec3(10, height() - 15, -1.5));
if (rgb2DGeode.valid() && depth2DGeode.valid())
{
......@@ -830,7 +919,8 @@ Pixhawk3DWidget::resizeHUD(void)
void
Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ,
double robotRoll, double robotPitch, double robotYaw)
double robotRoll, double robotPitch, double robotYaw,
const QString& utmZone)
{
resizeHUD();
......@@ -840,6 +930,31 @@ Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ,
std::ostringstream oss;
oss.setf(std::ios::fixed, std::ios::floatfield);
oss.precision(2);
if (frame == MAV_FRAME_GLOBAL)
{
double latitude, longitude;
Imagery::UTMtoLL(robotX, robotY, utmZone, latitude, longitude);
double cursorLatitude, cursorLongitude;
Imagery::UTMtoLL(cursorPosition.first, cursorPosition.second,
utmZone, cursorLatitude, cursorLongitude);
oss.precision(6);
oss << " Lat = " << latitude <<
" Lon = " << longitude;
oss.precision(2);
oss << " Altitude = " << -robotZ <<
" r = " << robotRoll <<
" p = " << robotPitch <<
" y = " << robotYaw;
oss.precision(6);
oss << " Cursor [" << cursorLatitude <<
" " << cursorLongitude << "]";
}
else if (frame == MAV_FRAME_LOCAL)
{
oss << " x = " << robotX <<
" y = " << robotY <<
" z = " << robotZ <<
......@@ -848,6 +963,8 @@ Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ,
" y = " << robotYaw <<
" Cursor [" << cursorPosition.first <<
" " << cursorPosition.second << "]";
}
statusText->setText(oss.str());
bool darkBackground = true;
......@@ -1001,136 +1118,10 @@ Pixhawk3DWidget::updateImagery(double originX, double originY, double originZ,
mapNode->update();
}
void
Pixhawk3DWidget::updateTarget(void)
{
static double radius = 0.2;
static bool expand = true;
if (radius < 0.1)
{
expand = true;
}
else if (radius > 0.25)
{
expand = false;
}
if (targetNode->getNumDrawables() > 0)
{
targetNode->removeDrawables(0, targetNode->getNumDrawables());
}
osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
osg::ref_ptr<osg::Sphere> sphere = new osg::Sphere;
sphere->setRadius(radius);
sd->setShape(sphere);
sd->setColor(osg::Vec4(0.0f, 0.7f, 1.0f, 1.0f));
targetNode->addDrawable(sd);
if (expand)
{
radius += 0.02;
}
else
{
radius -= 0.02;
}
}
void
Pixhawk3DWidget::updateWaypoints(void)
{
if (uas)
{
double latitude = uas->getLatitude();
double longitude = uas->getLongitude();
double robotX, robotY;
QString utmZone;
Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
double robotZ = -uas->getAltitude();
if (waypointsNode->getNumChildren() > 0)
{
waypointsNode->removeChild(0, waypointsNode->getNumChildren());
}
const QVector<Waypoint *>& list = uas->getWaypointManager().getWaypointList();
for (int i = 0; i < list.size(); i++)
{
Waypoint* wp = list.at(i);
osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
osg::ref_ptr<osg::Cylinder> cylinder =
new osg::Cylinder(osg::Vec3d(0.0, 0.0, wp->getZ() / 2.0),
wp->getOrbit(),
fabs(wp->getZ()));
sd->setShape(cylinder);
sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
if (wp->getCurrent())
{
sd->setColor(osg::Vec4(1.0f, 0.3f, 0.3f, 0.5f));
}
else
{
sd->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
}
osg::ref_ptr<osg::Geode> geode = new osg::Geode;
geode->addDrawable(sd);
char wpLabel[10];
sprintf(wpLabel, "wp%d", i);
geode->setName(wpLabel);
double wpX, wpY;
Imagery::LLtoUTM(wp->getY(), wp->getX(), wpX, wpY, utmZone);
if (i < list.size() - 1)
{
double nextWpX, nextWpY;
Imagery::LLtoUTM(list.at(i+1)->getY(), list.at(i+1)->getX(),
nextWpX, nextWpY, utmZone);
osg::ref_ptr<osg::Geometry> geometry = new osg::Geometry;
osg::ref_ptr<osg::Vec3dArray> vertices = new osg::Vec3dArray;
vertices->push_back(osg::Vec3d(0.0, 0.0, wp->getZ()));
vertices->push_back(osg::Vec3d(nextWpY - wpY,
nextWpX - wpX,
list.at(i+1)->getZ()));
geometry->setVertexArray(vertices);
osg::ref_ptr<osg::Vec4Array> colors = new osg::Vec4Array;
colors->push_back(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
geometry->setColorArray(colors);
geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, 2));
osg::ref_ptr<osg::LineWidth> linewidth(new osg::LineWidth());
linewidth->setWidth(2.0f);
geometry->getOrCreateStateSet()->setAttributeAndModes(linewidth, osg::StateAttribute::ON);
geometry->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
geode->addDrawable(geometry);
}
osg::ref_ptr<osg::PositionAttitudeTransform> pat =
new osg::PositionAttitudeTransform;
pat->setPosition(osg::Vec3d(wpY - robotY,
wpX - robotX,
robotZ));
waypointsNode->addChild(pat);
pat->addChild(geode);
}
}
waypointGroupNode->update(frame, uas);
}
float colormap_jet[128][3] =
......@@ -1315,34 +1306,6 @@ Pixhawk3DWidget::updateRGBD(void)
}
#endif
void
Pixhawk3DWidget::markTarget(void)
{
double robotZ = 0.0f;
if (uas != NULL)
{
robotZ = uas->getLocalZ();
}
std::pair<double,double> cursorWorldCoords =
getGlobalCursorPosition(getMouseX(), getMouseY(), -robotZ);
double targetX = cursorWorldCoords.first;
double targetY = cursorWorldCoords.second;
double targetZ = robotZ;
targetPosition->setPosition(osg::Vec3d(targetY, targetX, -targetZ));
displayTarget = true;
if (uas)
{
uas->setTargetPosition(targetX, targetY, targetZ, 0.0f);
}
targetButton->setChecked(false);
}
int
Pixhawk3DWidget::findWaypoint(int mouseX, int mouseY)
{
......
src/ui/map3D/Pixhawk3DWidget.h
View file @ a7e4f7d0
......@@ -37,6 +37,7 @@
#include "HUDScaleGeode.h"
#include "Imagery.h"
#include "ImageWindowGeode.h"
#include "WaypointGroupNode.h"
#ifdef QGC_LIBFREENECT_ENABLED
#include "Freenect.h"
......@@ -61,6 +62,7 @@ public slots:
void setActiveUAS(UASInterface* uas);
private slots:
void selectFrame(QString text);
void showGrid(int state);
void showTrail(int state);
void showWaypoints(int state);
......@@ -86,29 +88,34 @@ protected:
UASInterface* uas;
private:
void getPose(double& x, double& y, double& z,
double& roll, double& pitch, double& yaw,
QString& utmZone);
void getPose(double& x, double& y, double& z,
double& roll, double& pitch, double& yaw);
void getPosition(double& x, double& y, double& z,
QString& utmZone);
void getPosition(double& x, double& y, double& z);
osg::ref_ptr<osg::Geode> createGrid(void);
osg::ref_ptr<osg::Geode> createTrail(void);
osg::ref_ptr<Imagery> createMap(void);
osg::ref_ptr<osg::Node> createTarget(void);
osg::ref_ptr<osg::Group> createWaypoints(void);
osg::ref_ptr<osg::Geode> createRGBD3D(void);
void setupHUD(void);
void resizeHUD(void);
void updateHUD(double robotX, double robotY, double robotZ,
double robotRoll, double robotPitch, double robotYaw);
double robotRoll, double robotPitch, double robotYaw,
const QString& utmZone);
void updateTrail(double robotX, double robotY, double robotZ);
void updateImagery(double originX, double originY, double originZ,
const QString& zone);
void updateTarget(void);
void updateWaypoints(void);
#ifdef QGC_LIBFREENECT_ENABLED
void updateRGBD(void);
#endif
void markTarget(void);
int findWaypoint(int mouseX, int mouseY);
void showInsertWaypointMenu(const QPoint& cursorPos);
void showEditWaypointMenu(const QPoint& cursorPos);
......@@ -123,7 +130,6 @@ private:
bool displayGrid;
bool displayTrail;
bool displayImagery;
bool displayTarget;
bool displayWaypoints;
bool displayRGBD2D;
bool displayRGBD3D;
......@@ -147,9 +153,7 @@ private:
osg::ref_ptr<osg::Geometry> trailGeometry;
osg::ref_ptr<osg::DrawArrays> trailDrawArrays;
osg::ref_ptr<Imagery> mapNode;
osg::ref_ptr<osg::Geode> targetNode;
osg::ref_ptr<osg::PositionAttitudeTransform> targetPosition;
osg::ref_ptr<osg::Group> waypointsNode;
osg::ref_ptr<WaypointGroupNode> waypointGroupNode;
osg::ref_ptr<osg::Geode> rgbd3DNode;
#ifdef QGC_LIBFREENECT_ENABLED
QScopedPointer<Freenect> freenect;
......@@ -161,8 +165,7 @@ private:
QVector< osg::ref_ptr<osg::Node> > vehicleModels;
QPushButton* targetButton;
MAV_FRAME frame;
double lastRobotX, lastRobotY, lastRobotZ;
};
......
src/ui/map3D/Q3DWidget.cc
View file @ a7e4f7d0
......@@ -168,6 +168,7 @@ Q3DWidget::createHUD(void)
hudGroup->setStateSet(hudStateSet);
hudStateSet->setMode(GL_DEPTH_TEST, osg::StateAttribute::OFF);
hudStateSet->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
hudStateSet->setMode(GL_BLEND, osg::StateAttribute::ON);
hudStateSet->setRenderingHint(osg::StateSet::TRANSPARENT_BIN);
hudStateSet->setRenderBinDetails(11, "RenderBin");
......
src/ui/map3D/WaypointGroupNode.cc 0 → 100644
View file @ a7e4f7d0
/*=====================================================================
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 WaypointGroupNode.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
*/
#include "WaypointGroupNode.h"
#include <osg/LineWidth>
#include <osg/PositionAttitudeTransform>
#include <osg/ShapeDrawable>
#include "Imagery.h"
WaypointGroupNode::WaypointGroupNode()
{
}
void
WaypointGroupNode::init(void)
{
}
void
WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
{
if (uas)
{
double robotX, robotY, robotZ;
if (frame == MAV_FRAME_GLOBAL)
{
double latitude = uas->getLatitude();
double longitude = uas->getLongitude();
double altitude = uas->getAltitude();
QString utmZone;
Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
robotZ = -altitude;
}
else if (frame == MAV_FRAME_LOCAL)
{
robotX = uas->getLocalX();
robotY = uas->getLocalY();
robotZ = uas->getLocalZ();
}
if (getNumChildren() > 0)
{
removeChild(0, getNumChildren());
}
const QVector<Waypoint *>& list = uas->getWaypointManager().getWaypointList();
for (int i = 0; i < list.size(); i++)
{
Waypoint* wp = list.at(i);
double wpX, wpY, wpZ;
getPosition(wp, wpX, wpY, wpZ);
osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
osg::ref_ptr<osg::Cylinder> cylinder =
new osg::Cylinder(osg::Vec3d(0.0, 0.0, -wpZ / 2.0),
wp->getOrbit(),
fabs(wpZ));
sd->setShape(cylinder);
sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
if (wp->getCurrent())
{
sd->setColor(osg::Vec4(1.0f, 0.3f, 0.3f, 0.5f));
}
else
{
sd->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
}
osg::ref_ptr<osg::Geode> geode = new osg::Geode;
geode->addDrawable(sd);
char wpLabel[10];
sprintf(wpLabel, "wp%d", i);
geode->setName(wpLabel);
if (i < list.size() - 1)
{
double nextWpX, nextWpY, nextWpZ;
getPosition(list.at(i + 1), nextWpX, nextWpY, nextWpZ);
osg::ref_ptr<osg::Geometry> geometry = new osg::Geometry;
osg::ref_ptr<osg::Vec3dArray> vertices = new osg::Vec3dArray;
vertices->push_back(osg::Vec3d(0.0, 0.0, -wpZ));
vertices->push_back(osg::Vec3d(nextWpY - wpY,
nextWpX - wpX,
-nextWpZ));
geometry->setVertexArray(vertices);
osg::ref_ptr<osg::Vec4Array> colors = new osg::Vec4Array;
colors->push_back(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
geometry->setColorArray(colors);
geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, 2));
osg::ref_ptr<osg::LineWidth> linewidth(new osg::LineWidth());
linewidth->setWidth(2.0f);
geometry->getOrCreateStateSet()->setAttributeAndModes(linewidth, osg::StateAttribute::ON);
geometry->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
geode->addDrawable(geometry);
}
osg::ref_ptr<osg::PositionAttitudeTransform> pat =
new osg::PositionAttitudeTransform;
pat->setPosition(osg::Vec3d(wpY - robotY,
wpX - robotX,
robotZ));
addChild(pat);
pat->addChild(geode);
}
}
}
void
WaypointGroupNode::getPosition(Waypoint* wp, double &x, double &y, double &z)
{
if (wp->getFrame() == MAV_FRAME_GLOBAL)
{
double latitude = wp->getY();
double longitude = wp->getX();
double altitude = wp->getZ();
QString utmZone;
Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
z = -altitude;
}
else if (wp->getFrame() == MAV_FRAME_LOCAL)
{
x = wp->getX();
y = wp->getY();
z = wp->getZ();
}
}
src/ui/map3D/WaypointGroupNode.h 0 → 100644
View file @ a7e4f7d0
/*=====================================================================
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 WaypointGroupNode.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
*/
#ifndef WAYPOINTGROUPNODE_H
#define WAYPOINTGROUPNODE_H
#include <osg/Group>
#include "UASInterface.h"
class WaypointGroupNode : public osg::Group
{
public:
WaypointGroupNode();
void init(void);
void update(MAV_FRAME frame, UASInterface* uas);
private:
void getPosition(Waypoint* wp, double& x, double& y, double& z);
};
#endif // WAYPOINTGROUPNODE_H
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