Pixhawk3DWidget.cc 41.7 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
#include "QGC.h"

47 48 49 50 51
#ifdef QGC_PROTOBUF_ENABLED
#include <tr1/memory>
#include <pixhawk.pb.h>
#endif

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

    // generate Pixhawk Cheetah model
75 76
    vehicleModel = PixhawkCheetahGeode::instance();
    egocentricMap->addChild(vehicleModel);
77 78 79 80 81 82 83 84 85

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

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

86 87
    // generate map model
    mapNode = createMap();
88
    rollingMap->addChild(mapNode);
89

90
    // generate waypoint model
91 92 93
    waypointGroupNode = new WaypointGroupNode;
    waypointGroupNode->init();
    rollingMap->addChild(waypointGroupNode);
94

95 96 97 98
    // generate target model
    targetNode = createTarget();
    rollingMap->addChild(targetNode);

99
    // generate RGBD model
100 101
    rgbd3DNode = createRGBD3D();
    egocentricMap->addChild(rgbd3DNode);
102

103 104
    setupHUD();

105 106 107
    // find available vehicle models in models folder
    vehicleModels = findVehicleModels();

108 109 110 111 112 113 114 115 116 117 118 119 120 121 122
    buildLayout();

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

Pixhawk3DWidget::~Pixhawk3DWidget()
{

}

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

    this->uas = uas;
}

134 135 136
void
Pixhawk3DWidget::selectFrame(QString text)
{
137
    if (text.compare("Global") == 0) {
138
        frame = MAV_FRAME_GLOBAL;
139
    } else if (text.compare("Local") == 0) {
LM's avatar
LM committed
140
        frame = MAV_FRAME_LOCAL_NED;
141 142 143 144 145 146 147
    }

    getPosition(lastRobotX, lastRobotY, lastRobotZ);

    recenter();
}

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

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

        displayTrail = true;
167
    } else {
168 169 170 171
        displayTrail = false;
    }
}

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

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

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

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

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

208
    recenterCamera(robotY, robotX, -robotZ);
209 210 211
}

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

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

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

            target.set(cursorWorldCoords.first, cursorWorldCoords.second);
LM's avatar
LM committed
232
        } else if (frame == MAV_FRAME_LOCAL_NED) {
233 234 235
            double z = uas->getLocalZ();

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

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

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

        enableTarget = true;
    }
}

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

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

            wp = new Waypoint(0, longitude, latitude, altitude);
LM's avatar
LM committed
267
        } else if (frame == MAV_FRAME_LOCAL_NED) {
268 269 270
            double z = uas->getLocalZ();

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

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

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

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

void
Pixhawk3DWidget::setWaypoint(void)
{
293
    if (uas) {
294
        const QVector<Waypoint *> waypoints =
pixhawk's avatar
pixhawk committed
295
            uas->getWaypointManager()->getWaypointEditableList();
296
        Waypoint* waypoint = waypoints.at(selectedWpIndex);
297

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

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

            waypoint->setX(longitude);
            waypoint->setY(latitude);
            waypoint->setZ(altitude);
LM's avatar
LM committed
315
        } else if (frame == MAV_FRAME_LOCAL_NED) {
316 317 318
            double z = uas->getLocalZ();

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

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

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

void
Pixhawk3DWidget::setWaypointAltitude(void)
{
339
    if (uas) {
340
        bool ok;
341
        const QVector<Waypoint *> waypoints =
pixhawk's avatar
pixhawk committed
342
            uas->getWaypointManager()->getWaypointEditableList();
343 344
        Waypoint* waypoint = waypoints.at(selectedWpIndex);

345
        double altitude = waypoint->getZ();
LM's avatar
LM committed
346
        if (frame == MAV_FRAME_LOCAL_NED) {
347 348 349
            altitude = -altitude;
        }

350
        double newAltitude =
351 352 353 354
            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) {
355
                waypoint->setZ(newAltitude);
LM's avatar
LM committed
356
            } else if (frame == MAV_FRAME_LOCAL_NED) {
357 358
                waypoint->setZ(-newAltitude);
            }
359
        }
360 361 362 363 364 365
    }
}

void
Pixhawk3DWidget::clearAllWaypoints(void)
{
366
    if (uas) {
367
        const QVector<Waypoint *> waypoints =
pixhawk's avatar
pixhawk committed
368
            uas->getWaypointManager()->getWaypointEditableList();
369
        for (int i = waypoints.size() - 1; i >= 0; --i) {
370
            uas->getWaypointManager()->removeWaypoint(i);
371
        }
372 373 374
    }
}

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

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

    // add Pixhawk Bravo model
384
    nodes.push_back(PixhawkCheetahGeode::instance());
385

hengli's avatar
hengli committed
386 387 388
    // 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);
389
    sphereDrawable->setColor(osg::Vec4f(0.5f, 0.0f, 0.5f, 1.0f));
hengli's avatar
hengli committed
390 391 392 393 394
    osg::ref_ptr<osg::Geode> sphereGeode = new osg::Geode;
    sphereGeode->addDrawable(sphereDrawable);
    sphereGeode->setName("Sphere (0.1m)");
    nodes.push_back(sphereGeode);

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

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

lm's avatar
lm committed
407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428
//    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());
//        }
//    }

429 430 431 432 433 434
    return nodes;
}

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

440 441 442 443 444 445 446 447 448 449 450 451
    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);

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

458
    QLabel* modelLabel = new QLabel("Vehicle", this);
459
    QComboBox* modelComboBox = new QComboBox(this);
460
    for (int i = 0; i < vehicleModels.size(); ++i) {
461 462 463 464 465 466 467 468 469 470 471 472 473
        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);
474 475 476 477 478 479 480 481 482 483 484 485 486 487 488
    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);
489 490
    setLayout(layout);

491 492
    connect(frameComboBox, SIGNAL(currentIndexChanged(QString)),
            this, SLOT(selectFrame(QString)));
493 494 495 496 497 498
    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)));
499 500
    connect(mapComboBox, SIGNAL(currentIndexChanged(int)),
            this, SLOT(selectMapSource(int)));
501 502 503 504 505 506
    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)));
}
507

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

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

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

        recenterCamera(robotY, robotX, -robotZ);
525 526
    }

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

        moveCamera(dx, dy, dz);
533 534
    }

535 536 537 538
    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)));
539

540
    if (displayTrail) {
541 542 543
        updateTrail(robotX, robotY, robotZ);
    }

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

548
    if (displayWaypoints) {
549 550 551
        updateWaypoints();
    }

552
    if (enableTarget) {
553 554 555
        updateTarget(robotX, robotY);
    }

556 557 558
#ifdef QGC_PROTOBUF_ENABLED
    if (displayRGBD2D || displayRGBD3D) {
        updateRGBD(robotX, robotY, robotZ);
559
    }
560
#endif
561

562
    updateHUD(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw, utmZone);
563 564

    // set node visibility
565

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

    lastRobotX = robotX;
    lastRobotY = robotY;
    lastRobotZ = robotZ;
578 579
}

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

    Q3DWidget::keyPressEvent(event);
}

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

            return;
        }

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

            return;
        }
622 623 624 625 626
    }

    Q3DWidget::mousePressEvent(event);
}

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

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

            Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
            z = -altitude;
LM's avatar
LM committed
672
        } else if (frame == MAV_FRAME_LOCAL_NED) {
673 674 675 676 677 678 679 680 681 682 683 684 685 686
            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);
}

687 688 689 690
osg::ref_ptr<osg::Geode>
Pixhawk3DWidget::createGrid(void)
{
    osg::ref_ptr<osg::Geode> geode(new osg::Geode());
691 692 693 694
    osg::ref_ptr<osg::Geometry> fineGeometry(new osg::Geometry());
    osg::ref_ptr<osg::Geometry> coarseGeometry(new osg::Geometry());
    geode->addDrawable(fineGeometry);
    geode->addDrawable(coarseGeometry);
695 696 697 698

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

699 700
    osg::ref_ptr<osg::Vec3Array> fineCoords(new osg::Vec3Array);
    osg::ref_ptr<osg::Vec3Array> coarseCoords(new osg::Vec3Array);
701 702

    // draw a 20m x 20m grid with 0.25m resolution
703 704
    for (float i = -radius; i <= radius; i += resolution) {
        if (fabsf(i - floor(i + 0.5f)) < 0.01f) {
705 706 707 708
            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));
709
        } else {
710 711 712 713 714
            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));
        }
715 716
    }

717 718
    fineGeometry->setVertexArray(fineCoords);
    coarseGeometry->setVertexArray(coarseCoords);
719 720 721

    osg::ref_ptr<osg::Vec4Array> color(new osg::Vec4Array);
    color->push_back(osg::Vec4(0.5f, 0.5f, 0.5f, 1.0f));
722 723 724 725 726 727
    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,
728
                                  0, fineCoords->size()));
729
    coarseGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0,
730
                                    coarseCoords->size()));
731 732 733 734 735 736 737 738 739 740 741 742 743 744

    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);
745 746 747 748 749 750 751 752 753 754 755 756

    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());

757
    trailVertices = new osg::Vec3dArray;
758 759 760 761 762 763 764 765 766 767 768 769 770 771 772 773 774 775 776 777
    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;
}

778
osg::ref_ptr<Imagery>
779 780
Pixhawk3DWidget::createMap(void)
{
781
    return osg::ref_ptr<Imagery>(new Imagery());
782 783
}

784
osg::ref_ptr<osg::Geode>
785
Pixhawk3DWidget::createRGBD3D(void)
786
{
787
    int frameSize = 752 * 480;
788 789 790 791 792 793 794 795 796 797 798 799 800 801 802

    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;
803 804
}

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

    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;
}

825 826 827 828
void
Pixhawk3DWidget::setupHUD(void)
{
    osg::ref_ptr<osg::Vec4Array> hudColors(new osg::Vec4Array);
829 830
    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));
831 832

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

    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));

847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864
    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);
865 866 867 868

    scaleGeode = new HUDScaleGeode;
    scaleGeode->init();
    hudGroup->addChild(scaleGeode);
869 870 871
}

void
872
Pixhawk3DWidget::resizeHUD(void)
873
{
874
    int topHUDHeight = 25;
875 876 877
    int bottomHUDHeight = 25;

    osg::Vec3Array* vertices = static_cast<osg::Vec3Array*>(hudBackgroundGeometry->getVertexArray());
878
    if (vertices == NULL || vertices->size() != 8) {
879 880 881 882 883 884 885 886 887 888 889 890 891 892
        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);
893

894
    statusText->setPosition(osg::Vec3(10, height() - 15, -1.5));
895

896
    if (rgb2DGeode.valid() && depth2DGeode.valid()) {
897 898 899 900 901 902 903 904 905 906
        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
907
Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ,
908 909
                           double robotRoll, double robotPitch, double robotYaw,
                           const QString& utmZone)
910 911 912
{
    resizeHUD();

913
    std::pair<double,double> cursorPosition =
914
        getGlobalCursorPosition(getMouseX(), getMouseY(), -robotZ);
915 916 917 918

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

        oss.precision(2);
        oss << " Altitude = " << -robotZ <<
933 934 935
            " r = " << robotRoll <<
            " p = " << robotPitch <<
            " y = " << robotYaw;
936 937 938

        oss.precision(6);
        oss << " Cursor [" << cursorLatitude <<
939
            " " << cursorLongitude << "]";
LM's avatar
LM committed
940
    } else if (frame == MAV_FRAME_LOCAL_NED) {
941
        oss << " x = " << robotX <<
942 943 944 945 946 947 948
            " y = " << robotY <<
            " z = " << robotZ <<
            " r = " << robotRoll <<
            " p = " << robotPitch <<
            " y = " << robotYaw <<
            " Cursor [" << cursorPosition.first <<
            " " << cursorPosition.second << "]";
949 950
    }

951
    statusText->setText(oss.str());
952

953
    bool darkBackground = true;
954
    if (mapNode->getImageryType() == Imagery::GOOGLE_MAP) {
955 956 957 958 959
        darkBackground = false;
    }

    scaleGeode->update(height(), cameraParams.cameraFov,
                       cameraManipulator->getDistance(), darkBackground);
960 961 962
}

void
963
Pixhawk3DWidget::updateTrail(double robotX, double robotY, double robotZ)
964
{
965
    if (robotX == 0.0f || robotY == 0.0f || robotZ == 0.0f) {
966 967 968 969
        return;
    }

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

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

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

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

1003
void
1004
Pixhawk3DWidget::updateImagery(double originX, double originY, double originZ,
1005
                               const QString& zone)
1006
{
1007
    if (mapNode->getImageryType() == Imagery::BLANK_MAP) {
1008 1009 1010
        return;
    }

1011
    double viewingRadius = cameraManipulator->getDistance() * 10.0;
1012
    if (viewingRadius < 100.0) {
1013 1014 1015 1016
        viewingRadius = 100.0;
    }

    double minResolution = 0.25;
1017
    double centerResolution = cameraManipulator->getDistance() / 50.0;
1018 1019 1020
    double maxResolution = 1048576.0;

    Imagery::ImageryType imageryType = mapNode->getImageryType();
1021
    switch (imageryType) {
1022 1023 1024 1025 1026 1027 1028 1029 1030 1031
    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;
1032 1033
    default:
    {}
1034 1035 1036
    }

    double resolution = minResolution;
1037
    while (resolution * 2.0 < centerResolution) {
1038 1039
        resolution *= 2.0;
    }
1040
    if (resolution > maxResolution) {
1041 1042 1043 1044 1045 1046 1047
        resolution = maxResolution;
    }

    mapNode->draw3D(viewingRadius,
                    resolution,
                    cameraManipulator->getCenter().y(),
                    cameraManipulator->getCenter().x(),
1048 1049
                    originX,
                    originY,
1050
                    originZ,
1051 1052 1053
                    zone);

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

    mapNode->update();
}

1072 1073 1074
void
Pixhawk3DWidget::updateWaypoints(void)
{
1075
    waypointGroupNode->update(frame, uas);
1076
}
1077

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

1086 1087 1088 1089 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
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}
1215 1216
};

1217
#ifdef QGC_PROTOBUF_ENABLED
1218
void
1219
Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
1220
{
1221
    px::RGBDImage rgbdImage = uas->getRGBDImage();
1222 1223
    px::PointCloudXYZRGB pointCloud = uas->getPointCloud();

1224
    if (rgbdImage.rows() > 0 && rgbdImage.cols() > 0)
1225
    {
1226 1227 1228 1229 1230 1231 1232 1233
        rgbImage->setImage(rgbdImage.cols(), rgbdImage.rows(), 1,
                           GL_LUMINANCE, GL_LUMINANCE, GL_UNSIGNED_BYTE,
                           reinterpret_cast<unsigned char *>(&(*(rgbdImage.mutable_imagedata1()))[0]),
                           osg::Image::NO_DELETE);
        rgbImage->dirty();

        QByteArray coloredDepth(rgbdImage.cols() * rgbdImage.rows() * 3, 0);
        for (uint32_t r = 0; r < rgbdImage.rows(); ++r)
1234
        {
1235 1236 1237
            const float* depth = reinterpret_cast<const float*>(rgbdImage.imagedata2().c_str() + r * rgbdImage.step2());
            uint8_t* pixel = reinterpret_cast<uint8_t*>(coloredDepth.data()) + r * rgbdImage.cols() * 3;
            for (uint32_t c = 0; c < rgbdImage.cols(); ++c)
1238
            {
1239 1240 1241 1242
                if (depth[c] != 0)
                {
                    int idx = fminf(depth[c], 10.0f) / 10.0f * 127.0f;
                    idx = 127 - idx;
1243

1244 1245 1246 1247
                    pixel[0] = colormap_jet[idx][2] * 255.0f;
                    pixel[1] = colormap_jet[idx][1] * 255.0f;
                    pixel[2] = colormap_jet[idx][0] * 255.0f;
                }
1248

1249 1250
                pixel += 3;
            }
1251 1252
        }

1253 1254 1255 1256 1257 1258
        depthImage->setImage(rgbdImage.cols(), rgbdImage.rows(), 1,
                             GL_RGB, GL_RGB, GL_UNSIGNED_BYTE,
                             reinterpret_cast<unsigned char *>(coloredDepth.data()),
                             osg::Image::NO_DELETE);
        depthImage->dirty();
    }
1259 1260 1261 1262 1263

    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());
1264 1265 1266 1267 1268 1269 1270
    for (int i = 0; i < pointCloud.points_size(); ++i) {
        const px::PointCloudXYZRGB_PointXYZRGB& p = pointCloud.points(i);

        double x = p.x() - robotX;
        double y = p.y() - robotY;
        double z = p.z() - robotZ;

1271

1272
        (*vertices)[i].set(y, x, -z);
1273

1274
        if (enableRGBDColor) {
1275 1276 1277 1278 1279 1280 1281
            float rgb = p.rgb();

            float b = *(reinterpret_cast<unsigned char*>(&rgb)) / 255.0f;
            float g = *(1 + reinterpret_cast<unsigned char*>(&rgb)) / 255.0f;
            float r = *(2 + reinterpret_cast<unsigned char*>(&rgb)) / 255.0f;

            (*colors)[i].set(r, g, b, 1.0f);
1282
        } else {
1283 1284 1285 1286 1287 1288 1289
            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);
        }
1290 1291
    }

1292
    if (geometry->getNumPrimitiveSets() == 0) {
1293
        geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS,
1294
                                  0, pointCloud.points_size()));
1295
    } else {
1296
        osg::DrawArrays* drawarrays = static_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
1297
        drawarrays->setCount(pointCloud.points_size());
1298
    }
1299 1300
}
#endif
1301

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

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

    return -1;
}

1324 1325 1326
bool
Pixhawk3DWidget::findTarget(int mouseX, int mouseY)
{