Skip to content

Q
qgroundcontrol
Commit 7c3edae2 authored by hengli's avatar hengli
Browse files
Options

Fixed issue with transform from screen to world coordinates.

parent 1fb0417c
Show whitespace changes
Inline Side-by-side
Showing with 164 additions and 104 deletions
src/ui/map3D/Pixhawk3DWidget.cc
View file @ 7c3edae2
......@@ -38,10 +38,6 @@
#include <osg/LineWidth>
#include <osg/ShapeDrawable>
#include <osgEarth/Map>
#include <osgEarth/MapNode>
#include <osgEarthDrivers/gdal/GDALOptions>
#include "PixhawkCheetahGeode.h"
#include "UASManager.h"
#include "UASInterface.h"
......@@ -60,9 +56,6 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
init(15.0f);
setCameraParams(0.5f, 30.0f, 0.01f, 10000.0f);
osg::Node* imagery = osgDB::readNodeFile("/home/hengli/swissimage.earth");
root->addChild(imagery);
// generate Pixhawk Cheetah model
egocentricMap->addChild(PixhawkCheetahGeode::instance());
......@@ -74,6 +67,10 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
trailNode = createTrail();
rollingMap->addChild(trailNode);
// generate map model
mapNode = createMap();
root->addChild(mapNode);
// generate target model
targetNode = createTarget();
rollingMap->addChild(targetNode);
......@@ -85,7 +82,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
setupHUD();
setDisplayFunc(display, this);
// setMouseFunc(mouse, this);
setMouseFunc(mouse, this);
addTimerFunc(100, timer, this);
buildLayout();
......@@ -114,6 +111,10 @@ Pixhawk3DWidget::buildLayout(void)
waypointsCheckBox->setText("Waypoints");
waypointsCheckBox->setChecked(displayWaypoints);
targetButton = new QPushButton(this);
targetButton->setCheckable(true);
targetButton->setIcon(QIcon(QString::fromUtf8(":/images/status/weather-clear.svg")));
QPushButton* recenterButton = new QPushButton(this);
recenterButton->setText("Recenter Camera");
......@@ -127,19 +128,21 @@ Pixhawk3DWidget::buildLayout(void)
layout->addWidget(gridCheckBox, 1, 0);
layout->addWidget(trailCheckBox, 1, 1);
layout->addWidget(waypointsCheckBox, 1, 2);
layout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 3);
layout->addWidget(recenterButton, 1, 4);
layout->addWidget(lockCameraCheckBox, 1, 5);
layout->addItem(new QSpacerItem(20, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 3);
layout->addWidget(targetButton, 1, 4);
layout->addItem(new QSpacerItem(20, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 5);
layout->addWidget(recenterButton, 1, 6);
layout->addWidget(lockCameraCheckBox, 1, 7);
layout->setRowStretch(0, 100);
layout->setRowStretch(1, 1);
//layout->setColumnStretch(0, 1);
layout->setColumnStretch(2, 50);
setLayout(layout);
connect(gridCheckBox, SIGNAL(stateChanged(int)),
this, SLOT(showGrid(int)));
connect(trailCheckBox, SIGNAL(stateChanged(int)),
this, SLOT(showTrail(int)));
connect(waypointsCheckBox, SIGNAL(stateChanged(int)),
this, SLOT(showWaypoints(int)));
connect(recenterButton, SIGNAL(clicked()), this, SLOT(recenterCamera()));
connect(lockCameraCheckBox, SIGNAL(stateChanged(int)),
this, SLOT(toggleLockCamera(int)));
......@@ -174,6 +177,7 @@ Pixhawk3DWidget::displayHandler(void)
updateHUD(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw);
updateTrail(robotX, robotY, robotZ);
updateTarget(robotX, robotY, robotZ);
updateWaypoints();
// set node visibility
......@@ -195,12 +199,10 @@ void
Pixhawk3DWidget::mouseHandler(Qt::MouseButton button, MouseState state,
int32_t x, int32_t y)
{
if (button == Qt::RightButton && state == MOUSE_STATE_DOWN)
if (button == Qt::LeftButton && state == MOUSE_STATE_DOWN &&
targetButton->isChecked())
{
QMenu menu(this);
QAction* targetAction = menu.addAction(tr("Mark as Target"));
connect(targetAction, SIGNAL(triggered()), this, SLOT(markTarget()));
menu.exec(mapToGlobal(QPoint(x, y)));
markTarget();
}
}
......@@ -249,35 +251,6 @@ void Pixhawk3DWidget::setActiveUAS(UASInterface* uas)
this->uas = uas;
}
void
Pixhawk3DWidget::markTarget(void)
{
std::pair<float,float> mouseWorldCoords =
getGlobalCursorPosition(getLastMouseX(), getLastMouseY());
float robotX = 0.0f, robotY = 0.0f, robotZ = 0.0f;
if (uas != NULL)
{
robotX = uas->getLocalX();
robotY = uas->getLocalY();
robotZ = uas->getLocalZ();
}
targetPosition.x() = mouseWorldCoords.first + robotX;
targetPosition.y() = mouseWorldCoords.second + robotY;
targetPosition.z() = robotZ;
displayTarget = true;
if (uas)
{
uas->setTargetPosition(targetPosition.x(),
targetPosition.y(),
targetPosition.z(),
0.0f);
}
}
void
Pixhawk3DWidget::showGrid(int32_t state)
{
......@@ -309,6 +282,19 @@ Pixhawk3DWidget::showTrail(int32_t state)
}
}
void
Pixhawk3DWidget::showWaypoints(int state)
{
if (state == Qt::Checked)
{
displayWaypoints = true;
}
else
{
displayWaypoints = false;
}
}
void
Pixhawk3DWidget::recenterCamera(void)
{
......@@ -397,20 +383,29 @@ Pixhawk3DWidget::createTrail(void)
return geode;
}
osg::ref_ptr<osgEarth::MapNode>
Pixhawk3DWidget::createMap(void)
{
osg::ref_ptr<osg::Node> model = osgDB::readNodeFile("map.earth");
osg::ref_ptr<osgEarth::MapNode> node = osgEarth::MapNode::findMapNode(model);
return node;
}
osg::ref_ptr<osg::Group>
Pixhawk3DWidget::createTarget(void)
{
osg::ref_ptr<osg::Group> geode(new osg::Group());
osg::ref_ptr<osg::Group> group(new osg::Group());
return geode;
return group;
}
osg::ref_ptr<osg::Group>
Pixhawk3DWidget::createWaypoints(void)
{
osg::ref_ptr<osg::Group> geode(new osg::Group());
osg::ref_ptr<osg::Group> group(new osg::Group());
return geode;
return group;
}
void
......@@ -421,19 +416,31 @@ Pixhawk3DWidget::setupHUD(void)
hudBackgroundVertices->push_back(osg::Vec3(width(), height(), -1));
hudBackgroundVertices->push_back(osg::Vec3(width(), height() - 30, -1));
hudBackgroundVertices->push_back(osg::Vec3(0, height() - 30, -1));
hudBackgroundVertices->push_back(osg::Vec3(0, 0, -1));
hudBackgroundVertices->push_back(osg::Vec3(width(), 0, -1));
hudBackgroundVertices->push_back(osg::Vec3(width(), 25, -1));
hudBackgroundVertices->push_back(osg::Vec3(0, 25, -1));
osg::ref_ptr<osg::DrawElementsUInt> hudTopBackgroundIndices(
new osg::DrawElementsUInt(osg::PrimitiveSet::POLYGON, 0));
hudTopBackgroundIndices->push_back(0);
hudTopBackgroundIndices->push_back(1);
hudTopBackgroundIndices->push_back(2);
hudTopBackgroundIndices->push_back(3);
osg::ref_ptr<osg::DrawElementsUInt> hudBackgroundIndices(
osg::ref_ptr<osg::DrawElementsUInt> hudBottomBackgroundIndices(
new osg::DrawElementsUInt(osg::PrimitiveSet::POLYGON, 0));
hudBackgroundIndices->push_back(0);
hudBackgroundIndices->push_back(1);
hudBackgroundIndices->push_back(2);
hudBackgroundIndices->push_back(3);
hudBottomBackgroundIndices->push_back(4);
hudBottomBackgroundIndices->push_back(5);
hudBottomBackgroundIndices->push_back(6);
hudBottomBackgroundIndices->push_back(7);
osg::ref_ptr<osg::Vec4Array> hudColors(new osg::Vec4Array);
hudColors->push_back(osg::Vec4(0.0f, 0.0f, 0.0f, 0.2f));
hudBackgroundGeometry = new osg::Geometry;
hudBackgroundGeometry->addPrimitiveSet(hudBackgroundIndices);
hudBackgroundGeometry->addPrimitiveSet(hudTopBackgroundIndices);
hudBackgroundGeometry->addPrimitiveSet(hudBottomBackgroundIndices);
hudBackgroundGeometry->setVertexArray(hudBackgroundVertices);
hudBackgroundGeometry->setColorArray(hudColors);
hudBackgroundGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);
......@@ -459,12 +466,16 @@ Pixhawk3DWidget::updateHUD(float robotX, float robotY, float robotZ,
hudBackgroundVertices->push_back(osg::Vec3(width(), height(), -1));
hudBackgroundVertices->push_back(osg::Vec3(width(), height() - 30, -1));
hudBackgroundVertices->push_back(osg::Vec3(0, height() - 30, -1));
hudBackgroundVertices->push_back(osg::Vec3(0, 0, -1));
hudBackgroundVertices->push_back(osg::Vec3(width(), 0, -1));
hudBackgroundVertices->push_back(osg::Vec3(width(), 25, -1));
hudBackgroundVertices->push_back(osg::Vec3(0, 25, -1));
hudBackgroundGeometry->setVertexArray(hudBackgroundVertices);
statusText->setPosition(osg::Vec3(10, height() - 20, -1.5));
std::pair<float,float> cursorPosition =
getGlobalCursorPosition(getMouseX(), getMouseY());
std::pair<double,double> cursorPosition =
getGlobalCursorPosition(getMouseX(), getMouseY(), -robotZ);
std::ostringstream oss;
oss.setf(std::ios::fixed, std::ios::floatfield);
......@@ -546,6 +557,11 @@ Pixhawk3DWidget::updateTarget(float robotX, float robotY, float robotZ)
expand = false;
}
if (targetNode->getNumChildren() > 0)
{
targetNode->removeChild(0, targetNode->getNumChildren());
}
osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
osg::ref_ptr<osg::Sphere> sphere = new osg::Sphere;
sphere->setRadius(radius);
......@@ -618,3 +634,33 @@ Pixhawk3DWidget::updateWaypoints(void)
}
}
}
void
Pixhawk3DWidget::markTarget(void)
{
float robotZ = 0.0f;
if (uas != NULL)
{
robotZ = uas->getLocalZ();
}
std::pair<double,double> mouseWorldCoords =
getGlobalCursorPosition(getLastMouseX(), getLastMouseY(), -robotZ);
targetPosition.x() = mouseWorldCoords.first;
targetPosition.y() = mouseWorldCoords.second;
targetPosition.z() = robotZ;
displayTarget = true;
if (uas)
{
uas->setTargetPosition(targetPosition.x(),
targetPosition.y(),
targetPosition.z(),
0.0f);
}
targetButton->setChecked(false);
}
src/ui/map3D/Pixhawk3DWidget.h
View file @ 7c3edae2
......@@ -33,6 +33,7 @@
#define PIXHAWK3DWIDGET_H
#include <osgText/Text>
#include <osgEarth/MapNode>
#include "Q3DWidget.h"
......@@ -66,11 +67,11 @@ public:
public slots:
void setActiveUAS(UASInterface* uas);
void markTarget(void);
private slots:
void showGrid(int state);
void showTrail(int state);
void showWaypoints(int state);
void recenterCamera(void);
void toggleLockCamera(int state);
......@@ -80,6 +81,7 @@ protected:
private:
osg::ref_ptr<osg::Geode> createGrid(void);
osg::ref_ptr<osg::Geode> createTrail(void);
osg::ref_ptr<osgEarth::MapNode> createMap(void);
osg::ref_ptr<osg::Group> createTarget(void);
osg::ref_ptr<osg::Group> createWaypoints(void);
......@@ -91,6 +93,8 @@ private:
void updateTarget(float robotX, float robotY, float robotZ);
void updateWaypoints(void);
void markTarget(void);
double lastRedrawTime;
bool displayGrid;
......@@ -98,13 +102,6 @@ private:
bool displayTarget;
bool displayWaypoints;
typedef struct
{
float x;
float y;
float z;
} Pose3D;
bool lockCamera;
osg::ref_ptr<osg::Vec3Array> trailVertices;
......@@ -118,8 +115,11 @@ private:
osg::ref_ptr<osg::Geode> trailNode;
osg::ref_ptr<osg::Geometry> trailGeometry;
osg::ref_ptr<osg::DrawArrays> trailDrawArrays;
osg::ref_ptr<osgEarth::MapNode> mapNode;
osg::ref_ptr<osg::Group> targetNode;
osg::ref_ptr<osg::Group> waypointsNode;
QPushButton* targetButton;
};
#endif // PIXHAWK3DWIDGET_H
src/ui/map3D/Q3DWidget.cc
View file @ 7c3edae2
......@@ -262,24 +262,29 @@ Q3DWidget::addTimerFunc(uint32_t msecs, void(*func)(void *),
QTimer::singleShot(msecs, this, SLOT(userTimer()));
}
std::pair<float,float>
Q3DWidget::getGlobalCursorPosition(int32_t mouseX, int32_t mouseY)
std::pair<double,double>
Q3DWidget::getGlobalCursorPosition(int32_t mouseX, int32_t mouseY,
double z)
{
osgUtil::LineSegmentIntersector::Intersections intersections;
// normalize cursor position to value between -1 and 1
float x = -1.0f + static_cast<float>(2 * mouseX)
/ static_cast<float>(width());
float y = -1.0f + static_cast<float>(2 * (height() - mouseY))
/ static_cast<float>(height());
double x = -1.0f + static_cast<double>(2 * mouseX)
/ static_cast<double>(width());
double y = -1.0f + static_cast<double>(2 * (height() - mouseY))
/ static_cast<double>(height());
osg::Matrixd m = getCamera()->getViewMatrix()
* getCamera()->getProjectionMatrix();
osg::Matrixd invM = osg::Matrixd::inverse(m);
osg::Matrix pm = getCamera()->getProjectionMatrix();
osg::Matrix vm = getCamera()->getViewMatrix();
osg::Vec3d nearPoint = osg::Vec3d(x, y, -1.0) * invM;
osg::Vec3d farPoint = osg::Vec3d(x, y, 1.0) * invM;
osg::ref_ptr<osg::LineSegment> line =
projectNormalizedXYIntoObjectCoordinates(pm, vm, x, y);
new osg::LineSegment(nearPoint, farPoint);
osg::Plane p(line->start() - line->end(), 0.0);
osg::Plane p(osg::Vec3d(0.0, 0.0, 1.0), osg::Vec3d(0.0, 0.0, z));
osg::Vec3d projectedPoint;
getPlaneLineIntersection(p.asVec4(), *line, projectedPoint);
......@@ -618,24 +623,6 @@ Q3DWidget::convertKey(int key) const
}
}
osg::ref_ptr<osg::LineSegment>
Q3DWidget::projectNormalizedXYIntoObjectCoordinates(
const osg::Matrix& projectionMatrix,
const osg::Matrix& viewMatrix,
float x,
float y) const
{
osg::Matrix matrix = viewMatrix * projectionMatrix;
osg::Matrix inverseVP;
inverseVP.invert(matrix);
osg::Vec3 nearPoint = osg::Vec3(x, y, -1.0f) * inverseVP;
osg::Vec3 farPoint = osg::Vec3(x, y, 1.0f) * inverseVP;
return osg::ref_ptr<osg::LineSegment>(
new osg::LineSegment(nearPoint, farPoint));
}
bool
Q3DWidget::getPlaneLineIntersection(const osg::Vec4d& plane,
const osg::LineSegment& line,
......
src/ui/map3D/Q3DWidget.h
View file @ 7c3edae2
......@@ -73,8 +73,9 @@ public:
void addTimerFunc(uint msecs, void(*func)(void *),
void* clientData);
std::pair<float,float> getGlobalCursorPosition(int32_t mouseX,
int32_t mouseY);
std::pair<double,double> getGlobalCursorPosition(int32_t mouseX,
int32_t mouseY,
double z);
protected slots:
void redraw(void);
......@@ -104,11 +105,6 @@ protected:
float r2d(float angle);
float d2r(float angle);
osgGA::GUIEventAdapter::KeySymbol convertKey(int key) const;
osg::ref_ptr<osg::LineSegment> projectNormalizedXYIntoObjectCoordinates(
const osg::Matrix& projectionMatrix,
const osg::Matrix& viewMatrix,
float x,
float y) const;
bool getPlaneLineIntersection(const osg::Vec4d& plane,
const osg::LineSegment& line,
osg::Vec3d& isect);
......
src/ui/map3D/Q3DWidgetFactory.cc
View file @ 7c3edae2
/*=====================================================================
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 Q3DWidgetFactory.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
*/
#include "Q3DWidgetFactory.h"
#include "Pixhawk3DWidget.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