diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index abf2fa1bfe4002eeaf5979a4268709367d7db03d..ee6deb127a12ca66bba572b8ba715b2f241fb433 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -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")
diff --git a/src/input/Freenect.cc b/src/input/Freenect.cc
index cd6643287efe6c52e09349ee576bf128a3ebae6b..90cb5e06bd4b5fc05d2724c8138ab3dcfaa14eed 100644
--- a/src/input/Freenect.cc
+++ b/src/input/Freenect.cc
@@ -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);
diff --git a/src/input/Freenect.h b/src/input/Freenect.h
index 4ff4aa8571a56658dec53d7523ace3b9c36c0aeb..88b91550253f1f5edbaf017d0f38c876229e689b 100644
--- a/src/input/Freenect.h
+++ b/src/input/Freenect.h
@@ -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
diff --git a/src/ui/WaypointView.cc b/src/ui/WaypointView.cc
index 6732b0b79e175b5dfd1c3cdbbd73587b8b66ca8c..5d1804741e02f7c534e6c7e04c4937c664f93742 100644
--- a/src/ui/WaypointView.cc
+++ b/src/ui/WaypointView.cc
@@ -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();
diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc
index ab7ba5a553f5ff61d2c11a7d2b3086fc3dc99037..afda24aca4c407c61a02be57fde1a6e0429b11c9 100644
--- a/src/ui/map3D/Pixhawk3DWidget.cc
+++ b/src/ui/map3D/Pixhawk3DWidget.cc
@@ -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);
}
@@ -235,24 +241,40 @@ Pixhawk3DWidget::insertWaypoint(void)
{
if (uas)
{
- 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);
-
- std::pair<double,double> cursorWorldCoords =
- getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
-
- Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone,
- latitude, longitude);
-
- Waypoint* wp = new Waypoint(0,
- longitude,
- latitude,
- altitude);
- uas->getWaypointManager().addWaypoint(wp);
+ 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(latitude, longitude, x, y, utmZone);
+
+ std::pair<double,double> cursorWorldCoords =
+ getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
+
+ Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone,
+ latitude, longitude);
+
+ 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);
+ }
}
}
@@ -267,25 +289,40 @@ Pixhawk3DWidget::setWaypoint(void)
{
if (uas)
{
- 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);
-
- std::pair<double,double> cursorWorldCoords =
- getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
-
- 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);
+
+ if (frame == MAV_FRAME_GLOBAL)
+ {
+ double latitude = uas->getLatitude();
+ double longitude = uas->getLongitude();
+ double altitude = uas->getAltitude();
+ double x, y;
+ QString utmZone;
+ Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
+
+ std::pair<double,double> cursorWorldCoords =
+ getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
+
+ Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone,
+ latitude, longitude);
+
+ 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);
+ }
}
}
@@ -308,12 +345,25 @@ 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)
{
- waypoint->setZ(newAltitude);
+ 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,14 +930,41 @@ Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ,
std::ostringstream oss;
oss.setf(std::ios::fixed, std::ios::floatfield);
oss.precision(2);
- oss << " x = " << robotX <<
- " y = " << robotY <<
- " z = " << robotZ <<
- " r = " << robotRoll <<
- " p = " << robotPitch <<
- " y = " << robotYaw <<
- " Cursor [" << cursorPosition.first <<
- " " << cursorPosition.second << "]";
+ 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 <<
+ " r = " << robotRoll <<
+ " p = " << robotPitch <<
+ " 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)
{
diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h
index 87afd0ca27f96fa85320e5c68993b1dd930f852a..0cc499a131fa4e99c1a93be150ea6a63243115f9 100644
--- a/src/ui/map3D/Pixhawk3DWidget.h
+++ b/src/ui/map3D/Pixhawk3DWidget.h
@@ -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;
};
diff --git a/src/ui/map3D/Q3DWidget.cc b/src/ui/map3D/Q3DWidget.cc
index 797ff6120aea882c6c40e8bdca8b165ef5230d79..91e1ad093f29ff235f904dbabefed8a71aa0021d 100644
--- a/src/ui/map3D/Q3DWidget.cc
+++ b/src/ui/map3D/Q3DWidget.cc
@@ -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");
diff --git a/src/ui/map3D/WaypointGroupNode.cc b/src/ui/map3D/WaypointGroupNode.cc
new file mode 100644
index 0000000000000000000000000000000000000000..cc706157f47a0500d2195acdd278f1a3060e283f
--- /dev/null
+++ b/src/ui/map3D/WaypointGroupNode.cc
@@ -0,0 +1,173 @@
+/*=====================================================================
+
+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();
+ }
+}
diff --git a/src/ui/map3D/WaypointGroupNode.h b/src/ui/map3D/WaypointGroupNode.h
new file mode 100644
index 0000000000000000000000000000000000000000..1c1c4de8ec46e55c2f8ccb0bdfd1bd1e8d360f91
--- /dev/null
+++ b/src/ui/map3D/WaypointGroupNode.h
@@ -0,0 +1,51 @@
+/*=====================================================================
+
+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