Newer
Older
/*=====================================================================
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>
* @author Petri Tanskanen <mavteam@student.ethz.ch>
*
*/
#include "WaypointList.h"
#include "ui_WaypointList.h"
#include <UASInterface.h>
#include <UASManager.h>
#include <QDebug>
#include <QFileDialog>
tecnosapiens
committed
#include <QMessageBox>
#include <QMouseEvent>
WaypointList::WaypointList(QWidget *parent, UASInterface* uas) :
QWidget(parent),
uas(NULL),
mavX(0.0),
mavY(0.0),
mavZ(0.0),
mavYaw(0.0),
m_ui(new Ui::WaypointList)
//EDIT TAB
editableListLayout = new QVBoxLayout(m_ui->editableListWidget);
editableListLayout->setSpacing(0);
editableListLayout->setMargin(0);
editableListLayout->setAlignment(Qt::AlignTop);
m_ui->editableListWidget->setLayout(editableListLayout);
// 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(addEditable()));
// ADD WAYPOINT AT CURRENT POSITION
connect(m_ui->positionAddButton, SIGNAL(clicked()), this, SLOT(addCurrentPositionWaypoint()));
// SEND WAYPOINTS
connect(m_ui->transmitButton, SIGNAL(clicked()), this, SLOT(transmit()));
connect(m_ui->readButton, SIGNAL(clicked()), this, SLOT(read()));
// 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*)));
//VIEW TAB
viewOnlyListLayout = new QVBoxLayout(m_ui->viewOnlyListWidget);
viewOnlyListLayout->setSpacing(0);
viewOnlyListLayout->setMargin(0);
viewOnlyListLayout->setAlignment(Qt::AlignTop);
m_ui->viewOnlyListWidget->setLayout(viewOnlyListLayout);
// REFRESH VIEW TAB
connect(m_ui->refreshButton, SIGNAL(clicked()), this, SLOT(refresh()));
if (uas)
{
setUAS(uas);
}
// STATUS LABEL
updateStatusLabel("");
Mariano Lizarraga
committed
this->setVisible(false);
centerMapCoordinate.setX(0.0);
centerMapCoordinate.setY(0.0);
}
WaypointList::~WaypointList()
{
delete m_ui;
}
void WaypointList::updatePosition(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;
}
connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updatePosition(UASInterface*,double,double,double,quint64)));
connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
connect(uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)), this, SLOT(updateStatusLabel(const QString &)));
pixhawk
committed
connect(uas->getWaypointManager(), SIGNAL(waypointEditableListChanged(void)), this, SLOT(waypointEditableListChanged(void)));
connect(uas->getWaypointManager(), SIGNAL(waypointEditableChanged(int,Waypoint*)), this, SLOT(updateWaypointEditable(int,Waypoint*)));
connect(uas->getWaypointManager(), SIGNAL(waypointViewOnlyListChanged(void)), this, SLOT(waypointViewOnlyListChanged(void)));
connect(uas->getWaypointManager(), SIGNAL(waypointViewOnlyChanged(int,Waypoint*)), this, SLOT(updateWaypointViewOnly(int,Waypoint*)));
connect(uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointViewOnlyChanged(quint16)));
//connect(uas->getWaypointManager(),SIGNAL(loadWPFile()),this,SLOT(setIsLoadFileWP()));
//connect(uas->getWaypointManager(),SIGNAL(readGlobalWPFromUAS(bool)),this,SLOT(setIsReadGlobalWP(bool)));
QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)"));
QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)"));
}
uas->getWaypointManager()->readWaypoints(true);
}
}
void WaypointList::refresh()
{
if (uas)
{
uas->getWaypointManager()->readWaypoints(false);
const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointEditableList();
Waypoint *wp;
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->getParam1(), last->getParam2(), last->getParam3(), last->getParam4(),
last->getAutoContinue(), false, last->getFrame(), last->getAction());
uas->getWaypointManager()->addWaypointEditable(wp);
}
else
{
// Create first waypoint at current MAV position
addCurrentPositionWaypoint();
}
}
}
/*
void WaypointList::addViewOnly()
{
if (uas)
{
const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointViewOnlyList();
// Create waypoint with last frame
Waypoint *last = waypoints.at(waypoints.size()-1);
wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getParam1(), last->getParam2(), last->getParam3(), last->getParam4(),
last->getAutoContinue(), false, last->getFrame(), last->getAction());
pixhawk
committed
uas->getWaypointManager()->addWaypointEditable(wp);
}
else
{
// Create first waypoint at current MAV position
addCurrentPositionWaypoint();
Mariano Lizarraga
committed
}
*/
void WaypointList::addCurrentPositionWaypoint()
const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointEditableList();
Waypoint *wp;
Waypoint *last = 0;
if (waypoints.size() > 0)
last = waypoints.at(waypoints.size()-1);
}
if (uas->globalPositionKnown())
{
float acceptanceRadiusGlobal = 10.0f;
float holdTime = 0.0f;
float yawGlobal = 0.0f;
if (last)
tecnosapiens
committed
{
acceptanceRadiusGlobal = last->getAcceptanceRadius();
holdTime = last->getHoldTime();
yawGlobal = last->getYaw();
tecnosapiens
committed
}
// Create global frame waypoint per default
wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0, acceptanceRadiusGlobal, holdTime, yawGlobal, true, true, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT);
pixhawk
committed
uas->getWaypointManager()->addWaypointEditable(wp);
updateStatusLabel(tr("Added GLOBAL, ALTITUDE OVER GROUND waypoint"));
}
else if (uas->localPositionKnown())
{
float acceptanceRadiusLocal = 0.2f;
float holdTime = 0.5f;
if (last)
tecnosapiens
committed
{
acceptanceRadiusLocal = last->getAcceptanceRadius();
holdTime = last->getHoldTime();
tecnosapiens
committed
}
// Create local frame waypoint as second option
wp = new Waypoint(0, uas->getLocalX(), uas->getLocalY(), uas->getLocalZ(), uas->getYaw(), acceptanceRadiusLocal, holdTime, 0.0, true, true, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT);
pixhawk
committed
uas->getWaypointManager()->addWaypointEditable(wp);
}
else
{
// Do nothing
updateStatusLabel(tr("Not adding waypoint, no position known of MAV known yet."));
void WaypointList::updateStatusLabel(const QString &string)
{
// Status label in write widget
// Status label in read only widget
m_ui->viewStatusLabel->setText(string);
// Request UASWaypointManager to send the SET_CURRENT message to UAV
void WaypointList::changeCurrentWaypoint(quint16 seq)
// Request UASWaypointManager to set the new "current" and make sure all other waypoints are not "current"
void WaypointList::currentWaypointEditableChanged(quint16 seq)
uas->getWaypointManager()->setCurrentEditable(seq);
/*
const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointEditableList();
if (seq < waypoints.size())
{
for(int i = 0; i < waypoints.size(); i++)
{
WaypointEditableView* widget = wpEditableViews.find(waypoints[i]).value();
if (waypoints[i]->getId() == seq)
{
widget->setCurrent(true);
widget->setCurrent(false);
}
}
}
// Update waypointViews to correctly indicate the new current waypoint
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
void WaypointList::currentWaypointViewOnlyChanged(quint16 seq)
{
if (uas)
{
const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointViewOnlyList();
if (seq < waypoints.size())
{
for(int i = 0; i < waypoints.size(); i++)
{
WaypointViewOnlyView* widget = wpViewOnlyViews.find(waypoints[i]).value();
if (waypoints[i]->getId() == seq)
{
widget->setCurrent(true);
}
else
{
widget->setCurrent(false);
}
}
}
}
}
void WaypointList::updateWaypointEditable(int uas, Waypoint* wp)
wpv->updateValues();
}
void WaypointList::updateWaypointViewOnly(int uas, Waypoint* wp)
{
Q_UNUSED(uas);
WaypointViewOnlyView *wpv = wpViewOnlyViews.value(wp);
wpv->updateValues();
}
void WaypointList::waypointViewOnlyListChanged()
{
if (uas) {
// Prevent updates to prevent visual flicker
this->setUpdatesEnabled(false);
const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointViewOnlyList();
if (!wpViewOnlyViews.empty()) {
QMapIterator<Waypoint*,WaypointViewOnlyView*> viewIt(wpViewOnlyViews);
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()) {
WaypointViewOnlyView* widget = wpViewOnlyViews.find(cur).value();
widget->hide();
viewOnlyListLayout->removeWidget(widget);
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
wpViewOnlyViews.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 (!wpViewOnlyViews.contains(wp)) {
WaypointViewOnlyView* wpview = new WaypointViewOnlyView(wp, this);
wpViewOnlyViews.insert(wp, wpview);
connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16)));
viewOnlyListLayout->insertWidget(i, wpview);
}
WaypointViewOnlyView *wpv = wpViewOnlyViews.value(wp);
//check if ordering has changed
if(viewOnlyListLayout->itemAt(i)->widget() != wpv) {
viewOnlyListLayout->removeWidget(wpv);
viewOnlyListLayout->insertWidget(i, wpv);
}
wpv->updateValues(); // update the values of the ui elements in the view
}
this->setUpdatesEnabled(true);
loadFileGlobalWP = false;
}
}
pixhawk
committed
void WaypointList::waypointEditableListChanged()
// Prevent updates to prevent visual flicker
this->setUpdatesEnabled(false);
const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointEditableList();
if (!wpEditableViews.empty()) {
QMapIterator<Waypoint*,WaypointEditableView*> viewIt(wpEditableViews);
viewIt.toFront();
viewIt.next();
Waypoint *cur = viewIt.key();
int i;
for (i = 0; i < waypoints.size(); i++) {
if (waypoints[i] == cur) {
break;
WaypointEditableView* widget = wpEditableViews.find(cur).value();
widget->hide();
editableListLayout->removeWidget(widget);
wpEditableViews.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];
pixhawk
committed
WaypointEditableView* wpview = new WaypointEditableView(wp, this);
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(currentWaypointEditableChanged(quint16)));
}
//check if ordering has changed
if(editableListLayout->itemAt(i)->widget() != wpv) {
editableListLayout->removeWidget(wpv);
editableListLayout->insertWidget(i, wpv);
}
wpv->updateValues(); // update the values of the ui elements in the view
}
this->setUpdatesEnabled(true);
}
pixhawk
committed
//void WaypointList::waypointEditableListChanged()
//{
// if (uas)
// {
// // Prevent updates to prevent visual flicker
// this->setUpdatesEnabled(false);
// // Get all waypoints
// const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointEditableList();
//// // Store the current state, then check which widgets to update
//// // and which ones to delete
pixhawk
committed
//// WaypointEditableView* wpview;
pixhawk
committed
//// wpview = new WaypointEditableView(wp, this);
//// 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)));
//// }
//// else
//// {
//// // Update existing waypoints
//// }
//// // Mark as updated by removing from old list
//// oldWaypoints.removeAt(oldWaypoints.indexOf(wp));
//// wpview->updateValues(); // update the values of the ui elements in the view
//// }
//// // The old list now contains all wps to be deleted
//// foreach (Waypoint* wp, oldWaypoints)
//// {
//// // Delete waypoint view and entry in list
// QMapIterator<Waypoint*,WaypointEditableView*> viewIt(wpEditableViews);
// 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())
// {
// WaypointEditableView* widget = wpEditableViews.find(cur).value();
// // then add/update the views for each waypoint in the list
// for(int i = 0; i < waypoints.size(); i++)
// {
// Waypoint *wp = waypoints[i];
pixhawk
committed
// WaypointEditableView* wpview = new WaypointEditableView(wp, this);
// 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)));
// }
// wpv->updateValues(); // update the values of the ui elements in the view
// this->setUpdatesEnabled(true);
// }
//// loadFileGlobalWP = false;
//}
void WaypointList::moveUp(Waypoint* wp)
const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointEditableList();
//get the current position of wp in the local storage
int i;
for (i = 0; i < waypoints.size(); i++) {
if (waypoints[i] == wp)
break;
// if wp was found and its not the first entry, move it
if (i < waypoints.size() && i > 0) {
void WaypointList::moveDown(Waypoint* wp)
const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointEditableList();
//get the current position of wp in the local storage
int i;
for (i = 0; i < waypoints.size(); i++) {
if (waypoints[i] == wp)
break;
// if wp was found and its not the last entry, move it
}
}
}
void WaypointList::removeWaypoint(Waypoint* wp)
{
}
}
void WaypointList::changeEvent(QEvent *e)
{
switch (e->type()) {
case QEvent::LanguageChange:
m_ui->retranslateUi(this);
break;
default:
break;
}
}
void WaypointList::on_clearWPListButton_clicked()
{
if (uas) {
emit clearPathclicked();
const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointEditableList();
while(!waypoints.isEmpty()) { //for(int i = 0; i <= waypoints.size(); i++)
WaypointEditableView* widget = wpEditableViews.find(waypoints[0]).value();
// if(isGlobalWP)
// {
// emit clearPathclicked();
// }
}
///** @brief The MapWidget informs that a waypoint global was changed on the map */
tecnosapiens
committed
//void WaypointList::waypointGlobalChanged(QPointF coordinate, int indexWP)
//{
// if (uas)
// {
// const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointEditableList();
// if (waypoints.size() > 0)
// {
// Waypoint *temp = waypoints.at(indexWP);
tecnosapiens
committed
// temp->setX(coordinate.x());
// temp->setY(coordinate.y());
tecnosapiens
committed
// //WaypointGlobalView* widget = wpGlobalViews.find(waypoints[indexWP]).value();
// //widget->updateValues();
// }
// }
tecnosapiens
committed
tecnosapiens
committed
tecnosapiens
committed
///** @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);
//}
void WaypointList::clearWPWidget()
tecnosapiens
committed
{
const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointEditableList();
while(!waypoints.isEmpty()) { //for(int i = 0; i <= waypoints.size(); i++)
WaypointEditableView* widget = wpEditableViews.find(waypoints[0]).value();
tecnosapiens
committed
}
//void WaypointList::setIsLoadFileWP()
//{
// loadFileGlobalWP = true;
//}