ground/openpilotgcs/src/plugins/osgearthview/osgviewerwidget.cpp

   1 /********************************************************************************
   2  * @file       osgviewerwidget.cpp
   3  * @author     The OpenPilot Team Copyright (C) 2012.
   4  * @addtogroup GCSPlugins GCS Plugins
   5  * @{
   6  * @addtogroup OsgEarthview Plugin Widget
   7  * @{
   8  * @brief Osg Earth view of UAV
   9  *****************************************************************************/
  10 /*
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License as published by
  13  * the Free Software Foundation; either version 3 of the License, or
  14  * (at your option) any later version.
  15  *
  16  * This program is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  18  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  19  * for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License along
  22  * with this program; if not, write to the Free Software Foundation, Inc.,
  23  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  24  */
  25
  26 #include "osgviewerwidget.h"
  27 #include "osgearthviewwidget.h"
  28 #include <utils/stylehelper.h>
  29 #include <iostream>
  30 #include <QDebug>
  31 #include <QPainter>
  32 #include <QtOpenGL/QGLWidget>
  33 #include <cmath>
  34 #include <QtWidgets/QApplication>
  35 #include <QLabel>
  36 #include <QDebug>
  37
  38 #include <QtCore/QTimer>
  39 #include <QtWidgets/QApplication>
  40 #include <QGridLayout>
  41
  42
  43 #include <osg/Notify>
  44 #include <osg/PositionAttitudeTransform>
  45
  46 #include <osgUtil/Optimizer>
  47
  48 #include <osgGA/StateSetManipulator>
  49 #include <osgGA/GUIEventHandler>
  50 #include <osgGA/NodeTrackerManipulator>
  51
  52 #include <osgViewer/Viewer>
  53 #include <osgViewer/ViewerEventHandlers>
  54
  55 #include <osgEarth/MapNode>
  56 #include <osgEarth/XmlUtils>
  57 #include <osgEarth/Viewpoint>
  58
  59 #include <osgEarthSymbology/Color>
  60
  61 #include <osgEarthAnnotation/AnnotationRegistry>
  62 #include <osgEarthAnnotation/AnnotationData>
  63 #include <osgEarthAnnotation/Decluttering>
  64
  65 #include <osgEarthDrivers/kml/KML>
  66 #include <osgEarthDrivers/ocean_surface/OceanSurface>
  67 #include <osgEarthDrivers/cache_filesystem/FileSystemCache>
  68
  69 #include <osgEarthUtil/EarthManipulator>
  70 #include <osgEarthUtil/AutoClipPlaneHandler>
  71 #include <osgEarthUtil/Controls>
  72 #include <osgEarthUtil/SkyNode>
  73 #include <osgEarthUtil/LatLongFormatter>
  74 #include <osgEarthUtil/MouseCoordsTool>
  75 #include <osgEarthUtil/ObjectLocator>
  76
  77 using namespace osgEarth::Util;
  78 using namespace osgEarth::Util::Controls;
  79 using namespace osgEarth::Symbology;
  80 using namespace osgEarth::Drivers;
  81 using namespace osgEarth::Annotation;
  82
  83 #include <osgViewer/CompositeViewer>
  84 #include <osgViewer/ViewerEventHandlers>
  85
  86 #include <osgGA/TrackballManipulator>
  87
  88 #include <osgDB/ReadFile>
  89
  90 #include <osgQt/GraphicsWindowQt>
  91
  92 #include <iostream>
  93
  94 #include "utils/stylehelper.h"
  95 #include "utils/homelocationutil.h"
  96 #include "utils/worldmagmodel.h"
  97 #include "utils/coordinateconversions.h"
  98 #include "attitudestate.h"
  99 #include "gpspositionsensor.h"
 100 #include "homelocation.h"
 101 #include "positionstate.h"
 102 #include "systemsettings.h"
 103
 104 using namespace Utils;
 105
 106 OsgViewerWidget::OsgViewerWidget(QWidget *parent) : QWidget(parent)
 107 {
 108     setThreadingModel(osgViewer::ViewerBase::CullThreadPerCameraDrawThreadPerContext);
 109     setAttribute(Qt::WA_PaintOnScreen, true);
 110
 111     osg::Group *root = new osg::Group;
 112     osg::Node *earth = osgDB::readNodeFile("/Users/Cotton/Programming/osg/osgearth/tests/boston.earth");
 113     mapNode = osgEarth::MapNode::findMapNode(earth);
 114     if (!mapNode) {
 115         qDebug() << "Uhoh";
 116     }
 117
 118     root->addChild(earth);
 119
 120     osg::Node *airplane = createAirplane();
 121     uavPos = new osgEarth::Util::ObjectLocatorNode(mapNode->getMap());
 122     uavPos->getLocator()->setPosition(osg::Vec3d(-71.100549, 42.349273, 150));
 123     uavPos->addChild(airplane);
 124
 125     root->addChild(uavPos);
 126
 127     osgUtil::Optimizer optimizer;
 128     optimizer.optimize(root);
 129
 130     QWidget *viewWidget = createViewWidget(createCamera(0, 0, 200, 200, "Earth", false), root);
 131     viewWidget->show();
 132     setLayout(new QVBoxLayout(this));
 133
 134     viewWidget->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
 135     layout()->addWidget(viewWidget);
 136
 137
 138     connect(&_timer, SIGNAL(timeout()), this, SLOT(update()));
 139     _timer.start(10);
 140 }
 141
 142 OsgViewerWidget::~OsgViewerWidget()
 143 {}
 144
 145 QWidget *OsgViewerWidget::createViewWidget(osg::Camera *camera, osg::Node *scene)
 146 {
 147     osgViewer::View *view = new osgViewer::View;
 148
 149     view->setCamera(camera);
 150
 151     addView(view);
 152
 153     view->setSceneData(scene);
 154     view->addEventHandler(new osgViewer::StatsHandler);
 155     view->getDatabasePager()->setDoPreCompile(true);
 156
 157     manip = new EarthManipulator();
 158     view->setCameraManipulator(manip);
 159
 160 // osgGA::NodeTrackerManipulator *camTracker = new osgGA::NodeTrackerManipulator();
 161 // camTracker->setTrackNode(uavPos);
 162 // camTracker->setMinimumDistance(0.0001f);
 163 // camTracker->setDistance(0.001f);
 164 // camTracker->setTrackerMode(osgGA::NodeTrackerManipulator::NODE_CENTER);
 165 // view->setCameraManipulator(camTracker);
 166
 167     Grid *grid = new Grid();
 168     grid->setControl(0, 0, new LabelControl("OpenPilot"));
 169     ControlCanvas::get(view, true)->addControl(grid);
 170
 171     // zoom to a good startup position
 172     manip->setViewpoint(Viewpoint(-71.100549, 42.349273, 0, 24.261, -21.6, 350.0), 5.0);
 173     // manip->setViewpoint( Viewpoint(-71.100549, 42.349273, 0, 24.261, -81.6, 650.0), 5.0 );
 174     // manip->setHomeViewpoint(Viewpoint("Boston", osg::Vec3d(-71.0763, 42.34425, 0), 24.261, -21.6, 3450.0));
 175
 176     manip->setTetherNode(uavPos);
 177
 178     osgQt::GraphicsWindowQt *gw = dynamic_cast<osgQt::GraphicsWindowQt *>(camera->getGraphicsContext());
 179     return gw ? gw->getGLWidget() : NULL;
 180 }
 181
 182 osg::Camera *OsgViewerWidget::createCamera(int x, int y, int w, int h, const std::string & name = "", bool windowDecoration = false)
 183 {
 184     osg::DisplaySettings *ds = osg::DisplaySettings::instance().get();
 185     osg::ref_ptr<osg::GraphicsContext::Traits> traits = new osg::GraphicsContext::Traits;
 186
 187     traits->windowName = name;
 188     traits->windowDecoration = windowDecoration;
 189     traits->x       = x;
 190     traits->y       = y;
 191     traits->width   = w;
 192     traits->height  = h;
 193     traits->doubleBuffer = true;
 194     traits->alpha   = ds->getMinimumNumAlphaBits();
 195     traits->stencil = ds->getMinimumNumStencilBits();
 196     traits->sampleBuffers = ds->getMultiSamples();
 197     traits->samples = ds->getNumMultiSamples();
 198
 199     osg::ref_ptr<osg::Camera> camera = new osg::Camera;
 200     camera->setGraphicsContext(new osgQt::GraphicsWindowQt(traits.get()));
 201
 202     camera->setClearColor(osg::Vec4(0.2, 0.2, 0.6, 1.0));
 203     camera->setViewport(new osg::Viewport(0, 0, traits->width, traits->height));
 204     camera->setProjectionMatrixAsPerspective(
 205         30.0f, static_cast<double>(traits->width) / static_cast<double>(traits->height), 1.0f, 10000.0f);
 206     return camera.release();
 207 }
 208
 209 osg::Node *OsgViewerWidget::createAirplane()
 210 {
 211     osg::Group *model = new osg::Group;
 212     osg::Node *uav;
 213
 214     ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
 215
 216     Q_ASSERT(pm);
 217     UAVObjectManager *objMngr = pm->getObject<UAVObjectManager>();
 218     Q_ASSERT(objMngr);
 219
 220     SystemSettings *systemSettingsObj = SystemSettings::GetInstance(objMngr);
 221     SystemSettings::DataFields systemSettings = systemSettingsObj->getData();
 222
 223     qDebug() << "Frame type:" << systemSettingsObj->getField("AirframeType")->getValue().toString();
 224     // Get model that matches airframe type
 225     switch (systemSettings.AirframeType) {
 226     case SystemSettings::AIRFRAMETYPE_FIXEDWING:
 227     case SystemSettings::AIRFRAMETYPE_FIXEDWINGELEVON:
 228     case SystemSettings::AIRFRAMETYPE_FIXEDWINGVTAIL:
 229         uav = osgDB::readNodeFile("/Users/Cotton/Programming/OpenPilot/artwork/3D Model/planes/Easystar/easystar.3ds");
 230         break;
 231     default:
 232         uav = osgDB::readNodeFile("/Users/Cotton/Programming/OpenPilot/artwork/3D Model/multi/joes_cnc/J14-QT_+.3DS");
 233     }
 234
 235     if (uav) {
 236         uavAttitudeAndScale = new osg::MatrixTransform();
 237         uavAttitudeAndScale->setMatrix(osg::Matrixd::scale(0.2e0, 0.2e0, 0.2e0));
 238
 239         // Apply a rotation so model is NED before any other rotations
 240         osg::MatrixTransform *rotateModelNED = new osg::MatrixTransform();
 241         rotateModelNED->setMatrix(osg::Matrixd::scale(0.05e0, 0.05e0, 0.05e0) * osg::Matrixd::rotate(M_PI, osg::Vec3d(0, 0, 1)));
 242         rotateModelNED->addChild(uav);
 243
 244         uavAttitudeAndScale->addChild(rotateModelNED);
 245
 246         model->addChild(uavAttitudeAndScale);
 247     } else {
 248         qDebug() << "Bad model file";
 249     }
 250     return model;
 251 }
 252
 253 void OsgViewerWidget::paintEvent(QPaintEvent *event)
 254 {
 255     Q_UNUSED(event);
 256     ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
 257     UAVObjectManager *objMngr = pm->getObject<UAVObjectManager>();
 258
 259     PositionState *positionStateObj    = PositionState::GetInstance(objMngr);
 260     PositionState::DataFields positionState = positionStateObj->getData();
 261     double NED[3] = { positionState.North, positionState.East, positionState.Down };
 262
 263     bool positionStateUpdate = true;
 264     if (positionStateUpdate) {
 265         HomeLocation *homeLocationObj = HomeLocation::GetInstance(objMngr);
 266         HomeLocation::DataFields homeLocation = homeLocationObj->getData();
 267         double homeLLA[3] = { homeLocation.Latitude / 10.0e6, homeLocation.Longitude / 10.0e6, homeLocation.Altitude };
 268
 269         double LLA[3];
 270         CoordinateConversions().NED2LLA_HomeLLA(homeLLA, NED, LLA);
 271         uavPos->getLocator()->setPosition(osg::Vec3d(LLA[1], LLA[0], LLA[2])); // Note this takes longtitude first
 272     } else {
 273         GPSPositionSensor *gpsPosObj = GPSPositionSensor::GetInstance(objMngr);
 274         GPSPositionSensor::DataFields gpsPos = gpsPosObj->getData();
 275         uavPos->getLocator()->setPosition(osg::Vec3d(gpsPos.Longitude / 10.0e6, gpsPos.Latitude / 10.0e6, gpsPos.Altitude));
 276     }
 277
 278     // Set the attitude (reverse the attitude)
 279     AttitudeState *attitudeStateObj = AttitudeState::GetInstance(objMngr);
 280     AttitudeState::DataFields attitudeState = attitudeStateObj->getData();
 281     osg::Quat quat(attitudeState.q2, attitudeState.q3, attitudeState.q4, attitudeState.q1);
 282
 283     // Have to rotate the axes from OP NED frame to OSG frame (X east, Y north, Z down)
 284     double angle;
 285     osg::Vec3d axis;
 286     quat.getRotate(angle, axis);
 287     quat.makeRotate(angle, osg::Vec3d(axis[1], axis[0], -axis[2]));
 288     osg::Matrixd rot = osg::Matrixd::rotate(quat);
 289
 290     uavAttitudeAndScale->setMatrix(rot);
 291
 292     frame();
 293 }