rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-09 20:46:07 +01:00
Code Issues Releases Activity
5284195c29
LibrePilot/ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp

294 lines
10 KiB
C++
Raw Blame History

/********************************************************************************
* @file osgviewerwidget.cpp
* @author The OpenPilot Team Copyright (C) 2012.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup OsgEarthview Plugin Widget
* @{
* @brief Osg Earth view of UAV
*****************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "osgviewerwidget.h"
#include "osgearthviewwidget.h"
#include <utils/stylehelper.h>
#include <iostream>
#include <QDebug>
#include <QPainter>
#include <QtOpenGL/QGLWidget>
#include <cmath>
#include <QtGui/QApplication>
#include <QLabel>
#include <QDebug>
#include <QtCore/QTimer>
#include <QtGui/QApplication>
#include <QtGui/QGridLayout>
#include <osg/Notify>
#include <osg/PositionAttitudeTransform>
#include <osgUtil/Optimizer>
#include <osgGA/StateSetManipulator>
#include <osgGA/GUIEventHandler>
#include <osgGA/NodeTrackerManipulator>
#include <osgViewer/Viewer>
#include <osgViewer/ViewerEventHandlers>
#include <osgEarth/MapNode>
#include <osgEarth/XmlUtils>
#include <osgEarth/Viewpoint>
#include <osgEarthSymbology/Color>
#include <osgEarthAnnotation/AnnotationRegistry>
#include <osgEarthAnnotation/AnnotationData>
#include <osgEarthAnnotation/Decluttering>
#include <osgEarthDrivers/kml/KML>
#include <osgEarthDrivers/ocean_surface/OceanSurface>
#include <osgEarthDrivers/cache_filesystem/FileSystemCache>
#include <osgEarthUtil/EarthManipulator>
#include <osgEarthUtil/AutoClipPlaneHandler>
#include <osgEarthUtil/Controls>
#include <osgEarthUtil/SkyNode>
#include <osgEarthUtil/LatLongFormatter>
#include <osgEarthUtil/MouseCoordsTool>
#include <osgEarthUtil/ObjectLocator>
using namespace osgEarth::Util;
using namespace osgEarth::Util::Controls;
using namespace osgEarth::Symbology;
using namespace osgEarth::Drivers;
using namespace osgEarth::Annotation;
#include <osgViewer/CompositeViewer>
#include <osgViewer/ViewerEventHandlers>
#include <osgGA/TrackballManipulator>
#include <osgDB/ReadFile>
#include <osgQt/GraphicsWindowQt>
#include <iostream>
#include "utils/stylehelper.h"
#include "utils/homelocationutil.h"
#include "utils/worldmagmodel.h"
#include "utils/coordinateconversions.h"
#include "attitudestate.h"
#include "gpsposition.h"
#include "homelocation.h"
#include "positionstate.h"
#include "systemsettings.h"
using namespace Utils;
OsgViewerWidget::OsgViewerWidget(QWidget *parent) : QWidget(parent)
{
setThreadingModel(osgViewer::ViewerBase::CullThreadPerCameraDrawThreadPerContext);
setAttribute(Qt::WA_PaintOnScreen, true);
osg::Group *root = new osg::Group;
osg::Node *earth = osgDB::readNodeFile("/Users/Cotton/Programming/osg/osgearth/tests/boston.earth");
mapNode = osgEarth::MapNode::findMapNode(earth);
if (!mapNode) {
qDebug() << "Uhoh";
}
root->addChild(earth);
osg::Node *airplane = createAirplane();
uavPos = new osgEarth::Util::ObjectLocatorNode(mapNode->getMap());
uavPos->getLocator()->setPosition(osg::Vec3d(-71.100549, 42.349273, 150));
uavPos->addChild(airplane);
root->addChild(uavPos);
osgUtil::Optimizer optimizer;
optimizer.optimize(root);
QWidget *viewWidget = createViewWidget(createCamera(0, 0, 200, 200, "Earth", false), root);
viewWidget->show();
setLayout(new QVBoxLayout(this));
viewWidget->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
layout()->addWidget(viewWidget);
connect(&_timer, SIGNAL(timeout()), this, SLOT(update()));
_timer.start(10);
}
OsgViewerWidget::~OsgViewerWidget()
{}
QWidget *OsgViewerWidget::createViewWidget(osg::Camera *camera, osg::Node *scene)
{
osgViewer::View *view = new osgViewer::View;
view->setCamera(camera);
addView(view);
view->setSceneData(scene);
view->addEventHandler(new osgViewer::StatsHandler);
view->getDatabasePager()->setDoPreCompile(true);
manip = new EarthManipulator();
view->setCameraManipulator(manip);
// osgGA::NodeTrackerManipulator *camTracker = new osgGA::NodeTrackerManipulator();
// camTracker->setTrackNode(uavPos);
// camTracker->setMinimumDistance(0.0001f);
// camTracker->setDistance(0.001f);
// camTracker->setTrackerMode(osgGA::NodeTrackerManipulator::NODE_CENTER);
// view->setCameraManipulator(camTracker);
Grid *grid = new Grid();
grid->setControl(0, 0, new LabelControl("OpenPilot"));
ControlCanvas::get(view, true)->addControl(grid);
// zoom to a good startup position
manip->setViewpoint(Viewpoint(-71.100549, 42.349273, 0, 24.261, -21.6, 350.0), 5.0);
// manip->setViewpoint( Viewpoint(-71.100549, 42.349273, 0, 24.261, -81.6, 650.0), 5.0 );
// manip->setHomeViewpoint(Viewpoint("Boston", osg::Vec3d(-71.0763, 42.34425, 0), 24.261, -21.6, 3450.0));
manip->setTetherNode(uavPos);
osgQt::GraphicsWindowQt *gw = dynamic_cast<osgQt::GraphicsWindowQt *>(camera->getGraphicsContext());
return gw ? gw->getGLWidget() : NULL;
}
osg::Camera *OsgViewerWidget::createCamera(int x, int y, int w, int h, const std::string & name = "", bool windowDecoration = false)
{
osg::DisplaySettings *ds = osg::DisplaySettings::instance().get();
osg::ref_ptr<osg::GraphicsContext::Traits> traits = new osg::GraphicsContext::Traits;
traits->windowName = name;
traits->windowDecoration = windowDecoration;
traits->x = x;
traits->y = y;
traits->width = w;
traits->height = h;
traits->doubleBuffer = true;
traits->alpha = ds->getMinimumNumAlphaBits();
traits->stencil = ds->getMinimumNumStencilBits();
traits->sampleBuffers = ds->getMultiSamples();
traits->samples = ds->getNumMultiSamples();
osg::ref_ptr<osg::Camera> camera = new osg::Camera;
camera->setGraphicsContext(new osgQt::GraphicsWindowQt(traits.get()));
camera->setClearColor(osg::Vec4(0.2, 0.2, 0.6, 1.0));
camera->setViewport(new osg::Viewport(0, 0, traits->width, traits->height));
camera->setProjectionMatrixAsPerspective(
30.0f, static_cast<double>(traits->width) / static_cast<double>(traits->height), 1.0f, 10000.0f);
return camera.release();
}
osg::Node *OsgViewerWidget::createAirplane()
{
osg::Group *model = new osg::Group;
osg::Node *uav;
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
Q_ASSERT(pm);
UAVObjectManager *objMngr = pm->getObject<UAVObjectManager>();
Q_ASSERT(objMngr);
SystemSettings *systemSettingsObj = SystemSettings::GetInstance(objMngr);
SystemSettings::DataFields systemSettings = systemSettingsObj->getData();
qDebug() << "Frame type:" << systemSettingsObj->getField("AirframeType")->getValue().toString();
// Get model that matches airframe type
switch (systemSettings.AirframeType) {
case SystemSettings::AIRFRAMETYPE_FIXEDWING:
case SystemSettings::AIRFRAMETYPE_FIXEDWINGELEVON:
case SystemSettings::AIRFRAMETYPE_FIXEDWINGVTAIL:
uav = osgDB::readNodeFile("/Users/Cotton/Programming/OpenPilot/artwork/3D Model/planes/Easystar/easystar.3ds");
break;
default:
uav = osgDB::readNodeFile("/Users/Cotton/Programming/OpenPilot/artwork/3D Model/multi/joes_cnc/J14-QT_+.3DS");
}
if (uav) {
uavAttitudeAndScale = new osg::MatrixTransform();
uavAttitudeAndScale->setMatrix(osg::Matrixd::scale(0.2e0, 0.2e0, 0.2e0));
// Apply a rotation so model is NED before any other rotations
osg::MatrixTransform *rotateModelNED = new osg::MatrixTransform();
rotateModelNED->setMatrix(osg::Matrixd::scale(0.05e0, 0.05e0, 0.05e0) * osg::Matrixd::rotate(M_PI, osg::Vec3d(0, 0, 1)));
rotateModelNED->addChild(uav);
uavAttitudeAndScale->addChild(rotateModelNED);
model->addChild(uavAttitudeAndScale);
} else {
qDebug() << "Bad model file";
}
return model;
}
void OsgViewerWidget::paintEvent(QPaintEvent *event)
{
Q_UNUSED(event);
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objMngr = pm->getObject<UAVObjectManager>();
PositionState *positionStateObj = PositionState::GetInstance(objMngr);
PositionState::DataFields positionState = positionStateObj->getData();
double NED[3] = { positionState.North, positionState.East, positionState.Down };
bool positionStateUpdate = true;
if (positionStateUpdate) {
HomeLocation *homeLocationObj = HomeLocation::GetInstance(objMngr);
HomeLocation::DataFields homeLocation = homeLocationObj->getData();
double homeLLA[3] = { homeLocation.Latitude / 10.0e6, homeLocation.Longitude / 10.0e6, homeLocation.Altitude };
double LLA[3];
CoordinateConversions().NED2LLA_HomeLLA(homeLLA, NED, LLA);
uavPos->getLocator()->setPosition(osg::Vec3d(LLA[1], LLA[0], LLA[2])); // Note this takes longtitude first
} else {
GPSPosition *gpsPosObj = GPSPosition::GetInstance(objMngr);
GPSPosition::DataFields gpsPos = gpsPosObj->getData();
uavPos->getLocator()->setPosition(osg::Vec3d(gpsPos.Longitude / 10.0e6, gpsPos.Latitude / 10.0e6, gpsPos.Altitude));
}
// Set the attitude (reverse the attitude)
AttitudeState *attitudeStateObj = AttitudeState::GetInstance(objMngr);
AttitudeState::DataFields attitudeState = attitudeStateObj->getData();
osg::Quat quat(attitudeState.q2, attitudeState.q3, attitudeState.q4, attitudeState.q1);
// Have to rotate the axes from OP NED frame to OSG frame (X east, Y north, Z down)
double angle;
osg::Vec3d axis;
quat.getRotate(angle, axis);
quat.makeRotate(angle, osg::Vec3d(axis[1], axis[0], -axis[2]));
osg::Matrixd rot = osg::Matrixd::rotate(quat);
uavAttitudeAndScale->setMatrix(rot);
frame();
}
Reference in New Issue View Git Blame Copy Permalink