mainguiwindow.cpp 15.9 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
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75

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

76
77
78
79
}

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

88

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

93
94
95
96
97
98
99
100
101
102
    // 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);
103
    // ui->table_links->verticalHeader()->setDefaultSectionSize(20);
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
124
125
    // after scene is created, create CFlinker
    cf_linker = new CFLinker(ui, &crazyflies_vector, &scene->crazyfly_zones);

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

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

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

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

147
148
    // update Markers

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

159
    // ROS_INFO_STREAM("markers.size: " << p_msg->markers.size());
160

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

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

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

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

240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
    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);
        }
    }
256
}
roangel's avatar
roangel committed
257
#endif
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
330
331
332


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)
{
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
403
404
405
}

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
}
406
407
408
409
410

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

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

void MainGUIWindow::on_checkBox_vicon_crazyflies_toggled(bool checked)
{
    if(checked)
    {
        #ifdef CATKIN_MAKE
        for(int i = 0; i < crazyflies_vector.size(); i++)
        {
463
464
465
466
467
            if(!crazyflies_vector[i]->isAddedToScene())
            {
                scene->addItem(crazyflies_vector[i]);
                crazyflies_vector[i]->setAddedToScene(true);
            }
468
469
470
471
472
473
474
475
476
        }
        #endif
        ui->scaleSpinBox->setEnabled(true);
    }
    else
    {
        #ifdef CATKIN_MAKE
        for(int i = 0; i < crazyflies_vector.size(); i++)
        {
477
478
479
480
481
            if(crazyflies_vector[i]->isAddedToScene())
            {
                scene->removeItem(crazyflies_vector[i]);
                crazyflies_vector[i]->setAddedToScene(false);
            }
482
483
484
485
486
487
488
489
490
491
492
493
        }
        #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);
494
    }
495
    #endif
496
}
497
498
499
500
501

void MainGUIWindow::on_refresh_cfs_button_clicked()
{
    #ifdef CATKIN_MAKE
    ui->comboBoxCFs->clear();
502
    // TODO: grey out the ones that are already linked
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
542
543
544
545
    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
}
546
547
548
549
550
551



void MainGUIWindow::on_link_button_clicked()
{
    #ifdef CATKIN_MAKE
552
    cf_linker->link();
553
554
    #endif
}