mainguiwindow.cpp 42.1 KB
Newer Older
1
//    Copyright (C) 2018, ETH Zurich, D-ITET
flashingx's avatar
flashingx committed
2
3
4
5
//          Yvan Bosshard           byvan       @ee.ethz.ch
//          Michael Rogenmoser      michaero    @ee.ethz.ch
//          Tiago Salzmann          tiagos      @ee.ethz.ch
//    
6
//    Adapted from mainguiwindow of Angel Romero.
roangel's avatar
roangel committed
7
//
8
9
10
//    This file is part of D-FaLL-System.
//    
//    D-FaLL-System is free software: you can redistribute it and/or modify
roangel's avatar
roangel committed
11
12
13
//    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.
14
15
//    
//    D-FaLL-System is distributed in the hope that it will be useful,
roangel's avatar
roangel committed
16
17
18
//    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.
19
//    
roangel's avatar
roangel committed
20
//    You should have received a copy of the GNU General Public License
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
//    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
//    
//
//    ----------------------------------------------------------------------------------
//    DDDD        FFFFF        L     L           SSSS  Y   Y   SSSS  TTTTT  EEEEE  M   M
//    D   D       F      aaa   L     L          S       Y Y   S        T    E      MM MM
//    D   D  ---  FFFF  a   a  L     L     ---   SSS     Y     SSS     T    EEE    M M M
//    D   D       F     a  aa  L     L              S    Y        S    T    E      M   M
//    DDDD        F      aa a  LLLL  LLLL       SSSS     Y    SSSS     T    EEEEE  M   M
//
//
//    DESCRIPTION:
//    Teacher's GUI main window.
//
//    ----------------------------------------------------------------------------------
roangel's avatar
roangel committed
36
37


flashingx's avatar
flashingx committed
38
39
#include <regex>
#include <string>
40
41
42
43
44

#include <QObject>
#include <QDoubleSpinBox>
#include <QTextEdit>
#include <QString>
45
#include <QMetaType>
46
#include <QDir>
tiagos's avatar
tiagos committed
47
#include <QShortcut>
flashingx's avatar
flashingx committed
48
49
50
51
52
53
54

#include "mainguiwindow.h"
#include "ui_mainguiwindow.h"
#include "crazyFlyZoneTab.h"
#include "myGraphicsScene.h"
#include "myGraphicsView.h"
#include "channelLUT.h"
55

56
#ifdef CATKIN_MAKE
flashingx's avatar
flashingx committed
57
58
59
60
#include <ros/ros.h>
#include <ros/network.h>
#include <ros/package.h>

61
#include "d_fall_pps/UnlabeledMarker.h"
62
#include "d_fall_pps/CMRead.h"
63
#include "d_fall_pps/CrazyflieEntry.h"
64
65
66
#include "d_fall_pps/CMUpdate.h"
#include "d_fall_pps/CMCommand.h"
#include "CentralManagerService.h"
tiagos's avatar
tiagos committed
67

68
69
#endif

70
#define N_MAX_CRAZYFLIES           20 // protection number
71

72
// UWB definitions, have to match on nodes receiving these states
tiagos's avatar
tiagos committed
73
74
75
#define UWB_UPDATE_DISABLE      0
#define UWB_UPDATE_ENABLE       1
#define UWB_UPDATE_ANCHORS      5
tiagos's avatar
tiagos committed
76
77
78
#define UWB_CALIBRATE_ANCHORS   7

float offset[3] = {0};
79
std::vector<QCheckBox*> invertCheckBoxes;
tiagos's avatar
tiagos committed
80

roangel's avatar
roangel committed
81
#ifdef CATKIN_MAKE
82
using namespace d_fall_pps;
roangel's avatar
roangel committed
83
#endif
84

85
86
87
88
MainGUIWindow::MainGUIWindow(int argc, char **argv, QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainGUIWindow)
{
89
    #ifdef CATKIN_MAKE
90
    _rosNodeThread = new rosNodeThread(argc, argv, "my_GUI");
91
    #endif
92
93
94
    ui->setupUi(this);
    _init();
}
95

96

97
98
99
MainGUIWindow::~MainGUIWindow()
{
    delete ui;
tiagos's avatar
tiagos committed
100
    delete _rosNodeThread;
101
102
}

103
104
105
106
107
108
109
110
111
112
113
114
115
116
int MainGUIWindow::getTabIndexFromName(QString name)
{
    int found_name = -1;
    for(int i = 0; i < ui->tabWidget->count(); i++)
    {
        qDebug("name: %s", name.toStdString().c_str());
        qDebug("tabText: %s", ui->tabWidget->tabText(i).toStdString().c_str());
        if(name == ui->tabWidget->tabText(i))
        {
            found_name = i;
        }
    }
    return found_name;
}
117

roangel's avatar
roangel committed
118
void MainGUIWindow::doNumCrazyFlyZonesChanged(int n)
119
{
roangel's avatar
roangel committed
120
    // tabs number management, maybe do it in a different way so we dont have to remove and add everything?
121
    // first check if size of tabs is greater than size of vector or viceversa. Have we removed or added a zone?
122

123
    qDebug("tabWidgetCount : %d", ui->tabWidget->count());
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
    if(ui->tabWidget->count() > scene->crazyfly_zones.size())
    {
        // we removed one crazyfly_zone, n means index of the one we removed. Look for that index tab and remove it
        QString qstr = "CrazyFly ";
        qstr.append(QString::number(n+1));
        if(scene->crazyfly_zones.size() == 0)
        {
            ui->tabWidget->clear();
        }
        int found_index = getTabIndexFromName(qstr);
        if(found_index != -1)
        {
            ui->tabWidget->widget(found_index)->deleteLater();
            // ui->tabWidget->removeTab(found_index);
        }

        //  now unlink it from table also:
        #ifdef CATKIN_MAKE
        if(cf_linker->isCFZoneLinked(n))
        {
            cf_linker->unlink_cf_zone(n);
        }
        #endif
    }
    else if(ui->tabWidget->count() < scene->crazyfly_zones.size())
    {
        // we added one crazyfly_zone, n means index of the new one. New tab will be labeld index + 1
        QString qstr = "CrazyFly ";
        qstr.append(QString::number(n+1));
        crazyFlyZoneTab* widget = new crazyFlyZoneTab(n);
        ui->tabWidget->insertTab(n, widget, qstr);
        connect(widget, SIGNAL(centerButtonClickedSignal(int)), this, SLOT(centerViewIndex(int)));
    }
157

158
    updateComboBoxesCFZones();
159
160
161
162
}

void MainGUIWindow::_init()
{
163
164
165
166
167
168
169
170
    // initialize checkboxes, spinboxes,....
    ui->scaleSpinBox->setRange(0.1, 100);
    ui->scaleSpinBox->setSingleStep(0.1);
    ui->scaleSpinBox->setValue(1);

    ui->checkBox_vicon_crazyflies->setChecked(false);
    ui->scaleSpinBox->setEnabled(false);

171

172
    ui->graphicsView->setViewportUpdateMode(QGraphicsView::FullViewportUpdate);
173
174
    ui->graphicsView->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOn);
    ui->graphicsView->setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOn);
175

176
177
178
179
180
    // error messages
    // ui->err_message_cf->hide();
    // ui->err_message_cf_zone->hide();
    // ui->err_message_student_id->hide();

181
182
183
184
185
186
187
    ui->radioAddress_text->setReadOnly(true);

    QPalette *palette = new QPalette();
    palette->setColor(QPalette::Base,Qt::lightGray);
    palette->setColor(QPalette::Text,Qt::darkGray);
    ui->radioAddress_text->setPalette(*palette);

188
189
190
191
    ui->err_message_cf->setStyleSheet("QLabel { color : red; }");
    ui->err_message_cf_zone->setStyleSheet("QLabel { color : red; }");
    ui->err_message_student_id->setStyleSheet("QLabel { color : red; }");
    ui->err_message_radio_address->setStyleSheet("QLabel { color : red; }");
192

193
194
195
196
    ui->err_message_cf->clear();
    ui->err_message_cf_zone->clear();
    ui->err_message_student_id->clear();
    ui->err_message_radio_address->clear();
197

198
    // initialize table_links
199
    ui->table_links->setColumnCount(4);
200

201
202
203
204
205
    QFont fnt;
    fnt.setPointSize(7);
    ui->table_links->horizontalHeader()->setFont(fnt);

    ui->table_links->horizontalHeader()->setDefaultSectionSize(90);
206
    ui->table_links->verticalHeader()->setDefaultSectionSize(20);
207
208
209
210
211
212
213
214
215
216
217

    const int rowCount = ui->table_links->rowCount();
    const int columnCount = ui->table_links->columnCount();
    for(int i = 0; i < rowCount; ++i)
    {
    	for(int j = 0; j < columnCount; ++j)
        {
    		QTableWidgetItem* selectedItem = ui->table_links->item(i, j);
    		selectedItem->setFont(fnt);
    	}
    }
roangel's avatar
roangel committed
218
    ui->table_links->setSelectionBehavior(QAbstractItemView::SelectRows);
219
    QStringList horizontal_header;
220
    horizontal_header << "Student ID" << "CrazyFly" << "CrazyFly Zone" << "Radio Address";
221
    ui->table_links->setHorizontalHeaderLabels(horizontal_header);
222

223
    // scene
224
    scene = new myGraphicsScene(ui->frame_drawing);
225
    scene->setSceneRect(-100 * FROM_METERS_TO_UNITS, -100 * FROM_METERS_TO_UNITS, 200 * FROM_METERS_TO_UNITS, 200 * FROM_METERS_TO_UNITS);
226

227
    ui->graphicsView->setScene(scene);
228

229
    // after scene is created, create CFlinker
230
    #ifdef CATKIN_MAKE
231
    cf_linker = new CFLinker(ui, &crazyflies_vector, &scene->crazyfly_zones);
232
    #endif
233
    // connections
234
    QObject::connect(ui->tabWidget, SIGNAL(tabCloseRequested(int)), this, SLOT(doTabClosed(int)));
roangel's avatar
roangel committed
235
    QObject::connect(scene, SIGNAL(numCrazyFlyZonesChanged(int)), this, SLOT(doNumCrazyFlyZonesChanged(int)));
236
    QObject::connect(scene, SIGNAL(crazyFlyZoneSelected(int)), this, SLOT(setTabIndex(int)));
237
238
    QObject::connect(scene, SIGNAL(modeChanged(int)), this, SLOT(transitionToMode(int)));
    QObject::connect(scene, SIGNAL(numTablePiecesChanged(int)), this, SLOT(handleTablePiecesNumChanged(int)));
roangel's avatar
roangel committed
239
240

    ui->checkBox_vicon_highlight_markers->setEnabled(false);
241

roangel's avatar
roangel committed
242
    #ifdef CATKIN_MAKE
243
    _rosNodeThread->init();
244
    qRegisterMetaType<ptrToMessage>("ptrToMessage");
245
246
    qRegisterMetaType<std_msgs::Int32MultiArray>("std_msgs::Int32MultiArray");
    qRegisterMetaType<CrazyfliePositionData>("CrazyfliePositionData");
247
    QObject::connect(_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
248
249
    QObject::connect(_rosNodeThread, SIGNAL(newStudentIDs(const std_msgs::Int32MultiArray&)), this, SLOT(updateStudentIDs(const std_msgs::Int32MultiArray&)));
    QObject::connect(_rosNodeThread, SIGNAL(newPositionData(const CrazyfliePositionData&)), this, SLOT(updateCFPositions(const CrazyfliePositionData&)));
250
    QObject::connect(cf_linker, SIGNAL(updateComboBoxes()), this, SLOT(updateComboBoxes()));
251
252

    ros::NodeHandle nodeHandle("~");
253

254
    UWBServiceClient = nodeHandle.serviceClient<Anchors>("/UWBManagerService/UWBData", false);
255
    TeacherServiceClient = nodeHandle.serviceClient<CFIndex>("/TeacherService/Hangar", false);
256

257
    DBChangedPublisher = nodeHandle.advertise<std_msgs::Int32>("DBChanged", 1);
258
    emergencyStopPublisher = nodeHandle.advertise<std_msgs::Int32>("emergencyStop", 1);
259
260

    refreshStudents_publisher = nodeHandle.advertise<std_msgs::Int32>("refreshStudents", 1);
tiagos's avatar
tiagos committed
261
262
    UWBServiceClientUpdate_publisher = nodeHandle.advertise<std_msgs::Int32>("UWBUpdate", 1);
    UWBSettings_publisher = nodeHandle.advertise<std_msgs::Int32>("enableUWB", 1);
263
    QObject::connect(ui->checkBox_enable_UWB, SIGNAL(stateChanged(int)), this, SLOT(on_checkBox_enable_UWB_toggled(bool)));
tiagos's avatar
tiagos committed
264
265
266

    // add space bar shortcut to stop all motors
    ui->all_motors_off_button->setShortcut(tr("Space"));
tiagos's avatar
tiagos committed
267
    QShortcut* sk = new QShortcut(QKeySequence(tr("CTRL+C")), this, SLOT(close()));
tiagos's avatar
tiagos committed
268
269

    // set anchor table settings
tiagos's avatar
tiagos committed
270
    ui->table_anchorPos->verticalHeader()->setVisible(false);
tiagos's avatar
tiagos committed
271
272
273
274
275
276
277
278
279
280
    ui->table_anchorPos->setColumnCount(4);

    ui->table_anchorPos->horizontalHeader()->setFont(fnt);
    ui->table_anchorPos->verticalHeader()->setFont(fnt);
    ui->table_anchorPos->horizontalHeader()->setDefaultSectionSize(90);
    ui->table_anchorPos->verticalHeader()->setDefaultSectionSize(20);

    QStringList anchor_header;
    anchor_header << "Anchor id" << "x" << "y" << "z";
    ui->table_anchorPos->setHorizontalHeaderLabels(anchor_header);
281

282
283
284
285
286
287
288
    invertCheckBoxes.push_back(ui->invA1);
    invertCheckBoxes.push_back(ui->invA2);
    invertCheckBoxes.push_back(ui->invA3);
    invertCheckBoxes.push_back(ui->invA4);
    invertCheckBoxes.push_back(ui->invA5);
    invertCheckBoxes.push_back(ui->invA6);

289
290
    loadCFs(nodeHandle);

roangel's avatar
roangel committed
291
    #endif
292
}
293

294
295
296
void MainGUIWindow::doTabClosed(int tab_index)
{
    QString name = ui->tabWidget->tabText(tab_index);
roangel's avatar
roangel committed
297
    #ifdef CATKIN_MAKE
roangel's avatar
roangel committed
298
299
    int cf_zone_index = cf_linker->getCFZoneIndexFromName(name);
    scene->removeCrazyFlyZone(cf_zone_index);
roangel's avatar
roangel committed
300
    #endif
301
302
303
304
305
306
307
308
309
}

void MainGUIWindow::setTabIndex(int index)
{
    QString qstr = "CrazyFly ";
    qstr.append(QString::number(index + 1));
    ui->tabWidget->setCurrentIndex(getTabIndexFromName(qstr));
}

310
311
312
313
314
315
316
void MainGUIWindow::updateComboBoxes()
{
    updateComboBoxesCFs();
    updateComboBoxesCFZones();
}
void MainGUIWindow::updateComboBoxesCFs()
{
317
318
319
320
321
322
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
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
    ui->comboBoxCFs->clear();

    CFIndex cfi;

    if(!TeacherServiceClient.call(cfi))
    {
        ROS_ERROR("[Teacher GUI] could not load CF-Database!");
        return;
    }

    int size = crazyflies_vector.size();
    for(int i = 0; i < cfi.response.length; ++i)
    {
        if(!cf_linker->isCFLinked(cfi.response.data[i]))
        {
            QString qstr = QString::fromStdString(cfi.response.data[i]);
            ui->comboBoxCFs->addItem(qstr);
        }

        bool found = false;
        for(int j = 0; j < size; ++j)
        {
            if(crazyflies_vector[j]->getName() == cfi.response.data[i])
            {
                found = true;
                break;
            }
        }

        if(!found)
        {
            CrazyflieData* cfdata = new CrazyflieData();
            cfdata->crazyflieName = cfi.response.data[i];
            cfdata->x = i * 10;
            cfdata->y = 0;
            cfdata->z = 0;
            cfdata->roll = 0;
            cfdata->pitch = 0;
            cfdata->yaw = 0;
            //cfdata->acquiringTime = data.acquiringTime;
            cfdata->occluded = true;

            QString filename(":/images/center_rect.svg");
            crazyFly* tmp_p_crazyfly = new crazyFly(cfdata, filename);
            cf_positionsVector.push_back(tmp_p_crazyfly);
            scene->addItem(cf_positionsVector.back());
            cf_positionsVector.back()->setAddedToScene(true);
        }
    }


        
    /*#ifdef CATKIN_MAKE
370
371
372
373
374
375
376
377
378
    ui->comboBoxCFs->clear();
    for(int i = 0; i < crazyflies_vector.size(); i++)
    {
        if(!cf_linker->isCFLinked(crazyflies_vector[i]->getName()))
        {
            QString qstr = QString::fromStdString(crazyflies_vector[i]->getName());
            ui->comboBoxCFs->addItem(qstr);
        }
    }
379
    #endif*/
380
381
382
383
384
385
386
387
388
389
}

void MainGUIWindow::updateComboBoxesCFZones()
{
    ui->comboBoxCFZones->clear();
    #ifdef CATKIN_MAKE
    for(int i = 0; i < scene->crazyfly_zones.size(); i++)
    {
        if(!cf_linker->isCFZoneLinked(scene->crazyfly_zones[i]->getIndex()))
        {
390
            int cf_zone_index = scene->crazyfly_zones[i]->getIndex();
391
            QString qstr = "CrazyFlyZone ";
392
            qstr.append(QString::number(cf_zone_index + 1));
393
394
395
396
397
398
399
            ui->comboBoxCFZones->addItem(qstr);
        }
    }
    #endif
}


roangel's avatar
roangel committed
400
#ifdef CATKIN_MAKE
401
402
403
404
405
void MainGUIWindow::updateCFPositions(const CrazyfliePositionData& data)
{
    bool inScene = false;
    int index;

406
407
    //CrazyflieData cfdata = data;

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
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
    // Reconvert CFPD to CFD
    CrazyflieData cfdata;
    cfdata.crazyflieName = std::to_string(data.id);
    cfdata.x = data.x;
    cfdata.y = data.y;
    cfdata.z = data.z;
    cfdata.roll = data.roll;
    cfdata.pitch = data.pitch;
    cfdata.yaw = data.yaw;
    cfdata.acquiringTime = data.acquiringTime;
    cfdata.occluded = false;

    for(int i = 0; i < cf_positionsVector.size(); ++i)
    {
        if(cf_positionsVector[i]->getName() == cfdata.crazyflieName)
        {
            cf_positionsVector[i]->updateCF(&cfdata);
            inScene = true;
            index = i;
            break;
        }
    }

    if(!inScene)
    {
        QString filename(":/images/center_rect.svg");
        crazyFly* tmp_p_crazyfly = new crazyFly(&cfdata, filename);
        cf_positionsVector.push_back(tmp_p_crazyfly);
        index = cf_positionsVector.size() - 1;
    }

    if(ui->checkBox_vicon_crazyflies->checkState() == Qt::Checked)
    {
        if(!cf_positionsVector[index]->isAddedToScene())
        {
            scene->addItem(cf_positionsVector[index]);
            cf_positionsVector[index]->setAddedToScene(true);
        }
    }
}

void MainGUIWindow::updateStudentIDs(const std_msgs::Int32MultiArray& ids)
450
{
451
452
453
454
455
    ui->list_discovered_student_ids->clear();

    for(int i = 0; i < ids.data.size(); ++i)
        ui->list_discovered_student_ids->addItem(std::to_string(ids.data[i]).c_str());
}
456

457
458
void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to newViconData, from node
{
459
460
    // update Markers

461
    if(p_msg->markers.size() < markers_vector.size()) // some markers have dissapeared, received stuff is smaller than what we have
462
463
464
    {
        for(int i = p_msg->markers.size(); i < markers_vector.size(); i++)
        {
465
            scene->removeItem(markers_vector[i]); // remove objects from scene
466
            // ROS_INFO_STREAM("element index: " << i << " removed");
467
        }
468
        markers_vector.erase(markers_vector.begin() + p_msg->markers.size(), markers_vector.end()); //delete them
469
470
    }

471
    // ROS_INFO_STREAM("markers.size: " << p_msg->markers.size());
472

473
474
475
476
    for(int i = 0; i < p_msg->markers.size(); i++) // here, or new markers message is equal to current messages, or greater (some new markers)
    {
        if(i >= markers_vector.size()) //some new markers coming
        {
477
            // ROS_INFO_STREAM("element index: " << i << " added");
478
            Marker* tmp_p_marker = new Marker(&(p_msg->markers[i]));
479
480
            markers_vector.push_back(tmp_p_marker); // what happens with the new indexes? check if this is correct

481
            if(ui->checkBox_vicon_markers->checkState() == Qt::Checked) //only if markers checkbox info is checked..
482
483
484
485
486
487
488
            {
                scene->addItem(markers_vector[i]);
                if(ui->checkBox_vicon_highlight_markers->checkState() == Qt::Checked)
                {
                    markers_vector[i]->setHighlighted();
                }
            }
489
490
491
        }
        else
        {
492
            // ROS_INFO_STREAM("element index: " << i << " moved, already existed");
493
            markers_vector[i]->updateMarker(&(p_msg->markers[i]));
494
495
        }
    }
496
497

    // update Crazyflies
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
    // also: what happens if we dont go through one of the names? we need to remove that crazyfly
    int crazyfly_vector_size_before = crazyflies_vector.size(); //initial size of vector
    // in this loop, add new ones and update old ones
    for(int i = 0; i < p_msg->crazyflies.size(); i++)
    {
        bool name_found = false; // for each iteration, name_found starts in false
        int index_name_found;
        for(int j = 0; j < crazyfly_vector_size_before; j++)
        {
            if(crazyflies_vector[j]->getName() == p_msg->crazyflies[i].crazyflieName)
            {
                name_found = true; // name found. This can only happen once per i-iteration, names are unique
                index_name_found = j; // index in already existing vector, to update it later (really needed?)
            }
        }

        if(name_found)
        {
            crazyflies_vector[index_name_found]->updateCF(&(p_msg->crazyflies[i]));
        }
518
        else                    //name not found, newly arrived, add it to the vector
519
        {
roangel's avatar
roangel committed
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
            // now, if name follows our format, put the corresponding number. If not, put the unknown image
            std::string s = p_msg->crazyflies[i].crazyflieName;
            std::smatch m;
            std::regex e ("PPS_CF([0-9]{2})");

            QString filename(":/images/drone_fixed_");

            if(std::regex_search(s, m, e))
            {
                std::string found_string = m[1].str();
                filename.append(QString::fromStdString(found_string));
                filename.append(".svg");
            }
            else
            {
                filename.append("unk.svg");
            }

            crazyFly* tmp_p_crazyfly = new crazyFly(&(p_msg->crazyflies[i]), filename);
539
540
541
542
543
544
            crazyflies_vector.push_back(tmp_p_crazyfly);
        }

        if(ui->checkBox_vicon_crazyflies->checkState() == Qt::Checked)
        {
            for(int i = 0; i < crazyflies_vector.size(); i++) //check for occlussion
545
            {
546
547
                if(crazyflies_vector[i]->isOccluded())
                {
roangel's avatar
roangel committed
548
                    // ROS_INFO("===================OCCLUDED");
549
550
551
552
553
554
555
556
557
558
559
560
561
562
                    if(crazyflies_vector[i]->isAddedToScene())
                    {
                        scene->removeItem(crazyflies_vector[i]);
                        crazyflies_vector[i]->setAddedToScene(false);
                    }
                }
                else
                {
                    if(!crazyflies_vector[i]->isAddedToScene())
                    {
                        scene->addItem(crazyflies_vector[i]);
                        crazyflies_vector[i]->setAddedToScene(true);
                    }
                }
563
            }
564
565
        }
    }
566

567
    // in this loop, clean the ones that are not present anymore. UPDATE: this will apparently only happen when we tick and untick in Vicon
568
    int crazyfly_vector_size_after = crazyflies_vector.size();
569

570
    for(int j = crazyfly_vector_size_after - 1; j >= 0; j--)
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
    {
        bool name_found = false;
        for(int i = 0; i < p_msg->crazyflies.size(); i++)
        {
            if(crazyflies_vector[j]->getName() == p_msg->crazyflies[i].crazyflieName)
            {
                name_found = true;
            }
        }
        if(!name_found)
        {
            scene->removeItem(crazyflies_vector[j]);
            crazyflies_vector.erase(crazyflies_vector.begin() + j);
        }
    }
586
}
roangel's avatar
roangel committed
587
#endif
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
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
660
661
662


void MainGUIWindow::on_removeTable_clicked()
{
    if(scene->getMode() == myGraphicsScene::mode_table)
    {
        scene->removeTable();
    }
}

void MainGUIWindow::transitionToMode(int mode)
{
    switch(mode)
    {
        case myGraphicsScene::mode_table:
        {
            ui->removeTable->setDisabled(false);
            break;
        }
        case myGraphicsScene::mode_crazyfly_zones:
        {
            ui->removeTable->setDisabled(true);
            break;
        }
    }
}

void MainGUIWindow::on_radioButton_table_mode_toggled(bool checked)
{
    switch(scene->getMode())
    {
        case myGraphicsScene::mode_table:
        {
            // already in the mode we want, do nothing
            break;
        }
        case myGraphicsScene::mode_crazyfly_zones:
        {
            scene->setMode(myGraphicsScene::mode_table);
            break;
        }
        case myGraphicsScene::mode_locked:
        {
            scene->setMode(myGraphicsScene::mode_table);
            break;
        }
    }

}


void MainGUIWindow::on_radioButton_crazyfly_zones_mode_toggled(bool checked)
{
    switch(scene->getMode())
    {
        case myGraphicsScene::mode_table:
        {
            scene->setMode(myGraphicsScene::mode_crazyfly_zones);
            break;
        }
        case myGraphicsScene::mode_crazyfly_zones:
        {
            // already in the mode we want, do nothing
            break;
        }
        case myGraphicsScene::mode_locked:
        {
            scene->setMode(myGraphicsScene::mode_crazyfly_zones);
            break;
        }
    }
}

void MainGUIWindow::handleTablePiecesNumChanged(int newNum)
{
663

664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
}

void MainGUIWindow::on_radioButton_lock_mode_toggled(bool checked)
{
    switch(scene->getMode())
    {
        case myGraphicsScene::mode_table:
        {
            scene->setMode(myGraphicsScene::mode_locked);
            break;
        }
        case myGraphicsScene::mode_crazyfly_zones:
        {
            scene->setMode(myGraphicsScene::mode_locked);
            break;
        }
        case myGraphicsScene::mode_locked:
        {
            break;
        }
    }
}

void MainGUIWindow::on_checkBox_grid_toggled(bool checked)
{
    scene->setGrid(checked);
}

void MainGUIWindow::on_checkBox_table_toggled(bool checked)
{
    if(checked)
    {
        scene->showTable();
    }
    else
    {
        scene->hideTable();
    }
}

void MainGUIWindow::on_checkBox_crazyfly_zones_toggled(bool checked)
{
    if(checked)
    {
        scene->showCrazyFlyZones();
    }
    else
    {
        scene->hideCrazyFlyZones();
    }
}

void MainGUIWindow::on_tabWidget_currentChanged(int index)
{
718
719
720
721
722
723
    // // this index is tab index. Need to go to cf index
    // QString name = ui->tabWidget->tabText(index);
    // #ifdef CATKIN_MAKE
    // int cf_index = cf_linker->getCFZoneIndexFromName(name);
    // scene->setSelectedCrazyFlyZone(cf_index);
    // #endif
724
725
726
727
728
729
730
731
732
733
734
735
736
737
}

void MainGUIWindow::centerViewIndex(int index)
{
    ui->graphicsView->fitInView(scene->getRectFCrazyFlyZone(index), Qt::KeepAspectRatio);
    ui->graphicsView->scale(0.95, 0.95); // A bit back zoom, so we can see everything better
}


void MainGUIWindow::on_pushButton_fitAll_clicked()
{
    ui->graphicsView->fitInView(scene->itemsBoundingRect(), Qt::KeepAspectRatio);
    ui->graphicsView->scale(0.95, 0.95); // A bit back zoom, so we can see everything better
}
738
739
740
741
742

void MainGUIWindow::on_checkBox_vicon_markers_toggled(bool checked)
{
    if(checked)
    {
743
        #ifdef CATKIN_MAKE
744
745
746
747
        for(int i = 0; i < markers_vector.size(); i++)
        {
            scene->addItem(markers_vector[i]);
        }
748
        #endif
749
        ui->checkBox_vicon_highlight_markers->setCheckable(true);
roangel's avatar
roangel committed
750
        ui->checkBox_vicon_highlight_markers->setEnabled(true);
751
752
753
    }
    else
    {
754
        #ifdef CATKIN_MAKE
755
756
757
758
        for(int i = 0; i < markers_vector.size(); i++)
        {
            scene->removeItem(markers_vector[i]);
        }
759
        #endif
760
761
        ui->checkBox_vicon_highlight_markers->setChecked(false);
        ui->checkBox_vicon_highlight_markers->setCheckable(false);
roangel's avatar
roangel committed
762
        ui->checkBox_vicon_highlight_markers->setEnabled(false);
763
764
765
766
767
768
769
    }
}

void MainGUIWindow::on_checkBox_vicon_highlight_markers_toggled(bool checked)
{
    if(checked)
    {
770
        #ifdef CATKIN_MAKE
771
772
773
774
        for(int i = 0; i < markers_vector.size(); i++)
        {
            markers_vector[i]->setHighlighted();
        }
775
        #endif
776
777
778
    }
    else
    {
779
        #ifdef CATKIN_MAKE
780
781
782
783
        for(int i = 0; i < markers_vector.size(); i++)
        {
            markers_vector[i]->clearHighlighted();
        }
784
785
786
787
788
789
790
791
792
793
794
        #endif
    }
}

void MainGUIWindow::on_checkBox_vicon_crazyflies_toggled(bool checked)
{
    if(checked)
    {
        #ifdef CATKIN_MAKE
        for(int i = 0; i < crazyflies_vector.size(); i++)
        {
795
796
797
798
799
            if(!crazyflies_vector[i]->isAddedToScene())
            {
                scene->addItem(crazyflies_vector[i]);
                crazyflies_vector[i]->setAddedToScene(true);
            }
800
801
802
803
804
805
806
807
808
        }
        #endif
        ui->scaleSpinBox->setEnabled(true);
    }
    else
    {
        #ifdef CATKIN_MAKE
        for(int i = 0; i < crazyflies_vector.size(); i++)
        {
809
810
811
812
813
            if(crazyflies_vector[i]->isAddedToScene())
            {
                scene->removeItem(crazyflies_vector[i]);
                crazyflies_vector[i]->setAddedToScene(false);
            }
814
815
816
817
818
819
820
821
822
823
824
825
        }
        #endif
        ui->scaleSpinBox->setEnabled(false);
    }
}

void MainGUIWindow::on_scaleSpinBox_valueChanged(double arg1)
{
    #ifdef CATKIN_MAKE
    for(int i = 0; i < crazyflies_vector.size(); i++)
    {
        crazyflies_vector[i]->setScaleCFs(arg1);
826
    }
827
    #endif
828
}
829
830
831

void MainGUIWindow::on_refresh_cfs_button_clicked()
{
832
    updateComboBoxesCFs();
833
834
835
836
837
838
}

void MainGUIWindow::on_refresh_student_ids_button_clicked()
{
    #ifdef CATKIN_MAKE

839
840
841
842
    // Publish the button being pressed
    std_msgs::Int32 msg;
    msg.data = 1;
    this->refreshStudents_publisher.publish(msg);
843
844
845

    #endif
}
846
847
848
849

void MainGUIWindow::on_link_button_clicked()
{
    #ifdef CATKIN_MAKE
850
851
852
853
854

    bool error = false;
    if(ui->comboBoxCFs->count() == 0)
    {
        // plot error message
855
        ui->err_message_cf->setText("CF box is empty");
856
857
        error = true;
    }
858
859
860
861
    else
    {
        ui->err_message_cf->clear();
    }
862

863
864
865
    if(ui->comboBoxCFZones->count() == 0)
    {
        // plot error message
866
        ui->err_message_cf_zone->setText("CFZone box is empty");
867
868
        error = true;
    }
869
870
871
872
    else
    {
        ui->err_message_cf_zone->clear();
    }
873

874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
    if(cf_linker->isRadioAddressLinked(ui->radioAddress_text->text().toStdString()))
    {
        ui->err_message_radio_address->setText("Already in use");
        error = true;
    }
    else if(ui->radioAddress_text->text().toStdString() == "")
    {
        ui->err_message_radio_address->setText("Field is empty");
        error = true;
    }
    else
    {
        ui->err_message_radio_address->clear();
    }

889
    if(cf_linker->isStudentIDLinked(ui->spinBox_student_ids->value()))
890
    {
891
892
893
894
895
896
897
        // plot error message
        ui->err_message_student_id->setText("This StudentID has already been linked");
        error = true;
    }
    else
    {
        ui->err_message_student_id->clear();
898
899
900
901
    }

    if(!error)
    {
902
        cf_linker->link(ui->spinBox_student_ids->value(), cf_linker->getCFZoneIndexFromName(ui->comboBoxCFZones->currentText()), ui->comboBoxCFs->currentText().toStdString(), ui->radioAddress_text->text().toStdString());
903
    }
904
905
    #endif
}
906
907
908
909

void MainGUIWindow::on_unlink_button_clicked()
{
    #ifdef CATKIN_MAKE
910
    cf_linker->unlink_selection();
911
912
    #endif
}
913
914
915
916

void MainGUIWindow::on_save_in_DB_button_clicked()
{
    // we need to update and then save?
917
    CrazyflieDB tmp_db;
918
    for(int i = 0; i < cf_linker->links.size(); i++)
919
    {
920
921
        CrazyflieEntry tmp_entry;
        tmp_entry.crazyflieContext.crazyflieName = cf_linker->links[i].cf_name;
922
        tmp_entry.crazyflieContext.crazyflieAddress = cf_linker->links[i].radio_address;
923
924
        tmp_entry.crazyflieContext.localArea.crazyfly_zone_index = cf_linker->links[i].cf_zone_index;
        tmp_entry.studentID = cf_linker->links[i].student_id;
925

926
927
928
929
        for(int j = 0; j < scene->crazyfly_zones.size(); j++)
        {
            if(cf_linker->links[i].cf_zone_index == scene->crazyfly_zones[j]->getIndex())
            {
930
931
932
933
934
935
936
937
938
939
                double x_min = scene->crazyfly_zones[j]->sceneBoundingRect().bottomLeft().x();
                double y_min = - scene->crazyfly_zones[j]->sceneBoundingRect().bottomLeft().y();

                double x_max = scene->crazyfly_zones[j]->sceneBoundingRect().topRight().x();
                double y_max = -scene->crazyfly_zones[j]->sceneBoundingRect().topRight().y();

                tmp_entry.crazyflieContext.localArea.xmin = x_min * FROM_UNITS_TO_METERS;
                tmp_entry.crazyflieContext.localArea.xmax = x_max * FROM_UNITS_TO_METERS;
                tmp_entry.crazyflieContext.localArea.ymin = y_min * FROM_UNITS_TO_METERS;
                tmp_entry.crazyflieContext.localArea.ymax = y_max * FROM_UNITS_TO_METERS;
940
941
942

                tmp_entry.crazyflieContext.localArea.zmin = -0.2;
                tmp_entry.crazyflieContext.localArea.zmax = 2.0;
943
944
945
            }
        }
        tmp_db.crazyflieEntries.push_back(tmp_entry);
946
    }
947
948
949
950

    m_data_base = tmp_db;

    ROS_INFO_STREAM("database:\n" << m_data_base);
951
952
953
954

    // save the database in the file

    fill_database_file();
955
956
957
958
959

    // Now also publish a ROS message stating that we changed the DB, so the nodes can update it
    std_msgs::Int32 msg;
    msg.data = 1;
    this->DBChangedPublisher.publish(msg);
960
961
}

962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
void MainGUIWindow::clear_database_file()
{
    CrazyflieDB tmp_db;
    if(read_database_from_file(tmp_db) == 0)
    {
        for(int i = 0; i < tmp_db.crazyflieEntries.size(); i++)
        {
            CMUpdate updateCall;
            updateCall.request.mode = ENTRY_REMOVE;
            updateCall.request.crazyflieEntry.crazyflieContext.crazyflieName = tmp_db.crazyflieEntries[i].crazyflieContext.crazyflieName;
            if(_rosNodeThread->m_update_db_client.call(updateCall))
            {
                ROS_INFO("database changed in central manager service");
            }
            else
            {
                ROS_ERROR("Failed to remove entry in DB");
            }
        }
        save_database_file();
    }
    else
    {
        ROS_INFO("Failed to read DB");
    }
}

void MainGUIWindow::fill_database_file()
{
    clear_database_file();
    ROS_INFO("cleared data base file");
    ROS_INFO_STREAM("database:\n" << m_data_base);
    for(int i = 0; i < m_data_base.crazyflieEntries.size(); i++)
    {
        ROS_INFO("inserted 1 item in DB");
        insert_or_update_entry_database(m_data_base.crazyflieEntries[i]);
    }
    save_database_file();
}
For faster browsing, not all history is shown. View entire blame