MapWidget.cc 39.5 KB
Newer Older
1
/*==================================================================
pixhawk's avatar
pixhawk committed
2
3
4
5
======================================================================*/

/**
 * @file
lm's avatar
lm committed
6
 *   @brief Implementation of MapWidget
pixhawk's avatar
pixhawk committed
7
8
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
lm's avatar
lm committed
9
 *   @author Mariano Lizarraga
pixhawk's avatar
pixhawk committed
10
11
12
 *
 */

lm's avatar
lm committed
13
14
#include <QComboBox>
#include <QGridLayout>
pixhawk's avatar
pixhawk committed
15
#include <QDir>
16

17
#include "QGC.h"
pixhawk's avatar
pixhawk committed
18
19
#include "MapWidget.h"
#include "ui_MapWidget.h"
lm's avatar
lm committed
20
21
#include "UASInterface.h"
#include "UASManager.h"
22
23
#include "MAV2DIcon.h"
#include "Waypoint2DIcon.h"
24
#include "UASWaypointManager.h"
pixhawk's avatar
pixhawk committed
25

pixhawk's avatar
pixhawk committed
26
27
#include "MG.h"

28

pixhawk's avatar
pixhawk committed
29
MapWidget::MapWidget(QWidget *parent) :
30
31
32
33
34
35
36
37
38
    QWidget(parent),
    mc(NULL),
    zoomLevel(0),
    uasIcons(),
    uasTrails(),
    mav(NULL),
    lastUpdate(0),
    initialized(false),
    m_ui(new Ui::MapWidget)
pixhawk's avatar
pixhawk committed
39
40
{
    m_ui->setupUi(this);
lm's avatar
lm committed
41
    init();
42
43
44
45
}

void MapWidget::init()
{
46
    if (!initialized) {
lm's avatar
lm committed
47
48
49
50
51
52
53
54
55
56
57
        mc = new qmapcontrol::MapControl(this->size());
        // display the MapControl in the application
        QGridLayout* layout = new QGridLayout(this);
        layout->setMargin(0);
        layout->setSpacing(0);
        layout->addWidget(mc, 0, 0);
        setLayout(layout);

        //   VISUAL MAP STYLE
        QString buttonStyle("QAbstractButton { background-color: rgba(20, 20, 20, 45%); border-color: rgba(10, 10, 10, 50%)} QAbstractButton:checked { border: 2px solid #379AC3; }");
        mc->setPen(QGC::colorCyan.darker(400));
58

lm's avatar
lm committed
59
        waypointIsDrag = false;
60

lm's avatar
lm committed
61
62
        // Accept focus by clicking or keyboard
        this->setFocusPolicy(Qt::StrongFocus);
pixhawk's avatar
pixhawk committed
63

lm's avatar
lm committed
64
65
66
67
68
69
        // create MapControl

        mc->showScale(true);
        mc->showCoord(true);
        mc->enablePersistentCache();
        mc->setMouseTracking(true); // required to update the mouse position for diplay and capture
pixhawk's avatar
pixhawk committed
70

lm's avatar
lm committed
71
72
        // create MapAdapter to get maps from
        //TileMapAdapter* osmAdapter = new TileMapAdapter("tile.openstreetmap.org", "/%1/%2/%3.png", 256, 0, 17);
pixhawk's avatar
pixhawk committed
73

lm's avatar
lm committed
74
        qmapcontrol::MapAdapter* mapadapter_overlay = new qmapcontrol::YahooMapAdapter("us.maps3.yimg.com", "/aerial.maps.yimg.com/png?v=2.2&t=h&s=256&x=%2&y=%3&z=%1");
pixhawk's avatar
pixhawk committed
75

lm's avatar
lm committed
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
        // MAP BACKGROUND
        mapadapter = new qmapcontrol::GoogleSatMapAdapter();
        l = new qmapcontrol::MapLayer("Google Satellite", mapadapter);
        mc->addLayer(l);

        // STREET OVERLAY
        overlay = new qmapcontrol::MapLayer("Overlay", mapadapter_overlay);
        overlay->setVisible(false);
        mc->addLayer(overlay);

        // MAV FLIGHT TRACKS
        tracks = new qmapcontrol::MapLayer("Tracking", mapadapter);
        mc->addLayer(tracks);

        // WAYPOINT LAYER
        // create a layer with the mapadapter and type GeometryLayer (for waypoints)
        geomLayer = new qmapcontrol::GeometryLayer("Waypoints", mapadapter);
        mc->addLayer(geomLayer);



        //
        //    Layer* gsatLayer = new Layer("Google Satellite", gsat, Layer::MapLayer);
        //    mc->addLayer(gsatLayer);


        // Zurich, ETH

        int lastZoom = 16;
        double lastLat = 47.376889;
        double lastLon = 8.548056;

        QSettings settings;
        settings.beginGroup("QGC_MAPWIDGET");
        lastLat = settings.value("LAST_LATITUDE", lastLat).toDouble();
        lastLon = settings.value("LAST_LONGITUDE", lastLon).toDouble();
        lastZoom = settings.value("LAST_ZOOM", lastZoom).toInt();
        settings.endGroup();

        // SET INITIAL POSITION AND ZOOM
        // Set default zoom level
        mc->setZoom(lastZoom);
        mc->setView(QPointF(lastLon, lastLat));

        // Veracruz Mexico
        //mc->setView(QPointF(-96.105208,19.138955));

        // Add controls to select map provider
        /////////////////////////////////////////////////
        QActionGroup* mapproviderGroup = new QActionGroup(this);
Don Gagne's avatar
Don Gagne committed
126
127
128
129
130
        osmAction = new QAction(QIcon(":/res/openstreetmap.png"), tr("OpenStreetMap"), mapproviderGroup);
        yahooActionMap = new QAction(QIcon(":/res/yahoo.png"), tr("Yahoo: Map"), mapproviderGroup);
        yahooActionSatellite = new QAction(QIcon(":/res/yahoo.png"), tr("Yahoo: Satellite"), mapproviderGroup);
        googleActionMap = new QAction(QIcon(":/res/google.png"), tr("Google: Map"), mapproviderGroup);
        googleSatAction = new QAction(QIcon(":/res/google.png"), tr("Google: Sat"), mapproviderGroup);
lm's avatar
lm committed
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
        osmAction->setCheckable(true);
        yahooActionMap->setCheckable(true);
        yahooActionSatellite->setCheckable(true);
        googleActionMap->setCheckable(true);
        googleSatAction->setCheckable(true);
        googleSatAction->setChecked(true);
        connect(mapproviderGroup, SIGNAL(triggered(QAction*)),
                this, SLOT(mapproviderSelected(QAction*)));

        // Overlay seems currently broken
        //    yahooActionOverlay = new QAction(tr("Yahoo: street overlay"), this);
        //    yahooActionOverlay->setCheckable(true);
        //    yahooActionOverlay->setChecked(overlay->isVisible());
        //    connect(yahooActionOverlay, SIGNAL(toggled(bool)),
        //            overlay, SLOT(setVisible(bool)));

        //    mapproviderGroup->addAction(googleSatAction);
        //    mapproviderGroup->addAction(osmAction);
        //    mapproviderGroup->addAction(yahooActionOverlay);
        //    mapproviderGroup->addAction(googleActionMap);
        //    mapproviderGroup->addAction(yahooActionMap);
        //    mapproviderGroup->addAction(yahooActionSatellite);

        // Create map provider selection menu
        mapMenu = new QMenu(this);
        mapMenu->addActions(mapproviderGroup->actions());
        mapMenu->addSeparator();
        //    mapMenu->addAction(yahooActionOverlay);

        mapButton = new QPushButton(this);
        mapButton->setText("Map Source");
        mapButton->setMenu(mapMenu);
        mapButton->setStyleSheet(buttonStyle);

        // create buttons to control the map (zoom, GPS tracking and WP capture)
Don Gagne's avatar
Don Gagne committed
166
        QPushButton* zoomin = new QPushButton(QIcon(":/res/PlusSign"), "", this);
lm's avatar
lm committed
167
        zoomin->setStyleSheet(buttonStyle);
Don Gagne's avatar
Don Gagne committed
168
        QPushButton* zoomout = new QPushButton(QIcon(":/res/MinusSign"), "", this);
lm's avatar
lm committed
169
        zoomout->setStyleSheet(buttonStyle);
Don Gagne's avatar
Don Gagne committed
170
        createPath = new QPushButton(QIcon(":/res/BottomArrow"), "", this);
lm's avatar
lm committed
171
172
173
174
175
        createPath->setStyleSheet(buttonStyle);
        createPath->setToolTip(tr("Start / end waypoint add mode"));
        createPath->setStatusTip(tr("Start / end waypoint add mode"));
        //    clearTracking = new QPushButton(QIcon(""), "", this);
        //    clearTracking->setStyleSheet(buttonStyle);
Don Gagne's avatar
Don Gagne committed
176
        followgps = new QPushButton(QIcon(":/res/SystemLockScreen"), "", this);
lm's avatar
lm committed
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
        followgps->setStyleSheet(buttonStyle);
        followgps->setToolTip(tr("Follow the position of the current MAV with the map center"));
        followgps->setStatusTip(tr("Follow the position of the current MAV with the map center"));
        QPushButton* goToButton = new QPushButton(QIcon(""), "T", this);
        goToButton->setStyleSheet(buttonStyle);
        goToButton->setToolTip(tr("Enter a latitude/longitude position to move the map to"));
        goToButton->setStatusTip(tr("Enter a latitude/longitude position to move the map to"));

        zoomin->setMaximumWidth(30);
        zoomout->setMaximumWidth(30);
        createPath->setMaximumWidth(30);
        //    clearTracking->setMaximumWidth(30);
        followgps->setMaximumWidth(30);
        goToButton->setMaximumWidth(30);

        // Set checkable buttons
        // TODO: Currently checked buttons are are very difficult to distinguish when checked.
        //       create a style and the slots to change the background so it is easier to distinguish
        followgps->setCheckable(true);
        createPath->setCheckable(true);

        // add buttons to control the map (zoom, GPS tracking and WP capture)
        QGridLayout* innerlayout = new QGridLayout(mc);
        innerlayout->setMargin(3);
        innerlayout->setSpacing(3);
        innerlayout->addWidget(zoomin, 0, 0);
        innerlayout->addWidget(zoomout, 1, 0);
        innerlayout->addWidget(followgps, 2, 0);
        innerlayout->addWidget(createPath, 3, 0);
        //innerlayout->addWidget(clearTracking, 4, 0);
        // Add spacers to compress buttons on the top left
        innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 5, 0);
        innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 0, 1, 0, 7);
        innerlayout->addWidget(mapButton, 0, 6);
        innerlayout->addWidget(goToButton, 0, 7);
        innerlayout->setRowStretch(0, 1);
        innerlayout->setRowStretch(1, 100);
        mc->setLayout(innerlayout);

        // Configure the WP Path's pen
        pointPen = new QPen(QColor(0, 255,0));
        pointPen->setWidth(3);
        waypointPath = new qmapcontrol::LineString (wps, "Waypoint path", pointPen);
        mc->layer("Waypoints")->addGeometry(waypointPath);

lm's avatar
lm committed
222
223
224
225
226
227
228
229
230
231
232
        //Camera Control
        // CAMERA INDICATOR LAYER
        // create a layer with the mapadapter and type GeometryLayer (for camera indicator)
        camLayer = new qmapcontrol::GeometryLayer("Camera", mapadapter);
        mc->addLayer(camLayer);

        //camLine = new qmapcontrol::LineString(camPoints,"Camera Eje", camBorderPen);

        drawCamBorder = false;
        radioCamera = 10;

lm's avatar
lm committed
233
234
235
236
237
238
239
240
241
242
243
244
245
246
        // Done set state
        initialized = true;


        // Connect the required signals-slots
        connect(zoomin, SIGNAL(clicked(bool)),
                mc, SLOT(zoomIn()));

        connect(zoomout, SIGNAL(clicked(bool)),
                mc, SLOT(zoomOut()));

        connect(goToButton, SIGNAL(clicked()), this, SLOT(goTo()));

        QList<UASInterface*> systems = UASManager::instance()->getUASList();
247
        foreach(UASInterface* system, systems) {
lm's avatar
lm committed
248
249
            addUAS(system);
        }
pixhawk's avatar
pixhawk committed
250

lm's avatar
lm committed
251
252
        connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)),
                this, SLOT(addUAS(UASInterface*)));
pixhawk's avatar
pixhawk committed
253

lm's avatar
lm committed
254
255
        activeUASSet(UASManager::instance()->getActiveUAS());
        connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(activeUASSet(UASInterface*)));
pixhawk's avatar
pixhawk committed
256

lm's avatar
lm committed
257
258
        connect(mc, SIGNAL(mouseEventCoordinate(const QMouseEvent*, const QPointF)),
                this, SLOT(captureMapClick(const QMouseEvent*, const QPointF)));
pixhawk's avatar
pixhawk committed
259

lm's avatar
lm committed
260
261
        connect(createPath, SIGNAL(clicked(bool)),
                this, SLOT(createPathButtonClicked(bool)));
pixhawk's avatar
pixhawk committed
262

lm's avatar
lm committed
263
264
265
266
267
268
269
270
271
272
273
274

        connect(geomLayer, SIGNAL(geometryClicked(Geometry*,QPoint)),
                this, SLOT(captureGeometryClick(Geometry*, QPoint)));

        connect(geomLayer, SIGNAL(geometryDragged(Geometry*, QPointF)),
                this, SLOT(captureGeometryDrag(Geometry*, QPointF)));

        connect(geomLayer, SIGNAL(geometryEndDrag(Geometry*, QPointF)),
                this, SLOT(captureGeometryEndDrag(Geometry*, QPointF)));

        qDebug() << "CHECK END";
    }
lm's avatar
lm committed
275
276
}

277
278
279
void MapWidget::goTo()
{
    bool ok;
280
281
    QString text = QInputDialog::getText(this, tr("Please enter coordinates"),
                                         tr("Coordinates (Lat,Lon):"), QLineEdit::Normal,
282
                                         QString("%1,%2").arg(mc->currentCoordinate().y()).arg(mc->currentCoordinate().x()), &ok);
283
    if (ok && !text.isEmpty()) {
284
        QStringList split = text.split(",");
285
        if (split.length() == 2) {
286
287
288
289
290
291
            bool convert;
            double latitude = split.first().toDouble(&convert);
            ok &= convert;
            double longitude = split.last().toDouble(&convert);
            ok &= convert;

292
            if (ok) {
293
                mc->setView(QPointF(longitude, latitude));
294
295
296
            }
        }
    }
297
298
}

299

lm's avatar
lm committed
300
301
void MapWidget::mapproviderSelected(QAction* action)
{
302
    if (mc) {
lm's avatar
lm committed
303
304
        //delete mapadapter;
        mapButton->setText(action->text());
305
        if (action == osmAction) {
lm's avatar
lm committed
306
307
            int zoom = mapadapter->adaptedZoom();
            mc->setZoom(0);
pixhawk's avatar
pixhawk committed
308

lm's avatar
lm committed
309
310
311
            mapadapter = new qmapcontrol::OSMMapAdapter();
            l->setMapAdapter(mapadapter);
            geomLayer->setMapAdapter(mapadapter);
pixhawk's avatar
pixhawk committed
312

lm's avatar
lm committed
313
314
315
316
317
            if (isVisible()) mc->updateRequestNew();
            mc->setZoom(zoom);
            //        yahooActionOverlay->setEnabled(false);
            overlay->setVisible(false);
            //        yahooActionOverlay->setChecked(false);
pixhawk's avatar
pixhawk committed
318

319
        } else if (action == yahooActionMap) {
lm's avatar
lm committed
320
321
322
323
324
325
326
327
328
329
330
331
            int zoom = mapadapter->adaptedZoom();
            mc->setZoom(0);

            mapadapter = new qmapcontrol::YahooMapAdapter();
            l->setMapAdapter(mapadapter);
            geomLayer->setMapAdapter(mapadapter);

            if (isVisible()) mc->updateRequestNew();
            mc->setZoom(zoom);
            //        yahooActionOverlay->setEnabled(false);
            overlay->setVisible(false);
            //        yahooActionOverlay->setChecked(false);
332
        } else if (action == yahooActionSatellite) {
lm's avatar
lm committed
333
334
335
336
337
338
339
340
341
342
343
344
            int zoom = mapadapter->adaptedZoom();
            QPointF a = mc->currentCoordinate();
            mc->setZoom(0);

            mapadapter = new qmapcontrol::YahooMapAdapter("us.maps3.yimg.com", "/aerial.maps.yimg.com/png?v=1.7&t=a&s=256&x=%2&y=%3&z=%1");
            l->setMapAdapter(mapadapter);
            geomLayer->setMapAdapter(mapadapter);

            if (isVisible()) mc->updateRequestNew();
            mc->setZoom(zoom);
            overlay->setVisible(false);
            //        yahooActionOverlay->setEnabled(true);
345
        } else if (action == googleActionMap) {
lm's avatar
lm committed
346
347
348
349
350
351
352
353
354
355
356
            int zoom = mapadapter->adaptedZoom();
            mc->setZoom(0);
            mapadapter = new qmapcontrol::GoogleMapAdapter();
            l->setMapAdapter(mapadapter);
            geomLayer->setMapAdapter(mapadapter);

            if (isVisible()) mc->updateRequestNew();
            mc->setZoom(zoom);
            //        yahooActionOverlay->setEnabled(false);
            overlay->setVisible(false);
            //        yahooActionOverlay->setChecked(false);
357
        } else if (action == googleSatAction) {
lm's avatar
lm committed
358
359
360
361
362
363
364
365
366
367
368
            int zoom = mapadapter->adaptedZoom();
            mc->setZoom(0);
            mapadapter = new qmapcontrol::GoogleSatMapAdapter();
            l->setMapAdapter(mapadapter);
            geomLayer->setMapAdapter(mapadapter);

            if (isVisible()) mc->updateRequestNew();
            mc->setZoom(zoom);
            //        yahooActionOverlay->setEnabled(false);
            overlay->setVisible(false);
            //        yahooActionOverlay->setChecked(false);
369
        } else {
lm's avatar
lm committed
370
371
            mapButton->setText("Select..");
        }
lm's avatar
lm committed
372
    }
373
}
lm's avatar
lm committed
374

lm's avatar
lm committed
375

376
void MapWidget::createPathButtonClicked(bool checked)
lm's avatar
lm committed
377
{
378
    if (mc) {
lm's avatar
lm committed
379
        Q_UNUSED(checked);
pixhawk's avatar
pixhawk committed
380

381
        if (createPath->isChecked()) {
lm's avatar
lm committed
382
383
384
            // change the cursor shape
            this->setCursor(Qt::PointingHandCursor);
            mc->setMouseMode(qmapcontrol::MapControl::None);
pixhawk's avatar
pixhawk committed
385
386


lm's avatar
lm committed
387
388
            // emit signal start to create a Waypoint global
            //emit createGlobalWP(true, mc->currentCoordinate());
pixhawk's avatar
pixhawk committed
389

lm's avatar
lm committed
390
391
392
393
394
395
396
            //        // Clear the previous WP track
            //        // TODO: Move this to an actual clear track button and add a warning dialog
            //        mc->layer("Waypoints")->clearGeometries();
            //        wps.clear();
            //        path->setPoints(wps);
            //        mc->layer("Waypoints")->addGeometry(path);
            //        wpIndex.clear();
397
        } else {
pixhawk's avatar
pixhawk committed
398

lm's avatar
lm committed
399
400
401
402
            this->setCursor(Qt::ArrowCursor);
            mc->setMouseMode(qmapcontrol::MapControl::Panning);
        }
    }
403
}
404

405
406
407
408
409
410
411
/**
 * Captures a click on the map and if in create WP path mode, it adds the WP on MouseButtonRelease
 *
 * @param event The mouse event
 * @param coordinate The coordinate in which it occured the mouse event
 * @note  This slot is connected to the mouseEventCoordinate of the QMapControl object
 */
lm's avatar
lm committed
412

413
414
void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordinate)
{
415
    if (QEvent::MouseButtonRelease == event->type() && createPath->isChecked()) {
416
417
        // Create waypoint name
        QString str;
pixhawk's avatar
pixhawk committed
418

419
420
        // create the WP and set everything in the LineString to display the path
        Waypoint2DIcon* tempCirclePoint;
pixhawk's avatar
pixhawk committed
421

422
        if (mav) {
423
424
425
            double altitude = 0.0;
            double yaw = 0.0;
            int wpListCount = mav->getWaypointManager()->getWaypointList().count();
426
            if (wpListCount > 0) {
427
428
429
430
                altitude = mav->getWaypointManager()->getWaypointList().at(wpListCount-1)->getAltitude();
                yaw = mav->getWaypointManager()->getWaypointList().at(wpListCount-1)->getYaw();
            }
            mav->getWaypointManager()->addWaypoint(new Waypoint(wpListCount, coordinate.y(), coordinate.x(), altitude, yaw, true));
431
        } else {
432
            str = QString("%1").arg(waypointPath->numberOfPoints());
433
            tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle);
434
            wpIcons.append(tempCirclePoint);
pixhawk's avatar
pixhawk committed
435

436
            mc->layer("Waypoints")->addGeometry(tempCirclePoint);
pixhawk's avatar
pixhawk committed
437

438
439
440
441
442
            qmapcontrol::Point* tempPoint = new qmapcontrol::Point(coordinate.x(), coordinate.y(),str);
            wps.append(tempPoint);
            waypointPath->addPoint(tempPoint);

            // Refresh the screen
443
            if (isVisible()) mc->updateRequest(tempPoint->boundingBox().toRect());
444
        }
lm's avatar
lm committed
445

446
        // emit signal mouse was clicked
447
448
449
450
451
452
        //emit captureMapCoordinateClick(coordinate);
    }
}

void MapWidget::updateWaypoint(int uas, Waypoint* wp)
{
pixhawk's avatar
pixhawk committed
453
    // Update waypoint list and redraw map (last parameter)
454
455
456
    updateWaypoint(uas, wp, true);
}

457
458
459
460
/**
 * This function is called if a a single waypoint is updated and
 * also if the whole list changes.
 */
461
462
void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView)
{
463
    if (mc) {
lm's avatar
lm committed
464
        // Make sure this is the right UAS
465
        if (uas == this->mav->getUASID()) {
lm's avatar
lm committed
466
            // Only accept waypoints in global coordinate frame
467
            if (wp->getFrame() == MAV_FRAME_GLOBAL && wp->isNavigationType()) {
lm's avatar
lm committed
468
                // We're good, this is a global waypoint
469

lm's avatar
lm committed
470
                // Get the index of this waypoint
lm's avatar
lm committed
471
                // note the call to getGlobalFrameAndNavTypeIndexOf()
lm's avatar
lm committed
472
                // as we're only handling global waypoints
lm's avatar
lm committed
473
                int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getGlobalFrameAndNavTypeIndexOf(wp);
lm's avatar
lm committed
474
475
                // If not found, return (this should never happen, but helps safety)
                if (wpindex == -1) return;
pixhawk's avatar
pixhawk committed
476

lm's avatar
lm committed
477
                // Check if wp exists yet in map
478
                if (!(wpIcons.count() > wpindex)) {
lm's avatar
lm committed
479
                    // Waypoint is new, a new icon is created
480
                    QPointF coordinate;
481
482
                    coordinate.setX(wp->getLongitude());
                    coordinate.setY(wp->getLatitude());
lm's avatar
lm committed
483
                    createWaypointGraphAtMap(wpindex, coordinate);
484
                } else {
lm's avatar
lm committed
485
486
                    // Waypoint exists, update it if we're not
                    // currently dragging it with the mouse
487
                    if(!waypointIsDrag) {
lm's avatar
lm committed
488
489
490
491
492
493
                        QPointF coordinate;
                        coordinate.setX(wp->getLongitude());
                        coordinate.setY(wp->getLatitude());

                        Point* waypoint;
                        waypoint = wps.at(wpindex);
494
                        if (waypoint) {
lm's avatar
lm committed
495
496
497
498
499
500
501
502
503
504
                            // First set waypoint coordinate
                            waypoint->setCoordinate(coordinate);
                            // Now update icon position
                            wpIcons.at(wpindex)->setCoordinate(coordinate);
                            // Update pen
                            wpIcons.at(wpindex)->setPen(mavPens.value(uas));
                            // Then waypoint line coordinate
                            Point* linesegment = NULL;
                            // If the line segment already exists, just update it
                            // else create a new one
505
                            if (waypointPath->points().size() > wpindex) {
lm's avatar
lm committed
506
507
                                linesegment = waypointPath->points().at(wpindex);
                                if (linesegment) linesegment->setCoordinate(coordinate);
508
                            } else {
lm's avatar
lm committed
509
510
511
512
513
                                waypointPath->addPoint(waypoint);
                            }

                            // Update view
                            if (updateView) if (isVisible()) mc->updateRequest(waypoint->boundingBox().toRect());
514
                        }
515
516
                    }
                }
517
            } else {
lm's avatar
lm committed
518
519
520
521
                // Check if the index of this waypoint is larger than the global
                // waypoint list. This implies that the coordinate frame of this
                // waypoint was changed and the list containing only global
                // waypoints was shortened. Thus update the whole list
522
                if (waypointPath->points().count() > UASManager::instance()->getUASForId(uas)->getWaypointManager()->getGlobalFrameAndNavTypeCount()) {
lm's avatar
lm committed
523
524
                    updateWaypointList(uas);
                }
525
            }
526
        }
527
    }
pixhawk's avatar
pixhawk committed
528
529
}

530
void MapWidget::createWaypointGraphAtMap(int id, const QPointF coordinate)
531
{
532
    //if (!wpExists(coordinate))
533
    {
534
535
        // Create waypoint name
        QString str;
pixhawk's avatar
pixhawk committed
536

537
538
539
        // create the WP and set everything in the LineString to display the path
        //CirclePoint* tempCirclePoint = new CirclePoint(coordinate.x(), coordinate.y(), 10, str);
        Waypoint2DIcon* tempCirclePoint;
pixhawk's avatar
pixhawk committed
540

541
        if (mav) {
542
            int uas = mav->getUASID();
543
            str = QString("%1").arg(id);
544
545
            qDebug() << "Waypoint list count:" << str;
            tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle, mavPens.value(uas));
546
        } else {
547
            str = QString("%1").arg(id);
548
549
            tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle);
        }
pixhawk's avatar
pixhawk committed
550
551


552
        mc->layer("Waypoints")->addGeometry(tempCirclePoint);
pixhawk's avatar
pixhawk committed
553
        wpIcons.append(tempCirclePoint);
pixhawk's avatar
pixhawk committed
554

555
556
        Point* tempPoint = new Point(coordinate.x(), coordinate.y(),str);
        wps.append(tempPoint);
557
        waypointPath->addPoint(tempPoint);
pixhawk's avatar
pixhawk committed
558

559
        //wpIndex.insert(str,tempPoint);
tecnosapiens's avatar
tecnosapiens committed
560
        qDebug()<<"Funcion createWaypointGraphAtMap WP= "<<str<<" -> x= "<<tempPoint->latitude()<<" y= "<<tempPoint->longitude();
pixhawk's avatar
pixhawk committed
561

tecnosapiens's avatar
tecnosapiens committed
562
        // Refresh the screen
563
        if (isVisible()) if (isVisible()) mc->updateRequest(tempPoint->boundingBox().toRect());
564
    }
pixhawk's avatar
pixhawk committed
565

566
567
    ////    // emit signal mouse was clicked
    //    emit captureMapCoordinateClick(coordinate);
568
569
}

570
571
int MapWidget::wpExists(const QPointF coordinate)
{
572
573
    if (mc) {
        for (int i = 0; i < wps.size(); i++) {
lm's avatar
lm committed
574
            if (wps.at(i)->latitude() == coordinate.y() &&
575
                    wps.at(i)->longitude()== coordinate.x()) {
lm's avatar
lm committed
576
577
                return 1;
            }
578
        }
579
    }
580
    return 0;
581
582
}

583

584
585
void MapWidget::captureGeometryClick(Geometry* geom, QPoint point)
{
586
587
    Q_UNUSED(geom);
    Q_UNUSED(point);
pixhawk's avatar
pixhawk committed
588

lm's avatar
lm committed
589
    if (mc) mc->setMouseMode(qmapcontrol::MapControl::None);
590
591
}

592
593
void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate)
{
594
    waypointIsDrag = true;
pixhawk's avatar
pixhawk committed
595

596
    // Refresh the screen
597
    if (isVisible()) mc->updateRequest(geom->boundingBox().toRect());
pixhawk's avatar
pixhawk committed
598

599
    int temp = 0;
600
601
602
603
604

    // Get waypoint index in list
    bool wpIndexOk;
    int index = geom->name().toInt(&wpIndexOk);

605
    Waypoint2DIcon* point2Find = dynamic_cast <Waypoint2DIcon*> (geom);
pixhawk's avatar
pixhawk committed
606

607
    if (wpIndexOk && point2Find && wps.count() > index) {
608
        // Update visual
609
        point2Find->setCoordinate(coordinate);
610
611
        waypointPath->points().at(index)->setCoordinate(coordinate);
        if (isVisible()) mc->updateRequest(waypointPath->boundingBox().toRect());
pixhawk's avatar
pixhawk committed
612

613
        // Update waypoint data storage
614
        if (mav) {
lm's avatar
lm committed
615
            QVector<Waypoint*> wps = mav->getWaypointManager()->getGlobalFrameAndNavTypeWaypointList();
pixhawk's avatar
pixhawk committed
616

617
            if (wps.size() > index) {
618
                Waypoint* wp = wps.at(index);
619
620
                wp->setLatitude(coordinate.y());
                wp->setLongitude(coordinate.x());
621
                mav->getWaypointManager()->notifyOfChange(wp);
622
            }
623
        }
624
625
626
627
628

        // qDebug() << geom->name();
        temp = geom->get_myIndex();
        //qDebug() << temp;
        emit sendGeometryEndDrag(coordinate,temp);
629
    }
pixhawk's avatar
pixhawk committed
630

631
    waypointIsDrag = false;
632
633
}

634
void MapWidget::captureGeometryEndDrag(Geometry* geom, QPointF coordinate)
635
{
636
637
638
    Q_UNUSED(geom);
    Q_UNUSED(coordinate);
    // TODO: Investigate why when creating the waypoint path this slot is being called
pixhawk's avatar
pixhawk committed
639

640
    // Only change the mouse mode back to panning when not creating a WP path
641
    if (!createPath->isChecked()) {
642
643
644
        waypointIsDrag = false;
        mc->setMouseMode(qmapcontrol::MapControl::Panning);
    }
pixhawk's avatar
pixhawk committed
645

646
647
}

pixhawk's avatar
pixhawk committed
648
649
MapWidget::~MapWidget()
{
lm's avatar
lm committed
650
    delete mc;
pixhawk's avatar
pixhawk committed
651
652
    delete m_ui;
}
lm's avatar
lm committed
653
654
655
656
657
658
/**
 *
 * @param uas the UAS/MAV to monitor/display with the HUD
 */
void MapWidget::addUAS(UASInterface* uas)
{
659
    connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
660
    connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
661
    connect(uas, SIGNAL(systemSpecsChanged(int)), this, SLOT(updateSystemSpecs(int)));
662
663
}

pixhawk's avatar
pixhawk committed
664
665
666
667
668
/**
 * Update the whole list of waypoints. This is e.g. necessary if the list order changed.
 * The UAS manager will emit the appropriate signal whenever updating the list
 * is necessary.
 */
669
670
void MapWidget::updateWaypointList(int uas)
{
671
    if (mc) {
lm's avatar
lm committed
672
673
        // Get already existing waypoints
        UASInterface* uasInstance = UASManager::instance()->getUASForId(uas);
674
        if (uasInstance) {
lm's avatar
lm committed
675
676
677
            // Get update rect of old content, this is what will be redrawn
            // in the last step
            QRect updateRect = waypointPath->boundingBox().toRect();
678

lm's avatar
lm committed
679
680
            // Get all waypoints, including non-global waypoints
            QVector<Waypoint*> wpList = uasInstance->getWaypointManager()->getWaypointList();
681

lm's avatar
lm committed
682
            // Clear if necessary
683
            if (wpList.count() == 0) {
lm's avatar
lm committed
684
685
686
                clearWaypoints(uas);
                return;
            }
687

lm's avatar
lm committed
688
            // Trim internal list to number of global waypoints in the waypoint manager list
lm's avatar
lm committed
689
            int overSize = waypointPath->points().count() - uasInstance->getWaypointManager()->getGlobalFrameAndNavTypeCount();
690
            if (overSize > 0) {
lm's avatar
lm committed
691
692
693
                // Remove n waypoints at the end of the list
                // the remaining waypoints will be updated
                // in the next step
694
                for (int i = 0; i < overSize; ++i) {
lm's avatar
lm committed
695
696
697
698
699
                    wps.removeLast();
                    mc->layer("Waypoints")->removeGeometry(wpIcons.last());
                    wpIcons.removeLast();
                    waypointPath->points().removeLast();
                }
700
            }
pixhawk's avatar
pixhawk committed
701

lm's avatar
lm committed
702
            // Load all existing waypoints into map view
703
            foreach (Waypoint* wp, wpList) {
lm's avatar
lm committed
704
705
706
707
708
                // Block map draw updates, since we update everything in the next step
                // but update internal data structures.
                // Please note that updateWaypoint() ignores non-global waypoints
                updateWaypoint(mav->getUASID(), wp, false);
            }
709

lm's avatar
lm committed
710
711
712
            // Update view
            if (isVisible()) mc->updateRequest(updateRect);
        }
713
714
715
716
717
718
719
720
721
722
723
724
    }
}

void MapWidget::redoWaypoints(int uas)
{
    //    QObject* sender = QObject::sender();
    //    UASWaypointManager* manager = dynamic_cast<UASWaypointManager*>(sender);
    //    if (sender)
    //    {
    // Get waypoint list for this MAV

    // Clear all waypoints
725
    //clearWaypoints();
726
727
728
729
730
    // Re-add the updated waypoints

    //    }

    updateWaypointList(uas);
lm's avatar
lm committed
731
732
}

733
734
void MapWidget::activeUASSet(UASInterface* uas)
{
735
    // Disconnect old MAV
736
    if (mav) {
737
        // Disconnect the waypoint manager / data storage from the UI
738
        disconnect(mav->getWaypointManager(), SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int)));
739
        disconnect(mav->getWaypointManager(), SIGNAL(waypointChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*)));
740
741
742
        disconnect(this, SIGNAL(waypointCreated(Waypoint*)), mav->getWaypointManager(), SLOT(addWaypoint(Waypoint*)));
    }

743
    if (uas && mc) {
744
        mav = uas;
745
746
        QColor color = mav->getColor();
        color.setAlphaF(0.9);
747
        QPen* pen = new QPen(color);
748
        pen->setWidth(3.0);
749
750
751
752
        mavPens.insert(mav->getUASID(), pen);
        // FIXME Remove after refactoring
        waypointPath->setPen(pen);

753
        // Delete all waypoints and add waypoint from new system
754
755
        //redoWaypoints();
        updateWaypointList(uas->getUASID());
756

757
        // Connect the waypoint manager / data storage to the UI
758
        connect(mav->getWaypointManager(), SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int)));
759
        connect(mav->getWaypointManager(), SIGNAL(waypointChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*)));
760
        connect(this, SIGNAL(waypointCreated(Waypoint*)), mav->getWaypointManager(), SLOT(addWaypoint(Waypoint*)));
761

762
        updateSystemSpecs(mav->getUASID());
763
        updateSelectedSystem(mav->getUASID());
764
        mc->updateRequest(waypointPath->boundingBox().toRect());
765
766
767
    }
}

768
769
void MapWidget::updateSystemSpecs(int uas)
{
770
771
    if (mc) {
        foreach (qmapcontrol::Point* p, uasIcons.values()) {
lm's avatar
lm committed
772
            MAV2DIcon* icon = dynamic_cast<MAV2DIcon*>(p);
773
            if (icon && icon->getUASId() == uas) {
lm's avatar
lm committed
774
775
776
777
                // Set new airframe
                icon->setAirframe(UASManager::instance()->getUASForId(uas)->getAirframe());
                icon->drawIcon();
            }
778
779
780
781
        }
    }
}

782
783
void MapWidget::updateSelectedSystem(int uas)
{
784
785
    if (mc) {
        foreach (qmapcontrol::Point* p, uasIcons.values()) {
lm's avatar
lm committed
786
            MAV2DIcon* icon = dynamic_cast<MAV2DIcon*>(p);
787
            if (icon) {
lm's avatar
lm committed
788
789
790
                // Set as selected if ids match
                icon->setSelectedUAS((icon->getUASId() == uas));
            }
791
        }
792
793
794
    }
}

795
796
797
798
799
void MapWidget::updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 usec)
{
    Q_UNUSED(roll);
    Q_UNUSED(pitch);
    Q_UNUSED(usec);
800
    if (mc) {
lm's avatar
lm committed
801

802
        if (uas) {
lm's avatar
lm committed
803
            MAV2DIcon* icon = dynamic_cast<MAV2DIcon*>(uasIcons.value(uas->getUASID(), NULL));
804
            if (icon) {
lm's avatar
lm committed
805
806
                icon->setYaw(yaw);
            }
807
808
809
810
        }
    }
}

lm's avatar
lm committed
811
812
813
814
815
816
817
818
819
/**
 * Updates the global position of one MAV and append the last movement to the trail
 *
 * @param uas The unmanned air system
 * @param lat Latitude in WGS84 ellipsoid
 * @param lon Longitutde in WGS84 ellipsoid
 * @param alt Altitude over mean sea level
 * @param usec Timestamp of the position message in milliseconds FIXME will move to microseconds
 */
lm's avatar
lm committed
820
821
822
void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec)
{
    Q_UNUSED(usec);
pixhawk's avatar
pixhawk committed
823
    Q_UNUSED(alt); // FIXME Use altitude
824
    if (mc) {
lm's avatar
lm committed
825
826
827
828
829
830
831
832
833
834
835
836
        // create a LineString
        //QList<Point*> points;
        // Points with a circle
        // A QPen can be used to customize the
        //pointpen->setWidth(3);
        //points.append(new CirclePoint(lat, lon, 10, uas->getUASName(), Point::Middle, pointpen));

        qmapcontrol::Point* p;
        QPointF coordinate;
        coordinate.setX(lon);
        coordinate.setY(lat);

837
        if (!uasIcons.contains(uas->getUASID())) {
lm's avatar
lm committed
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
            // Get the UAS color
            QColor uasColor = uas->getColor();

            // Icon
            //QPen* pointpen = new QPen(uasColor);
            qDebug() << "2D MAP: ADDING" << uas->getUASName() << __FILE__ << __LINE__;
            p = new MAV2DIcon(uas, 68, uas->getSystemType(), uas->getColor(), QString("%1").arg(uas->getUASID()), qmapcontrol::Point::Middle);
            uasIcons.insert(uas->getUASID(), p);
            mc->layer("Waypoints")->addGeometry(p);

            // Line
            // A QPen also can use transparency

            //        QList<qmapcontrol::Point*> points;
            //        points.append(new qmapcontrol::Point(coordinate.x(), coordinate.y()));
            //        QPen* linepen = new QPen(uasColor.darker());
            //        linepen->setWidth(2);

            //        // Create tracking line string
            //        qmapcontrol::LineString* ls = new qmapcontrol::LineString(points, QString("%1").arg(uas->getUASID()), linepen);
            //        uasTrails.insert(uas->getUASID(), ls);

            //        // Add the LineString to the layer
            //        mc->layer("Waypoints")->addGeometry(ls);
862
        } else {
lm's avatar
lm committed
863
864
865
866
867
868
869
870
871
872
            //        p = dynamic_cast<MAV2DIcon*>(uasIcons.value(uas->getUASID()));
            //        if (p)
            //        {
            p = uasIcons.value(uas->getUASID());
            p->setCoordinate(QPointF(lon, lat));
            //p->setYaw(uas->getYaw());
            //        }
            // Extend trail
            //        uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(coordinate.x(), coordinate.y()));
        }
873

lm's avatar
lm committed
874
        if (isVisible()) mc->updateRequest(p->boundingBox().toRect());
875

lm's avatar
lm committed
876
        //if (isVisible()) mc->updateRequestNew();//(uasTrails.value(uas->getUASID())->boundingBox().toRect());
877

878
        if (this->mav && uas->getUASID() == this->mav->getUASID()) {
lm's avatar
lm committed
879
880
            // Limit the position update rate
            quint64 currTime = MG::TIME::getGroundTimeNow();
881
            if (currTime - lastUpdate > 120) {
lm's avatar
lm committed
882
883
                lastUpdate = currTime;
                // Sets the view to the interesting area
884
                if (followgps->isChecked()) {
lm's avatar
lm committed
885
                    updatePosition(0, lon, lat);
886
                } else {
lm's avatar
lm committed
887
888
889
                    // Refresh the screen
                    //if (isVisible()) mc->updateRequestNew();
                }
890
            }
891
        }
pixhawk's avatar
pixhawk committed
892
893
    }
}
pixhawk's avatar
pixhawk committed
894

lm's avatar
lm committed
895
896
897
/**
 * Center the view on this position
 */
lm's avatar
lm committed
898
void MapWidget::updatePosition(float time, double lat, double lon)
pixhawk's avatar
pixhawk committed
899
{
lm's avatar
lm committed
900
    Q_UNUSED(time);
901
    //gpsposition->setText(QString::number(time) + " / " + QString::number(lat) + " / " + QString::number(lon));
902
    if (followgps->isChecked() && isVisible() && mc) {
903
        if (mc) mc->setView(QPointF(lat, lon));
pixhawk's avatar
pixhawk committed
904
905
906
907
908
    }
}

void MapWidget::wheelEvent(QWheelEvent *event)
{
909
    if (mc) {
lm's avatar
lm committed
910
911
912
913
914
915
916
917
918
919
920
        int numDegrees = event->delta() / 8;
        int numSteps = numDegrees / 15;
        // Calculate new zoom level
        int newZoom = mc->currentZoom()+numSteps;
        // Set new zoom level, level is bounded by map control
        mc->setZoom(newZoom);
        // Detail zoom level is the number of steps zoomed in further
        // after the bounding has taken effect
        detailZoom = qAbs(qMin(0, mc->currentZoom()-newZoom));

        // visual field of camera
lm's avatar
lm committed
921
        updateCameraPosition(20*newZoom,0,"no");
lm's avatar
lm committed
922
    }
pixhawk's avatar
pixhawk committed
923
924
925
926
}

void MapWidget::keyPressEvent(QKeyEvent *event)
{
927
    if (mc) {
lm's avatar
lm committed
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
        switch (event->key()) {
        case Qt::Key_Plus:
            mc->zoomIn();
            break;
        case Qt::Key_Minus:
            mc->zoomOut();
            break;
        case Qt::Key_Left:
            mc->scrollLeft(this->width()/scrollStep);
            break;
        case Qt::Key_Right:
            mc->scrollRight(this->width()/scrollStep);
            break;
        case Qt::Key_Down:
            mc->scrollDown(this->width()/scrollStep);
            break;
        case Qt::Key_Up:
            mc->scrollUp(this->width()/scrollStep);
            break;
        default:
            QWidget::keyPressEvent(event);
        }
pixhawk's avatar
pixhawk committed
950
951
952
    }
}

953
void MapWidget::resizeEvent(QResizeEvent* event )
pixhawk's avatar
pixhawk committed
954
{
955
    Q_UNUSED(event);
956
957
958
    if (!initialized) {
        init();
    }
959
    if (mc) mc->resize(this->size());
pixhawk's avatar
pixhawk committed
960
961
}

pixhawk's avatar
pixhawk committed
962
963
964
void MapWidget::showEvent(QShowEvent* event)
{
    Q_UNUSED(event);
lm's avatar
lm committed
965
966
967
968
969
970
971
//    if (isVisible())
//    {
//	if (!initialized)
//	{
//            init();
//	}
//    }
pixhawk's avatar
pixhawk committed
972
973
974
975
976
}

void MapWidget::hideEvent(QHideEvent* event)
{
    Q_UNUSED(event);
977
    if (mc) {
lm's avatar
lm committed
978
979
980
981
982
983
984
985
986
        QSettings settings;
        settings.beginGroup("QGC_MAPWIDGET");
        QPointF currentPos = mc->currentCoordinate();
        settings.setValue("LAST_LATITUDE", currentPos.y());
        settings.setValue("LAST_LONGITUDE", currentPos.x());
        settings.setValue("LAST_ZOOM", mc->currentZoom());
        settings.endGroup();
        settings.sync();
    }
pixhawk's avatar
pixhawk committed
987
988
}

pixhawk's avatar
pixhawk committed
989
990
991
992
993
994
995
996
997
998
999
1000

void MapWidget::changeEvent(QEvent *e)
{
    QWidget::changeEvent(e);
    switch (e->type()) {
    case QEvent::LanguageChange:
        m_ui->retranslateUi(this);
        break;
    default:
        break;
    }
}
For faster browsing, not all history is shown. View entire blame