MainWindow.cpp 61.2 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
            ui->rf_status_label->clear();
485
486
487
            QPixmap rf_connected_pixmap(":/images/rf_connected.png");
            ui->rf_status_label->setPixmap(rf_connected_pixmap);
            ui->rf_status_label->setScaledContents(true);
488
            ui->rf_status_label->update();
489
            // ENABLE THE REMAINDER OF THE GUI
490
            enableGUI();
491
            break;
492
493
        }

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

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

535
        default:
536
        {
537
538
    		ROS_ERROR_STREAM("unexpected radio status: " << m_radio_status);
            break;
539
        }
540
541
542
543
    }
    this->m_radio_status = radio_status;
}

544
545
546



547
548
float MainWindow::fromVoltageToPercent(float voltage)
{
549
550
551
552
553
554
	// 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
555
	m_flying_state_mutex.lock();
556
557
558
559
560
561
562
563
564
565
	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;
	}
566
	m_flying_state_mutex.unlock();
567
568
	//voltage_when_empty = battery_voltage_empty_while_motors_off;
	//voltage_when_full  = battery_voltage_full_while_motors_off;
569

570
	// COMPUTE THE PERCENTAGE
571
	float percentage = 100.0f * (voltage-voltage_when_empty)/(voltage_when_full-voltage_when_empty);
572

573

574
575
	// CLIP THE PERCENTAGE TO BE BETWEEN [0,100]
    // > This should not happen to often
576
    if(percentage > 100.0f)
577
    {
578
        percentage = 100.0f;
579
    }
580
    if(percentage < 0.0f)
581
    {
582
        percentage = 0.0f;
583
    }
584

585
586
587
588
589
    return percentage;
}

void MainWindow::updateBatteryVoltage(float battery_voltage)
{
590
	// PUT THE VOLTAGE INTO THE CLASS VARIABLES
591
    m_battery_voltage = battery_voltage;
592

593
    // UPDATE THE BATTERY VOLTAGE FIELD
594
    voltage_field_mutex.lock();
roangel's avatar
roangel committed
595
    QString qstr = "";
roangel's avatar
roangel committed
596
    qstr.append(QString::number(battery_voltage, 'f', 2));
roangel's avatar
roangel committed
597
598
    qstr.append(" V");
    ui->voltage_field->setText(qstr);
599
    voltage_field_mutex.unlock();
600
601

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

604
	//ROS_INFO_STREAM("Battery percentage = " << battery_voltage_percentage );
605
606

	// UPDATE THE IMAGE DISPLAYED IN THE BATTERY VOLTAGE LABEL IMAGE
607
	battery_status_label_mutex.lock();
608
609
610
611
	switch(m_battery_state)
	{
		// WHEN THE BATTERY IS IN A LOW STATE
		case BATTERY_STATE_LOW:
612
        {
613
			// SET THE IMAGE FOR THE BATTERY STATUS LABEL
614
615
			if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_EMPTY)
			{
616
				ui->battery_status_label->clear();
617
618
619
620
				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;
621
				ui->battery_status_label->update();
622
			}
623
			break;
624
        }
625
626
627

		// WHEN THE BATTERY IS IN A NORMAL STATE
		case BATTERY_STATE_NORMAL:
628
        {
629
630
631
632
633
634

			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))
			)
635
			{
636
637
638
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_EMPTY)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
639
					ui->battery_status_label->clear();
640
641
642
643
					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;
644
					ui->battery_status_label->update();
645
				}
646
			}
647
648
649
650
651
			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))
			)
652
			{
653
654
655
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_20)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
656
					ui->battery_status_label->clear();
657
658
659
660
					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;
661
					ui->battery_status_label->update();
662
				}
663
			}
664
665
666
667
668
			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))
			)
669
			{
670
671
672
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_40)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
673
					ui->battery_status_label->clear();
674
675
676
677
					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;
678
					ui->battery_status_label->update();
679
				}
680
			}
681
682
683
684
685
			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))
			)
686
			{
687
688
689
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_60)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
690
					ui->battery_status_label->clear();
691
692
693
694
					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;
695
					ui->battery_status_label->update();
696
				}
697
			}
698
699
700
701
702
			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))
			)
703
			{
704
705
706
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_80)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
707
					ui->battery_status_label->clear();
708
709
710
711
					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;
712
					ui->battery_status_label->update();
713
				}
714
715
716
			}
			else
			{
717
718
719
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_FULL)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
720
					ui->battery_status_label->clear();
721
722
723
724
					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;
725
					ui->battery_status_label->update();
726
				}
727
728
			}
			break;
729
        }
730
731

		default:
732
        {
733
734
735
        	if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_UNKNOWN)
			{
	            // SET THE IMAGE FOR THE BATTERY STATUS LABEL
736
	            ui->battery_status_label->clear();
737
738
739
740
	            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;
741
				ui->battery_status_label->update();
742
	        }
743
			break;
744
        }
745
	}
746
	battery_status_label_mutex.unlock();
747
748
749
750
751
752
753
}

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

754
755
void MainWindow::crazyRadioStatusCallback(const std_msgs::Int32& msg)
{
756
    ROS_INFO("[Student GUI] Callback CrazyRadioStatus called");
757
758
759
    this->setCrazyRadioStatus(msg.data);
}

760
761
762
763
764
765
766
767
void MainWindow::loadCrazyflieContext()
{
	CMQuery contextCall;
	contextCall.request.studentID = m_student_id;
	ROS_INFO_STREAM("StudentID:" << m_student_id);

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

768
769
	if(centralManager.call(contextCall))
    {
770
771
		m_context = contextCall.response.crazyflieContext;
		ROS_INFO_STREAM("CrazyflieContext:\n" << m_context);
772
773
774
775
776
777
778
        // 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);
779
780
781
	}
    else
    {
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
		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;
}


804
805
void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to newViconData, from node
{
806
807
808
    for(std::vector<CrazyflieData>::const_iterator it = p_msg->crazyflies.begin(); it != p_msg->crazyflies.end(); ++it)
    {
		CrazyflieData global = *it;
809
810
811
812
813
814
        if(global.crazyflieName == m_context.crazyflieName)
        {
            CrazyflieData local = global;
            coordinatesToLocal(local);

            // now we have the local coordinates, put them in the labels
815
816
817
818
819
820
821
822
823
824
825
826
827
            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));
828

829
            // also update diff
830
831
832
833
834
835
836
837
838
            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));
839
        }
840
    }
841
}
842

843
844
845
846


//    ----------------------------------------------------------------------------------
// # RF Crazyradio Connect Disconnect
847
848
849
850
void MainWindow::on_RF_Connect_button_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_RECONNECT;
851
    this->crazyRadioCommandPublisher.publish(msg);
roangel's avatar
roangel committed
852
    ROS_INFO("command reconnect published");
853
}
854

855
856
857
858
859
860
861
862
863
864
865
866
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
867
void MainWindow::on_take_off_button_clicked()
868
{
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
    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);
886
}
887

888
889
890
891


//    ----------------------------------------------------------------------------------
// # Setpoint
892
void MainWindow::on_set_setpoint_button_safe_clicked()
893
894
{
    Setpoint msg_setpoint;
895
896
897

    // initialize setpoint to previous one

898
899
900
901
    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();
902

903
904
    if(!ui->new_setpoint_x_safe->text().isEmpty())
        msg_setpoint.x = (ui->new_setpoint_x_safe->text()).toFloat();
905

906
907
    if(!ui->new_setpoint_y_safe->text().isEmpty())
        msg_setpoint.y = (ui->new_setpoint_y_safe->text()).toFloat();
908