mainguiwindow.cpp 16.3 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
    #ifdef CATKIN_MAKE
    _rosNodeThread = new rosNodeThread(argc, argv, "/ViconDataPublisher/ViconData");
33
    cf_linker = new CFLinker(ui);
34
    #endif
35
36
37
    ui->setupUi(this);
    _init();
}
38

39

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

roangel's avatar
roangel committed
45
void MainGUIWindow::doNumCrazyFlyZonesChanged(int n)
46
{
roangel's avatar
roangel committed
47
    // tabs number management, maybe do it in a different way so we dont have to remove and add everything?
48
49
50
    ui->tabWidget->clear();
    for (int i = 0; i < n; i++)
    {
51
52
        QString qstr = "CrazyFly ";
        qstr.append(QString::number(i+1));
53
54
55
56
        crazyFlyZoneTab* widget = new crazyFlyZoneTab(i);
        ui->tabWidget->addTab(widget, qstr);
        connect(widget, SIGNAL(centerButtonClickedSignal(int)), this, SLOT(centerViewIndex(int)));
    }
roangel's avatar
roangel committed
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76

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

77
78
79
80
}

void MainGUIWindow::_init()
{
81
82
83
84
85
86
87
88
    // 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);

89

90
    ui->graphicsView->setViewportUpdateMode(QGraphicsView::FullViewportUpdate);
91
92
    ui->graphicsView->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOn);
    ui->graphicsView->setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOn);
93

94
95
96
97
98
99
100
101
102
103
    // initialize table_links
    ui->table_links->setColumnCount(3);
    QStringList horizontal_header;
    horizontal_header << "Student ID" << "CrazyFly" << "CrazyFly Zone";
    ui->table_links->setHorizontalHeaderLabels(horizontal_header);
    QFont fnt;
    fnt.setPointSize(7);
    ui->table_links->horizontalHeader()->setFont(fnt);

    ui->table_links->horizontalHeader()->setDefaultSectionSize(90);
104
    ui->table_links->verticalHeader()->setDefaultSectionSize(20);
105
106
107
108
109
110
111
112
113
114
115
116

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

117
    // scene
118
    scene = new myGraphicsScene(ui->frame_drawing);
119
    scene->setSceneRect(-100 * FROM_METERS_TO_UNITS, -100 * FROM_METERS_TO_UNITS, 200 * FROM_METERS_TO_UNITS, 200 * FROM_METERS_TO_UNITS);
120

121
    ui->graphicsView->setScene(scene);
122

123
    // connections
124
    QObject::connect(ui->tabWidget, SIGNAL(tabCloseRequested(int)), scene, SLOT(removeCrazyFlyZone(int)));
roangel's avatar
roangel committed
125
    QObject::connect(scene, SIGNAL(numCrazyFlyZonesChanged(int)), this, SLOT(doNumCrazyFlyZonesChanged(int)));
126
127
128
129
    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
130
131

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

roangel's avatar
roangel committed
133
    #ifdef CATKIN_MAKE
134
    _rosNodeThread->init();
135
    qRegisterMetaType<ptrToMessage>("ptrToMessage");
136
    QObject::connect(_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
roangel's avatar
roangel committed
137
    #endif
138
}
139

roangel's avatar
roangel committed
140
#ifdef CATKIN_MAKE
141
void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to newViconData, from node
142
{
143

144
145
    // update Markers

146
    if(p_msg->markers.size() < markers_vector.size()) // some markers have dissapeared, received stuff is smaller than what we have
147
148
149
    {
        for(int i = p_msg->markers.size(); i < markers_vector.size(); i++)
        {
150
            scene->removeItem(markers_vector[i]); // remove objects from scene
151
            // ROS_INFO_STREAM("element index: " << i << " removed");
152
        }
153
        markers_vector.erase(markers_vector.begin() + p_msg->markers.size(), markers_vector.end()); //delete them
154
155
    }

156
    // ROS_INFO_STREAM("markers.size: " << p_msg->markers.size());
157

158
159
160
161
    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
        {
162
            // ROS_INFO_STREAM("element index: " << i << " added");
163
            Marker* tmp_p_marker = new Marker(&(p_msg->markers[i]));
164
165
            markers_vector.push_back(tmp_p_marker); // what happens with the new indexes? check if this is correct

166
            if(ui->checkBox_vicon_markers->checkState() == Qt::Checked) //only if markers checkbox info is checked..
167
168
169
170
171
172
173
            {
                scene->addItem(markers_vector[i]);
                if(ui->checkBox_vicon_highlight_markers->checkState() == Qt::Checked)
                {
                    markers_vector[i]->setHighlighted();
                }
            }
174
175
176
        }
        else
        {
177
            // ROS_INFO_STREAM("element index: " << i << " moved, already existed");
178
            markers_vector[i]->updateMarker(&(p_msg->markers[i]));
179
180
        }
    }
181
182

    // update Crazyflies
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
    // 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]));
        }
203
        else                    //name not found, newly arrived, add it to the vector
204
205
        {
            crazyFly* tmp_p_crazyfly = new crazyFly(&(p_msg->crazyflies[i]));
206
207
208
209
210
211
            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
212
            {
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
                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);
                    }
                }
230
            }
231
232
        }
    }
233

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

237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
    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);
        }
    }
253
}
roangel's avatar
roangel committed
254
#endif
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
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


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

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
}
403
404
405
406
407

void MainGUIWindow::on_checkBox_vicon_markers_toggled(bool checked)
{
    if(checked)
    {
408
        #ifdef CATKIN_MAKE
409
410
411
412
        for(int i = 0; i < markers_vector.size(); i++)
        {
            scene->addItem(markers_vector[i]);
        }
413
        #endif
414
        ui->checkBox_vicon_highlight_markers->setCheckable(true);
roangel's avatar
roangel committed
415
        ui->checkBox_vicon_highlight_markers->setEnabled(true);
416
417
418
    }
    else
    {
419
        #ifdef CATKIN_MAKE
420
421
422
423
        for(int i = 0; i < markers_vector.size(); i++)
        {
            scene->removeItem(markers_vector[i]);
        }
424
        #endif
425
426
        ui->checkBox_vicon_highlight_markers->setChecked(false);
        ui->checkBox_vicon_highlight_markers->setCheckable(false);
roangel's avatar
roangel committed
427
        ui->checkBox_vicon_highlight_markers->setEnabled(false);
428
429
430
431
432
433
434
    }
}

void MainGUIWindow::on_checkBox_vicon_highlight_markers_toggled(bool checked)
{
    if(checked)
    {
435
        #ifdef CATKIN_MAKE
436
437
438
439
        for(int i = 0; i < markers_vector.size(); i++)
        {
            markers_vector[i]->setHighlighted();
        }
440
        #endif
441
442
443
    }
    else
    {
444
        #ifdef CATKIN_MAKE
445
446
447
448
        for(int i = 0; i < markers_vector.size(); i++)
        {
            markers_vector[i]->clearHighlighted();
        }
449
450
451
452
453
454
455
456
457
458
459
        #endif
    }
}

void MainGUIWindow::on_checkBox_vicon_crazyflies_toggled(bool checked)
{
    if(checked)
    {
        #ifdef CATKIN_MAKE
        for(int i = 0; i < crazyflies_vector.size(); i++)
        {
460
461
462
463
464
            if(!crazyflies_vector[i]->isAddedToScene())
            {
                scene->addItem(crazyflies_vector[i]);
                crazyflies_vector[i]->setAddedToScene(true);
            }
465
466
467
468
469
470
471
472
473
        }
        #endif
        ui->scaleSpinBox->setEnabled(true);
    }
    else
    {
        #ifdef CATKIN_MAKE
        for(int i = 0; i < crazyflies_vector.size(); i++)
        {
474
475
476
477
478
            if(crazyflies_vector[i]->isAddedToScene())
            {
                scene->removeItem(crazyflies_vector[i]);
                crazyflies_vector[i]->setAddedToScene(false);
            }
479
480
481
482
483
484
485
486
487
488
489
490
        }
        #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);
491
    }
492
    #endif
493
}
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541

void MainGUIWindow::on_refresh_cfs_button_clicked()
{
    #ifdef CATKIN_MAKE
    ui->comboBoxCFs->clear();
    for(int i = 0; i < crazyflies_vector.size(); i++)
    {
        QString qstr = QString::fromStdString(crazyflies_vector[i]->getName());
        ui->comboBoxCFs->addItem(qstr);
    }
    #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
}
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565



void MainGUIWindow::on_link_button_clicked()
{
    #ifdef CATKIN_MAKE
    QString cfzone_str = ui->comboBoxCFZones->currentText();
    crazyFly* p_tmp_cf;

    for(int i = 0; i < crazyflies_vector.size(); i++)
    {
        if(ui->comboBoxCFs->currentText().toStdString() == crazyflies_vector[i]->getName())
        {
            ROS_INFO("crazyfly to link found!--------------------------");
            p_tmp_cf = crazyflies_vector[i];
        }
    }

    int cf_zone_index = cfzone_str.split(" ")[1].toInt() - 1;
    crazyFlyZone* p_tmp_cf_zone = scene->crazyfly_zones[cf_zone_index];
    ROS_INFO("cf_zone_index: %d", cf_zone_index);
    cf_linker->link(ui->spinBox_student_ids->value(), p_tmp_cf, p_tmp_cf_zone);
    #endif
}