mainguiwindow.cpp 17.7 KB
Newer Older
1
#include "mainguiwindow.h"
2
#include "ui_mainguiwindow.h"
3
#include "crazyFlyZoneTab.h"
4
5
#include "myGraphicsScene.h"
#include "myGraphicsView.h"
6
7
8
9
10

#include <QObject>
#include <QDoubleSpinBox>
#include <QTextEdit>
#include <QString>
11
#include <QMetaType>
12
#include <QDir>
13
#include <regex>
14

15
16
17
18
#ifdef CATKIN_MAKE
#include "d_fall_pps/UnlabeledMarker.h"
#endif

19
#include <string>
20

21
#define N_MAX_CRAZYFLIES           20 // protection number
22

roangel's avatar
roangel committed
23
#ifdef CATKIN_MAKE
24
using namespace d_fall_pps;
roangel's avatar
roangel committed
25
#endif
26

27
28
29
30
MainGUIWindow::MainGUIWindow(int argc, char **argv, QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainGUIWindow)
{
31
32
33
    #ifdef CATKIN_MAKE
    _rosNodeThread = new rosNodeThread(argc, argv, "/ViconDataPublisher/ViconData");
    #endif
34
35
36
    ui->setupUi(this);
    _init();
}
37

38

39
40
41
42
43
MainGUIWindow::~MainGUIWindow()
{
    delete ui;
}

roangel's avatar
roangel committed
44
void MainGUIWindow::doNumCrazyFlyZonesChanged(int n)
45
{
roangel's avatar
roangel committed
46
    // tabs number management, maybe do it in a different way so we dont have to remove and add everything?
47
48
49
    ui->tabWidget->clear();
    for (int i = 0; i < n; i++)
    {
50
51
        QString qstr = "CrazyFly ";
        qstr.append(QString::number(i+1));
52
53
54
55
        crazyFlyZoneTab* widget = new crazyFlyZoneTab(i);
        ui->tabWidget->addTab(widget, qstr);
        connect(widget, SIGNAL(centerButtonClickedSignal(int)), this, SLOT(centerViewIndex(int)));
    }
roangel's avatar
roangel committed
56

57
58
59
    ui->comboBoxCFZones->clear();
    #ifdef CATKIN_MAKE
    for(int i = 0; i < scene->crazyfly_zones.size(); i++)
roangel's avatar
roangel committed
60
    {
61
        if(!cf_linker->isCFZoneLinked(scene->crazyfly_zones[i]->getIndex()))
roangel's avatar
roangel committed
62
63
64
65
66
67
        {
            QString qstr = "CrazyFlyZone ";
            qstr.append(QString::number(i+1));
            ui->comboBoxCFZones->addItem(qstr);
        }
    }
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
    #endif
    // add options to QComboBox of CFZones
    // int current_count = ui->comboBoxCFZones->count();
    // if(n > current_count)
    // {
    //     for(int i = current_count; i < n; i++)
    //     {
    //         QString qstr = "CrazyFlyZone ";
    //         qstr.append(QString::number(i+1));
    //         ui->comboBoxCFZones->addItem(qstr);
    //     }
    // }
    // else if(n < current_count)
    // {
    //     for(int i = current_count; i >= n; i--)
    //     {
    //         ui->comboBoxCFZones->removeItem(i);
    //     }
    // }
roangel's avatar
roangel committed
87

88
89
90
91
}

void MainGUIWindow::_init()
{
92
93
94
95
96
97
98
99
    // 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);

100

101
    ui->graphicsView->setViewportUpdateMode(QGraphicsView::FullViewportUpdate);
102
103
    ui->graphicsView->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOn);
    ui->graphicsView->setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOn);
104

105
106
107
108
109
    // error messages
    // ui->err_message_cf->hide();
    // ui->err_message_cf_zone->hide();
    // ui->err_message_student_id->hide();

roangel's avatar
roangel committed
110
111
112
   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; }");
113

roangel's avatar
roangel committed
114
115
116
   ui->err_message_cf->clear();
   ui->err_message_cf_zone->clear();
   ui->err_message_student_id->clear();
117

118
119
    // initialize table_links
    ui->table_links->setColumnCount(3);
120

121
122
123
124
125
    QFont fnt;
    fnt.setPointSize(7);
    ui->table_links->horizontalHeader()->setFont(fnt);

    ui->table_links->horizontalHeader()->setDefaultSectionSize(90);
126
    ui->table_links->verticalHeader()->setDefaultSectionSize(20);
127
128
129
130
131
132
133
134
135
136
137

    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
138
    ui->table_links->setSelectionBehavior(QAbstractItemView::SelectRows);
139
140
141
    QStringList horizontal_header;
    horizontal_header << "Student ID" << "CrazyFly" << "CrazyFly Zone";
    ui->table_links->setHorizontalHeaderLabels(horizontal_header);
142

143
    // scene
144
    scene = new myGraphicsScene(ui->frame_drawing);
145
    scene->setSceneRect(-100 * FROM_METERS_TO_UNITS, -100 * FROM_METERS_TO_UNITS, 200 * FROM_METERS_TO_UNITS, 200 * FROM_METERS_TO_UNITS);
146

147
    ui->graphicsView->setScene(scene);
148

149
    // after scene is created, create CFlinker
150
    #ifdef CATKIN_MAKE
151
    cf_linker = new CFLinker(ui, &crazyflies_vector, &scene->crazyfly_zones);
152
    #endif
153
    // connections
154
    QObject::connect(ui->tabWidget, SIGNAL(tabCloseRequested(int)), scene, SLOT(removeCrazyFlyZone(int)));
roangel's avatar
roangel committed
155
    QObject::connect(scene, SIGNAL(numCrazyFlyZonesChanged(int)), this, SLOT(doNumCrazyFlyZonesChanged(int)));
156
157
158
159
    QObject::connect(ui->tabWidget, SIGNAL(currentChanged(int)), scene, SLOT(setSelectedCrazyFlyZone(int)));
    QObject::connect(scene, SIGNAL(crazyFlyZoneSelected(int)), ui->tabWidget, SLOT(setCurrentIndex(int)));
    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
160
161

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

roangel's avatar
roangel committed
163
    #ifdef CATKIN_MAKE
164
    _rosNodeThread->init();
165
    qRegisterMetaType<ptrToMessage>("ptrToMessage");
166
    QObject::connect(_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
roangel's avatar
roangel committed
167
    #endif
168
}
169

roangel's avatar
roangel committed
170
#ifdef CATKIN_MAKE
171
void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to newViconData, from node
172
{
173

174
175
    // update Markers

176
    if(p_msg->markers.size() < markers_vector.size()) // some markers have dissapeared, received stuff is smaller than what we have
177
178
179
    {
        for(int i = p_msg->markers.size(); i < markers_vector.size(); i++)
        {
180
            scene->removeItem(markers_vector[i]); // remove objects from scene
181
            // ROS_INFO_STREAM("element index: " << i << " removed");
182
        }
183
        markers_vector.erase(markers_vector.begin() + p_msg->markers.size(), markers_vector.end()); //delete them
184
185
    }

186
    // ROS_INFO_STREAM("markers.size: " << p_msg->markers.size());
187

188
189
190
191
    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
        {
192
            // ROS_INFO_STREAM("element index: " << i << " added");
193
            Marker* tmp_p_marker = new Marker(&(p_msg->markers[i]));
194
195
            markers_vector.push_back(tmp_p_marker); // what happens with the new indexes? check if this is correct

196
            if(ui->checkBox_vicon_markers->checkState() == Qt::Checked) //only if markers checkbox info is checked..
197
198
199
200
201
202
203
            {
                scene->addItem(markers_vector[i]);
                if(ui->checkBox_vicon_highlight_markers->checkState() == Qt::Checked)
                {
                    markers_vector[i]->setHighlighted();
                }
            }
204
205
206
        }
        else
        {
207
            // ROS_INFO_STREAM("element index: " << i << " moved, already existed");
208
            markers_vector[i]->updateMarker(&(p_msg->markers[i]));
209
210
        }
    }
211
212

    // update Crazyflies
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
    // 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]));
        }
233
        else                    //name not found, newly arrived, add it to the vector
234
235
        {
            crazyFly* tmp_p_crazyfly = new crazyFly(&(p_msg->crazyflies[i]));
236
237
238
239
240
241
            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
242
            {
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
                if(crazyflies_vector[i]->isOccluded())
                {
                    ROS_INFO("===================OCCLUDED");
                    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);
                    }
                }
260
            }
261
262
        }
    }
263

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

267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
    for(int j = 0; j < crazyfly_vector_size_after; j++)
    {
        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);
        }
    }
283
}
roangel's avatar
roangel committed
284
#endif
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
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


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)
{
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
391
392
393
394
395
396
397
398
399
400
401
402
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
}

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)
{
    if(index >= 0)
    {
        scene->setSelectedCrazyFlyZone(index);
    }
}

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
}
433
434
435
436
437

void MainGUIWindow::on_checkBox_vicon_markers_toggled(bool checked)
{
    if(checked)
    {
438
        #ifdef CATKIN_MAKE
439
440
441
442
        for(int i = 0; i < markers_vector.size(); i++)
        {
            scene->addItem(markers_vector[i]);
        }
443
        #endif
444
        ui->checkBox_vicon_highlight_markers->setCheckable(true);
roangel's avatar
roangel committed
445
        ui->checkBox_vicon_highlight_markers->setEnabled(true);
446
447
448
    }
    else
    {
449
        #ifdef CATKIN_MAKE
450
451
452
453
        for(int i = 0; i < markers_vector.size(); i++)
        {
            scene->removeItem(markers_vector[i]);
        }
454
        #endif
455
456
        ui->checkBox_vicon_highlight_markers->setChecked(false);
        ui->checkBox_vicon_highlight_markers->setCheckable(false);
roangel's avatar
roangel committed
457
        ui->checkBox_vicon_highlight_markers->setEnabled(false);
458
459
460
461
462
463
464
    }
}

void MainGUIWindow::on_checkBox_vicon_highlight_markers_toggled(bool checked)
{
    if(checked)
    {
465
        #ifdef CATKIN_MAKE
466
467
468
469
        for(int i = 0; i < markers_vector.size(); i++)
        {
            markers_vector[i]->setHighlighted();
        }
470
        #endif
471
472
473
    }
    else
    {
474
        #ifdef CATKIN_MAKE
475
476
477
478
        for(int i = 0; i < markers_vector.size(); i++)
        {
            markers_vector[i]->clearHighlighted();
        }
479
480
481
482
483
484
485
486
487
488
489
        #endif
    }
}

void MainGUIWindow::on_checkBox_vicon_crazyflies_toggled(bool checked)
{
    if(checked)
    {
        #ifdef CATKIN_MAKE
        for(int i = 0; i < crazyflies_vector.size(); i++)
        {
490
491
492
493
494
            if(!crazyflies_vector[i]->isAddedToScene())
            {
                scene->addItem(crazyflies_vector[i]);
                crazyflies_vector[i]->setAddedToScene(true);
            }
495
496
497
498
499
500
501
502
503
        }
        #endif
        ui->scaleSpinBox->setEnabled(true);
    }
    else
    {
        #ifdef CATKIN_MAKE
        for(int i = 0; i < crazyflies_vector.size(); i++)
        {
504
505
506
507
508
            if(crazyflies_vector[i]->isAddedToScene())
            {
                scene->removeItem(crazyflies_vector[i]);
                crazyflies_vector[i]->setAddedToScene(false);
            }
509
510
511
512
513
514
515
516
517
518
519
520
        }
        #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);
521
    }
522
    #endif
523
}
524
525
526
527
528
529
530

void MainGUIWindow::on_refresh_cfs_button_clicked()
{
    #ifdef CATKIN_MAKE
    ui->comboBoxCFs->clear();
    for(int i = 0; i < crazyflies_vector.size(); i++)
    {
531
532
533
534
535
        if(!cf_linker->isCFLinked(crazyflies_vector[i]->getName()))
        {
            QString qstr = QString::fromStdString(crazyflies_vector[i]->getName());
            ui->comboBoxCFs->addItem(qstr);
        }
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
    }
    #endif
}

void MainGUIWindow::on_refresh_student_ids_button_clicked()
{
    #ifdef CATKIN_MAKE
    ui->list_discovered_student_ids->clear();

    // \/(\d)\/PPSClient
    ros::V_string v_str;
    ros::master::getNodes(v_str);
    for(int i = 0; i < v_str.size(); i++)
    {
        std::string s = v_str[i];
        std::smatch m;
        std::regex e ("\\/(\\d)\\/PPSClient");

        // std::regex e("\\/PPSClien(.)");

        // while(std::regex_search(s, m, e))
        // {
        //     for (int i = 0; i < m.size(); i++)
        //     {
        //         ROS_INFO("FOUND: %s", m[i].str().c_str());
        //         // std::cout << "FOUND" << m[i] << "\n";
        //     }
        //     s = m.suffix().str();
        // }

        if(std::regex_search(s, m, e))
        {
            // ROS_INFO("===============================================FOUND: %s", m[1].str().c_str()); // one because we are interested ONLY in the first match
            std::string found_string = m[1].str();
            ui->list_discovered_student_ids->addItem(found_string.c_str());
        }
    }
    #endif
}
575
576
577
578
579
580



void MainGUIWindow::on_link_button_clicked()
{
    #ifdef CATKIN_MAKE
581
582
583
584
585

    bool error = false;
    if(ui->comboBoxCFs->count() == 0)
    {
        // plot error message
586
        ui->err_message_cf->setText("CF box is empty");
587
588
        error = true;
    }
589
590
591
592
    else
    {
        ui->err_message_cf->clear();
    }
593
594
595
    if(ui->comboBoxCFZones->count() == 0)
    {
        // plot error message
596
        ui->err_message_cf_zone->setText("CFZone box is empty");
597
598
        error = true;
    }
599
600
601
602
    else
    {
        ui->err_message_cf_zone->clear();
    }
603

604
    if(cf_linker->isStudentIDLinked(ui->spinBox_student_ids->value()))
605
    {
606
607
608
609
610
611
612
        // plot error message
        ui->err_message_student_id->setText("This StudentID has already been linked");
        error = true;
    }
    else
    {
        ui->err_message_student_id->clear();
613
614
615
616
617
618
    }

    if(!error)
    {
        cf_linker->link();
    }
619
620
    #endif
}
621
622
623
624
625
626
627

void MainGUIWindow::on_unlink_button_clicked()
{
    #ifdef CATKIN_MAKE
    cf_linker->unlink();
    #endif
}