MainWindow.cpp 61.4 KB
Newer Older
1
//    Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero
roangel's avatar
roangel committed
2
//
3
4
5
//    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
6
7
8
//    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.
9
10
//    
//    D-FaLL-System is distributed in the hope that it will be useful,
roangel's avatar
roangel committed
11
12
13
//    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.
14
//    
roangel's avatar
roangel committed
15
//    You should have received a copy of the GNU General Public License
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
//    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:
//    Main window of the Student's GUI
//
//    ----------------------------------------------------------------------------------

roangel's avatar
roangel committed
32

roangel's avatar
roangel committed
33
34
#include "MainWindow.h"
#include "ui_MainWindow.h"
35
#include <string>
Paul Beuchat's avatar
Paul Beuchat committed
36
#include <QShortcut>
roangel's avatar
roangel committed
37

38
39
#include <ros/ros.h>
#include <ros/network.h>
40
#include <ros/package.h>
41

42
43
44
45
#include "d_fall_pps/CMQuery.h"

#include "d_fall_pps/ViconData.h"

46
47
#include "d_fall_pps/CustomButton.h"

48
MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
roangel's avatar
roangel committed
49
    QMainWindow(parent),
50
    ui(new Ui::MainWindow),
51
    m_battery_level(0)
roangel's avatar
roangel committed
52
{
53

roangel's avatar
roangel committed
54
    ui->setupUi(this);
55
56

    m_rosNodeThread = new rosNodeThread(argc, argv, "student_GUI");
57
    m_rosNodeThread->init();
58

59
60
    setCrazyRadioStatus(DISCONNECTED);

61
    m_ros_namespace = ros::this_node::getNamespace();
62
    ROS_INFO("[Student GUI] node namespace: %s", m_ros_namespace.c_str());
63

64
65
    qRegisterMetaType<ptrToMessage>("ptrToMessage");
    QObject::connect(m_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
66

67
    ros::NodeHandle nodeHandle(m_ros_namespace);
68

69

70
    // SUBSCRIBERS AND PUBLISHERS:
71
72
73
74
75
76
77
78
    // > For the Demo Controller SETPOINTS and CUSTOM COMMANDS
    demoSetpointPublisher     = nodeHandle.advertise<Setpoint>("DemoControllerService/Setpoint", 1);
    demoSetpointSubscriber    = nodeHandle.subscribe("DemoControllerService/Setpoint", 1, &MainWindow::demoSetpointCallback, this);
    demoCustomButtonPublisher = nodeHandle.advertise<CustomButton>("DemoControllerService/GUIButton", 1);
    // > For the Student Controller SETPOINTS and CUSTOM COMMANDS
    studentSetpointPublisher     = nodeHandle.advertise<Setpoint>("StudentControllerService/Setpoint", 1);
    studentSetpointSubscriber    = nodeHandle.subscribe("StudentControllerService/Setpoint", 1, &MainWindow::studentSetpointCallback, this);
    studentCustomButtonPublisher = nodeHandle.advertise<CustomButton>("StudentControllerService/GUIButton", 1);
79
    // > For the MPC Controller SETPOINTS
80
81
    mpcSetpointPublisher  = nodeHandle.advertise<Setpoint>("MpcControllerService/Setpoint", 1);
    mpcSetpointSubscriber = nodeHandle.subscribe("MpcControllerService/Setpoint", 1, &MainWindow::mpcSetpointCallback, this);
82

83
84
85
86
87
88
89
90
91
92
93

    // > For the Remote Controller subscribe action
    remoteSubscribePublisher = nodeHandle.advertise<ViconSubscribeObjectName>("RemoteControllerService/ViconSubscribeObjectName", 1);
    // > For the Remote Controller activate action
    remoteActivatePublisher = nodeHandle.advertise<std_msgs::Int32>("RemoteControllerService/Activate", 1);
    // > For the Remote Controller data
    remoteDataSubscriber = nodeHandle.subscribe("RemoteControllerService/RemoteData", 1, &MainWindow::remoteDataCallback, this);;
    // > For the Remote Controller data
    remoteControlSetpointSubscriber = nodeHandle.subscribe("RemoteControllerService/RemoteControlSetpoint", 1, &MainWindow::remoteControlSetpointCallback, this);;


94
95
96
97
98
99
100
101
102
    // > For the TUNING CONTROLLER "test" button publisher
    tuningActivateTestPublisher = nodeHandle.advertise<std_msgs::Int32>("TuningControllerService/ActivateTest", 1);
    // > For the TUNING CONTOLLER "gain" sliders
    tuningHorizontalGainPublisher = nodeHandle.advertise<std_msgs::Int32>("TuningControllerService/HorizontalGain", 1);
    tuningVerticalGainPublisher = nodeHandle.advertise<std_msgs::Int32>("TuningControllerService/VerticalGain", 1);
    tuningHeadingGainPublisher = nodeHandle.advertise<std_msgs::Int32>("TuningControllerService/HeadingGain", 1);



103
    // subscribers
roangel's avatar
roangel committed
104
    crazyRadioStatusSubscriber = nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &MainWindow::crazyRadioStatusCallback, this);
105

106
107
    CFBatterySubscriber = nodeHandle.subscribe("CrazyRadio/CFBattery", 1, &MainWindow::CFBatteryCallback, this);

108
109
    flyingStateSubscriber = nodeHandle.subscribe("PPSClient/flyingState", 1, &MainWindow::flyingStateChangedCallback, this);

110
111
    batteryStateSubscriber = nodeHandle.subscribe("PPSClient/batteryState", 1, &MainWindow::batteryStateChangedCallback, this);

112
113
    controllerUsedSubscriber = nodeHandle.subscribe("PPSClient/controllerUsed", 1, &MainWindow::controllerUsedChangedCallback, this);

114

115
    safeSetpointSubscriber = nodeHandle.subscribe("SafeControllerService/Setpoint", 1, &MainWindow::safeSetpointCallback, this);
116
    DBChangedSubscriber = nodeHandle.subscribe("/my_GUI/DBChanged", 1, &MainWindow::DBChangedCallback, this);
117

118
    ros::NodeHandle my_nodeHandle("~");
119
    controllerSetpointPublisher = my_nodeHandle.advertise<Setpoint>("ControllerSetpoint", 1);
120

121

122
    // communication with PPS Client, just to make it possible to communicate through terminal also we use PPSClient's name
123
124
    //ros::NodeHandle nh_PPSClient(m_ros_namespace + "/PPSClient");
    ros::NodeHandle nh_PPSClient("PPSClient");
125
    crazyRadioCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
126
    PPSClientCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("Command", 1);    
127

128

129
    // > For publishing a message that requests the
130
131
132
133
134
    //   YAML parameters to be re-loaded from file
    // > The message contents specify which controller
    //   the parameters should be re-loaded for
    requestLoadControllerYamlPublisher = nh_PPSClient.advertise<std_msgs::Int32>("requestLoadControllerYaml", 1);

135
136
137

    // Subscriber for locking the load the controller YAML
    // parameters when the Coordintor GUI requests a load
138
    requestLoadControllerYaml_from_my_GUI_Subscriber = nodeHandle.subscribe("/my_GUI/requestLoadControllerYaml", 1, &MainWindow::requestLoadControllerYaml_from_my_GUI_Callback, this);
139
140

    // First get student ID
141
    if(!nh_PPSClient.getParam("agentID", m_student_id))
142
    {
143
		ROS_ERROR("Failed to get agentID");
144
145
	}

146
147
148
149
150
151
    // Then, Central manager
    centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
    loadCrazyflieContext();



152

153
154
155
156
    // Load default setpoint from the "SafeController" namespace of the "ParameterService"
    std::vector<float> default_setpoint(4);
    ros::NodeHandle nodeHandle_to_own_agent_parameter_service("ParameterService");
    ros::NodeHandle nodeHandle_for_safeController(nodeHandle_to_own_agent_parameter_service, "SafeController");
157

158
    if(!nodeHandle_for_safeController.getParam("defaultSetpoint", default_setpoint))
roangel's avatar
roangel committed
159
    {
160
        ROS_ERROR_STREAM("The StudentGUI could not find parameter 'defaultSetpoint', as called from main(...)");
roangel's avatar
roangel committed
161
162
    }

163
    // Copy the default setpoint into respective text fields of the GUI
164
165
166
167
    ui->current_setpoint_x_safe->setText(QString::number(default_setpoint[0]));
    ui->current_setpoint_y_safe->setText(QString::number(default_setpoint[1]));
    ui->current_setpoint_z_safe->setText(QString::number(default_setpoint[2]));
    ui->current_setpoint_yaw_safe->setText(QString::number(default_setpoint[3]));
roangel's avatar
roangel committed
168

169

170
    disableGUI();
171
    highlightSafeControllerTab();
172
173

    // INITIALISE THE BATTERY STATE AS NORMAL
roangel's avatar
roangel committed
174
    m_battery_state = BATTERY_STATE_NORMAL;
175

176
177
178
    // SET THE BATTERY VOLTAGE FIELD TO BE BLANK
    QString qstr = "-.-- V";
    ui->voltage_field->setText(qstr);
179
    // SET THE IMAGE FOR THE BATTERY STATUS LABEL
180
181
182
    QPixmap battery_unknown_pixmap(":/images/battery_unknown.png");
    ui->battery_status_label->setPixmap(battery_unknown_pixmap);
    ui->battery_status_label->setScaledContents(true);
183
    m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
184

185
    // SET THE IMAGE FOR THE CRAZY RADIO STATUS LABEL
186
187
188
189
    QPixmap rf_disconnected_pixmap(":/images/rf_disconnected.png");
    ui->rf_status_label->setPixmap(rf_disconnected_pixmap);
    ui->rf_status_label->setScaledContents(true);

190
191
192
193
194
195
    // SET THE IMAGE FOR THE FLYING STATE LABEL
    QPixmap flying_state_off_pixmap(":/images/flying_state_off.png");
    ui->flying_state_label->setPixmap(flying_state_off_pixmap);
    ui->flying_state_label->setScaledContents(true);


196
197
198
199
200
201

    //QPixmap battery_80_pixmap(":/images/battery_80.png");
	//m_battery_80_pixmap = battery_80_pixmap.scaled(50,70,Qt::IgnoreAspectRatio);
	// The syntax for "scaled" is (int width, int height, ...)


202
203
204
    ui->error_label->setStyleSheet("QLabel { color : red; }");
    ui->error_label->clear();

205
206
207
208
209
210
211
    // Add keyboard shortcuts
    // > for "all motors off", press the space bar
    ui->motors_OFF_button->setShortcut(tr("Space"));
    // > for "kill GUI node", press "CTRL+C" while the GUI window is the focus
    QShortcut* close_GUI_shortcut = new QShortcut(QKeySequence(tr("CTRL+C")), this, SLOT(close()));


212
213
214
    initialize_demo_setpoint();
    initialize_student_setpoint();
    initialize_mpc_setpoint();
roangel's avatar
roangel committed
215
216
}

217

roangel's avatar
roangel committed
218
219
220
221
MainWindow::~MainWindow()
{
    delete ui;
}
222

223
224
void MainWindow::disableGUI()
{
225
226
227
    ui->motors_OFF_button->setEnabled(false);
    ui->take_off_button->setEnabled(false);
    ui->land_button->setEnabled(false);
228
229
230
231
}

void MainWindow::enableGUI()
{
232
    ui->motors_OFF_button->setEnabled(true);
roangel's avatar
roangel committed
233
234
    if(m_battery_state == BATTERY_STATE_NORMAL)
    {
235
236
        ui->take_off_button->setEnabled(true);
        ui->land_button->setEnabled(true);
roangel's avatar
roangel committed
237
    }
238
239
}

240
241
242
243
void MainWindow::highlightSafeControllerTab()
{
    ui->tabWidget->tabBar()->setTabTextColor(0, Qt::green);
    ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
244
245
    ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
246
    ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
247
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
248
}
249
void MainWindow::highlightDemoControllerTab()
250
251
252
{
    ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(1, Qt::green);
253
254
    ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
255
    ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
256
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
257
258
259
260
261
262
263
}
void MainWindow::highlightStudentControllerTab()
{
    ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(2, Qt::green);
    ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
264
    ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
265
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
266
267
268
269
270
271
272
}
void MainWindow::highlightMpcControllerTab()
{
    ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(3, Qt::green);
273
    ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
274
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
275
276
277
278
279
280
281
282
}
void MainWindow::highlightRemoteControllerTab()
{
    ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(4, Qt::green);
283
284
285
286
287
288
289
290
291
292
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
}
void MainWindow::highlightTuningControllerTab()
{
    ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::green);
293
294
}

295
296
297
void MainWindow::DBChangedCallback(const std_msgs::Int32& msg)
{
    loadCrazyflieContext();
298
    ROS_INFO("context reloaded in student_GUI");
299
300
}

301
302
303
304
305
306
307
void MainWindow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
{
    switch(msg.data)
    {
        case SAFE_CONTROLLER:
            highlightSafeControllerTab();
            break;
308
        case DEMO_CONTROLLER:
309
310
311
312
313
314
315
            highlightDemoControllerTab();
            break;
        case STUDENT_CONTROLLER:
            highlightStudentControllerTab();
            break;
        case MPC_CONTROLLER:
            highlightMpcControllerTab();
316
            break;
317
318
        case REMOTE_CONTROLLER:
            highlightRemoteControllerTab();
319
            break;
320
321
        case TUNING_CONTROLLER:
            highlightTuningControllerTab();
322
            break;
323
324
325
326
327
        default:
            break;
    }
}

328
void MainWindow::safeSetpointCallback(const Setpoint& newSetpoint)
329
{
330
    m_safe_setpoint = newSetpoint;
331
    // here we get the new setpoint, need to update it in GUI
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
    ui->current_setpoint_x_safe->setText(QString::number(newSetpoint.x, 'f', 3));
    ui->current_setpoint_y_safe->setText(QString::number(newSetpoint.y, 'f', 3));
    ui->current_setpoint_z_safe->setText(QString::number(newSetpoint.z, 'f', 3));
    ui->current_setpoint_yaw_safe->setText(QString::number(newSetpoint.yaw * RAD2DEG, 'f', 1));
}

void MainWindow::demoSetpointCallback(const Setpoint& newSetpoint)
{
    m_demo_setpoint = newSetpoint;
    // here we get the new setpoint, need to update it in GUI
    ui->current_setpoint_x_demo->setText(QString::number(newSetpoint.x, 'f', 3));
    ui->current_setpoint_y_demo->setText(QString::number(newSetpoint.y, 'f', 3));
    ui->current_setpoint_z_demo->setText(QString::number(newSetpoint.z, 'f', 3));
    ui->current_setpoint_yaw_demo->setText(QString::number(newSetpoint.yaw * RAD2DEG, 'f', 1));
}

void MainWindow::studentSetpointCallback(const Setpoint& newSetpoint)
{
    m_student_setpoint = newSetpoint;
    // here we get the new setpoint, need to update it in GUI
    ui->current_setpoint_x_student->setText(QString::number(newSetpoint.x, 'f', 3));
    ui->current_setpoint_y_student->setText(QString::number(newSetpoint.y, 'f', 3));
    ui->current_setpoint_z_student->setText(QString::number(newSetpoint.z, 'f', 3));
    ui->current_setpoint_yaw_student->setText(QString::number(newSetpoint.yaw * RAD2DEG, 'f', 1));
356
357
}

358
void MainWindow::mpcSetpointCallback(const Setpoint& newSetpoint)
359
{
360
    m_mpc_setpoint = newSetpoint;
361
    // here we get the new setpoint, need to update it in GUI
362
363
364
365
    ui->current_setpoint_x_mpc->setText(QString::number(newSetpoint.x, 'f', 3));
    ui->current_setpoint_y_mpc->setText(QString::number(newSetpoint.y, 'f', 3));
    ui->current_setpoint_z_mpc->setText(QString::number(newSetpoint.z, 'f', 3));
    ui->current_setpoint_yaw_mpc->setText(QString::number(newSetpoint.yaw * RAD2DEG, 'f', 1));
366
367
}

368
369
void MainWindow::flyingStateChangedCallback(const std_msgs::Int32& msg)
{
370
	// PUT THE CURRENT STATE INTO THE CLASS VARIABLE
371
	m_flying_state_mutex.lock();
372
	m_flying_state = msg.data;
373
	m_flying_state_mutex.unlock();
374
375

	// UPDATE THE LABEL TO DISPLAY THE FLYING STATE
376
    //QString qstr = "Flying State: ";
377
378
379
    switch(msg.data)
    {
        case STATE_MOTORS_OFF:
380
381
382
        {
            //qstr.append("Motors OFF");
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
383
            ui->flying_state_label->clear();
384
385
386
            QPixmap flying_state_off_pixmap(":/images/flying_state_off.png");
            ui->flying_state_label->setPixmap(flying_state_off_pixmap);
            ui->flying_state_label->setScaledContents(true);
387
            ui->flying_state_label->update();
388
            break;
389
390
        }

391
        case STATE_TAKE_OFF:
392
393
394
        {
            //qstr.append("Take OFF");
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
395
            ui->flying_state_label->clear();
396
397
398
            QPixmap flying_state_enabling_pixmap(":/images/flying_state_enabling.png");
            ui->flying_state_label->setPixmap(flying_state_enabling_pixmap);
            ui->flying_state_label->setScaledContents(true);
399
            ui->flying_state_label->update();
400
            break;
401
402
        }

403
        case STATE_FLYING:
404
405
406
        {
            //qstr.append("Flying");
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
407
            ui->flying_state_label->clear();
408
409
410
            QPixmap flying_state_flying_pixmap(":/images/flying_state_flying.png");
            ui->flying_state_label->setPixmap(flying_state_flying_pixmap);
            ui->flying_state_label->setScaledContents(true);
411
            ui->flying_state_label->update();
412
            break;
413
414
        }

415
        case STATE_LAND:
416
417
418
        {
            //qstr.append("Land");
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
419
            ui->flying_state_label->clear();
420
421
422
            QPixmap flying_state_disabling_pixmap(":/images/flying_state_disabling.png");
            ui->flying_state_label->setPixmap(flying_state_disabling_pixmap);
            ui->flying_state_label->setScaledContents(true);
423
            ui->flying_state_label->update();
424
            break;
425
426
        }

427
        default:
428
429
        {
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
430
            ui->flying_state_label->clear();
431
432
433
            QPixmap flying_state_unknown_pixmap(":/images/flying_state_unknown.png");
            ui->flying_state_label->setPixmap(flying_state_unknown_pixmap);
            ui->flying_state_label->setScaledContents(true);
434
            ui->flying_state_label->update();
435
            break;
436
        }
437
    }
438
    //ui->flying_state_label->setText(qstr);
439
440
}

441
442
void MainWindow::batteryStateChangedCallback(const std_msgs::Int32& msg)
{
443
444
445
446
447
    // switch case with unabling buttons motors off, take off, etc... when battery is low
    QString qstr = "";
    switch(msg.data)
    {
        case BATTERY_STATE_LOW:
448
        {
449
            // DISABLE THE TAKE OFF AND LAND BUTTONS
450
451
452
453
            ui->take_off_button->setEnabled(false);
            ui->land_button->setEnabled(false);
            // ui->groupBox_4->setEnabled(false);

454
			// SET THE CLASS VARIABLE FOR TRACKING THE BATTERY STATE
roangel's avatar
roangel committed
455
            m_battery_state = BATTERY_STATE_LOW;
456
            break;
457
458
        }

459
        case BATTERY_STATE_NORMAL:
460
        {
461
462
463
464
            // ui->groupBox_4->setEnabled(true);
            ui->take_off_button->setEnabled(true);
            ui->land_button->setEnabled(true);

465
            // SET THE CLASS VARIABLE FOR TRACKING THE BATTERY STATE
roangel's avatar
roangel committed
466
            m_battery_state = BATTERY_STATE_NORMAL;
467
            break;
468
469
        }

470
471
472
        default:
            break;
    }
473
474
475
}


476
477
478
479
480
481
void MainWindow::setCrazyRadioStatus(int radio_status)
{
    // add more things whenever the status is changed
    switch(radio_status)
    {
        case CONNECTED:
482
483
        {
            // SET THE APPROPRIATE IMAGE FOR THE RADIOSTATUS LABEL
484
485
            rf_status_label_mutex.lock();
            //ui->rf_status_label->clear();
486
487
488
            QPixmap rf_connected_pixmap(":/images/rf_connected.png");
            ui->rf_status_label->setPixmap(rf_connected_pixmap);
            ui->rf_status_label->setScaledContents(true);
489
490
            //ui->rf_status_label->update();
            rf_status_label_mutex.unlock();
491
            // ENABLE THE REMAINDER OF THE GUI
492
            enableGUI();
493
            break;
494
495
        }

496
        case CONNECTING:
497
498
        {
            // SET THE APPROPRIATE IMAGE FOR THE RADIO STATUS LABEL
499
500
            rf_status_label_mutex.lock();
            //ui->rf_status_label->clear();
501
502
503
            QPixmap rf_connecting_pixmap(":/images/rf_connecting.png");
            ui->rf_status_label->setPixmap(rf_connecting_pixmap);
            ui->rf_status_label->setScaledContents(true);
504
505
            //ui->rf_status_label->update();
            rf_status_label_mutex.unlock();
506
            break;
507
508
        }

509
        case DISCONNECTED:
510
511
        {
            // SET THE APPROPRIATE IMAGE FOR THE RADIO STATUS LABEL
512
513
            rf_status_label_mutex.lock();
            //ui->rf_status_label->clear();
514
515
516
            QPixmap rf_disconnected_pixmap(":/images/rf_disconnected.png");
            ui->rf_status_label->setPixmap(rf_disconnected_pixmap);
            ui->rf_status_label->setScaledContents(true);
517
518
            //ui->rf_status_label->update();
            rf_status_label_mutex.unlock();
519
520
            // SET THE BATTERY VOLTAGE FIELD TO BE BLANK
            QString qstr = "-.-- V";
521
            voltage_field_mutex.lock();
522
            ui->voltage_field->setText(qstr);
523
            voltage_field_mutex.unlock();
524
            // SET THE APPROPRIATE IMAGE FOR THE BATTERY STATUS LABEL
525
            battery_status_label_mutex.lock();
526
527
            if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_UNKNOWN)
			{
528
				ui->battery_status_label->clear();
529
530
531
532
	            QPixmap battery_unknown_pixmap(":/images/battery_unknown.png");
	            ui->battery_status_label->setPixmap(battery_unknown_pixmap);
	            ui->battery_status_label->setScaledContents(true);
	            m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
533
	            ui->battery_status_label->update();
534
	        }
535
            battery_status_label_mutex.unlock();
536
            // DISABLE THE REMAINDER OF THE GUI
537
            disableGUI();
538
            break;
539
540
        }

541
        default:
542
        {
543
544
    		ROS_ERROR_STREAM("unexpected radio status: " << m_radio_status);
            break;
545
        }
546
547
548
549
    }
    this->m_radio_status = radio_status;
}

550
551
552



553
554
float MainWindow::fromVoltageToPercent(float voltage)
{
555
556
557
558
559
560
	// INITIALISE THE LOCAL VARIABLE FOR THE VOLTAGE WHEN FULL/EMPTY
	float voltage_when_full;
	float voltage_when_empty;

	// COMPUTE THE PERCENTAGE DIFFERENTLY DEPENDING ON
	// THE CURRENT FLYING STATE
561
	m_flying_state_mutex.lock();
562
563
564
565
566
567
568
569
570
571
	if (m_flying_state == STATE_MOTORS_OFF)
	{
		voltage_when_empty = battery_voltage_empty_while_motors_off;
		voltage_when_full  = battery_voltage_full_while_motors_off;
	}
	else
	{
		voltage_when_empty = battery_voltage_empty_while_flying;
		voltage_when_full  = battery_voltage_full_while_flying;
	}
572
	m_flying_state_mutex.unlock();
573
574
	//voltage_when_empty = battery_voltage_empty_while_motors_off;
	//voltage_when_full  = battery_voltage_full_while_motors_off;
575

576
	// COMPUTE THE PERCENTAGE
577
	float percentage = 100.0f * (voltage-voltage_when_empty)/(voltage_when_full-voltage_when_empty);
578

579

580
581
	// CLIP THE PERCENTAGE TO BE BETWEEN [0,100]
    // > This should not happen to often
582
    if(percentage > 100.0f)
583
    {
584
        percentage = 100.0f;
585
    }
586
    if(percentage < 0.0f)
587
    {
588
        percentage = 0.0f;
589
    }
590

591
592
593
594
595
    return percentage;
}

void MainWindow::updateBatteryVoltage(float battery_voltage)
{
596
	// PUT THE VOLTAGE INTO THE CLASS VARIABLES
597
    m_battery_voltage = battery_voltage;
598

599
    // UPDATE THE BATTERY VOLTAGE FIELD
600
    voltage_field_mutex.lock();
roangel's avatar
roangel committed
601
    QString qstr = "";
roangel's avatar
roangel committed
602
    qstr.append(QString::number(battery_voltage, 'f', 2));
roangel's avatar
roangel committed
603
604
    qstr.append(" V");
    ui->voltage_field->setText(qstr);
605
    voltage_field_mutex.unlock();
606
607

	// COMPUTE THE BATTERY VOLTAGE AS A PERCENTAGE
608
609
	float battery_voltage_percentage = fromVoltageToPercent(battery_voltage);

610
	//ROS_INFO_STREAM("Battery percentage = " << battery_voltage_percentage );
611
612

	// UPDATE THE IMAGE DISPLAYED IN THE BATTERY VOLTAGE LABEL IMAGE
613
	battery_status_label_mutex.lock();
614
615
616
617
	switch(m_battery_state)
	{
		// WHEN THE BATTERY IS IN A LOW STATE
		case BATTERY_STATE_LOW:
618
        {
619
			// SET THE IMAGE FOR THE BATTERY STATUS LABEL
620
621
			if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_EMPTY)
			{
622
				ui->battery_status_label->clear();
623
624
625
626
				QPixmap battery_empty_pixmap(":/images/battery_empty.png");
				ui->battery_status_label->setPixmap(battery_empty_pixmap);
				ui->battery_status_label->setScaledContents(true);
				m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_EMPTY;
627
				ui->battery_status_label->update();
628
			}
629
			break;
630
        }
631
632
633

		// WHEN THE BATTERY IS IN A NORMAL STATE
		case BATTERY_STATE_NORMAL:
634
        {
635
636
637
638
639
640

			if (
				((m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_EMPTY) && (battery_voltage_percentage <= 0.0f))
				||
				((m_battery_label_image_current_index == BATTERY_LABEL_IMAGE_INDEX_EMPTY) && (battery_voltage_percentage <= 2.0f))
			)
641
			{
642
643
644
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_EMPTY)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
645
					ui->battery_status_label->clear();
646
647
648
649
					QPixmap battery_empty_pixmap(":/images/battery_empty.png");
					ui->battery_status_label->setPixmap(battery_empty_pixmap);
					ui->battery_status_label->setScaledContents(true);
					m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_EMPTY;
650
					ui->battery_status_label->update();
651
				}
652
			}
653
654
655
656
657
			else if (
				((m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_20) && (battery_voltage_percentage <= 20.0f))
				||
				((m_battery_label_image_current_index == BATTERY_LABEL_IMAGE_INDEX_20) && (battery_voltage_percentage <= 22.0f))
			)
658
			{
659
660
661
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_20)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
662
					ui->battery_status_label->clear();
663
664
665
666
					QPixmap battery_20_pixmap(":/images/battery_20.png");
					ui->battery_status_label->setPixmap(battery_20_pixmap);
					ui->battery_status_label->setScaledContents(true);
					m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_20;
667
					ui->battery_status_label->update();
668
				}
669
			}
670
671
672
673
674
			else if (
				((m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_40) && (battery_voltage_percentage <= 40.0f))
				||
				((m_battery_label_image_current_index == BATTERY_LABEL_IMAGE_INDEX_40) && (battery_voltage_percentage <= 42.0f))
			)
675
			{
676
677
678
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_40)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
679
					ui->battery_status_label->clear();
680
681
682
683
					QPixmap battery_40_pixmap(":/images/battery_40.png");
					ui->battery_status_label->setPixmap(battery_40_pixmap);
					ui->battery_status_label->setScaledContents(true);
					m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_40;
684
					ui->battery_status_label->update();
685
				}
686
			}
687
688
689
690
691
			else if (
				((m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_60) && (battery_voltage_percentage <= 60.0f))
				||
				((m_battery_label_image_current_index == BATTERY_LABEL_IMAGE_INDEX_60) && (battery_voltage_percentage <= 62.0f))
			)
692
			{
693
694
695
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_60)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
696
					ui->battery_status_label->clear();
697
698
699
700
					QPixmap battery_60_pixmap(":/images/battery_60.png");
					ui->battery_status_label->setPixmap(battery_60_pixmap);
					ui->battery_status_label->setScaledContents(true);
					m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_60;
701
					ui->battery_status_label->update();
702
				}
703
			}
704
705
706
707
708
			else if (
				((m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_80) && (battery_voltage_percentage <= 80.0f))
				||
				((m_battery_label_image_current_index == BATTERY_LABEL_IMAGE_INDEX_80) && (battery_voltage_percentage <= 82.0f))
			)
709
			{
710
711
712
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_80)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
713
					ui->battery_status_label->clear();
714
715
716
717
					QPixmap battery_80_pixmap(":/images/battery_80.png");
					ui->battery_status_label->setPixmap(battery_80_pixmap);
					ui->battery_status_label->setScaledContents(true);
					m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_80;
718
					ui->battery_status_label->update();
719
				}
720
721
722
			}
			else
			{
723
724
725
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_FULL)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
726
					ui->battery_status_label->clear();
727
728
729
730
					QPixmap battery_full_pixmap(":/images/battery_full.png");
					ui->battery_status_label->setPixmap(battery_full_pixmap);
					ui->battery_status_label->setScaledContents(true);
					m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_FULL;
731
					ui->battery_status_label->update();
732
				}
733
734
			}
			break;
735
        }
736
737

		default:
738
        {
739
740
741
        	if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_UNKNOWN)
			{
	            // SET THE IMAGE FOR THE BATTERY STATUS LABEL
742
	            ui->battery_status_label->clear();
743
744
745
746
	            QPixmap battery_unknown_pixmap(":/images/battery_unknown.png");
	            ui->battery_status_label->setPixmap(battery_unknown_pixmap);
	            ui->battery_status_label->setScaledContents(true);
	            m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
747
				ui->battery_status_label->update();
748
	        }
749
			break;
750
        }
751
	}
752
	battery_status_label_mutex.unlock();
753
754
755
756
757
758
759
}

void MainWindow::CFBatteryCallback(const std_msgs::Float32& msg)
{
    updateBatteryVoltage(msg.data);
}

760
761
void MainWindow::crazyRadioStatusCallback(const std_msgs::Int32& msg)
{
762
    ROS_INFO("[Student GUI] Callback CrazyRadioStatus called");
763
764
765
    this->setCrazyRadioStatus(msg.data);
}

766
767
768
769
770
771
772
773
void MainWindow::loadCrazyflieContext()
{
	CMQuery contextCall;
	contextCall.request.studentID = m_student_id;
	ROS_INFO_STREAM("StudentID:" << m_student_id);

	centralManager.waitForExistence(ros::Duration(-1));

774
775
	if(centralManager.call(contextCall))
    {
776
777
		m_context = contextCall.response.crazyflieContext;
		ROS_INFO_STREAM("CrazyflieContext:\n" << m_context);
778
779
780
781
782
783
784
        // we now have the m_context variable with the current context. Put CF Name in label

        QString qstr = "StudentID ";
        qstr.append(QString::number(m_student_id));
        qstr.append(" connected to CF ");
        qstr.append(QString::fromStdString(m_context.crazyflieName));
        ui->groupBox->setTitle(qstr);
785
786
787
	}
    else
    {
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
		ROS_ERROR("Failed to load context");
	}

	ros::NodeHandle nh("CrazyRadio");
	nh.setParam("crazyFlieAddress", m_context.crazyflieAddress);
}

void MainWindow::coordinatesToLocal(CrazyflieData& cf)
{
	AreaBounds area = m_context.localArea;
	float originX = (area.xmin + area.xmax) / 2.0;
	float originY = (area.ymin + area.ymax) / 2.0;
    // change Z origin to zero, i.e., to the table height, zero of global coordinates, instead of middle of the box
    float originZ = 0.0;
	// float originZ = (area.zmin + area.zmax) / 2.0;

	cf.x -= originX;
	cf.y -= originY;
	cf.z -= originZ;
}


810
811
void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to newViconData, from node
{
812
813
814
    for(std::vector<CrazyflieData>::const_iterator it = p_msg->crazyflies.begin(); it != p_msg->crazyflies.end(); ++it)
    {
		CrazyflieData global = *it;
815
816
817
818
819
820
        if(global.crazyflieName == m_context.crazyflieName)
        {
            CrazyflieData local = global;
            coordinatesToLocal(local);

            // now we have the local coordinates, put them in the labels
821
822
823
824
825
826
827
828
829
830
831
832
833
            ui->current_x_safe->setText(QString::number(local.x, 'f', 3));
            ui->current_y_safe->setText(QString::number(local.y, 'f', 3));
            ui->current_z_safe->setText(QString::number(local.z, 'f', 3));
            ui->current_yaw_safe->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
            ui->current_pitch_safe->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
            ui->current_roll_safe->setText(QString::number(local.roll * RAD2DEG, 'f', 1));

            ui->current_x_demo->setText(QString::number(local.x, 'f', 3));
            ui->current_y_demo->setText(QString::number(local.y, 'f', 3));
            ui->current_z_demo->setText(QString::number(local.z, 'f', 3));
            ui->current_yaw_demo->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
            ui->current_pitch_demo->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
            ui->current_roll_demo->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
834

835
            // also update diff
836
837
838
839
840
841
842
843
844
            ui->diff_x_safe->setText(QString::number(m_safe_setpoint.x - local.x, 'f', 3));
            ui->diff_y_safe->setText(QString::number(m_safe_setpoint.y - local.y, 'f', 3));
            ui->diff_z_safe->setText(QString::number(m_safe_setpoint.z - local.z, 'f', 3));
            ui->diff_yaw_safe->setText(QString::number((m_safe_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));

            ui->diff_x_demo->setText(QString::number(m_demo_setpoint.x - local.x, 'f', 3));
            ui->diff_y_demo->setText(QString::number(m_demo_setpoint.y - local.y, 'f', 3));
            ui->diff_z_demo->setText(QString::number(m_demo_setpoint.z - local.z, 'f', 3));
            ui->diff_yaw_demo->setText(QString::number((m_demo_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
845
        }
846
    }
847
}
848

849
850
851
852


//    ----------------------------------------------------------------------------------
// # RF Crazyradio Connect Disconnect
853
854
855
856
void MainWindow::on_RF_Connect_button_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_RECONNECT;
857
    this->crazyRadioCommandPublisher.publish(msg);
roangel's avatar
roangel committed
858
    ROS_INFO("command reconnect published");
859
}
860

861
862
863
864
865
866
867
868
869
870
871
872
void MainWindow::on_RF_disconnect_button_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_DISCONNECT;
    this->crazyRadioCommandPublisher.publish(msg);
    ROS_INFO("command disconnect published");
}



//    ----------------------------------------------------------------------------------
// # Take off, lanf, motors off
873
void MainWindow::on_take_off_button_clicked()
874
{
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
    std_msgs::Int32 msg;
    msg.data = CMD_CRAZYFLY_TAKE_OFF;
    this->PPSClientCommandPublisher.publish(msg);
}

void MainWindow::on_land_button_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_CRAZYFLY_LAND;
    this->PPSClientCommandPublisher.publish(msg);
}

void MainWindow::on_motors_OFF_button_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_CRAZYFLY_MOTORS_OFF;
    this->PPSClientCommandPublisher.publish(msg);
892
}
893

894
895
896
897


//    ----------------------------------------------------------------------------------
// # Setpoint
898
void MainWindow::on_set_setpoint_button_safe_clicked()
899
900
{
    Setpoint msg_setpoint;
901
902
903

    // initialize setpoint to previous one

904
905
906
907
    msg_setpoint.x = (ui->current_setpoint_x_safe->text()).toFloat();
    msg_setpoint.y = (ui->current_setpoint_y_safe->text()).toFloat();
    msg_setpoint.z = (ui->current_setpoint_z_safe->text()).toFloat();
    msg_setpoint.yaw = (ui->current_setpoint_yaw_safe->text()).toFloat();
908

909
910
    if(!ui->new_setpoint_x_safe->text().isEmpty())
        msg_setpoint.x = (ui->new_setpoint_x_safe->text()).toFloat();
911