Pixhawk3DWidget.cc 40.8 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36
///*=====================================================================
//
//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 Pixhawk3DWidget.
 *
 *   @author Lionel Heng <hengli@student.ethz.ch>
 *
 */

#include "Pixhawk3DWidget.h"

#include <sstream>

#include <osg/Geode>
37
#include <osg/Image>
38
#include <osgDB/ReadFile>
39 40 41 42 43
#include <osg/LineWidth>
#include <osg/ShapeDrawable>

#include "PixhawkCheetahGeode.h"
#include "UASManager.h"
44

45 46 47
#include "QGC.h"

Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65
    : Q3DWidget(parent)
    , uas(NULL)
    , mode(DEFAULT_MODE)
    , selectedWpIndex(-1)
    , displayGrid(true)
    , displayTrail(false)
    , displayImagery(true)
    , displayWaypoints(true)
    , displayRGBD2D(false)
    , displayRGBD3D(false)
    , enableRGBDColor(true)
    , enableTarget(false)
    , followCamera(true)
    , enableFreenect(false)
    , frame(MAV_FRAME_GLOBAL)
    , lastRobotX(0.0f)
    , lastRobotY(0.0f)
    , lastRobotZ(0.0f)
66
{
67
    setCameraParams(2.0f, 30.0f, 0.01f, 10000.0f);
68 69 70
    init(15.0f);

    // generate Pixhawk Cheetah model
71 72
    vehicleModel = PixhawkCheetahGeode::instance();
    egocentricMap->addChild(vehicleModel);
73 74 75 76 77 78 79 80 81

    // generate grid model
    gridNode = createGrid();
    rollingMap->addChild(gridNode);

    // generate empty trail model
    trailNode = createTrail();
    rollingMap->addChild(trailNode);

82 83
    // generate map model
    mapNode = createMap();
84
    rollingMap->addChild(mapNode);
85

86
    // generate waypoint model
87 88 89
    waypointGroupNode = new WaypointGroupNode;
    waypointGroupNode->init();
    rollingMap->addChild(waypointGroupNode);
90

91 92 93 94
    // generate target model
    targetNode = createTarget();
    rollingMap->addChild(targetNode);

95 96
#ifdef QGC_LIBFREENECT_ENABLED
    freenect.reset(new Freenect());
97
    enableFreenect = freenect->init();
98 99
#endif

100
    // generate RGBD model
101
    if (enableFreenect) {
102 103 104
        rgbd3DNode = createRGBD3D();
        egocentricMap->addChild(rgbd3DNode);
    }
105

106 107
    setupHUD();

108 109 110
    // find available vehicle models in models folder
    vehicleModels = findVehicleModels();

111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127
    buildLayout();

    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
            this, SLOT(setActiveUAS(UASInterface*)));
}

Pixhawk3DWidget::~Pixhawk3DWidget()
{

}

/**
 *
 * @param uas the UAS/MAV to monitor/display with the HUD
 */
void Pixhawk3DWidget::setActiveUAS(UASInterface* uas)
{
128
    if (this->uas != NULL && this->uas != uas) {
129 130 131 132 133 134 135
        // Disconnect any previously connected active MAV
        //disconnect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64)));
    }

    this->uas = uas;
}

136 137 138
void
Pixhawk3DWidget::selectFrame(QString text)
{
139
    if (text.compare("Global") == 0) {
140
        frame = MAV_FRAME_GLOBAL;
141
    } else if (text.compare("Local") == 0) {
142 143 144 145 146 147 148 149
        frame = MAV_FRAME_LOCAL;
    }

    getPosition(lastRobotX, lastRobotY, lastRobotZ);

    recenter();
}

150 151 152
void
Pixhawk3DWidget::showGrid(int32_t state)
{
153
    if (state == Qt::Checked) {
154
        displayGrid = true;
155
    } else {
156 157 158 159 160 161 162
        displayGrid = false;
    }
}

void
Pixhawk3DWidget::showTrail(int32_t state)
{
163 164
    if (state == Qt::Checked) {
        if (!displayTrail) {
165
            trail.clear();
166 167 168
        }

        displayTrail = true;
169
    } else {
170 171 172 173
        displayTrail = false;
    }
}

174 175 176
void
Pixhawk3DWidget::showWaypoints(int state)
{
177
    if (state == Qt::Checked) {
178
        displayWaypoints = true;
179
    } else {
180 181 182 183
        displayWaypoints = false;
    }
}

184 185 186 187
void
Pixhawk3DWidget::selectMapSource(int index)
{
    mapNode->setImageryType(static_cast<Imagery::ImageryType>(index));
188

189
    if (mapNode->getImageryType() == Imagery::BLANK_MAP) {
190
        displayImagery = false;
191
    } else {
192 193
        displayImagery = true;
    }
194 195
}

196 197 198
void
Pixhawk3DWidget::selectVehicleModel(int index)
{
199
    egocentricMap->removeChild(vehicleModel);
200
    vehicleModel = vehicleModels.at(index);
201
    egocentricMap->addChild(vehicleModel);
202 203
}

204
void
205
Pixhawk3DWidget::recenter(void)
206
{
207
    double robotX = 0.0f, robotY = 0.0f, robotZ = 0.0f;
208
    getPosition(robotX, robotY, robotZ);
209

210
    recenterCamera(robotY, robotX, -robotZ);
211 212 213
}

void
214
Pixhawk3DWidget::toggleFollowCamera(int32_t state)
215
{
216
    if (state == Qt::Checked) {
217
        followCamera = true;
218
    } else {
219
        followCamera = false;
220 221
    }
}
222

223 224 225
void
Pixhawk3DWidget::selectTarget(void)
{
226 227
    if (uas) {
        if (frame == MAV_FRAME_GLOBAL) {
228 229 230
            double altitude = uas->getAltitude();

            std::pair<double,double> cursorWorldCoords =
231
                getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
232 233

            target.set(cursorWorldCoords.first, cursorWorldCoords.second);
234
        } else if (frame == MAV_FRAME_LOCAL) {
235 236 237
            double z = uas->getLocalZ();

            std::pair<double,double> cursorWorldCoords =
238
                getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
239 240 241 242 243 244 245 246 247 248

            target.set(cursorWorldCoords.first, cursorWorldCoords.second);
        }

        uas->setTargetPosition(target.x(), target.y(), 0.0, 0.0);

        enableTarget = true;
    }
}

249 250 251
void
Pixhawk3DWidget::insertWaypoint(void)
{
252
    if (uas) {
253
        Waypoint* wp = NULL;
254
        if (frame == MAV_FRAME_GLOBAL) {
255 256 257 258 259 260 261 262
            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 =
263
                getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
264 265 266 267 268

            Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone,
                             latitude, longitude);

            wp = new Waypoint(0, longitude, latitude, altitude);
269
        } else if (frame == MAV_FRAME_LOCAL) {
270 271 272
            double z = uas->getLocalZ();

            std::pair<double,double> cursorWorldCoords =
273
                getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
274 275 276 277 278

            wp = new Waypoint(0, cursorWorldCoords.first,
                              cursorWorldCoords.second, z);
        }

279
        if (wp) {
280
            wp->setFrame(frame);
281
            uas->getWaypointManager()->addWaypoint(wp);
282
        }
283 284 285 286 287 288 289 290 291 292 293 294
    }
}

void
Pixhawk3DWidget::moveWaypoint(void)
{
    mode = MOVE_WAYPOINT_MODE;
}

void
Pixhawk3DWidget::setWaypoint(void)
{
295
    if (uas) {
296
        const QVector<Waypoint *> waypoints =
297
            uas->getWaypointManager()->getWaypointList();
298
        Waypoint* waypoint = waypoints.at(selectedWpIndex);
299

300
        if (frame == MAV_FRAME_GLOBAL) {
301 302 303 304 305 306 307 308
            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 =
309
                getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
310 311 312 313 314 315 316

            Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone,
                             latitude, longitude);

            waypoint->setX(longitude);
            waypoint->setY(latitude);
            waypoint->setZ(altitude);
317
        } else if (frame == MAV_FRAME_LOCAL) {
318 319 320
            double z = uas->getLocalZ();

            std::pair<double,double> cursorWorldCoords =
321
                getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
322 323 324 325 326

            waypoint->setX(cursorWorldCoords.first);
            waypoint->setY(cursorWorldCoords.second);
            waypoint->setZ(z);
        }
327 328 329 330 331 332
    }
}

void
Pixhawk3DWidget::deleteWaypoint(void)
{
333
    if (uas) {
334
        uas->getWaypointManager()->removeWaypoint(selectedWpIndex);
335 336 337 338 339 340
    }
}

void
Pixhawk3DWidget::setWaypointAltitude(void)
{
341
    if (uas) {
342
        bool ok;
343
        const QVector<Waypoint *> waypoints =
344
            uas->getWaypointManager()->getWaypointList();
345 346
        Waypoint* waypoint = waypoints.at(selectedWpIndex);

347
        double altitude = waypoint->getZ();
348
        if (frame == MAV_FRAME_LOCAL) {
349 350 351
            altitude = -altitude;
        }

352
        double newAltitude =
353 354 355 356
            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) {
357
                waypoint->setZ(newAltitude);
358
            } else if (frame == MAV_FRAME_LOCAL) {
359 360
                waypoint->setZ(-newAltitude);
            }
361
        }
362 363 364 365 366 367
    }
}

void
Pixhawk3DWidget::clearAllWaypoints(void)
{
368
    if (uas) {
369
        const QVector<Waypoint *> waypoints =
370 371
            uas->getWaypointManager()->getWaypointList();
        for (int i = waypoints.size() - 1; i >= 0; --i) {
372
            uas->getWaypointManager()->removeWaypoint(i);
373
        }
374 375 376
    }
}

377 378 379 380
QVector< osg::ref_ptr<osg::Node> >
Pixhawk3DWidget::findVehicleModels(void)
{
    QDir directory("models");
lm's avatar
lm committed
381
    QStringList files = directory.entryList(QStringList("*.osg"), QDir::Files);
382 383 384 385

    QVector< osg::ref_ptr<osg::Node> > nodes;

    // add Pixhawk Bravo model
386
    nodes.push_back(PixhawkCheetahGeode::instance());
387

hengli's avatar
hengli committed
388 389 390
    // add sphere of 0.05m radius
    osg::ref_ptr<osg::Sphere> sphere = new osg::Sphere(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.05f);
    osg::ref_ptr<osg::ShapeDrawable> sphereDrawable = new osg::ShapeDrawable(sphere);
391
    sphereDrawable->setColor(osg::Vec4f(0.5f, 0.0f, 0.5f, 1.0f));
hengli's avatar
hengli committed
392 393 394 395 396
    osg::ref_ptr<osg::Geode> sphereGeode = new osg::Geode;
    sphereGeode->addDrawable(sphereDrawable);
    sphereGeode->setName("Sphere (0.1m)");
    nodes.push_back(sphereGeode);

397
    // add all other models in folder
398
    for (int i = 0; i < files.size(); ++i) {
399
        osg::ref_ptr<osg::Node> node =
400
            osgDB::readNodeFile(directory.absoluteFilePath(files[i]).toStdString().c_str());
401

402
        if (node) {
403
            nodes.push_back(node);
404
        } else {
405
            printf("%s\n", QString("ERROR: Could not load file " + directory.absoluteFilePath(files[i]) + "\n").toStdString().c_str());
lm's avatar
lm committed
406
        }
407 408
    }

lm's avatar
lm committed
409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430
//    QStringList dirs = directory.entryList(QDir::Dirs);
//    // Add models in subfolders
//    for (int i = 0; i < dirs.size(); ++i)
//    {
//        // Handling the current directory
//        QStringList currFiles = QDir(dirs[i]).entryList(QStringList("*.ac"), QDir::Files);

//        // Load the file
//        osg::ref_ptr<osg::Node> node =
//                osgDB::readNodeFile(directory.absoluteFilePath(currFiles.first()).toStdString().c_str());

//        if (node)
//        {

//        nodes.push_back(node);
//        }
//        else
//        {
//            printf(QString("ERROR: Could not load file " + directory.absoluteFilePath(files[i]) + "\n").toStdString().c_str());
//        }
//    }

431 432 433 434 435 436
    return nodes;
}

void
Pixhawk3DWidget::buildLayout(void)
{
437 438 439 440 441
    QComboBox* frameComboBox = new QComboBox(this);
    frameComboBox->addItem("Global");
    frameComboBox->addItem("Local");
    frameComboBox->setFixedWidth(70);

442 443 444 445 446 447 448 449 450 451 452 453
    QCheckBox* gridCheckBox = new QCheckBox(this);
    gridCheckBox->setText("Grid");
    gridCheckBox->setChecked(displayGrid);

    QCheckBox* trailCheckBox = new QCheckBox(this);
    trailCheckBox->setText("Trail");
    trailCheckBox->setChecked(displayTrail);

    QCheckBox* waypointsCheckBox = new QCheckBox(this);
    waypointsCheckBox->setText("Waypoints");
    waypointsCheckBox->setChecked(displayWaypoints);

454 455 456 457 458 459
    QLabel* mapLabel = new QLabel("Map", this);
    QComboBox* mapComboBox = new QComboBox(this);
    mapComboBox->addItem("None");
    mapComboBox->addItem("Map (Google)");
    mapComboBox->addItem("Satellite (Google)");

460
    QLabel* modelLabel = new QLabel("Vehicle", this);
461
    QComboBox* modelComboBox = new QComboBox(this);
462
    for (int i = 0; i < vehicleModels.size(); ++i) {
463 464 465 466 467 468 469 470 471 472 473 474 475
        modelComboBox->addItem(vehicleModels[i]->getName().c_str());
    }

    QPushButton* recenterButton = new QPushButton(this);
    recenterButton->setText("Recenter Camera");

    QCheckBox* followCameraCheckBox = new QCheckBox(this);
    followCameraCheckBox->setText("Follow Camera");
    followCameraCheckBox->setChecked(followCamera);

    QGridLayout* layout = new QGridLayout(this);
    layout->setMargin(0);
    layout->setSpacing(2);
476 477 478 479 480 481 482 483 484 485 486 487 488 489 490
    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);
491 492
    setLayout(layout);

493 494
    connect(frameComboBox, SIGNAL(currentIndexChanged(QString)),
            this, SLOT(selectFrame(QString)));
495 496 497 498 499 500
    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)));
501 502
    connect(mapComboBox, SIGNAL(currentIndexChanged(int)),
            this, SLOT(selectMapSource(int)));
503 504 505 506 507 508
    connect(modelComboBox, SIGNAL(currentIndexChanged(int)),
            this, SLOT(selectVehicleModel(int)));
    connect(recenterButton, SIGNAL(clicked()), this, SLOT(recenter()));
    connect(followCameraCheckBox, SIGNAL(stateChanged(int)),
            this, SLOT(toggleFollowCamera(int)));
}
509

510 511 512
void
Pixhawk3DWidget::display(void)
{
513
    if (uas == NULL) {
514 515 516
        return;
    }

517
    double robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw;
518
    QString utmZone;
519
    getPose(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw, utmZone);
520

521
    if (lastRobotX == 0.0f && lastRobotY == 0.0f && lastRobotZ == 0.0f) {
522 523 524 525 526
        lastRobotX = robotX;
        lastRobotY = robotY;
        lastRobotZ = robotZ;

        recenterCamera(robotY, robotX, -robotZ);
527 528
    }

529
    if (followCamera) {
530 531 532
        double dx = robotY - lastRobotY;
        double dy = robotX - lastRobotX;
        double dz = lastRobotZ - robotZ;
533 534

        moveCamera(dx, dy, dz);
535 536
    }

537 538 539 540
    robotPosition->setPosition(osg::Vec3d(robotY, robotX, -robotZ));
    robotAttitude->setAttitude(osg::Quat(-robotYaw, osg::Vec3d(0.0f, 0.0f, 1.0f),
                                         robotPitch, osg::Vec3d(1.0f, 0.0f, 0.0f),
                                         robotRoll, osg::Vec3d(0.0f, 1.0f, 0.0f)));
541

542
    if (displayTrail) {
543 544 545
        updateTrail(robotX, robotY, robotZ);
    }

546
    if (frame == MAV_FRAME_GLOBAL && displayImagery) {
547
        updateImagery(robotX, robotY, robotZ, utmZone);
548 549
    }

550
    if (displayWaypoints) {
551 552 553
        updateWaypoints();
    }

554
    if (enableTarget) {
555 556 557
        updateTarget(robotX, robotY);
    }

558
#ifdef QGC_LIBFREENECT_ENABLED
559
    if (enableFreenect && (displayRGBD2D || displayRGBD3D)) {
560 561
        updateRGBD();
    }
562
#endif
563
    updateHUD(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw, utmZone);
564 565

    // set node visibility
566

567 568
    rollingMap->setChildValue(gridNode, displayGrid);
    rollingMap->setChildValue(trailNode, displayTrail);
569
    rollingMap->setChildValue(mapNode, displayImagery);
570
    rollingMap->setChildValue(waypointGroupNode, displayWaypoints);
571
    rollingMap->setChildValue(targetNode, enableTarget);
572
    if (enableFreenect) {
573 574
        egocentricMap->setChildValue(rgbd3DNode, displayRGBD3D);
    }
575 576
    hudGroup->setChildValue(rgb2DGeode, displayRGBD2D);
    hudGroup->setChildValue(depth2DGeode, displayRGBD2D);
577 578 579 580

    lastRobotX = robotX;
    lastRobotY = robotY;
    lastRobotZ = robotZ;
581 582
}

583 584 585
void
Pixhawk3DWidget::keyPressEvent(QKeyEvent* event)
{
586 587
    if (!event->text().isEmpty()) {
        switch (*(event->text().toAscii().data())) {
588 589 590
        case '1':
            displayRGBD2D = !displayRGBD2D;
            break;
591 592 593
        case '2':
            displayRGBD3D = !displayRGBD3D;
            break;
594 595
        case 'c':
        case 'C':
596 597
            enableRGBDColor = !enableRGBDColor;
            break;
598 599 600 601 602 603
        }
    }

    Q3DWidget::keyPressEvent(event);
}

604 605 606
void
Pixhawk3DWidget::mousePressEvent(QMouseEvent* event)
{
607 608
    if (event->button() == Qt::LeftButton) {
        if (mode == MOVE_WAYPOINT_MODE) {
609 610 611 612 613 614
            setWaypoint();
            mode = DEFAULT_MODE;

            return;
        }

615
        if (event->modifiers() == Qt::ShiftModifier) {
616
            selectedWpIndex = findWaypoint(event->x(), event->y());
617
            if (selectedWpIndex == -1) {
618
                showInsertWaypointMenu(event->globalPos());
619
            } else {
620 621 622 623 624
                showEditWaypointMenu(event->globalPos());
            }

            return;
        }
625 626 627 628 629
    }

    Q3DWidget::mousePressEvent(event);
}

630 631 632 633 634
void
Pixhawk3DWidget::getPose(double& x, double& y, double& z,
                         double& roll, double& pitch, double& yaw,
                         QString& utmZone)
{
635 636
    if (uas) {
        if (frame == MAV_FRAME_GLOBAL) {
637 638 639 640 641 642
            double latitude = uas->getLatitude();
            double longitude = uas->getLongitude();
            double altitude = uas->getAltitude();

            Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
            z = -altitude;
643
        } else if (frame == MAV_FRAME_LOCAL) {
644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667
            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)
{
668 669
    if (uas) {
        if (frame == MAV_FRAME_GLOBAL) {
670 671 672 673 674 675
            double latitude = uas->getLatitude();
            double longitude = uas->getLongitude();
            double altitude = uas->getAltitude();

            Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
            z = -altitude;
676
        } else if (frame == MAV_FRAME_LOCAL) {
677 678 679 680 681 682 683 684 685 686 687 688 689 690
            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);
}

691 692 693 694
osg::ref_ptr<osg::Geode>
Pixhawk3DWidget::createGrid(void)
{
    osg::ref_ptr<osg::Geode> geode(new osg::Geode());
695 696 697 698
    osg::ref_ptr<osg::Geometry> fineGeometry(new osg::Geometry());
    osg::ref_ptr<osg::Geometry> coarseGeometry(new osg::Geometry());
    geode->addDrawable(fineGeometry);
    geode->addDrawable(coarseGeometry);
699 700 701 702

    float radius = 10.0f;
    float resolution = 0.25f;

703 704
    osg::ref_ptr<osg::Vec3Array> fineCoords(new osg::Vec3Array);
    osg::ref_ptr<osg::Vec3Array> coarseCoords(new osg::Vec3Array);
705 706

    // draw a 20m x 20m grid with 0.25m resolution
707 708
    for (float i = -radius; i <= radius; i += resolution) {
        if (fabsf(i - floor(i + 0.5f)) < 0.01f) {
709 710 711 712
            coarseCoords->push_back(osg::Vec3(i, -radius, 0.0f));
            coarseCoords->push_back(osg::Vec3(i, radius, 0.0f));
            coarseCoords->push_back(osg::Vec3(-radius, i, 0.0f));
            coarseCoords->push_back(osg::Vec3(radius, i, 0.0f));
713
        } else {
714 715 716 717 718
            fineCoords->push_back(osg::Vec3(i, -radius, 0.0f));
            fineCoords->push_back(osg::Vec3(i, radius, 0.0f));
            fineCoords->push_back(osg::Vec3(-radius, i, 0.0f));
            fineCoords->push_back(osg::Vec3(radius, i, 0.0f));
        }
719 720
    }

721 722
    fineGeometry->setVertexArray(fineCoords);
    coarseGeometry->setVertexArray(coarseCoords);
723 724 725

    osg::ref_ptr<osg::Vec4Array> color(new osg::Vec4Array);
    color->push_back(osg::Vec4(0.5f, 0.5f, 0.5f, 1.0f));
726 727 728 729 730 731
    fineGeometry->setColorArray(color);
    coarseGeometry->setColorArray(color);
    fineGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);
    coarseGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);

    fineGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES,
732
                                  0, fineCoords->size()));
733
    coarseGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0,
734
                                    coarseCoords->size()));
735 736 737 738 739 740 741 742 743 744 745 746 747 748

    osg::ref_ptr<osg::StateSet> fineStateset(new osg::StateSet);
    osg::ref_ptr<osg::LineWidth> fineLinewidth(new osg::LineWidth());
    fineLinewidth->setWidth(0.25f);
    fineStateset->setAttributeAndModes(fineLinewidth, osg::StateAttribute::ON);
    fineStateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
    fineGeometry->setStateSet(fineStateset);

    osg::ref_ptr<osg::StateSet> coarseStateset(new osg::StateSet);
    osg::ref_ptr<osg::LineWidth> coarseLinewidth(new osg::LineWidth());
    coarseLinewidth->setWidth(2.0f);
    coarseStateset->setAttributeAndModes(coarseLinewidth, osg::StateAttribute::ON);
    coarseStateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
    coarseGeometry->setStateSet(coarseStateset);
749 750 751 752 753 754 755 756 757 758 759 760

    return geode;
}

osg::ref_ptr<osg::Geode>
Pixhawk3DWidget::createTrail(void)
{
    osg::ref_ptr<osg::Geode> geode(new osg::Geode());
    trailGeometry = new osg::Geometry();
    trailGeometry->setUseDisplayList(false);
    geode->addDrawable(trailGeometry.get());

761
    trailVertices = new osg::Vec3dArray;
762 763 764 765 766 767 768 769 770 771 772 773 774 775 776 777 778 779 780 781
    trailGeometry->setVertexArray(trailVertices);

    trailDrawArrays = new osg::DrawArrays(osg::PrimitiveSet::LINE_STRIP);
    trailGeometry->addPrimitiveSet(trailDrawArrays);

    osg::ref_ptr<osg::Vec4Array> color(new osg::Vec4Array);
    color->push_back(osg::Vec4(1.0f, 0.0f, 0.0f, 1.0f));
    trailGeometry->setColorArray(color);
    trailGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);

    osg::ref_ptr<osg::StateSet> stateset(new osg::StateSet);
    osg::ref_ptr<osg::LineWidth> linewidth(new osg::LineWidth());
    linewidth->setWidth(1.0f);
    stateset->setAttributeAndModes(linewidth, osg::StateAttribute::ON);
    stateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
    trailGeometry->setStateSet(stateset);

    return geode;
}

782
osg::ref_ptr<Imagery>
783 784
Pixhawk3DWidget::createMap(void)
{
785
    return osg::ref_ptr<Imagery>(new Imagery());
786 787
}

788
osg::ref_ptr<osg::Geode>
789
Pixhawk3DWidget::createRGBD3D(void)
790
{
791 792 793 794 795 796 797 798 799 800 801 802 803 804 805 806
    int frameSize = 640 * 480;

    osg::ref_ptr<osg::Geode> geode(new osg::Geode);
    osg::ref_ptr<osg::Geometry> geometry(new osg::Geometry);

    osg::ref_ptr<osg::Vec3Array> vertices(new osg::Vec3Array(frameSize));
    geometry->setVertexArray(vertices);

    osg::ref_ptr<osg::Vec4Array> colors(new osg::Vec4Array(frameSize));
    geometry->setColorArray(colors);
    geometry->setColorBinding(osg::Geometry::BIND_PER_VERTEX);
    geometry->setUseDisplayList(false);

    geode->addDrawable(geometry);

    return geode;
807 808
}

809 810 811 812
osg::ref_ptr<osg::Node>
Pixhawk3DWidget::createTarget(void)
{
    osg::ref_ptr<osg::PositionAttitudeTransform> pat =
813
        new osg::PositionAttitudeTransform;
814 815 816 817 818 819 820 821 822 823 824 825 826 827 828

    pat->setPosition(osg::Vec3d(0.0, 0.0, 0.0));

    osg::ref_ptr<osg::Sphere> sphere = new osg::Sphere(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.1f);
    osg::ref_ptr<osg::ShapeDrawable> sphereDrawable = new osg::ShapeDrawable(sphere);
    sphereDrawable->setColor(osg::Vec4f(0.0f, 1.0f, 0.0f, 1.0f));
    osg::ref_ptr<osg::Geode> sphereGeode = new osg::Geode;
    sphereGeode->addDrawable(sphereDrawable);
    sphereGeode->setName("Target");

    pat->addChild(sphereGeode);

    return pat;
}

829 830 831 832
void
Pixhawk3DWidget::setupHUD(void)
{
    osg::ref_ptr<osg::Vec4Array> hudColors(new osg::Vec4Array);
833 834
    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));
835 836

    hudBackgroundGeometry = new osg::Geometry;
837
    hudBackgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON,
838
                                           0, 4));
839
    hudBackgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON,
840
                                           4, 4));
841
    hudBackgroundGeometry->setColorArray(hudColors);
842
    hudBackgroundGeometry->setColorBinding(osg::Geometry::BIND_PER_PRIMITIVE_SET);
843
    hudBackgroundGeometry->setUseDisplayList(false);
844 845 846 847 848 849 850

    statusText = new osgText::Text;
    statusText->setCharacterSize(11);
    statusText->setFont("images/Vera.ttf");
    statusText->setAxisAlignment(osgText::Text::SCREEN);
    statusText->setColor(osg::Vec4(255, 255, 255, 1));

851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868
    resizeHUD();

    osg::ref_ptr<osg::Geode> statusGeode = new osg::Geode;
    statusGeode->addDrawable(hudBackgroundGeometry);
    statusGeode->addDrawable(statusText);
    hudGroup->addChild(statusGeode);

    rgbImage = new osg::Image;
    rgb2DGeode = new ImageWindowGeode("RGB Image",
                                      osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f),
                                      rgbImage);
    hudGroup->addChild(rgb2DGeode);

    depthImage = new osg::Image;
    depth2DGeode = new ImageWindowGeode("Depth Image",
                                        osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f),
                                        depthImage);
    hudGroup->addChild(depth2DGeode);
869 870 871 872

    scaleGeode = new HUDScaleGeode;
    scaleGeode->init();
    hudGroup->addChild(scaleGeode);
873 874 875
}

void
876
Pixhawk3DWidget::resizeHUD(void)
877
{
878
    int topHUDHeight = 25;
879 880 881
    int bottomHUDHeight = 25;

    osg::Vec3Array* vertices = static_cast<osg::Vec3Array*>(hudBackgroundGeometry->getVertexArray());
882
    if (vertices == NULL || vertices->size() != 8) {
883 884 885 886 887 888 889 890 891 892 893 894 895 896
        osg::ref_ptr<osg::Vec3Array> newVertices = new osg::Vec3Array(8);
        hudBackgroundGeometry->setVertexArray(newVertices);

        vertices = static_cast<osg::Vec3Array*>(hudBackgroundGeometry->getVertexArray());
    }

    (*vertices)[0] = osg::Vec3(0, height(), -1);
    (*vertices)[1] = osg::Vec3(width(), height(), -1);
    (*vertices)[2] = osg::Vec3(width(), height() - topHUDHeight, -1);
    (*vertices)[3] = osg::Vec3(0, height() - topHUDHeight, -1);
    (*vertices)[4] = osg::Vec3(0, 0, -1);
    (*vertices)[5] = osg::Vec3(width(), 0, -1);
    (*vertices)[6] = osg::Vec3(width(), bottomHUDHeight, -1);
    (*vertices)[7] = osg::Vec3(0, bottomHUDHeight, -1);
897

898
    statusText->setPosition(osg::Vec3(10, height() - 15, -1.5));
899

900
    if (rgb2DGeode.valid() && depth2DGeode.valid()) {
901 902 903 904 905 906 907 908 909 910
        int windowWidth = (width() - 20) / 2;
        int windowHeight = 3 * windowWidth / 4;
        rgb2DGeode->setAttributes(10, (height() - windowHeight) / 2,
                                  windowWidth, windowHeight);
        depth2DGeode->setAttributes(width() / 2, (height() - windowHeight) / 2,
                                    windowWidth, windowHeight);
    }
}

void
911
Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ,
912 913
                           double robotRoll, double robotPitch, double robotYaw,
                           const QString& utmZone)
914 915 916
{
    resizeHUD();

917
    std::pair<double,double> cursorPosition =
918
        getGlobalCursorPosition(getMouseX(), getMouseY(), -robotZ);
919 920 921 922

    std::ostringstream oss;
    oss.setf(std::ios::fixed, std::ios::floatfield);
    oss.precision(2);
923
    if (frame == MAV_FRAME_GLOBAL) {
924 925 926 927 928 929 930 931 932
        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 <<
933
            " Lon = " << longitude;
934 935 936

        oss.precision(2);
        oss << " Altitude = " << -robotZ <<
937 938 939
            " r = " << robotRoll <<
            " p = " << robotPitch <<
            " y = " << robotYaw;
940 941 942

        oss.precision(6);
        oss << " Cursor [" << cursorLatitude <<
943 944
            " " << cursorLongitude << "]";
    } else if (frame == MAV_FRAME_LOCAL) {
945
        oss << " x = " << robotX <<
946 947 948 949 950 951 952
            " y = " << robotY <<
            " z = " << robotZ <<
            " r = " << robotRoll <<
            " p = " << robotPitch <<
            " y = " << robotYaw <<
            " Cursor [" << cursorPosition.first <<
            " " << cursorPosition.second << "]";
953 954
    }

955
    statusText->setText(oss.str());
956

957
    bool darkBackground = true;
958
    if (mapNode->getImageryType() == Imagery::GOOGLE_MAP) {
959 960 961 962 963
        darkBackground = false;
    }

    scaleGeode->update(height(), cameraParams.cameraFov,
                       cameraManipulator->getDistance(), darkBackground);
964 965 966
}

void
967
Pixhawk3DWidget::updateTrail(double robotX, double robotY, double robotZ)
968
{
969
    if (robotX == 0.0f || robotY == 0.0f || robotZ == 0.0f) {
970 971 972 973
        return;
    }

    bool addToTrail = false;
974
    if (trail.size() > 0) {
975
        if (fabs(robotX - trail[trail.size() - 1].x()) > 0.01f ||
976 977
                fabs(robotY - trail[trail.size() - 1].y()) > 0.01f ||
                fabs(robotZ - trail[trail.size() - 1].z()) > 0.01f) {
978 979
            addToTrail = true;
        }
980
    } else {
981 982 983
        addToTrail = true;
    }

984
    if (addToTrail) {
985
        osg::Vec3d p(robotX, robotY, robotZ);
986
        if (trail.size() == trail.capacity()) {
987
            memcpy(trail.data(), trail.data() + 1,
988
                   (trail.size() - 1) * sizeof(osg::Vec3d));
989
            trail[trail.size() - 1] = p;
990
        } else {
991 992 993 994 995
            trail.append(p);
        }
    }

    trailVertices->clear();
996
    for (int i = 0; i < trail.size(); ++i) {
997 998 999
        trailVertices->push_back(osg::Vec3d(trail[i].y() - robotY,
                                            trail[i].x() - robotX,
                                            -(trail[i].z() - robotZ)));
1000 1001 1002 1003 1004 1005 1006
    }

    trailDrawArrays->setFirst(0);
    trailDrawArrays->setCount(trailVertices->size());
    trailGeometry->dirtyBound();
}

1007
void
1008
Pixhawk3DWidget::updateImagery(double originX, double originY, double originZ,
1009
                               const QString& zone)
1010
{
1011
    if (mapNode->getImageryType() == Imagery::BLANK_MAP) {
1012 1013 1014
        return;
    }

1015
    double viewingRadius = cameraManipulator->getDistance() * 10.0;
1016
    if (viewingRadius < 100.0) {
1017 1018 1019 1020
        viewingRadius = 100.0;
    }

    double minResolution = 0.25;
1021
    double centerResolution = cameraManipulator->getDistance() / 50.0;
1022 1023 1024
    double maxResolution = 1048576.0;

    Imagery::ImageryType imageryType = mapNode->getImageryType();
1025
    switch (imageryType) {
1026 1027 1028 1029 1030 1031 1032 1033 1034 1035
    case Imagery::GOOGLE_MAP:
        minResolution = 0.25;
        break;
    case Imagery::GOOGLE_SATELLITE:
        minResolution = 0.5;
        break;
    case Imagery::SWISSTOPO_SATELLITE:
        minResolution = 0.25;
        maxResolution = 0.25;
        break;
1036 1037
    default:
    {}
1038 1039 1040
    }

    double resolution = minResolution;
1041
    while (resolution * 2.0 < centerResolution) {
1042 1043
        resolution *= 2.0;
    }
1044
    if (resolution > maxResolution) {
1045 1046 1047 1048 1049 1050 1051
        resolution = maxResolution;
    }

    mapNode->draw3D(viewingRadius,
                    resolution,
                    cameraManipulator->getCenter().y(),
                    cameraManipulator->getCenter().x(),
1052 1053
                    originX,
                    originY,
1054
                    originZ,
1055 1056 1057
                    zone);

    // prefetch map tiles
1058
    if (resolution / 2.0 >= minResolution) {
1059 1060 1061 1062 1063 1064
        mapNode->prefetch3D(viewingRadius / 2.0,
                            resolution / 2.0,
                            cameraManipulator->getCenter().y(),
                            cameraManipulator->getCenter().x(),
                            zone);
    }
1065
    if (resolution * 2.0 <= maxResolution) {
1066 1067 1068 1069 1070 1071 1072 1073 1074 1075
        mapNode->prefetch3D(viewingRadius * 2.0,
                            resolution * 2.0,
                            cameraManipulator->getCenter().y(),
                            cameraManipulator->getCenter().x(),
                            zone);
    }

    mapNode->update();
}

1076 1077 1078
void
Pixhawk3DWidget::updateWaypoints(void)
{
1079
    waypointGroupNode->update(frame, uas);
1080
}
1081

1082 1083 1084 1085
void
Pixhawk3DWidget::updateTarget(double robotX, double robotY)
{
    osg::PositionAttitudeTransform* pat =
1086
        static_cast<osg::PositionAttitudeTransform*>(targetNode.get());
1087 1088 1089
    pat->setPosition(osg::Vec3d(target.y() - robotY, target.x() - robotX, 0.0));
}

1090 1091 1092 1093 1094 1095 1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109 1110 1111 1112 1113 1114 1115 1116 1117 1118 1119 1120 1121 1122 1123 1124 1125 1126 1127 1128 1129 1130 1131 1132 1133 1134 1135 1136 1137 1138 1139 1140 1141 1142 1143 1144 1145 1146 1147 1148 1149 1150 1151 1152 1153 1154 1155 1156 1157 1158 1159 1160 1161 1162 1163 1164 1165 1166 1167 1168 1169 1170 1171 1172 1173 1174 1175 1176 1177 1178 1179 1180 1181 1182 1183 1184 1185 1186 1187 1188 1189 1190 1191 1192 1193 1194 1195 1196 1197 1198 1199 1200 1201 1202 1203 1204 1205 1206 1207 1208 1209 1210 1211 1212 1213 1214 1215 1216 1217 1218
float colormap_jet[128][3] = {
    {0.0f,0.0f,0.53125f},
    {0.0f,0.0f,0.5625f},
    {0.0f,0.0f,0.59375f},
    {0.0f,0.0f,0.625f},
    {0.0f,0.0f,0.65625f},
    {0.0f,0.0f,0.6875f},
    {0.0f,0.0f,0.71875f},
    {0.0f,0.0f,0.75f},
    {0.0f,0.0f,0.78125f},
    {0.0f,0.0f,0.8125f},
    {0.0f,0.0f,0.84375f},
    {0.0f,0.0f,0.875f},
    {0.0f,0.0f,0.90625f},
    {0.0f,0.0f,0.9375f},
    {0.0f,0.0f,0.96875f},
    {0.0f,0.0f,1.0f},
    {0.0f,0.03125f,1.0f},
    {0.0f,0.0625f,1.0f},
    {0.0f,0.09375f,1.0f},
    {0.0f,0.125f,1.0f},
    {0.0f,0.15625f,1.0f},
    {0.0f,0.1875f,1.0f},
    {0.0f,0.21875f,1.0f},
    {0.0f,0.25f,1.0f},
    {0.0f,0.28125f,1.0f},
    {0.0f,0.3125f,1.0f},
    {0.0f,0.34375f,1.0f},
    {0.0f,0.375f,1.0f},
    {0.0f,0.40625f,1.0f},
    {0.0f,0.4375f,1.0f},
    {0.0f,0.46875f,1.0f},
    {0.0f,0.5f,1.0f},
    {0.0f,0.53125f,1.0f},
    {0.0f,0.5625f,1.0f},
    {0.0f,0.59375f,1.0f},
    {0.0f,0.625f,1.0f},
    {0.0f,0.65625f,1.0f},
    {0.0f,0.6875f,1.0f},
    {0.0f,0.71875f,1.0f},
    {0.0f,0.75f,1.0f},
    {0.0f,0.78125f,1.0f},
    {0.0f,0.8125f,1.0f},
    {0.0f,0.84375f,1.0f},
    {0.0f,0.875f,1.0f},
    {0.0f,0.90625f,1.0f},
    {0.0f,0.9375f,1.0f},
    {0.0f,0.96875f,1.0f},
    {0.0f,1.0f,1.0f},
    {0.03125f,1.0f,0.96875f},
    {0.0625f,1.0f,0.9375f},
    {0.09375f,1.0f,0.90625f},
    {0.125f,1.0f,0.875f},
    {0.15625f,1.0f,0.84375f},
    {0.1875f,1.0f,0.8125f},
    {0.21875f,1.0f,0.78125f},
    {0.25f,1.0f,0.75f},
    {0.28125f,1.0f,0.71875f},
    {0.3125f,1.0f,0.6875f},
    {0.34375f,1.0f,0.65625f},
    {0.375f,1.0f,0.625f},
    {0.40625f,1.0f,0.59375f},
    {0.4375f,1.0f,0.5625f},
    {0.46875f,1.0f,0.53125f},
    {0.5f,1.0f,0.5f},
    {0.53125f,1.0f,0.46875f},
    {0.5625f,1.0f,0.4375f},
    {0.59375f,1.0f,0.40625f},
    {0.625f,1.0f,0.375f},
    {0.65625f,1.0f,0.34375f},
    {0.6875f,1.0f,0.3125f},
    {0.71875f,1.0f,0.28125f},
    {0.75f,1.0f,0.25f},
    {0.78125f,1.0f,0.21875f},
    {0.8125f,1.0f,0.1875f},
    {0.84375f,1.0f,0.15625f},
    {0.875f,1.0f,0.125f},
    {0.90625f,1.0f,0.09375f},
    {0.9375f,1.0f,0.0625f},
    {0.96875f,1.0f,0.03125f},
    {1.0f,1.0f,0.0f},
    {1.0f,0.96875f,0.0f},
    {1.0f,0.9375f,0.0f},
    {1.0f,0.90625f,0.0f},
    {1.0f,0.875f,0.0f},
    {1.0f,0.84375f,0.0f},
    {1.0f,0.8125f,0.0f},
    {1.0f,0.78125f,0.0f},
    {1.0f,0.75f,0.0f},
    {1.0f,0.71875f,0.0f},
    {1.0f,0.6875f,0.0f},
    {1.0f,0.65625f,0.0f},
    {1.0f,0.625f,0.0f},
    {1.0f,0.59375f,0.0f},
    {1.0f,0.5625f,0.0f},
    {1.0f,0.53125f,0.0f},
    {1.0f,0.5f,0.0f},
    {1.0f,0.46875f,0.0f},
    {1.0f,0.4375f,0.0f},
    {1.0f,0.40625f,0.0f},
    {1.0f,0.375f,0.0f},
    {1.0f,0.34375f,0.0f},
    {1.0f,0.3125f,0.0f},
    {1.0f,0.28125f,0.0f},
    {1.0f,0.25f,0.0f},
    {1.0f,0.21875f,0.0f},
    {1.0f,0.1875f,0.0f},
    {1.0f,0.15625f,0.0f},
    {1.0f,0.125f,0.0f},
    {1.0f,0.09375f,0.0f},
    {1.0f,0.0625f,0.0f},
    {1.0f,0.03125f,0.0f},
    {1.0f,0.0f,0.0f},
    {0.96875f,0.0f,0.0f},
    {0.9375f,0.0f,0.0f},
    {0.90625f,0.0f,0.0f},
    {0.875f,0.0f,0.0f},
    {0.84375f,0.0f,0.0f},
    {0.8125f,0.0f,0.0f},
    {0.78125f,0.0f,0.0f},
    {0.75f,0.0f,0.0f},
    {0.71875f,0.0f,0.0f},
    {0.6875f,0.0f,0.0f},
    {0.65625f,0.0f,0.0f},
    {0.625f,0.0f,0.0f},
    {0.59375f,0.0f,0.0f},
    {0.5625f,0.0f,0.0f},
    {0.53125f,0.0f,0.0f},
    {0.5f,0.0f,0.0f}
1219 1220
};

1221 1222 1223 1224
#ifdef QGC_LIBFREENECT_ENABLED
void
Pixhawk3DWidget::updateRGBD(void)
{
1225
    QSharedPointer<QByteArray> rgb = freenect->getRgbData();
1226
    if (!rgb.isNull()) {
1227 1228 1229 1230 1231 1232 1233 1234
        rgbImage->setImage(640, 480, 1,
                           GL_RGB, GL_RGB, GL_UNSIGNED_BYTE,
                           reinterpret_cast<unsigned char *>(rgb->data()),
                           osg::Image::NO_DELETE);
        rgbImage->dirty();
    }

    QSharedPointer<QByteArray> coloredDepth = freenect->getColoredDepthData();
1235
    if (!coloredDepth.isNull()) {
1236 1237 1238 1239 1240 1241 1242 1243
        depthImage->setImage(640, 480, 1,
                             GL_RGB, GL_RGB, GL_UNSIGNED_BYTE,
                             reinterpret_cast<unsigned char *>(coloredDepth->data()),
                             osg::Image::NO_DELETE);
        depthImage->dirty();
    }

    QSharedPointer< QVector<Freenect::Vector6D> > pointCloud =
1244
        freenect->get6DPointCloudData();
1245 1246 1247 1248 1249

    osg::Geometry* geometry = rgbd3DNode->getDrawable(0)->asGeometry();

    osg::Vec3Array* vertices = static_cast<osg::Vec3Array*>(geometry->getVertexArray());
    osg::Vec4Array* colors = static_cast<osg::Vec4Array*>(geometry->getColorArray());
1250
    for (int i = 0; i < pointCloud->size(); ++i) {
1251 1252 1253
        double x = pointCloud->at(i).x;
        double y = pointCloud->at(i).y;
        double z = pointCloud->at(i).z;
1254 1255
        (*vertices)[i].set(x, z, -y);

1256
        if (enableRGBDColor) {
1257 1258 1259
            (*colors)[i].set(pointCloud->at(i).r / 255.0f,
                             pointCloud->at(i).g / 255.0f,
                             pointCloud->at(i).b / 255.0f,
1260
                             1.0f);
1261
        } else {
1262 1263 1264 1265 1266 1267 1268
            double dist = sqrt(x * x + y * y + z * z);
            int colorIndex = static_cast<int>(fmin(dist / 7.0 * 127.0, 127.0));
            (*colors)[i].set(colormap_jet[colorIndex][0],
                             colormap_jet[colorIndex][1],
                             colormap_jet[colorIndex][2],
                             1.0f);
        }
1269 1270
    }

1271
    if (geometry->getNumPrimitiveSets() == 0) {
1272
        geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS,
1273 1274
                                  0, pointCloud->size()));
    } else {
1275
        osg::DrawArrays* drawarrays = static_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
1276
        drawarrays->setCount(pointCloud->size());
1277
    }
1278 1279
}
#endif
1280

1281 1282 1283
int
Pixhawk3DWidget::findWaypoint(int mouseX, int mouseY)
{
1284
    if (getSceneData() != NULL) {
1285 1286
        osgUtil::LineSegmentIntersector::Intersections intersections;

1287
        if (computeIntersections(mouseX, height() - mouseY, intersections)) {
1288
            for (osgUtil::LineSegmentIntersector::Intersections::iterator
1289 1290
                    it = intersections.begin(); it != intersections.end(); it++) {
                for (uint i = 0 ; i < it->nodePath.size(); ++i) {
1291
                    std::string nodeName = it->nodePath[i]->getName();
1292
                    if (nodeName.substr(0, 2).compare("wp") == 0) {
1293
                        return atoi(nodeName.substr(2).c_str());
1294 1295 1296 1297 1298 1299 1300 1301 1302
                    }
                }
            }
        }
    }

    return -1;
}

1303 1304 1305
bool
Pixhawk3DWidget::findTarget(int mouseX, int mouseY)
{
1306
    if (getSceneData() != NULL) {
1307 1308
        osgUtil::LineSegmentIntersector::Intersections intersections;

1309
        if (computeIntersections(mouseX, height() - mouseY, intersections)) {
1310
            for (osgUtil::LineSegmentIntersector::Intersections::iterator
1311 1312
                    it = intersections.begin(); it != intersections.end(); it++) {
                for (uint i = 0 ; i < it->nodePath.size(); ++i) {
1313
                    std::string nodeName = it->nodePath[i]->getName();
1314
                    if (nodeName.compare("Target") == 0) {
1315 1316 1317 1318 1319 1320 1321 1322 1323 1324
                        return true;
                    }
                }
            }
        }
    }

    return false;
}

1325 1326 1327 1328 1329 1330
void
Pixhawk3DWidget::showInsertWaypointMenu(const QPoint &cursorPos)
{
    QMenu menu;
    menu.addAction("Insert new waypoint", this, SLOT(insertWaypoint()));
    menu.addAction("Clear all waypoints", this, SLOT(clearAllWaypoints()));
1331
    menu.addAction("Select target", this, SLOT(selectTarget()));
1332 1333 1334 1335 1336 1337 1338 1339 1340
    menu.exec(cursorPos);
}

void
Pixhawk3DWidget::showEditWaypointMenu(const QPoint &cursorPos)
{
    QMenu menu;

    QString text;
1341
    text = QString("Move waypoint %1").arg(QString::number(selectedWpIndex));
1342 1343
    menu.addAction(text, this, SLOT(moveWaypoint()));

1344
    text = QString("Change altitude of waypoint %1").arg(QString::number(selectedWpIndex));
1345 1346
    menu.addAction(text, this, SLOT(setWaypointAltitude()));

1347
    text = QString("Delete waypoint %1").arg(QString::number(selectedWpIndex));
1348 1349 1350 1351 1352
    menu.addAction(text, this, SLOT(deleteWaypoint()));

    menu.addAction("Clear all waypoints", this, SLOT(clearAllWaypoints()));
    menu.exec(cursorPos);
}