MapWidget.cc 31.8 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>
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 30 31
MapWidget::MapWidget(QWidget *parent) :
        QWidget(parent),
        zoomLevel(0),
32 33
        uasIcons(),
        uasTrails(),
34
        mav(NULL),
35
        lastUpdate(0),
pixhawk's avatar
pixhawk committed
36 37 38
        m_ui(new Ui::MapWidget)
{
    m_ui->setupUi(this);
39 40 41
    mc = new qmapcontrol::MapControl(QSize(320, 240));

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

45 46 47 48 49 50 51 52








53
    waypointIsDrag = false;
pixhawk's avatar
pixhawk committed
54

pixhawk's avatar
pixhawk committed
55 56
    // Accept focus by clicking or keyboard
    this->setFocusPolicy(Qt::StrongFocus);
pixhawk's avatar
pixhawk committed
57

pixhawk's avatar
pixhawk committed
58
    // create MapControl
59

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

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

68
    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
69

lm's avatar
lm committed
70
    // MAP BACKGROUND
71 72
    mapadapter = new qmapcontrol::GoogleSatMapAdapter();
    l = new qmapcontrol::MapLayer("Google Satellite", mapadapter);
lm's avatar
lm committed
73
    mc->addLayer(l);
pixhawk's avatar
pixhawk committed
74

lm's avatar
lm committed
75
    // STREET OVERLAY
76
    overlay = new qmapcontrol::MapLayer("Overlay", mapadapter_overlay);
lm's avatar
lm committed
77 78
    overlay->setVisible(false);
    mc->addLayer(overlay);
pixhawk's avatar
pixhawk committed
79

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

lm's avatar
lm committed
84 85
    // WAYPOINT LAYER
    // create a layer with the mapadapter and type GeometryLayer (for waypoints)
86
    geomLayer = new qmapcontrol::GeometryLayer("Waypoints", mapadapter);
87
    mc->addLayer(geomLayer);
pixhawk's avatar
pixhawk committed
88 89 90



91 92 93
    //
    //    Layer* gsatLayer = new Layer("Google Satellite", gsat, Layer::MapLayer);
    //    mc->addLayer(gsatLayer);
pixhawk's avatar
pixhawk committed
94

lm's avatar
lm committed
95 96
    // SET INITIAL POSITION AND ZOOM
    // Set default zoom level
pixhawk's avatar
pixhawk committed
97
    mc->setZoom(17);
lm's avatar
lm committed
98
    // Zurich, ETH
99
    mc->setView(QPointF(8.548056,47.376389));
pixhawk's avatar
pixhawk committed
100

101 102
    // Veracruz Mexico
    //mc->setView(QPointF(-96.105208,19.138955));
pixhawk's avatar
pixhawk committed
103

lm's avatar
lm committed
104 105 106
    // Add controls to select map provider
    /////////////////////////////////////////////////
    QActionGroup* mapproviderGroup = new QActionGroup(this);
107 108 109 110 111
    osmAction = new QAction(QIcon(":/images/mapproviders/openstreetmap.png"), tr("OpenStreetMap"), mapproviderGroup);
    yahooActionMap = new QAction(QIcon(":/images/mapproviders/yahoo.png"), tr("Yahoo: Map"), mapproviderGroup);
    yahooActionSatellite = new QAction(QIcon(":/images/mapproviders/yahoo.png"), tr("Yahoo: Satellite"), mapproviderGroup);
    googleActionMap = new QAction(QIcon(":/images/mapproviders/google.png"), tr("Google: Map"), mapproviderGroup);
    googleSatAction = new QAction(QIcon(":/images/mapproviders/google.png"), tr("Google: Sat"), mapproviderGroup);
lm's avatar
lm committed
112 113 114 115 116 117 118 119
    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*)));
pixhawk's avatar
pixhawk committed
120

lm's avatar
lm committed
121
    // Overlay seems currently broken
122 123 124 125 126
    //    yahooActionOverlay = new QAction(tr("Yahoo: street overlay"), this);
    //    yahooActionOverlay->setCheckable(true);
    //    yahooActionOverlay->setChecked(overlay->isVisible());
    //    connect(yahooActionOverlay, SIGNAL(toggled(bool)),
    //            overlay, SLOT(setVisible(bool)));
pixhawk's avatar
pixhawk committed
127

128 129 130 131 132 133
    //    mapproviderGroup->addAction(googleSatAction);
    //    mapproviderGroup->addAction(osmAction);
    //    mapproviderGroup->addAction(yahooActionOverlay);
    //    mapproviderGroup->addAction(googleActionMap);
    //    mapproviderGroup->addAction(yahooActionMap);
    //    mapproviderGroup->addAction(yahooActionSatellite);
pixhawk's avatar
pixhawk committed
134

lm's avatar
lm committed
135 136 137 138
    // Create map provider selection menu
    mapMenu = new QMenu(this);
    mapMenu->addActions(mapproviderGroup->actions());
    mapMenu->addSeparator();
139
    //    mapMenu->addAction(yahooActionOverlay);
pixhawk's avatar
pixhawk committed
140

lm's avatar
lm committed
141 142 143
    mapButton = new QPushButton(this);
    mapButton->setText("Map Source");
    mapButton->setMenu(mapMenu);
144
    mapButton->setStyleSheet(buttonStyle);
pixhawk's avatar
pixhawk committed
145

pixhawk's avatar
pixhawk committed
146
    // display the MapControl in the application
lm's avatar
lm committed
147
    QGridLayout* layout = new QGridLayout(this);
pixhawk's avatar
pixhawk committed
148
    layout->setMargin(0);
149 150
    layout->setSpacing(0);
    layout->addWidget(mc, 0, 0);
pixhawk's avatar
pixhawk committed
151
    setLayout(layout);
pixhawk's avatar
pixhawk committed
152

153
    // create buttons to control the map (zoom, GPS tracking and WP capture)
pixhawk's avatar
pixhawk committed
154
    QPushButton* zoomin = new QPushButton(QIcon(":/images/actions/list-add.svg"), "", this);
155
    zoomin->setStyleSheet(buttonStyle);
pixhawk's avatar
pixhawk committed
156
    QPushButton* zoomout = new QPushButton(QIcon(":/images/actions/list-remove.svg"), "", this);
157
    zoomout->setStyleSheet(buttonStyle);
158
    createPath = new QPushButton(QIcon(":/images/actions/go-bottom.svg"), "", this);
159
    createPath->setStyleSheet(buttonStyle);
160
    clearTracking = new QPushButton(QIcon(""), "", this);
161
    clearTracking->setStyleSheet(buttonStyle);
pixhawk's avatar
pixhawk committed
162
    followgps = new QPushButton(QIcon(":/images/actions/system-lock-screen.svg"), "", this);
163 164 165
    followgps->setStyleSheet(buttonStyle);
    QPushButton* goToButton = new QPushButton(QIcon(""), "T", this);
    goToButton->setStyleSheet(buttonStyle);
pixhawk's avatar
pixhawk committed
166

167 168 169 170 171
    zoomin->setMaximumWidth(30);
    zoomout->setMaximumWidth(30);
    createPath->setMaximumWidth(30);
    clearTracking->setMaximumWidth(30);
    followgps->setMaximumWidth(30);
172
    goToButton->setMaximumWidth(30);
pixhawk's avatar
pixhawk committed
173

174 175 176 177 178
    // 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);
pixhawk's avatar
pixhawk committed
179

180
    // add buttons to control the map (zoom, GPS tracking and WP capture)
lm's avatar
lm committed
181
    QGridLayout* innerlayout = new QGridLayout(mc);
182 183
    innerlayout->setMargin(3);
    innerlayout->setSpacing(3);
lm's avatar
lm committed
184 185 186 187
    innerlayout->addWidget(zoomin, 0, 0);
    innerlayout->addWidget(zoomout, 1, 0);
    innerlayout->addWidget(followgps, 2, 0);
    innerlayout->addWidget(createPath, 3, 0);
188
    innerlayout->addWidget(clearTracking, 4, 0);
lm's avatar
lm committed
189
    // Add spacers to compress buttons on the top left
190
    innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 5, 0);
191
    innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 0, 1, 0, 7);
192 193
    innerlayout->addWidget(mapButton, 0, 6);
    innerlayout->addWidget(goToButton, 0, 7);
lm's avatar
lm committed
194 195
    innerlayout->setRowStretch(0, 1);
    innerlayout->setRowStretch(1, 100);
pixhawk's avatar
pixhawk committed
196
    mc->setLayout(innerlayout);
pixhawk's avatar
pixhawk committed
197 198


199
    // Connect the required signals-slots
200 201
    connect(zoomin, SIGNAL(clicked(bool)),
            mc, SLOT(zoomIn()));
pixhawk's avatar
pixhawk committed
202

203 204
    connect(zoomout, SIGNAL(clicked(bool)),
            mc, SLOT(zoomOut()));
pixhawk's avatar
pixhawk committed
205

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

pixhawk's avatar
pixhawk committed
208 209 210 211 212 213
    QList<UASInterface*> systems = UASManager::instance()->getUASList();
    foreach(UASInterface* system, systems)
    {
        addUAS(system);
    }

214 215
    connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)),
            this, SLOT(addUAS(UASInterface*)));
pixhawk's avatar
pixhawk committed
216 217

    activeUASSet(UASManager::instance()->getActiveUAS());
218
    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(activeUASSet(UASInterface*)));
pixhawk's avatar
pixhawk committed
219

220 221
    connect(mc, SIGNAL(mouseEventCoordinate(const QMouseEvent*, const QPointF)),
            this, SLOT(captureMapClick(const QMouseEvent*, const QPointF)));
pixhawk's avatar
pixhawk committed
222

223
    connect(createPath, SIGNAL(clicked(bool)),
224
            this, SLOT(createPathButtonClicked(bool)));
pixhawk's avatar
pixhawk committed
225 226


227 228
    connect(geomLayer, SIGNAL(geometryClicked(Geometry*,QPoint)),
            this, SLOT(captureGeometryClick(Geometry*, QPoint)));
pixhawk's avatar
pixhawk committed
229

230 231
    connect(geomLayer, SIGNAL(geometryDragged(Geometry*, QPointF)),
            this, SLOT(captureGeometryDrag(Geometry*, QPointF)));
pixhawk's avatar
pixhawk committed
232

233 234
    connect(geomLayer, SIGNAL(geometryEndDrag(Geometry*, QPointF)),
            this, SLOT(captureGeometryEndDrag(Geometry*, QPointF)));
pixhawk's avatar
pixhawk committed
235

236 237 238
    // Configure the WP Path's pen
    pointPen = new QPen(QColor(0, 255,0));
    pointPen->setWidth(3);
239 240
    waypointPath = new qmapcontrol::LineString (wps, "Waypoint path", pointPen);
    mc->layer("Waypoints")->addGeometry(waypointPath);
pixhawk's avatar
pixhawk committed
241

242 243 244 245 246
    //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);
pixhawk's avatar
pixhawk committed
247

248
    //camLine = new qmapcontrol::LineString(camPoints,"Camera Eje", camBorderPen);
pixhawk's avatar
pixhawk committed
249

250 251
    drawCamBorder = false;
    radioCamera = 10;
lm's avatar
lm committed
252 253
}

254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278
void MapWidget::goTo()
{
    bool ok;
         QString text = QInputDialog::getText(this, tr("Please enter coordinates"),
                                              tr("Coordinates (Lat,Lon):"), QLineEdit::Normal,
                                              QString("%1,%2").arg(mc->currentCoordinate().x()).arg(mc->currentCoordinate().y()), &ok);
         if (ok && !text.isEmpty())
         {
             QStringList split = text.split(",");
             if (split.length() == 2)
             {
                 bool convert;
                 double latitude = split.first().toDouble(&convert);
                 ok &= convert;
                 double longitude = split.last().toDouble(&convert);
                 ok &= convert;

                 if (ok)
                 {
                     mc->setView(QPointF(latitude, longitude));
                 }
             }
         }
}

279

lm's avatar
lm committed
280 281 282 283 284 285 286 287
void MapWidget::mapproviderSelected(QAction* action)
{
    //delete mapadapter;
    mapButton->setText(action->text());
    if (action == osmAction)
    {
        int zoom = mapadapter->adaptedZoom();
        mc->setZoom(0);
pixhawk's avatar
pixhawk committed
288

289
        mapadapter = new qmapcontrol::OSMMapAdapter();
lm's avatar
lm committed
290 291
        l->setMapAdapter(mapadapter);
        geomLayer->setMapAdapter(mapadapter);
pixhawk's avatar
pixhawk committed
292

lm's avatar
lm committed
293 294
        mc->updateRequestNew();
        mc->setZoom(zoom);
295
        //        yahooActionOverlay->setEnabled(false);
lm's avatar
lm committed
296
        overlay->setVisible(false);
297
        //        yahooActionOverlay->setChecked(false);
pixhawk's avatar
pixhawk committed
298

lm's avatar
lm committed
299 300 301 302 303
    }
    else if (action == yahooActionMap)
    {
        int zoom = mapadapter->adaptedZoom();
        mc->setZoom(0);
pixhawk's avatar
pixhawk committed
304

305
        mapadapter = new qmapcontrol::YahooMapAdapter();
lm's avatar
lm committed
306 307
        l->setMapAdapter(mapadapter);
        geomLayer->setMapAdapter(mapadapter);
pixhawk's avatar
pixhawk committed
308

lm's avatar
lm committed
309 310
        mc->updateRequestNew();
        mc->setZoom(zoom);
311
        //        yahooActionOverlay->setEnabled(false);
lm's avatar
lm committed
312
        overlay->setVisible(false);
313
        //        yahooActionOverlay->setChecked(false);
lm's avatar
lm committed
314 315 316 317 318 319
    }
    else if (action == yahooActionSatellite)
    {
        int zoom = mapadapter->adaptedZoom();
        QPointF a = mc->currentCoordinate();
        mc->setZoom(0);
pixhawk's avatar
pixhawk committed
320

321
        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");
lm's avatar
lm committed
322
        l->setMapAdapter(mapadapter);
pixhawk's avatar
pixhawk committed
323

lm's avatar
lm committed
324 325
        mc->updateRequestNew();
        mc->setZoom(zoom);
326
        //        yahooActionOverlay->setEnabled(true);
lm's avatar
lm committed
327 328 329 330 331
    }
    else if (action == googleActionMap)
    {
        int zoom = mapadapter->adaptedZoom();
        mc->setZoom(0);
332
        mapadapter = new qmapcontrol::GoogleMapAdapter();
lm's avatar
lm committed
333 334
        l->setMapAdapter(mapadapter);
        geomLayer->setMapAdapter(mapadapter);
pixhawk's avatar
pixhawk committed
335

lm's avatar
lm committed
336 337
        mc->updateRequestNew();
        mc->setZoom(zoom);
338
        //        yahooActionOverlay->setEnabled(false);
lm's avatar
lm committed
339
        overlay->setVisible(false);
340
        //        yahooActionOverlay->setChecked(false);
lm's avatar
lm committed
341 342 343 344 345
    }
    else if (action == googleSatAction)
    {
        int zoom = mapadapter->adaptedZoom();
        mc->setZoom(0);
346
        mapadapter = new qmapcontrol::GoogleSatMapAdapter();
lm's avatar
lm committed
347 348
        l->setMapAdapter(mapadapter);
        geomLayer->setMapAdapter(mapadapter); 
pixhawk's avatar
pixhawk committed
349

lm's avatar
lm committed
350 351
        mc->updateRequestNew();
        mc->setZoom(zoom);
352
        //        yahooActionOverlay->setEnabled(false);
lm's avatar
lm committed
353
        overlay->setVisible(false);
354
        //        yahooActionOverlay->setChecked(false);
lm's avatar
lm committed
355 356 357 358 359
    }
    else
    {
        mapButton->setText("Select..");
    }
360
}
lm's avatar
lm committed
361

lm's avatar
lm committed
362

363
void MapWidget::createPathButtonClicked(bool checked)
lm's avatar
lm committed
364
{
365
    Q_UNUSED(checked);
pixhawk's avatar
pixhawk committed
366

lm's avatar
lm committed
367 368 369 370
    if (createPath->isChecked())
    {
        // change the cursor shape
        this->setCursor(Qt::PointingHandCursor);
371
        mc->setMouseMode(qmapcontrol::MapControl::None);
pixhawk's avatar
pixhawk committed
372 373


374
        // emit signal start to create a Waypoint global
375
        //emit createGlobalWP(true, mc->currentCoordinate());
pixhawk's avatar
pixhawk committed
376

377 378 379 380 381 382 383
        //        // 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();
384 385 386
    }
    else
    {
pixhawk's avatar
pixhawk committed
387

lm's avatar
lm committed
388
        this->setCursor(Qt::ArrowCursor);
389
        mc->setMouseMode(qmapcontrol::MapControl::Panning);
lm's avatar
lm committed
390
    }
pixhawk's avatar
pixhawk committed
391

392
}
393

394 395 396 397 398 399 400
/**
 * 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
 */
401

402 403
void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordinate)
{
404
    if (QEvent::MouseButtonRelease == event->type() && createPath->isChecked())
405
    {
406 407
        // Create waypoint name
        QString str;
pixhawk's avatar
pixhawk committed
408

409 410
        // create the WP and set everything in the LineString to display the path
        Waypoint2DIcon* tempCirclePoint;
pixhawk's avatar
pixhawk committed
411

412 413
        if (mav)
        {
414
            mav->getWaypointManager()->addWaypoint(new Waypoint(mav->getWaypointManager()->getWaypointList().count(), coordinate.x(), coordinate.y()));
415 416 417
        }
        else
        {
418
            str = QString("%1").arg(waypointPath->numberOfPoints());
419
            tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle);
pixhawk's avatar
pixhawk committed
420

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

423 424 425 426 427 428 429
            qmapcontrol::Point* tempPoint = new qmapcontrol::Point(coordinate.x(), coordinate.y(),str);
            wps.append(tempPoint);
            waypointPath->addPoint(tempPoint);

            // Refresh the screen
            mc->updateRequest(tempPoint->boundingBox().toRect());
        }
pixhawk's avatar
pixhawk committed
430

431
        // emit signal mouse was clicked
432 433 434 435 436 437
        //emit captureMapCoordinateClick(coordinate);
    }
}

void MapWidget::updateWaypoint(int uas, Waypoint* wp)
{
438
    //qDebug() << "UPDATING WP" << wp->getId() << __FILE__ << __LINE__;
439 440
    if (uas == this->mav->getUASID())
    {
441
        int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getIndexOf(wp);
442
        // Create waypoint name
443
        QString str = QString("%1").arg(wpindex);
444
        // Check if wp exists yet
445
        if (!(wps.count() > wpindex))
446 447 448 449
        {
            QPointF coordinate;
            coordinate.setX(wp->getX());
            coordinate.setY(wp->getY());
450
            createWaypointGraphAtMap(wpindex, coordinate);
451

452 453 454 455 456 457 458
            qDebug() << "Waypoint Index did not contain" << str;
        }
        else
        {
            // Waypoint exists, update it
            if(!waypointIsDrag)
            {
459
                qDebug() <<"indice WP= "<< wpindex <<"\n";
460 461 462 463 464 465

                QPointF coordinate;
                coordinate.setX(wp->getX());
                coordinate.setY(wp->getY());

                Point* waypoint;
466
                waypoint = wps.at(wpindex);//wpIndex[str];
467 468 469 470
                if (waypoint)
                {
                    // First set waypoint coordinate
                    waypoint->setCoordinate(coordinate);
pixhawk's avatar
pixhawk committed
471
                    // Now update icon position
472 473 474
                    //mc->layer("Waypoints")->removeGeometry(wpIcons.at(wpindex));
                    wpIcons.at(wpindex)->setCoordinate(coordinate);
                    //mc->layer("Waypoints")->addGeometry(wpIcons.at(wpindex));
475
                    // Then waypoint line coordinate
476
                    Point* linesegment = NULL;
477
                    if (waypointPath->points().size() > wpindex)
478
                    {
479
                        linesegment = waypointPath->points().at(wpindex);
480
                    }
481 482 483 484 485 486

                    if (linesegment)
                    {
                        linesegment->setCoordinate(coordinate);
                    }

487
                    //point2Find = dynamic_cast <Point*> (mc->layer("Waypoints")->get_Geometry(wpindex));
488 489 490 491 492
                    //point2Find->setCoordinate(coordinate);
                    mc->updateRequest(waypoint->boundingBox().toRect());
                }
            }
        }
493
    }
pixhawk's avatar
pixhawk committed
494 495
}

496
void MapWidget::createWaypointGraphAtMap(int id, const QPointF coordinate)
497
{
498 499
    if (!wpExists(coordinate))
    {
500 501
        // Create waypoint name
        QString str;
pixhawk's avatar
pixhawk committed
502

503 504 505
        // 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
506

507 508
        if (mav)
        {
509
            int uas = mav->getUASID();
510
            str = QString("%1").arg(id);
511 512
            qDebug() << "Waypoint list count:" << str;
            tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle, mavPens.value(uas));
513 514 515
        }
        else
        {
516
            str = QString("%1").arg(id);
517 518
            tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle);
        }
pixhawk's avatar
pixhawk committed
519 520


521
        mc->layer("Waypoints")->addGeometry(tempCirclePoint);
pixhawk's avatar
pixhawk committed
522
        wpIcons.append(tempCirclePoint);
pixhawk's avatar
pixhawk committed
523

524 525
        Point* tempPoint = new Point(coordinate.x(), coordinate.y(),str);
        wps.append(tempPoint);
526
        waypointPath->addPoint(tempPoint);
pixhawk's avatar
pixhawk committed
527

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

tecnosapiens's avatar
tecnosapiens committed
531
        // Refresh the screen
532
        mc->updateRequest(tempPoint->boundingBox().toRect());
533
    }
pixhawk's avatar
pixhawk committed
534

535 536
    ////    // emit signal mouse was clicked
    //    emit captureMapCoordinateClick(coordinate);
537 538
}

539 540
int MapWidget::wpExists(const QPointF coordinate)
{
541 542
    for (int i = 0; i < wps.size(); i++){
        if (wps.at(i)->latitude() == coordinate.y() &&
543 544
            wps.at(i)->longitude()== coordinate.x())
        {
545 546
            return 1;
        }
547
    }
548
    return 0;
549 550
}

551

552 553
void MapWidget::captureGeometryClick(Geometry* geom, QPoint point)
{
554 555
    Q_UNUSED(geom);
    Q_UNUSED(point);
pixhawk's avatar
pixhawk committed
556

557
    mc->setMouseMode(qmapcontrol::MapControl::None);
558 559
}

560 561
void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate)
{
562
    waypointIsDrag = true;
pixhawk's avatar
pixhawk committed
563

564
    // Refresh the screen
565
    mc->updateRequest(geom->boundingBox().toRect());
pixhawk's avatar
pixhawk committed
566

567
    int temp = 0;
568 569 570 571 572

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

573
    qmapcontrol::Point* point2Find;
574
    point2Find = wps.at(geom->name().toInt());
pixhawk's avatar
pixhawk committed
575

576 577 578
    if (point2Find)
    {
        point2Find->setCoordinate(coordinate);
pixhawk's avatar
pixhawk committed
579

580 581 582 583
        point2Find = dynamic_cast <qmapcontrol::Point*> (geom);
        if (point2Find)
        {
            point2Find->setCoordinate(coordinate);
pixhawk's avatar
pixhawk committed
584

585 586 587 588 589 590 591 592 593 594 595 596 597 598 599
            if (wpIndexOk)
            {
                mc->updateRequest(point2Find->boundingBox().toRect());
                if (mav)
                {
                        QVector<Waypoint*> wps = mav->getWaypointManager()->getWaypointList();

                    if (wps.size() > index)
                    {
                        wps.at(index)->setX(coordinate.x());
                        wps.at(index)->setY(coordinate.y());
                        mav->getWaypointManager()->notifyOfChange(wps.at(index));
                    }
                }
            }
600 601 602 603 604 605
            // qDebug() << geom->name();
            temp = geom->get_myIndex();
            //qDebug() << temp;
            emit sendGeometryEndDrag(coordinate,temp);
        }
    }
pixhawk's avatar
pixhawk committed
606

607 608
}

609
void MapWidget::captureGeometryEndDrag(Geometry* geom, QPointF coordinate)
610
{
611 612 613
    Q_UNUSED(geom);
    Q_UNUSED(coordinate);
    // TODO: Investigate why when creating the waypoint path this slot is being called
pixhawk's avatar
pixhawk committed
614

615 616 617 618 619 620
    // Only change the mouse mode back to panning when not creating a WP path
    if (!createPath->isChecked())
    {
        waypointIsDrag = false;
        mc->setMouseMode(qmapcontrol::MapControl::Panning);
    }
pixhawk's avatar
pixhawk committed
621

622 623
}

pixhawk's avatar
pixhawk committed
624 625 626 627
MapWidget::~MapWidget()
{
    delete m_ui;
}
lm's avatar
lm committed
628 629 630 631 632 633
/**
 *
 * @param uas the UAS/MAV to monitor/display with the HUD
 */
void MapWidget::addUAS(UASInterface* uas)
{
634
    connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659
    //connect(uas->getWaypointManager(), SIGNAL(waypointListChanged()), this, SLOT(redoWaypoints()));
}

void MapWidget::updateWaypointList(int uas)
{
    // Get already existing waypoints
    UASInterface* uasInstance = UASManager::instance()->getUASForId(uas);
    if (uasInstance)
    {
        QVector<Waypoint*> wpList = uasInstance->getWaypointManager()->getWaypointList();

        // Clear if necessary
        if (wpList.count() == 0)
        {
            clearWaypoints(uas);
            return;
        }

        // Load all existing waypoints into map view
        foreach (Waypoint* wp, wpList)
        {
            updateWaypoint(mav->getUASID(), wp);
        }

        // Delete now unused wps
660
        if (wps.count() > wpList.count())
661
        {
pixhawk's avatar
pixhawk committed
662
            mc->layer("Waypoints")->removeGeometry(waypointPath);
663
            for (int i = wpList.count(); i < wps.count(); ++i)
664 665 666
            {
                QRect updateRect = wps.at(i)->boundingBox().toRect();
                wps.removeAt(i);
pixhawk's avatar
pixhawk committed
667
                mc->layer("Waypoints")->removeGeometry(wpIcons.at(i));
668
                waypointPath->points().removeAt(i);
pixhawk's avatar
pixhawk committed
669 670
                //Point* linesegment = waypointPath->points().at(mav->getWaypointManager()->getWaypointList().indexOf(wp));

671 672
                mc->updateRequest(updateRect);
            }
pixhawk's avatar
pixhawk committed
673
            mc->layer("Waypoints")->addGeometry(waypointPath);
674
        }
pixhawk's avatar
pixhawk committed
675 676 677

        // Clear and rebuild linestring

678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695
    }
}

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
    clearWaypoints();
    // Re-add the updated waypoints

    //    }

    updateWaypointList(uas);
lm's avatar
lm committed
696 697
}

698 699
void MapWidget::activeUASSet(UASInterface* uas)
{
700 701 702 703 704 705 706 707 708 709 710 711 712
    // Disconnect old MAV
    if (mav)
    {
        // clear path create on the map
        disconnect(mav->getWaypointManager(), SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int)));
        // add Waypoint widget in the WaypointList widget when mouse clicked
        disconnect(this, SIGNAL(waypointCreated(Waypoint*)), mav->getWaypointManager(), SLOT(addWaypoint(Waypoint*)));


        // it notifies that a waypoint global goes to do create and a map graphic too
        //connect(waypointsDockWidget->widget(), SIGNAL(createWaypointAtMap(QPointF)), mapWidget, SLOT(createWaypointGraphAtMap(QPointF)));
    }

713 714 715
    if (uas)
    {
        mav = uas;
716
        QColor color = mav->getColor().lighter(100);
717 718
        color.setAlphaF(0.6);
        QPen* pen = new QPen(color);
719 720 721 722 723 724 725 726 727 728 729 730
        pen->setWidth(2.0);
        mavPens.insert(mav->getUASID(), pen);
        // FIXME Remove after refactoring
        waypointPath->setPen(pen);

        // Delete all waypoints and start fresh
        redoWaypoints();

        // clear path create on the map
        connect(mav->getWaypointManager(), SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int)));
        // add Waypoint widget in the WaypointList widget when mouse clicked
        connect(this, SIGNAL(waypointCreated(Waypoint*)), mav->getWaypointManager(), SLOT(addWaypoint(Waypoint*)));
731 732 733
    }
}

lm's avatar
lm committed
734 735 736 737 738 739 740 741 742
/**
 * 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
743 744 745
void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec)
{
    Q_UNUSED(usec);
746
    Q_UNUSED(alt); // FIXME Use altitude
747 748 749 750 751 752 753 754

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

755 756 757 758
    qmapcontrol::Point* p;
    QPointF coordinate;
    coordinate.setX(lat);
    coordinate.setY(lon);
759 760 761 762 763 764 765

    if (!uasIcons.contains(uas->getUASID()))
    {
        // Get the UAS color
        QColor uasColor = uas->getColor();

        // Icon
766 767 768 769
        //QPen* pointpen = new QPen(uasColor);
        qDebug() << "2D MAP: ADDING" << uas->getUASName() << __FILE__ << __LINE__;
        //p = new MAV2DIcon(lat, lon, 20, uas->getUASName(), qmapcontrol::Point::Middle, mavPens.value(uas->getUASID()));
        p = new Waypoint2DIcon(lat, lon, 20, QString("%1").arg(uas->getUASID()), qmapcontrol::Point::Middle);
770
        uasIcons.insert(uas->getUASID(), p);
771
        mc->layer("Waypoints")->addGeometry(p);
772 773 774 775 776

        // Line
        // A QPen also can use transparency

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

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

        // Add the LineString to the layer
786
        mc->layer("Waypoints")->addGeometry(ls);
787 788 789
    }
    else
    {
790 791 792 793
//        p = dynamic_cast<MAV2DIcon*>(uasIcons.value(uas->getUASID()));
//        if (p)
//        {
         p = uasIcons.value(uas->getUASID());
794
            p->setCoordinate(QPointF(lat, lon));
795 796
            //p->setYaw(uas->getYaw());
//        }
797
        // Extend trail
798
        uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(coordinate.x(), coordinate.y()));
799 800
    }

801
mc->updateRequest(p->boundingBox().toRect());
802

803 804 805 806
    //mc->updateRequestNew();//(uasTrails.value(uas->getUASID())->boundingBox().toRect());


    // Limit the position update rate
pixhawk's avatar
pixhawk committed
807
    quint64 currTime = MG::TIME::getGroundTimeNow();
808
    if (currTime - lastUpdate > 120)
pixhawk's avatar
pixhawk committed
809 810
    {
        lastUpdate = currTime;
811 812
        // Sets the view to the interesting area
        if (followgps->isChecked())
813
        {
814
            updatePosition(0, lat, lon);
815 816 817
        }
        else
        {
818 819
            // Refresh the screen
            //mc->updateRequestNew();
820
        }
pixhawk's avatar
pixhawk committed
821 822
    }
}
pixhawk's avatar
pixhawk committed
823

lm's avatar
lm committed
824 825 826
/**
 * Center the view on this position
 */
lm's avatar
lm committed
827
void MapWidget::updatePosition(float time, double lat, double lon)
pixhawk's avatar
pixhawk committed
828
{
lm's avatar
lm committed
829
    Q_UNUSED(time);
830
    //gpsposition->setText(QString::number(time) + " / " + QString::number(lat) + " / " + QString::number(lon));
831
    if (followgps->isChecked() && isVisible())
pixhawk's avatar
pixhawk committed
832
    {
lm's avatar
lm committed
833
        mc->setView(QPointF(lat, lon));
pixhawk's avatar
pixhawk committed
834 835 836 837 838 839 840 841 842 843 844 845 846 847
    }
}

void MapWidget::wheelEvent(QWheelEvent *event)
{
    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));
pixhawk's avatar
pixhawk committed
848

849
    // visual field of camera
850
    updateCameraPosition(20*newZoom,0,"no");
pixhawk's avatar
pixhawk committed
851

pixhawk's avatar
pixhawk committed
852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875 876 877 878 879
}

void MapWidget::keyPressEvent(QKeyEvent *event)
{
    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);
    }
}

880
void MapWidget::resizeEvent(QResizeEvent* event )
pixhawk's avatar
pixhawk committed
881
{
882 883
    Q_UNUSED(event);
    mc->resize(this->size());
pixhawk's avatar
pixhawk committed
884 885
}

886 887 888 889 890 891 892 893 894 895
void MapWidget::showEvent(QShowEvent* event)
{
    Q_UNUSED(event);
}

void MapWidget::hideEvent(QHideEvent* event)
{
    Q_UNUSED(event);
}

pixhawk's avatar
pixhawk committed
896 897 898 899 900 901 902 903 904 905 906 907

void MapWidget::changeEvent(QEvent *e)
{
    QWidget::changeEvent(e);
    switch (e->type()) {
    case QEvent::LanguageChange:
        m_ui->retranslateUi(this);
        break;
    default:
        break;
    }
}
908

909
void MapWidget::clearWaypoints(int uas)
910 911
{
    // Clear the previous WP track
pixhawk's avatar
pixhawk committed
912

913
    //mc->layer("Waypoints")->clearGeometries();
914
    wps.clear();
915
    waypointPath->points().clear();
916 917
    //delete waypointPath;
    //waypointPath = new
918
    mc->layer("Waypoints")->addGeometry(waypointPath);
919
    //wpIndex.clear();
920
    mc->updateRequestNew();//(waypointPath->boundingBox().toRect());
pixhawk's avatar
pixhawk committed
921

922 923 924 925
    if(createPath->isChecked())
    {
        createPath->click();
    }
926 927

    qDebug() << "CLEARING WAYPOINTS";
928
}
pixhawk's avatar
pixhawk committed
929

930
void MapWidget::clearPath(int uas)
931 932 933 934 935 936 937 938 939 940
{
    mc->layer("Tracking")->clearGeometries();
    foreach (qmapcontrol::LineString* ls, uasTrails)
    {
        QPen* linepen = ls->pen();
        delete ls;
        qmapcontrol::LineString* lsNew = new qmapcontrol::LineString(QList<qmapcontrol::Point*>(), "", linepen);
        mc->layer("Tracking")->addGeometry(lsNew);
    }
    // FIXME update this with update request only for bounding box of trails
941
    mc->updateRequestNew();//(QRect(0, 0, width(), height()));
942
}
943

944 945
void MapWidget::updateCameraPosition(double radio, double bearing, QString dir)
{
946
    // FIXME Mariano
947 948
    //camPoints.clear();
    QPointF currentPos = mc->currentCoordinate();
949
    //    QPointF actualPos = getPointxBearing_Range(currentPos.y(),currentPos.x(),bearing,distance);
pixhawk's avatar
pixhawk committed
950

951 952
    //    qmapcontrol::Point* tempPoint1 = new qmapcontrol::Point(currentPos.x(), currentPos.y(),"inicial",qmapcontrol::Point::Middle);
    //    qmapcontrol::Point* tempPoint2 = new qmapcontrol::Point(actualPos.x(), actualPos.y(),"final",qmapcontrol::Point::Middle);
pixhawk's avatar
pixhawk committed
953

954 955
    //    camPoints.append(tempPoint1);
    //    camPoints.append(tempPoint2);
pixhawk's avatar
pixhawk committed
956

957
    //    camLine->setPoints(camPoints);
pixhawk's avatar
pixhawk committed
958

959
    QPen* camBorderPen = new QPen(QColor(255,0,0));
960
    camBorderPen->setWidth(2);
pixhawk's avatar
pixhawk committed
961

962
    //radio = mc->currentZoom()
pixhawk's avatar
pixhawk committed
963

964 965 966 967
    if(drawCamBorder)
    {
        //clear camera borders
        mc->layer("Camera")->clearGeometries();
pixhawk's avatar
pixhawk committed
968

969 970
        //create a camera borders
        qmapcontrol::CirclePoint* camBorder = new qmapcontrol::CirclePoint(currentPos.x(), currentPos.y(), radio, "camBorder", qmapcontrol::Point::Middle, camBorderPen);
pixhawk's avatar
pixhawk committed
971

972
        //camBorder->setCoordinate(currentPos);
pixhawk's avatar
pixhawk committed
973

974
        mc->layer("Camera")->addGeometry(camBorder);
975
        // mc->layer("Camera")->addGeometry(camLine);
976
        mc->updateRequestNew();
pixhawk's avatar
pixhawk committed
977

978
    }
979 980 981 982 983
    else
    {
        //clear camera borders
        mc->layer("Camera")->clearGeometries();
        mc->updateRequestNew();
pixhawk's avatar
pixhawk committed
984

985
    }
pixhawk's avatar
pixhawk committed
986 987


988 989 990 991 992 993
}

void MapWidget::drawBorderCamAtMap(bool status)
{
    drawCamBorder = status;
    updateCameraPosition(20,0,"no");
pixhawk's avatar
pixhawk committed
994

995 996 997 998 999
}

QPointF MapWidget::getPointxBearing_Range(double lat1, double lon1, double bearing, double distance)
{
    QPointF temp;
pixhawk's avatar
pixhawk committed
1000

1001
    double rad = M_PI/180;
pixhawk's avatar
pixhawk committed
1002

1003 1004 1005
    bearing = bearing*rad;
    temp.setX((lon1 + ((distance/60) * (sin(bearing)))));
    temp.setY((lat1 + ((distance/60) * (cos(bearing)))));
pixhawk's avatar
pixhawk committed
1006

1007 1008 1009
    return temp;
}