MainWindow.cpp 69.8 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>
roangel's avatar
roangel committed
36

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

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

#include "d_fall_pps/ViconData.h"

45
46
#include "d_fall_pps/CustomButton.h"

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

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

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

58
59
    setCrazyRadioStatus(DISCONNECTED);

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

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

66
    ros::NodeHandle nodeHandle(m_ros_namespace);
67

68

69
    // SUBSCRIBERS AND PUBLISHERS:
70
71
72
73
74
75
76
77
    // > 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);
78
    // > For the MPC Controller SETPOINTS
79
80
    mpcSetpointPublisher  = nodeHandle.advertise<Setpoint>("MpcControllerService/Setpoint", 1);
    mpcSetpointSubscriber = nodeHandle.subscribe("MpcControllerService/Setpoint", 1, &MainWindow::mpcSetpointCallback, this);
81

82
83
84
85
86
87
88
89
90
91
92

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


93
94
95
96
97
98
99
100
    // > 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);


101
102
103
104
105
106
107
    // > For the PICKER CONTROLLER
    pickerButtonPressedPublisher  =  nodeHandle.advertise<std_msgs::Int32>("PickerControllerService/ButtonPressed", 1);
    pickerZSetpointPublisher      =  nodeHandle.advertise<std_msgs::Float32>("PickerControllerService/ZSetpoint", 1);
    pickerYawSetpointPublisher    =  nodeHandle.advertise<std_msgs::Float32>("PickerControllerService/YawSetpoint", 1);
    pickerMassPublisher           =  nodeHandle.advertise<std_msgs::Float32>("PickerControllerService/Mass", 1);
    pickerXAdjustmentPublisher    =  nodeHandle.advertise<std_msgs::Float32>("PickerControllerService/XAdjustment", 1);
    pickerYAdjustmentPublisher    =  nodeHandle.advertise<std_msgs::Float32>("PickerControllerService/YAdjustment", 1);
108
    pickerSetpointPublisher       =  nodeHandle.advertise<Setpoint>("PickerControllerService/Setpoint", 1);
109
    pickerSetpointSubscriber      =  nodeHandle.subscribe("PickerControllerService/Setpoint", 1, &MainWindow::pickerSetpointCallback, this);
110
    pickerSetpointToGUISubscriber =  nodeHandle.subscribe("PickerControllerService/SetpointToGUI", 1, &MainWindow::pickerSetpointCallback, this);
111

112
113
114
115
116
117
118
    // SET ALL SLIDERS AND DIALS TO DEFAULT VALUES
    ui->picker_z_slider->setValue( 40 );
    ui->picker_mass_slider->setValue( 29 );
    ui->picker_yaw_dial->setValue( 0 );
    ui->picker_x_slider->setValue( 0 );
    ui->picker_y_slider->setValue( 0 );

119

120

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

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

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

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

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

132

133
    safeSetpointSubscriber = nodeHandle.subscribe("SafeControllerService/Setpoint", 1, &MainWindow::safeSetpointCallback, this);
134
    DBChangedSubscriber = nodeHandle.subscribe("/my_GUI/DBChanged", 1, &MainWindow::DBChangedCallback, this);
135

136
    ros::NodeHandle my_nodeHandle("~");
137
    controllerSetpointPublisher = my_nodeHandle.advertise<Setpoint>("ControllerSetpoint", 1);
138

139

140
    // communication with PPS Client, just to make it possible to communicate through terminal also we use PPSClient's name
141
142
    //ros::NodeHandle nh_PPSClient(m_ros_namespace + "/PPSClient");
    ros::NodeHandle nh_PPSClient("PPSClient");
143
    crazyRadioCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
144
    PPSClientCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("Command", 1);    
145

146

147
    // > For publishing a message that requests the
148
149
150
151
152
    //   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);

153
154
155

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

    // First get student ID
159
    if(!nh_PPSClient.getParam("agentID", m_student_id))
160
    {
161
		ROS_ERROR("Failed to get agentID");
162
163
	}

164
165
166
167
168
169
    // Then, Central manager
    centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
    loadCrazyflieContext();



170

171
172
173
174
    // 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");
175

176
    if(!nodeHandle_for_safeController.getParam("defaultSetpoint", default_setpoint))
roangel's avatar
roangel committed
177
    {
178
        ROS_ERROR_STREAM("The StudentGUI could not find parameter 'defaultSetpoint', as called from main(...)");
roangel's avatar
roangel committed
179
180
    }

181
    // Copy the default setpoint into respective text fields of the GUI
182
183
184
185
    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
186

187

188
    disableGUI();
189
    highlightSafeControllerTab();
190
191

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

194
195
196
    // SET THE BATTERY VOLTAGE FIELD TO BE BLANK
    QString qstr = "-.-- V";
    ui->voltage_field->setText(qstr);
197
    // SET THE IMAGE FOR THE BATTERY STATUS LABEL
198
199
200
    QPixmap battery_unknown_pixmap(":/images/battery_unknown.png");
    ui->battery_status_label->setPixmap(battery_unknown_pixmap);
    ui->battery_status_label->setScaledContents(true);
201
    m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
202

203
    // SET THE IMAGE FOR THE CRAZY RADIO STATUS LABEL
204
205
206
207
    QPixmap rf_disconnected_pixmap(":/images/rf_disconnected.png");
    ui->rf_status_label->setPixmap(rf_disconnected_pixmap);
    ui->rf_status_label->setScaledContents(true);

208
209
210
211
212
213
    // 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);


214
215
216
217
218
219

    //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, ...)


220
221
222
    ui->error_label->setStyleSheet("QLabel { color : red; }");
    ui->error_label->clear();

223
224
225
226
    // 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
227
    m_close_GUI_shortcut = new QShortcut(QKeySequence(tr("CTRL+C")), this, SLOT(close()));
228
229


230
231
232
    initialize_demo_setpoint();
    initialize_student_setpoint();
    initialize_mpc_setpoint();
233
    initialize_picker_setpoint();
roangel's avatar
roangel committed
234
235
}

236

roangel's avatar
roangel committed
237
238
239
240
MainWindow::~MainWindow()
{
    delete ui;
}
241

242
243
void MainWindow::disableGUI()
{
244
245
246
    ui->motors_OFF_button->setEnabled(false);
    ui->take_off_button->setEnabled(false);
    ui->land_button->setEnabled(false);
247
248
249
250
}

void MainWindow::enableGUI()
{
251
    ui->motors_OFF_button->setEnabled(true);
roangel's avatar
roangel committed
252
253
    if(m_battery_state == BATTERY_STATE_NORMAL)
    {
254
255
        ui->take_off_button->setEnabled(true);
        ui->land_button->setEnabled(true);
roangel's avatar
roangel committed
256
    }
257
258
}

259
260
261
262
void MainWindow::highlightSafeControllerTab()
{
    ui->tabWidget->tabBar()->setTabTextColor(0, Qt::green);
    ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
263
264
    ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
265
    ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
266
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
267
    ui->tabWidget->tabBar()->setTabTextColor(6, Qt::black);
268
}
269
void MainWindow::highlightPickerControllerTab()
270
271
272
{
    ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(1, Qt::green);
273
274
    ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
275
    ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
276
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
277
    ui->tabWidget->tabBar()->setTabTextColor(6, Qt::black);
278
}
279
void MainWindow::highlightDemoControllerTab()
280
281
282
283
284
{
    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);
285
    ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
286
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
287
    ui->tabWidget->tabBar()->setTabTextColor(6, Qt::black);
288
}
289
void MainWindow::highlightStudentControllerTab()
290
291
292
293
294
{
    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);
295
    ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
296
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
297
    ui->tabWidget->tabBar()->setTabTextColor(6, Qt::black);
298
}
299
void MainWindow::highlightMpcControllerTab()
300
301
302
303
304
305
{
    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);
306
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
307
    ui->tabWidget->tabBar()->setTabTextColor(6, Qt::black);
308
}
309
void MainWindow::highlightRemoteControllerTab()
310
311
312
313
314
315
316
{
    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);
317
    ui->tabWidget->tabBar()->setTabTextColor(6, Qt::black);
318
}
319
320
321
322
323
324
325
326
327
328
329
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::black);
    ui->tabWidget->tabBar()->setTabTextColor(6, Qt::green);
}

330

331
332
333
void MainWindow::DBChangedCallback(const std_msgs::Int32& msg)
{
    loadCrazyflieContext();
334
    ROS_INFO("context reloaded in student_GUI");
335
336
}

337
338
339
340
341
342
343
void MainWindow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
{
    switch(msg.data)
    {
        case SAFE_CONTROLLER:
            highlightSafeControllerTab();
            break;
344
        case DEMO_CONTROLLER:
345
346
347
348
349
350
351
            highlightDemoControllerTab();
            break;
        case STUDENT_CONTROLLER:
            highlightStudentControllerTab();
            break;
        case MPC_CONTROLLER:
            highlightMpcControllerTab();
352
            break;
353
354
        case REMOTE_CONTROLLER:
            highlightRemoteControllerTab();
355
            break;
356
357
        case TUNING_CONTROLLER:
            highlightTuningControllerTab();
358
            break;
359
360
361
        case PICKER_CONTROLLER:
            highlightPickerControllerTab();
            break;
362
363
364
365
366
        default:
            break;
    }
}

367
void MainWindow::safeSetpointCallback(const Setpoint& newSetpoint)
368
{
369
    m_safe_setpoint = newSetpoint;
370
    // here we get the new setpoint, need to update it in GUI
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
    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));
395
396
}

397
void MainWindow::mpcSetpointCallback(const Setpoint& newSetpoint)
398
{
399
    m_mpc_setpoint = newSetpoint;
400
    // here we get the new setpoint, need to update it in GUI
401
402
403
404
    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));
405
406
}

407
408
409
410
void MainWindow::pickerSetpointCallback(const Setpoint& newSetpoint)
{
    m_picker_setpoint = newSetpoint;
    // here we get the new setpoint, need to update it in GUI
411
    ui->picker_z_slider->setValue( int(newSetpoint.z*100.0) );
412
413
414
415
    ui->picker_yaw_dial->setValue( int(newSetpoint.yaw * RAD2DEG) );
    
}

416
417
void MainWindow::flyingStateChangedCallback(const std_msgs::Int32& msg)
{
418
	// PUT THE CURRENT STATE INTO THE CLASS VARIABLE
419
	m_flying_state_mutex.lock();
420
	m_flying_state = msg.data;
421
	m_flying_state_mutex.unlock();
422
423

	// UPDATE THE LABEL TO DISPLAY THE FLYING STATE
424
    //QString qstr = "Flying State: ";
425
426
427
    switch(msg.data)
    {
        case STATE_MOTORS_OFF:
428
429
430
        {
            //qstr.append("Motors OFF");
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
431
            ui->flying_state_label->clear();
432
433
434
            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);
435
            ui->flying_state_label->update();
436
            break;
437
438
        }

439
        case STATE_TAKE_OFF:
440
441
442
        {
            //qstr.append("Take OFF");
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
443
            ui->flying_state_label->clear();
444
445
446
            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);
447
            ui->flying_state_label->update();
448
            break;
449
450
        }

451
        case STATE_FLYING:
452
453
454
        {
            //qstr.append("Flying");
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
455
            ui->flying_state_label->clear();
456
457
458
            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);
459
            ui->flying_state_label->update();
460
            break;
461
462
        }

463
        case STATE_LAND:
464
465
466
        {
            //qstr.append("Land");
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
467
            ui->flying_state_label->clear();
468
469
470
            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);
471
            ui->flying_state_label->update();
472
            break;
473
474
        }

475
        default:
476
477
        {
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
478
            ui->flying_state_label->clear();
479
480
481
            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);
482
            ui->flying_state_label->update();
483
            break;
484
        }
485
    }
486
    //ui->flying_state_label->setText(qstr);
487
488
}

489
490
void MainWindow::batteryStateChangedCallback(const std_msgs::Int32& msg)
{
491
492
493
494
495
    // switch case with unabling buttons motors off, take off, etc... when battery is low
    QString qstr = "";
    switch(msg.data)
    {
        case BATTERY_STATE_LOW:
496
        {
497
            // DISABLE THE TAKE OFF AND LAND BUTTONS
498
499
500
501
            ui->take_off_button->setEnabled(false);
            ui->land_button->setEnabled(false);
            // ui->groupBox_4->setEnabled(false);

502
			// SET THE CLASS VARIABLE FOR TRACKING THE BATTERY STATE
roangel's avatar
roangel committed
503
            m_battery_state = BATTERY_STATE_LOW;
504
            break;
505
506
        }

507
        case BATTERY_STATE_NORMAL:
508
        {
509
510
511
512
            // ui->groupBox_4->setEnabled(true);
            ui->take_off_button->setEnabled(true);
            ui->land_button->setEnabled(true);

513
            // SET THE CLASS VARIABLE FOR TRACKING THE BATTERY STATE
roangel's avatar
roangel committed
514
            m_battery_state = BATTERY_STATE_NORMAL;
515
            break;
516
517
        }

518
519
520
        default:
            break;
    }
521
522
523
}


524
525
526
527
528
529
void MainWindow::setCrazyRadioStatus(int radio_status)
{
    // add more things whenever the status is changed
    switch(radio_status)
    {
        case CONNECTED:
530
531
        {
            // SET THE APPROPRIATE IMAGE FOR THE RADIOSTATUS LABEL
532
533
            rf_status_label_mutex.lock();
            //ui->rf_status_label->clear();
534
535
536
            QPixmap rf_connected_pixmap(":/images/rf_connected.png");
            ui->rf_status_label->setPixmap(rf_connected_pixmap);
            ui->rf_status_label->setScaledContents(true);
537
538
            //ui->rf_status_label->update();
            rf_status_label_mutex.unlock();
539
            // ENABLE THE REMAINDER OF THE GUI
540
            enableGUI();
541
            break;
542
543
        }

544
        case CONNECTING:
545
546
        {
            // SET THE APPROPRIATE IMAGE FOR THE RADIO STATUS LABEL
547
548
            rf_status_label_mutex.lock();
            //ui->rf_status_label->clear();
549
550
551
            QPixmap rf_connecting_pixmap(":/images/rf_connecting.png");
            ui->rf_status_label->setPixmap(rf_connecting_pixmap);
            ui->rf_status_label->setScaledContents(true);
552
553
            //ui->rf_status_label->update();
            rf_status_label_mutex.unlock();
554
            break;
555
556
        }

557
        case DISCONNECTED:
558
559
        {
            // SET THE APPROPRIATE IMAGE FOR THE RADIO STATUS LABEL
560
561
            rf_status_label_mutex.lock();
            //ui->rf_status_label->clear();
562
563
564
            QPixmap rf_disconnected_pixmap(":/images/rf_disconnected.png");
            ui->rf_status_label->setPixmap(rf_disconnected_pixmap);
            ui->rf_status_label->setScaledContents(true);
565
566
            //ui->rf_status_label->update();
            rf_status_label_mutex.unlock();
567
568
            // SET THE BATTERY VOLTAGE FIELD TO BE BLANK
            QString qstr = "-.-- V";
569
            voltage_field_mutex.lock();
570
            ui->voltage_field->setText(qstr);
571
            voltage_field_mutex.unlock();
572
            // SET THE APPROPRIATE IMAGE FOR THE BATTERY STATUS LABEL
573
            battery_status_label_mutex.lock();
574
575
            if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_UNKNOWN)
			{
576
				ui->battery_status_label->clear();
577
578
579
580
	            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;
581
	            ui->battery_status_label->update();
582
	        }
583
            battery_status_label_mutex.unlock();
584
            // DISABLE THE REMAINDER OF THE GUI
585
            disableGUI();
586
            break;
587
588
        }

589
        default:
590
        {
591
592
    		ROS_ERROR_STREAM("unexpected radio status: " << m_radio_status);
            break;
593
        }
594
595
596
597
    }
    this->m_radio_status = radio_status;
}

598
599
600



601
602
float MainWindow::fromVoltageToPercent(float voltage)
{
603
604
605
606
607
608
	// 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
609
	m_flying_state_mutex.lock();
610
611
612
613
614
615
616
617
618
619
	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;
	}
620
	m_flying_state_mutex.unlock();
621
622
	//voltage_when_empty = battery_voltage_empty_while_motors_off;
	//voltage_when_full  = battery_voltage_full_while_motors_off;
623

624
	// COMPUTE THE PERCENTAGE
625
	float percentage = 100.0f * (voltage-voltage_when_empty)/(voltage_when_full-voltage_when_empty);
626

627

628
629
	// CLIP THE PERCENTAGE TO BE BETWEEN [0,100]
    // > This should not happen to often
630
    if(percentage > 100.0f)
631
    {
632
        percentage = 100.0f;
633
    }
634
    if(percentage < 0.0f)
635
    {
636
        percentage = 0.0f;
637
    }
638

639
640
641
642
643
    return percentage;
}

void MainWindow::updateBatteryVoltage(float battery_voltage)
{
644
	// PUT THE VOLTAGE INTO THE CLASS VARIABLES
645
    m_battery_voltage = battery_voltage;
646

647
    // UPDATE THE BATTERY VOLTAGE FIELD
648
    voltage_field_mutex.lock();
roangel's avatar
roangel committed
649
    QString qstr = "";
roangel's avatar
roangel committed
650
    qstr.append(QString::number(battery_voltage, 'f', 2));
roangel's avatar
roangel committed
651
652
    qstr.append(" V");
    ui->voltage_field->setText(qstr);
653
    voltage_field_mutex.unlock();
654
655

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

658
	//ROS_INFO_STREAM("Battery percentage = " << battery_voltage_percentage );
659
660

	// UPDATE THE IMAGE DISPLAYED IN THE BATTERY VOLTAGE LABEL IMAGE
661
	battery_status_label_mutex.lock();
662
663
664
665
	switch(m_battery_state)
	{
		// WHEN THE BATTERY IS IN A LOW STATE
		case BATTERY_STATE_LOW:
666
        {
667
			// SET THE IMAGE FOR THE BATTERY STATUS LABEL
668
669
			if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_EMPTY)
			{
670
				ui->battery_status_label->clear();
671
672
673
674
				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;
675
				ui->battery_status_label->update();
676
			}
677
			break;
678
        }
679
680
681

		// WHEN THE BATTERY IS IN A NORMAL STATE
		case BATTERY_STATE_NORMAL:
682
        {
683
684
685
686
687
688

			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))
			)
689
			{
690
691
692
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_EMPTY)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
693
					ui->battery_status_label->clear();
694
695
696
697
					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;
698
					ui->battery_status_label->update();
699
				}
700
			}
701
702
703
704
705
			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))
			)
706
			{
707
708
709
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_20)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
710
					ui->battery_status_label->clear();
711
712
713
714
					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;
715
					ui->battery_status_label->update();
716
				}
717
			}
718
719
720
721
722
			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))
			)
723
			{
724
725
726
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_40)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
727
					ui->battery_status_label->clear();
728
729
730
731
					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;
732
					ui->battery_status_label->update();
733
				}
734
			}
735
736
737
738
739
			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))
			)
740
			{
741
742
743
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_60)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
744
					ui->battery_status_label->clear();
745
746
747
748
					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;
749
					ui->battery_status_label->update();
750
				}
751
			}
752
753
754
755
756
			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))
			)
757
			{
758
759
760
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_80)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
761
					ui->battery_status_label->clear();
762
763
764
765
					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;
766
					ui->battery_status_label->update();
767
				}
768
769
770
			}
			else
			{
771
772
773
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_FULL)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
774
					ui->battery_status_label->clear();
775
776
777
778
					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;
779
					ui->battery_status_label->update();
780
				}
781
782
			}
			break;
783
        }
784
785

		default:
786
        {
787
788
789
        	if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_UNKNOWN)
			{
	            // SET THE IMAGE FOR THE BATTERY STATUS LABEL
790
	            ui->battery_status_label->clear();
791
792
793
794
	            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;
795
				ui->battery_status_label->update();
796
	        }
797
			break;
798
        }
799
	}
800
	battery_status_label_mutex.unlock();
801
802
803
804
805
806
807
}

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

808
809
void MainWindow::crazyRadioStatusCallback(const std_msgs::Int32& msg)
{
810
    ROS_INFO("[Student GUI] Callback CrazyRadioStatus called");
811
812
813
    this->setCrazyRadioStatus(msg.data);
}

814
815
816
817
818
819
820
821
void MainWindow::loadCrazyflieContext()
{
	CMQuery contextCall;
	contextCall.request.studentID = m_student_id;
	ROS_INFO_STREAM("StudentID:" << m_student_id);

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

822
823
	if(centralManager.call(contextCall))
    {
824
825
		m_context = contextCall.response.crazyflieContext;
		ROS_INFO_STREAM("CrazyflieContext:\n" << m_context);
826
827
828
829
830
831
832
        // 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);
833
834
835
	}
    else
    {
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
		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;
}


858
859
void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to newViconData, from node
{
860
861
862
    for(std::vector<CrazyflieData>::const_iterator it = p_msg->crazyflies.begin(); it != p_msg->crazyflies.end(); ++it)
    {
		CrazyflieData global = *it;
863
864
865
866
867
868
        if(global.crazyflieName == m_context.crazyflieName)
        {
            CrazyflieData local = global;
            coordinatesToLocal(local);

            // now we have the local coordinates, put them in the labels
869
870
871
872
873
874
875
876
877
878
879
880
881
            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));