WaypointList.cc 15.7 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 <QMessageBox>
41
#include <QMouseEvent>
pixhawk's avatar
pixhawk committed
42 43 44 45

WaypointList::WaypointList(QWidget *parent, UASInterface* uas) :
        QWidget(parent),
        uas(NULL),
46 47 48 49
        mavX(0.0),
        mavY(0.0),
        mavZ(0.0),
        mavYaw(0.0),
pixhawk's avatar
pixhawk committed
50 51 52 53 54 55 56 57 58 59 60 61 62 63 64
        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);

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

65 66 67
    // ADD WAYPOINT AT CURRENT POSITION
    connect(m_ui->positionAddButton, SIGNAL(clicked()), this, SLOT(addCurrentPositonWaypoint()));

pixhawk's avatar
pixhawk committed
68 69 70
    // SEND WAYPOINTS
    connect(m_ui->transmitButton, SIGNAL(clicked()), this, SLOT(transmit()));

71
    // REQUEST WAYPOINTS
72
    connect(m_ui->readButton, SIGNAL(clicked()), this, SLOT(read()));
73

pixhawk's avatar
pixhawk committed
74 75 76 77
    // SAVE/LOAD WAYPOINTS
    connect(m_ui->saveButton, SIGNAL(clicked()), this, SLOT(saveWaypoints()));
    connect(m_ui->loadButton, SIGNAL(clicked()), this, SLOT(loadWaypoints()));

78
    //connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*)));
pixhawk's avatar
pixhawk committed
79

80 81


pixhawk's avatar
pixhawk committed
82 83
    // SET UAS AFTER ALL SIGNALS/SLOTS ARE CONNECTED
    setUAS(uas);
84 85 86

    // STATUS LABEL
    updateStatusLabel("");
87 88

    this->setVisible(false);
89
    loadFileGlobalWP = false;
90
    readGlobalWP = false;
91 92 93
    centerMapCoordinate.setX(0.0);
    centerMapCoordinate.setY(0.0);

pixhawk's avatar
pixhawk committed
94 95 96 97 98 99 100
}

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

101
void WaypointList::updatePosition(UASInterface* uas, double x, double y, double z, quint64 usec)
102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118
{
    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
119 120 121 122 123
void WaypointList::setUAS(UASInterface* uas)
{
    if (this->uas == NULL && uas != NULL)
    {
        this->uas = uas;
pixhawk's avatar
pixhawk committed
124

125
        connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)),  this, SLOT(updatePosition(UASInterface*,double,double,double,quint64)));
126
        connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)),       this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
pixhawk's avatar
pixhawk committed
127

pixhawk's avatar
pixhawk committed
128 129
        connect(uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)),        this, SLOT(updateStatusLabel(const QString &)));
        connect(uas->getWaypointManager(), SIGNAL(waypointListChanged(void)),                  this, SLOT(waypointListChanged(void)));
130
        connect(uas->getWaypointManager(), SIGNAL(waypointChanged(int,Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*)));
pixhawk's avatar
pixhawk committed
131
        connect(uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)),            this, SLOT(currentWaypointChanged(quint16)));
132 133
        //connect(uas->getWaypointManager(),SIGNAL(loadWPFile()),this,SLOT(setIsLoadFileWP()));
        //connect(uas->getWaypointManager(),SIGNAL(readGlobalWPFromUAS(bool)),this,SLOT(setIsReadGlobalWP(bool)));
134

pixhawk's avatar
pixhawk committed
135 136 137
    }
}

pixhawk's avatar
pixhawk committed
138 139 140 141 142
void WaypointList::saveWaypoints()
{
    if (uas)
    {
        QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)"));
pixhawk's avatar
pixhawk committed
143
        uas->getWaypointManager()->saveWaypoints(fileName);
pixhawk's avatar
pixhawk committed
144 145 146 147 148 149 150 151
    }
}

void WaypointList::loadWaypoints()
{
    if (uas)
    {
        QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)"));
pixhawk's avatar
pixhawk committed
152
        uas->getWaypointManager()->loadWaypoints(fileName);
153
    }
pixhawk's avatar
pixhawk committed
154 155 156 157 158 159
}

void WaypointList::transmit()
{
    if (uas)
    {
pixhawk's avatar
pixhawk committed
160
        uas->getWaypointManager()->writeWaypoints();
pixhawk's avatar
pixhawk committed
161 162 163 164 165 166 167
    }
}

void WaypointList::read()
{
    if (uas)
    {
pixhawk's avatar
pixhawk committed
168
        uas->getWaypointManager()->readWaypoints();
pixhawk's avatar
pixhawk committed
169 170 171 172 173 174 175
    }
}

void WaypointList::add()
{
    if (uas)
    {
pixhawk's avatar
pixhawk committed
176 177
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
        Waypoint *wp;
178 179


pixhawk's avatar
pixhawk committed
180 181 182 183 184 185 186 187 188 189 190
        if (waypoints.size() > 0)
        {
            // Create waypoint with last frame
            Waypoint *last = waypoints.at(waypoints.size()-1);
            wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getAcceptanceRadius(),
                              last->getHoldTime(), last->getFrame(), last->getAction());
            uas->getWaypointManager()->addWaypoint(wp);
        }
        else
        {
            // Create global frame waypoint per default
lm's avatar
lm committed
191
            wp = new Waypoint(0, uas->getLongitude(), uas->getLatitude(), uas->getAltitude(), 0.0, true, true, 0.15, 0, MAV_FRAME_GLOBAL, MAV_CMD_NAV_WAYPOINT);
pixhawk's avatar
pixhawk committed
192
            uas->getWaypointManager()->addWaypoint(wp);
193
        }
pixhawk's avatar
pixhawk committed
194 195 196 197
    }
}

void WaypointList::addCurrentPositonWaypoint()
pixhawk's avatar
pixhawk committed
198
{
199
    /* TODO: implement with new waypoint structure
pixhawk's avatar
pixhawk committed
200
    if (uas)
pixhawk's avatar
pixhawk committed
201
    {
202
        // For Global Waypoints
203 204 205 206 207
        //if(isGlobalWP)
        //{
            //isLocalWP = false;
        //}
        //else
pixhawk's avatar
pixhawk committed
208
        {
pixhawk's avatar
pixhawk committed
209
            const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
210 211 212 213
            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());
pixhawk's avatar
pixhawk committed
214
                uas->getWaypointManager()->addWaypoint(wp);
215 216 217 218
            }
            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);
pixhawk's avatar
pixhawk committed
219
                uas->getWaypointManager()->addWaypoint(wp);
220 221
            }

222
             //isLocalWP = true;
pixhawk's avatar
pixhawk committed
223
        }
pixhawk's avatar
pixhawk committed
224
    }
225
    */
pixhawk's avatar
pixhawk committed
226 227
}

pixhawk's avatar
pixhawk committed
228 229 230 231 232
void WaypointList::updateStatusLabel(const QString &string)
{
    m_ui->statusLabel->setText(string);
}

233
void WaypointList::changeCurrentWaypoint(quint16 seq)
pixhawk's avatar
pixhawk committed
234
{
pixhawk's avatar
pixhawk committed
235
    if (uas)
236
    {
pixhawk's avatar
pixhawk committed
237
        uas->getWaypointManager()->setCurrentWaypoint(seq);
238
    }
pixhawk's avatar
pixhawk committed
239 240
}

241
void WaypointList::currentWaypointChanged(quint16 seq)
242
{
pixhawk's avatar
pixhawk committed
243
    if (uas)
244
    {
pixhawk's avatar
pixhawk committed
245
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265

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

266 267 268 269 270 271 272
void WaypointList::updateWaypoint(int uas, Waypoint* wp)
{
    Q_UNUSED(uas);
    WaypointView *wpv = wpViews.value(wp);
    wpv->updateValues();
}

273
void WaypointList::waypointListChanged()
pixhawk's avatar
pixhawk committed
274
{
pixhawk's avatar
pixhawk committed
275
    if (uas)
pixhawk's avatar
pixhawk committed
276
    {
pixhawk's avatar
pixhawk committed
277 278
        // Prevent updates to prevent visual flicker
        this->setUpdatesEnabled(false);
pixhawk's avatar
pixhawk committed
279
        // Get all waypoints
pixhawk's avatar
pixhawk committed
280 281
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();

pixhawk's avatar
pixhawk committed
282 283 284 285 286 287 288 289
        // Store the current state, then check which widgets to update
        // and which ones to delete
        QList<Waypoint*> oldWaypoints = wpViews.keys();

        foreach (Waypoint* wp, waypoints)
        {
            // Create any new waypoint
            if (!wpViews.contains(wp))
290
            {
pixhawk's avatar
pixhawk committed
291 292 293 294 295 296 297
                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)));
pixhawk's avatar
pixhawk committed
298
            }
pixhawk's avatar
pixhawk committed
299
            else
pixhawk's avatar
pixhawk committed
300
            {
pixhawk's avatar
pixhawk committed
301 302 303 304 305 306 307 308 309
                // Update existing waypoints

                // Mark as updated by removing from old list
                oldWaypoints.removeAt(oldWaypoints.indexOf(wp));
            }
            WaypointView *wpv = wpViews.value(wp);
            wpv->updateValues();    // update the values of the ui elements in the view
            listLayout->addWidget(wpv);
        }
310

pixhawk's avatar
pixhawk committed
311 312 313 314 315 316 317 318
        // The old list now contains all wps to be deleted
        foreach (Waypoint* wp, oldWaypoints)
        {
            // Delete waypoint view and entry in list
            WaypointView* wpv = wpViews.value(wp);
            if (wpv)
            {
                wpv->deleteLater();
319
            }
pixhawk's avatar
pixhawk committed
320
            wpViews.remove(wp);
pixhawk's avatar
pixhawk committed
321
        }
322

pixhawk's avatar
pixhawk committed
323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347
//        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);
//                }
//            }
//        }
348

pixhawk's avatar
pixhawk committed
349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370
//        // 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);

//        }
        this->setUpdatesEnabled(true);
    }
    loadFileGlobalWP = false;
pixhawk's avatar
pixhawk committed
371 372
}

373
void WaypointList::moveUp(Waypoint* wp)
pixhawk's avatar
pixhawk committed
374
{
375
    if (uas)
pixhawk's avatar
pixhawk committed
376
    {
pixhawk's avatar
pixhawk committed
377
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
378 379 380 381

        //get the current position of wp in the local storage
        int i;
        for (i = 0; i < waypoints.size(); i++)
382
        {
383 384
            if (waypoints[i] == wp)
                break;
385 386
        }

387 388
        // if wp was found and its not the first entry, move it
        if (i < waypoints.size() && i > 0)
pixhawk's avatar
pixhawk committed
389
        {
pixhawk's avatar
pixhawk committed
390
            uas->getWaypointManager()->moveWaypoint(i, i-1);
pixhawk's avatar
pixhawk committed
391 392 393 394
        }
    }
}

395
void WaypointList::moveDown(Waypoint* wp)
pixhawk's avatar
pixhawk committed
396
{
397
    if (uas)
pixhawk's avatar
pixhawk committed
398
    {
pixhawk's avatar
pixhawk committed
399
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
400

401 402 403
        //get the current position of wp in the local storage
        int i;
        for (i = 0; i < waypoints.size(); i++)
pixhawk's avatar
pixhawk committed
404
        {
405 406
            if (waypoints[i] == wp)
                break;
pixhawk's avatar
pixhawk committed
407 408
        }

409 410
        // if wp was found and its not the last entry, move it
        if (i < waypoints.size()-1)
pixhawk's avatar
pixhawk committed
411
        {
pixhawk's avatar
pixhawk committed
412
            uas->getWaypointManager()->moveWaypoint(i, i+1);
pixhawk's avatar
pixhawk committed
413 414 415 416 417 418
        }
    }
}

void WaypointList::removeWaypoint(Waypoint* wp)
{
419
    if (uas)
pixhawk's avatar
pixhawk committed
420
    {
pixhawk's avatar
pixhawk committed
421
        uas->getWaypointManager()->removeWaypoint(wp->getId());
pixhawk's avatar
pixhawk committed
422 423 424 425 426 427 428 429 430 431 432 433 434 435
    }
}

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

436 437 438 439


void WaypointList::on_clearWPListButton_clicked()
{
440

tecnosapiens's avatar
tecnosapiens committed
441

442 443
    if (uas)
    {
tecnosapiens's avatar
tecnosapiens committed
444
           emit clearPathclicked();
pixhawk's avatar
pixhawk committed
445
          const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
446 447 448 449
            while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++)
            {
                WaypointView* widget = wpViews.find(waypoints[0]).value();
                widget->remove();
450
        }
451
    }
tecnosapiens's avatar
tecnosapiens committed
452 453
    else
    {
454 455 456 457
//        if(isGlobalWP)
//        {
//           emit clearPathclicked();
//        }
tecnosapiens's avatar
tecnosapiens committed
458
    }
459 460
}

pixhawk's avatar
pixhawk committed
461
///** @brief The MapWidget informs that a waypoint global was changed on the map */
462

pixhawk's avatar
pixhawk committed
463 464 465 466 467 468 469 470
//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);
471

pixhawk's avatar
pixhawk committed
472 473
//            temp->setX(coordinate.x());
//            temp->setY(coordinate.y());
474

pixhawk's avatar
pixhawk committed
475 476 477 478
//            //WaypointGlobalView* widget = wpGlobalViews.find(waypoints[indexWP]).value();
//            //widget->updateValues();
//        }
//    }
479 480


pixhawk's avatar
pixhawk committed
481
//}
482

483 484 485 486 487 488 489 490 491 492 493 494 495
///** @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);


//}

496
void WaypointList::clearWPWidget()
497 498 499
{
  if (uas)
  {
pixhawk's avatar
pixhawk committed
500
    const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
501 502 503 504 505 506
      while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++)
      {
          WaypointView* widget = wpViews.find(waypoints[0]).value();
          widget->remove();
      }
  }
507
}
508

pixhawk's avatar
pixhawk committed
509 510 511 512
//void WaypointList::setIsLoadFileWP()
//{
//    loadFileGlobalWP = true;
//}
513

pixhawk's avatar
pixhawk committed
514 515 516 517 518 519
//void WaypointList::setIsReadGlobalWP(bool value)
//{
//    // FIXME James Check this
//    Q_UNUSED(value);
//    // readGlobalWP = value;
//}