mainguiwindow.cpp 12.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

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

18
#include <string>
19

20
#define N_MAX_CRAZYFLIES           20 // protection number
21

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

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

40

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

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

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

78
79
80
81
}

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

90

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

95
96

    // scene
97
    scene = new myGraphicsScene(ui->frame_drawing);
98
    scene->setSceneRect(-100 * FROM_METERS_TO_UNITS, -100 * FROM_METERS_TO_UNITS, 200 * FROM_METERS_TO_UNITS, 200 * FROM_METERS_TO_UNITS);
99

100
    ui->graphicsView->setScene(scene);
101

102
    // connections
103
    QObject::connect(ui->tabWidget, SIGNAL(tabCloseRequested(int)), scene, SLOT(removeCrazyFlyZone(int)));
roangel's avatar
roangel committed
104
    QObject::connect(scene, SIGNAL(numCrazyFlyZonesChanged(int)), this, SLOT(doNumCrazyFlyZonesChanged(int)));
105
106
107
108
    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
109
110

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

roangel's avatar
roangel committed
112
    #ifdef CATKIN_MAKE
113
    _rosNodeThread->init();
114
    qRegisterMetaType<ptrToMessage>("ptrToMessage");
115
    QObject::connect(_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
roangel's avatar
roangel committed
116
    #endif
117
}
118

roangel's avatar
roangel committed
119
#ifdef CATKIN_MAKE
120
void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to newViconData, from node
121
{
122

123
124
    // update Markers

125
    if(p_msg->markers.size() < markers_vector.size()) // some markers have dissapeared, received stuff is smaller than what we have
126
127
128
    {
        for(int i = p_msg->markers.size(); i < markers_vector.size(); i++)
        {
129
130
            scene->removeItem(markers_vector[i]); // remove objects from scene
            ROS_INFO_STREAM("element index: " << i << " removed");
131
        }
132
        markers_vector.erase(markers_vector.begin() + p_msg->markers.size(), markers_vector.end()); //delete them
133
134
    }

135
136
    ROS_INFO_STREAM("markers.size: " << p_msg->markers.size());

137
138
139
140
    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
        {
141
            ROS_INFO_STREAM("element index: " << i << " added");
142
            Marker* tmp_p_marker = new Marker(&(p_msg->markers[i]));
143
144
            markers_vector.push_back(tmp_p_marker); // what happens with the new indexes? check if this is correct

145
            if(ui->checkBox_vicon_markers->checkState() == Qt::Checked) //only if markers checkbox info is checked..
146
147
148
149
150
151
152
            {
                scene->addItem(markers_vector[i]);
                if(ui->checkBox_vicon_highlight_markers->checkState() == Qt::Checked)
                {
                    markers_vector[i]->setHighlighted();
                }
            }
153
154
155
        }
        else
        {
156
            ROS_INFO_STREAM("element index: " << i << " moved, already existed");
157
            markers_vector[i]->updateMarker(&(p_msg->markers[i]));
158
159
        }
    }
160
161

    // update Crazyflies
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
    // 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]));
        }
        else
        {
            crazyFly* tmp_p_crazyfly = new crazyFly(&(p_msg->crazyflies[i]));
185
186
187
188
            if(ui->checkBox_vicon_crazyflies->checkState() == Qt::Checked)
            {
                scene->addItem(tmp_p_crazyfly);
            }
189
190
191
            crazyflies_vector.push_back(tmp_p_crazyfly);
        }
    }
192

193
194
    // in this loop, clean the ones that are not present anymore
    int crazyfly_vector_size_after = crazyflies_vector.size();
195

196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
    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);
        }
    }
212
}
roangel's avatar
roangel committed
213
#endif
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
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


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

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
}
362
363
364
365
366

void MainGUIWindow::on_checkBox_vicon_markers_toggled(bool checked)
{
    if(checked)
    {
367
        #ifdef CATKIN_MAKE
368
369
370
371
        for(int i = 0; i < markers_vector.size(); i++)
        {
            scene->addItem(markers_vector[i]);
        }
372
        #endif
373
        ui->checkBox_vicon_highlight_markers->setCheckable(true);
roangel's avatar
roangel committed
374
        ui->checkBox_vicon_highlight_markers->setEnabled(true);
375
376
377
    }
    else
    {
378
        #ifdef CATKIN_MAKE
379
380
381
382
        for(int i = 0; i < markers_vector.size(); i++)
        {
            scene->removeItem(markers_vector[i]);
        }
383
        #endif
384
385
        ui->checkBox_vicon_highlight_markers->setChecked(false);
        ui->checkBox_vicon_highlight_markers->setCheckable(false);
roangel's avatar
roangel committed
386
        ui->checkBox_vicon_highlight_markers->setEnabled(false);
387
388
389
390
391
392
393
    }
}

void MainGUIWindow::on_checkBox_vicon_highlight_markers_toggled(bool checked)
{
    if(checked)
    {
394
        #ifdef CATKIN_MAKE
395
396
397
398
        for(int i = 0; i < markers_vector.size(); i++)
        {
            markers_vector[i]->setHighlighted();
        }
399
        #endif
400
401
402
    }
    else
    {
403
        #ifdef CATKIN_MAKE
404
405
406
407
        for(int i = 0; i < markers_vector.size(); i++)
        {
            markers_vector[i]->clearHighlighted();
        }
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
        #endif
    }
}

void MainGUIWindow::on_checkBox_vicon_crazyflies_toggled(bool checked)
{
    if(checked)
    {
        #ifdef CATKIN_MAKE
        for(int i = 0; i < crazyflies_vector.size(); i++)
        {
            scene->addItem(crazyflies_vector[i]);
        }
        #endif
        ui->scaleSpinBox->setEnabled(true);
    }
    else
    {
        #ifdef CATKIN_MAKE
        for(int i = 0; i < crazyflies_vector.size(); i++)
        {
            scene->removeItem(crazyflies_vector[i]);
        }
        #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);
442
    }
443
    #endif
444
}