WaypointList.cc 18.8 KB
Newer Older
pixhawk's avatar
pixhawk committed
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
/*=====================================================================

PIXHAWK Micro Air Vehicle Flying Robotics Toolkit

(c) 2009, 2010 PIXHAWK PROJECT  <http://pixhawk.ethz.ch>

This file is part of the PIXHAWK project

    PIXHAWK 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.

    PIXHAWK 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 PIXHAWK. If not, see <http://www.gnu.org/licenses/>.

======================================================================*/

/**
 * @file
 *   @brief Waypoint list widget
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *   @author Benjamin Knecht <mavteam@student.ethz.ch>
30
 *   @author Petri Tanskanen <mavteam@student.ethz.ch>
pixhawk's avatar
pixhawk committed
31
32
33
34
35
36
37
38
39
 *
 */

#include "WaypointList.h"
#include "ui_WaypointList.h"
#include <UASInterface.h>
#include <UASManager.h>
#include <QDebug>
#include <QFileDialog>
40
#include "WaypointGlobalView.h"
41
#include <QMessageBox>
42
#include <QMouseEvent>
pixhawk's avatar
pixhawk committed
43
44
45
46

WaypointList::WaypointList(QWidget *parent, UASInterface* uas) :
        QWidget(parent),
        uas(NULL),
47
48
49
50
        mavX(0.0),
        mavY(0.0),
        mavZ(0.0),
        mavYaw(0.0),
pixhawk's avatar
pixhawk committed
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
        m_ui(new Ui::WaypointList)
{
    m_ui->setupUi(this);

    listLayout = new QVBoxLayout(m_ui->listWidget);
    listLayout->setSpacing(6);
    listLayout->setMargin(0);
    listLayout->setAlignment(Qt::AlignTop);
    m_ui->listWidget->setLayout(listLayout);

    this->uas = NULL;

    // ADD WAYPOINT
    // Connect add action, set right button icon and connect action to this class
    connect(m_ui->addButton, SIGNAL(clicked()), m_ui->actionAddWaypoint, SIGNAL(triggered()));
    connect(m_ui->actionAddWaypoint, SIGNAL(triggered()), this, SLOT(add()));

68
69
70
    // ADD WAYPOINT AT CURRENT POSITION
    connect(m_ui->positionAddButton, SIGNAL(clicked()), this, SLOT(addCurrentPositonWaypoint()));

pixhawk's avatar
pixhawk committed
71
72
73
    // SEND WAYPOINTS
    connect(m_ui->transmitButton, SIGNAL(clicked()), this, SLOT(transmit()));

74
    // REQUEST WAYPOINTS
75
    connect(m_ui->readButton, SIGNAL(clicked()), this, SLOT(read()));
76

pixhawk's avatar
pixhawk committed
77
78
79
80
81
82
    // SAVE/LOAD WAYPOINTS
    connect(m_ui->saveButton, SIGNAL(clicked()), this, SLOT(saveWaypoints()));
    connect(m_ui->loadButton, SIGNAL(clicked()), this, SLOT(loadWaypoints()));

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

83
84


pixhawk's avatar
pixhawk committed
85
86
    // SET UAS AFTER ALL SIGNALS/SLOTS ARE CONNECTED
    setUAS(uas);
87
88
89

    // STATUS LABEL
    updateStatusLabel("");
90
91

    this->setVisible(false);
92
    isGlobalWP = false;
93
    isLocalWP = false;
94
95
96
    centerMapCoordinate.setX(0.0);
    centerMapCoordinate.setY(0.0);

pixhawk's avatar
pixhawk committed
97
98
99
100
101
102
103
}

WaypointList::~WaypointList()
{
    delete m_ui;
}

104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
void WaypointList::updateLocalPosition(UASInterface* uas, double x, double y, double z, quint64 usec)
{
    Q_UNUSED(uas);
    Q_UNUSED(usec);
    mavX = x;
    mavY = y;
    mavZ = z;
}

void WaypointList::updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 usec)
{
    Q_UNUSED(uas);
    Q_UNUSED(usec);
    Q_UNUSED(roll);
    Q_UNUSED(pitch);
    mavYaw = yaw;
}

pixhawk's avatar
pixhawk committed
122
123
124
125
126
void WaypointList::setUAS(UASInterface* uas)
{
    if (this->uas == NULL && uas != NULL)
    {
        this->uas = uas;
pixhawk's avatar
pixhawk committed
127

128
129
        connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)),  this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
        connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)),       this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
pixhawk's avatar
pixhawk committed
130

131
132
133
        connect(&uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)),        this, SLOT(updateStatusLabel(const QString &)));
        connect(&uas->getWaypointManager(), SIGNAL(waypointListChanged(void)),                  this, SLOT(waypointListChanged(void)));
        connect(&uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)),            this, SLOT(currentWaypointChanged(quint16)));
pixhawk's avatar
pixhawk committed
134
135
136
    }
}

pixhawk's avatar
pixhawk committed
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
166
167
168
169
170
171
172
173
174
void WaypointList::saveWaypoints()
{
    if (uas)
    {
        QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)"));
        uas->getWaypointManager().localSaveWaypoints(fileName);
    }
}

void WaypointList::loadWaypoints()
{
    if (uas)
    {
        QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)"));
        uas->getWaypointManager().localLoadWaypoints(fileName);
    }
}

void WaypointList::transmit()
{
    if (uas)
    {
        uas->getWaypointManager().writeWaypoints();
    }
}

void WaypointList::read()
{
    if (uas)
    {
        uas->getWaypointManager().readWaypoints();
    }
}

void WaypointList::add()
{
    if (uas)
    {
175
176
        if(isGlobalWP)
        {
177
178
179
180
            const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
            if (waypoints.size() > 0)
            {
                Waypoint *last = waypoints.at(waypoints.size()-1);
181
                Waypoint *wp = new Waypoint(0, centerMapCoordinate.x(), centerMapCoordinate.y(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime());
182
183
184
185
                uas->getWaypointManager().localAddWaypoint(wp);
            }
            else
            {
186
                Waypoint *wp = new Waypoint(0, centerMapCoordinate.x(), centerMapCoordinate.y(), -0.8, 0.0, true, true, 0.15, 2000);
187
188
189
                uas->getWaypointManager().localAddWaypoint(wp);
            }

190
191
192
193
194
195
196
197
198
199
200
201
202
203
             emit createWaypointAtMap(centerMapCoordinate);
        }
        else
        {

            const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
            if (waypoints.size() > 0)
            {
                Waypoint *last = waypoints.at(waypoints.size()-1);
                Waypoint *wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime());
                uas->getWaypointManager().localAddWaypoint(wp);
            }
            else
            {
204
205


206
207
208
                    isLocalWP = true;
                    Waypoint *wp = new Waypoint(0, 1.1, 1.1, -0.8, 0.0, true, true, 0.15, 2000);
                    uas->getWaypointManager().localAddWaypoint(wp);
209
210


211
212
            }
        }
pixhawk's avatar
pixhawk committed
213
214
215
216
    }
}

void WaypointList::addCurrentPositonWaypoint()
pixhawk's avatar
pixhawk committed
217
{
pixhawk's avatar
pixhawk committed
218
    if (uas)
pixhawk's avatar
pixhawk committed
219
    {
220
221
        // For Global Waypoints
        if(isGlobalWP)
pixhawk's avatar
pixhawk committed
222
        {
223
            isLocalWP = false;
pixhawk's avatar
pixhawk committed
224
225
226
        }
        else
        {
227
228
229
230
231
232
233
234
235
236
237
238
239
240
            const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
            if (waypoints.size() > 0)
            {
                Waypoint *last = waypoints.at(waypoints.size()-1);
                Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime());
                uas->getWaypointManager().localAddWaypoint(wp);
            }
            else
            {
                Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., true, true, 0.15, 2000);
                uas->getWaypointManager().localAddWaypoint(wp);
            }

             isLocalWP = true;
pixhawk's avatar
pixhawk committed
241
        }
pixhawk's avatar
pixhawk committed
242
243
244
    }
}

pixhawk's avatar
pixhawk committed
245
246
247
248
249
void WaypointList::updateStatusLabel(const QString &string)
{
    m_ui->statusLabel->setText(string);
}

250
void WaypointList::changeCurrentWaypoint(quint16 seq)
pixhawk's avatar
pixhawk committed
251
{
pixhawk's avatar
pixhawk committed
252
    if (uas)
253
    {
254
        uas->getWaypointManager().setCurrentWaypoint(seq);
255
    }
pixhawk's avatar
pixhawk committed
256
257
}

258
void WaypointList::currentWaypointChanged(quint16 seq)
259
{
pixhawk's avatar
pixhawk committed
260
    if (uas)
261
    {
262
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282

        if (seq < waypoints.size())
        {
            for(int i = 0; i < waypoints.size(); i++)
            {
                WaypointView* widget = wpViews.find(waypoints[i]).value();

                if (waypoints[i]->getId() == seq)
                {
                    widget->setCurrent(true);
                }
                else
                {
                    widget->setCurrent(false);
                }
            }
        }
    }
}

283
void WaypointList::waypointListChanged()
pixhawk's avatar
pixhawk committed
284
{
pixhawk's avatar
pixhawk committed
285
    if (uas)
pixhawk's avatar
pixhawk committed
286
    {
287
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
pixhawk's avatar
pixhawk committed
288

289
290
            // For Global Waypoints
            if(isGlobalWP)
pixhawk's avatar
pixhawk committed
291
            {
292
                isLocalWP = false;
293
294
                // first remove all views of non existing waypoints
                if (!wpGlobalViews.empty())
295
                {
296
297
298
                    QMapIterator<Waypoint*,WaypointGlobalView*> viewIt(wpGlobalViews);
                    viewIt.toFront();
                    while(viewIt.hasNext())
299
                    {
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
                        viewIt.next();
                        Waypoint *cur = viewIt.key();
                        int i;
                        for (i = 0; i < waypoints.size(); i++)
                        {
                            if (waypoints[i] == cur)
                            {
                                break;
                            }
                        }
                        if (i == waypoints.size())
                        {
                            WaypointGlobalView* widget = wpGlobalViews.find(cur).value();
                            widget->hide();
                            listLayout->removeWidget(widget);
                            wpGlobalViews.remove(cur);
                        }
317
                    }
318
                }
319
320
321

                // then add/update the views for each waypoint in the list
                for(int i = 0; i < waypoints.size(); i++)
322
                {
323
324
325
326
327
                    Waypoint *wp = waypoints[i];
                    if (!wpGlobalViews.contains(wp))
                    {
                        WaypointGlobalView* wpview = new WaypointGlobalView(wp, this);
                        wpGlobalViews.insert(wp, wpview);
328
329
                        connect(wpview, SIGNAL(removeWaypoint(Waypoint*)),      this, SLOT(removeWaypoint(Waypoint*)));
                        connect(wpview, SIGNAL(changePositionWP(Waypoint*)), this, SLOT(changeWPPositionBySpinBox(Waypoint*)));
330
331
//                        connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)),    this, SLOT(moveDown(Waypoint*)));
//                        connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)),      this, SLOT(moveUp(Waypoint*)));
332
                       // connect(wpview, SIGNAL(changePositionWP(Waypoint*)), this, SLOT(waypointGlobalPositionChanged(Waypoint*)));
333
334
335
336
337
338
//                        connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16)));
//                        connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16)));
                    }
                WaypointGlobalView *wpgv = wpGlobalViews.value(wp);
                wpgv->updateValues();
                listLayout->addWidget(wpgv);
pixhawk's avatar
pixhawk committed
339
            }
340

341
342
            }
            else
343
            {
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
                // for local Waypoints
                // first remove all views of non existing waypoints
                if (!wpViews.empty())
                {
                    QMapIterator<Waypoint*,WaypointView*> viewIt(wpViews);
                    viewIt.toFront();
                    while(viewIt.hasNext())
                    {
                        viewIt.next();
                        Waypoint *cur = viewIt.key();
                        int i;
                        for (i = 0; i < waypoints.size(); i++)
                        {
                            if (waypoints[i] == cur)
                            {
                                break;
                            }
                        }
                        if (i == waypoints.size())
                        {
                            WaypointView* widget = wpViews.find(cur).value();
                            widget->hide();
                            listLayout->removeWidget(widget);
                            wpViews.remove(cur);
                        }
                    }
                }

                // then add/update the views for each waypoint in the list
                for(int i = 0; i < waypoints.size(); i++)
                {
                    Waypoint *wp = waypoints[i];
                    if (!wpViews.contains(wp))
                    {
                        WaypointView* wpview = new WaypointView(wp, this);
                        wpViews.insert(wp, wpview);
                        connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)),    this, SLOT(moveDown(Waypoint*)));
                        connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)),      this, SLOT(moveUp(Waypoint*)));
                        connect(wpview, SIGNAL(removeWaypoint(Waypoint*)),      this, SLOT(removeWaypoint(Waypoint*)));
                        connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16)));
                        connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16)));
                    }
                WaypointView *wpv = wpViews.value(wp);
                wpv->updateValues();    // update the values of the ui elements in the view
                listLayout->addWidget(wpv);

            }
391
392
            }

393
394


pixhawk's avatar
pixhawk committed
395
        }
396
397


pixhawk's avatar
pixhawk committed
398
399
}

400
void WaypointList::moveUp(Waypoint* wp)
pixhawk's avatar
pixhawk committed
401
{
402
    if (uas)
pixhawk's avatar
pixhawk committed
403
    {
404
405
406
407
408
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();

        //get the current position of wp in the local storage
        int i;
        for (i = 0; i < waypoints.size(); i++)
409
        {
410
411
            if (waypoints[i] == wp)
                break;
412
413
        }

414
415
        // if wp was found and its not the first entry, move it
        if (i < waypoints.size() && i > 0)
pixhawk's avatar
pixhawk committed
416
        {
417
            uas->getWaypointManager().localMoveWaypoint(i, i-1);
pixhawk's avatar
pixhawk committed
418
419
420
421
        }
    }
}

422
void WaypointList::moveDown(Waypoint* wp)
pixhawk's avatar
pixhawk committed
423
{
424
    if (uas)
pixhawk's avatar
pixhawk committed
425
    {
426
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
427

428
429
430
        //get the current position of wp in the local storage
        int i;
        for (i = 0; i < waypoints.size(); i++)
pixhawk's avatar
pixhawk committed
431
        {
432
433
            if (waypoints[i] == wp)
                break;
pixhawk's avatar
pixhawk committed
434
435
        }

436
437
        // if wp was found and its not the last entry, move it
        if (i < waypoints.size()-1)
pixhawk's avatar
pixhawk committed
438
        {
439
            uas->getWaypointManager().localMoveWaypoint(i, i+1);
pixhawk's avatar
pixhawk committed
440
441
442
443
444
445
        }
    }
}

void WaypointList::removeWaypoint(Waypoint* wp)
{
446
    if (uas)
pixhawk's avatar
pixhawk committed
447
    {
448
        uas->getWaypointManager().localRemoveWaypoint(wp->getId());
pixhawk's avatar
pixhawk committed
449
450
451
452
453
454
455
456
457
458
459
460
461
462
    }
}

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

463
464
465
466


void WaypointList::on_clearWPListButton_clicked()
{
467

468
469
    if (uas)
    {
470
471
        if(isGlobalWP)
        {
472
473
            emit clearPathclicked();

474
475
476
477
478
479
480
481
482
            const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
              while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++)
              {

                  WaypointGlobalView* widget = wpGlobalViews.find(waypoints[0]).value();

                  widget->remove();
              }

483
484


485
486
487
488
489
490
491
492
493
494
              isGlobalWP = false;



        }
        else
        {


          const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
495
496
            while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++)
            {
497

498
                WaypointView* widget = wpViews.find(waypoints[0]).value();
499

500
                widget->remove();
501

502

503
            }
504

505
        }
506
507

    }
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
}

/** @brief Add a waypoint by mouse click over the map */
void WaypointList::addWaypointMouse(QPointF coordinate)
{
    if (uas)
    {
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
        if (waypoints.size() > 0)
        {
            Waypoint *last = waypoints.at(waypoints.size()-1);
            Waypoint *wp = new Waypoint(0, coordinate.x(), coordinate.y(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime());
            uas->getWaypointManager().localAddWaypoint(wp);
        }
        else
        {
            Waypoint *wp = new Waypoint(0, coordinate.x(), coordinate.y(), -0.8, 0.0, true, true, 0.15, 2000);
            uas->getWaypointManager().localAddWaypoint(wp);
        }
527
528


529
530
    }

531
}
532

533
 /** @brief Notifies the user that a global waypoint will be created */
534
void WaypointList::setIsWPGlobal(bool value, QPointF centerCoordinate)
535
{
536
    centerMapCoordinate = centerCoordinate;
537
538
539
540
541
542
543


    if(isLocalWP)
    {
        if(wpViews.size()!= 0)
        {

544
545
            int ret = QMessageBox::warning(this, tr("QGroundControl"),
                                    tr("There are Local Waypoints created.\n"
546
547
548
549
550
551
                                       "Do you want to clear them?"),
                                    QMessageBox::Ok | QMessageBox::Cancel);

            if(ret)
            {
                clearLocalWPWidget();
552
553


554
            }
555

556
        }
557
558
        isLocalWP = !value;
        isGlobalWP = value;
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590


    }
    else
    {
        isGlobalWP = value;
    }

}

/** @brief The MapWidget informs that a waypoint global was changed on the map */

void WaypointList::waypointGlobalChanged(QPointF coordinate, int indexWP)
{
    if (uas)
    {
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
        if (waypoints.size() > 0)
        {
            Waypoint *temp = waypoints.at(indexWP);

            temp->setX(coordinate.x());
            temp->setY(coordinate.y());

            WaypointGlobalView* widget = wpGlobalViews.find(waypoints[indexWP]).value();
            widget->updateValues();
        }
    }


}

591
592
593
594
595
596
597
598
599
600
601
602
603
///** @brief The MapWidget informs that a waypoint global was changed on the map */

//void WaypointList::waypointGlobalPositionChanged(Waypoint* wp)
//{
//    QPointF coordinate;
//    coordinate.setX(wp->getX());
//    coordinate.setY(wp->getY());

//   emit ChangeWaypointGlobalPosition(wp->getId(), coordinate);


//}

604
605
606
607
608
609
610
611
612
613
614
615
616
617
void WaypointList::clearLocalWPWidget()
{
  if (uas)
  {
    const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
      while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++)
      {

          WaypointView* widget = wpViews.find(waypoints[0]).value();

          widget->remove();

      }
  }
618
}
619
620
621
622
623
624

void WaypointList::changeWPPositionBySpinBox(Waypoint *wp)
{

    emit changePositionWPGlobalBySpinBox(wp->getId(), wp->getY(), wp->getX());
}