MainWindow.cpp 71.2 KB
Newer Older
1
//    Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero
roangel's avatar
roangel committed
2
//
3
//    This file is part of D-FaLL-System.
pragash1's avatar
pragash1 committed
4
//
5
//    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.
pragash1's avatar
pragash1 committed
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.
pragash1's avatar
pragash1 committed
14
//
roangel's avatar
roangel committed
15
//    You should have received a copy of the GNU General Public License
16
//    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
pragash1's avatar
pragash1 committed
17
//
18
19
20
21
22
23
24
25
26
27
28
29
30
31
//
//    ----------------------------------------------------------------------------------
//    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

pragash1's avatar
pragash1 committed
112
113
114
115
116
117
118



    //TODO add Publisher and Subscriber for DroneX



119
120
121
122
123
124
125
    // 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 );

126

127

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

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

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

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

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

139

140
    safeSetpointSubscriber = nodeHandle.subscribe("SafeControllerService/Setpoint", 1, &MainWindow::safeSetpointCallback, this);
141
    DBChangedSubscriber = nodeHandle.subscribe("/my_GUI/DBChanged", 1, &MainWindow::DBChangedCallback, this);
142

143
    ros::NodeHandle my_nodeHandle("~");
144
    controllerSetpointPublisher = my_nodeHandle.advertise<Setpoint>("ControllerSetpoint", 1);
145

146

147
    // communication with PPS Client, just to make it possible to communicate through terminal also we use PPSClient's name
148
149
    //ros::NodeHandle nh_PPSClient(m_ros_namespace + "/PPSClient");
    ros::NodeHandle nh_PPSClient("PPSClient");
150
    crazyRadioCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
pragash1's avatar
pragash1 committed
151
    PPSClientCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("Command", 1);
152

153

154
    // > For publishing a message that requests the
155
156
157
158
159
    //   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);

160
161
162

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

    // First get student ID
166
    if(!nh_PPSClient.getParam("agentID", m_student_id))
167
    {
168
		ROS_ERROR("Failed to get agentID");
169
170
	}

171
172
173
174
175
176
    // Then, Central manager
    centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
    loadCrazyflieContext();



177

178
179
180
181
    // 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");
182

183
    if(!nodeHandle_for_safeController.getParam("defaultSetpoint", default_setpoint))
roangel's avatar
roangel committed
184
    {
185
        ROS_ERROR_STREAM("The StudentGUI could not find parameter 'defaultSetpoint', as called from main(...)");
roangel's avatar
roangel committed
186
187
    }

188
    // Copy the default setpoint into respective text fields of the GUI
189
190
191
192
    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
193

194

195
    disableGUI();
196
    highlightSafeControllerTab();
197
198

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

201
202
203
    // SET THE BATTERY VOLTAGE FIELD TO BE BLANK
    QString qstr = "-.-- V";
    ui->voltage_field->setText(qstr);
204
    // SET THE IMAGE FOR THE BATTERY STATUS LABEL
205
206
207
    QPixmap battery_unknown_pixmap(":/images/battery_unknown.png");
    ui->battery_status_label->setPixmap(battery_unknown_pixmap);
    ui->battery_status_label->setScaledContents(true);
208
    m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
209

210
    // SET THE IMAGE FOR THE CRAZY RADIO STATUS LABEL
211
212
213
214
    QPixmap rf_disconnected_pixmap(":/images/rf_disconnected.png");
    ui->rf_status_label->setPixmap(rf_disconnected_pixmap);
    ui->rf_status_label->setScaledContents(true);

215
216
217
218
219
220
    // 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);


221
222
223
224
225
226

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


227
228
229
    ui->error_label->setStyleSheet("QLabel { color : red; }");
    ui->error_label->clear();

230
231
232
233
    // 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
234
    m_close_GUI_shortcut = new QShortcut(QKeySequence(tr("CTRL+C")), this, SLOT(close()));
235
236


237
238
239
    initialize_demo_setpoint();
    initialize_student_setpoint();
    initialize_mpc_setpoint();
240
    initialize_picker_setpoint();
pragash1's avatar
pragash1 committed
241
242
    //TODO add initialize_dronex_setpoint();

roangel's avatar
roangel committed
243
244
}

245

roangel's avatar
roangel committed
246
247
248
249
MainWindow::~MainWindow()
{
    delete ui;
}
250

251
252
void MainWindow::disableGUI()
{
253
254
255
    ui->motors_OFF_button->setEnabled(false);
    ui->take_off_button->setEnabled(false);
    ui->land_button->setEnabled(false);
256
257
258
259
}

void MainWindow::enableGUI()
{
260
    ui->motors_OFF_button->setEnabled(true);
roangel's avatar
roangel committed
261
262
    if(m_battery_state == BATTERY_STATE_NORMAL)
    {
263
264
        ui->take_off_button->setEnabled(true);
        ui->land_button->setEnabled(true);
roangel's avatar
roangel committed
265
    }
266
267
}

268
269
270
271
void MainWindow::highlightSafeControllerTab()
{
    ui->tabWidget->tabBar()->setTabTextColor(0, Qt::green);
    ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
272
273
    ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
274
    ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
275
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
276
    ui->tabWidget->tabBar()->setTabTextColor(6, Qt::black);
pragash1's avatar
pragash1 committed
277
    ui->tabWidget->tabBar()->setTabTextColor(7, Qt::black);
278
}
279
void MainWindow::highlightPickerControllerTab()
280
281
282
{
    ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(1, Qt::green);
283
284
    ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
    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);
pragash1's avatar
pragash1 committed
288
    ui->tabWidget->tabBar()->setTabTextColor(7, Qt::black);
289
}
290
void MainWindow::highlightDemoControllerTab()
291
292
293
294
295
{
    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);
296
    ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
297
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
298
    ui->tabWidget->tabBar()->setTabTextColor(6, Qt::black);
pragash1's avatar
pragash1 committed
299
    ui->tabWidget->tabBar()->setTabTextColor(7, Qt::black);
300
}
301
void MainWindow::highlightStudentControllerTab()
302
303
304
305
306
{
    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);
307
    ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
308
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
309
    ui->tabWidget->tabBar()->setTabTextColor(6, Qt::black);
pragash1's avatar
pragash1 committed
310
    ui->tabWidget->tabBar()->setTabTextColor(7, Qt::black);
311
}
312
void MainWindow::highlightMpcControllerTab()
313
314
315
316
317
318
{
    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);
319
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
320
    ui->tabWidget->tabBar()->setTabTextColor(6, Qt::black);
pragash1's avatar
pragash1 committed
321
    ui->tabWidget->tabBar()->setTabTextColor(7, Qt::black);
322
}
323
void MainWindow::highlightRemoteControllerTab()
324
325
326
327
328
329
330
{
    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);
331
    ui->tabWidget->tabBar()->setTabTextColor(6, Qt::black);
pragash1's avatar
pragash1 committed
332
    ui->tabWidget->tabBar()->setTabTextColor(7, Qt::black);
333
}
334
335
336
337
338
339
340
341
342
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);
pragash1's avatar
pragash1 committed
343
344
345
346
347
348
349
350
351
352
353
354
    ui->tabWidget->tabBar()->setTabTextColor(7, Qt::black);
}
void MainWindow::highlightDroneXControllerTab()
{
    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::black);
    ui->tabWidget->tabBar()->setTabTextColor(7, Qt::green);
355
}
356

357
358
359
void MainWindow::DBChangedCallback(const std_msgs::Int32& msg)
{
    loadCrazyflieContext();
360
    ROS_INFO("context reloaded in student_GUI");
361
362
}

363
364
365
366
367
368
369
void MainWindow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
{
    switch(msg.data)
    {
        case SAFE_CONTROLLER:
            highlightSafeControllerTab();
            break;
370
        case DEMO_CONTROLLER:
371
372
373
374
375
376
377
            highlightDemoControllerTab();
            break;
        case STUDENT_CONTROLLER:
            highlightStudentControllerTab();
            break;
        case MPC_CONTROLLER:
            highlightMpcControllerTab();
378
            break;
379
380
        case REMOTE_CONTROLLER:
            highlightRemoteControllerTab();
381
            break;
382
383
        case TUNING_CONTROLLER:
            highlightTuningControllerTab();
384
            break;
385
386
387
        case PICKER_CONTROLLER:
            highlightPickerControllerTab();
            break;
pragash1's avatar
pragash1 committed
388
389
        case DRONEX_CONTROLLER:
            highlightDroneXControllerTab();
390
391
392
393
394
        default:
            break;
    }
}

395
void MainWindow::safeSetpointCallback(const Setpoint& newSetpoint)
396
{
397
    m_safe_setpoint = newSetpoint;
398
    // here we get the new setpoint, need to update it in GUI
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
    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));
423
424
}

425
void MainWindow::mpcSetpointCallback(const Setpoint& newSetpoint)
426
{
427
    m_mpc_setpoint = newSetpoint;
428
    // here we get the new setpoint, need to update it in GUI
429
430
431
432
    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));
433
434
}

435
436
437
438
void MainWindow::pickerSetpointCallback(const Setpoint& newSetpoint)
{
    m_picker_setpoint = newSetpoint;
    // here we get the new setpoint, need to update it in GUI
439
    ui->picker_z_slider->setValue( int(newSetpoint.z*100.0) );
440
    ui->picker_yaw_dial->setValue( int(newSetpoint.yaw * RAD2DEG) );
pragash1's avatar
pragash1 committed
441

442
443
}

444
445
void MainWindow::flyingStateChangedCallback(const std_msgs::Int32& msg)
{
446
	// PUT THE CURRENT STATE INTO THE CLASS VARIABLE
447
	m_flying_state_mutex.lock();
448
	m_flying_state = msg.data;
449
	m_flying_state_mutex.unlock();
450
451

	// UPDATE THE LABEL TO DISPLAY THE FLYING STATE
452
    //QString qstr = "Flying State: ";
453
454
455
    switch(msg.data)
    {
        case STATE_MOTORS_OFF:
456
457
458
        {
            //qstr.append("Motors OFF");
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
459
            ui->flying_state_label->clear();
460
461
462
            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);
463
            ui->flying_state_label->update();
464
            break;
465
466
        }

467
        case STATE_TAKE_OFF:
468
469
470
        {
            //qstr.append("Take OFF");
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
471
            ui->flying_state_label->clear();
472
473
474
            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);
475
            ui->flying_state_label->update();
476
            break;
477
478
        }

479
        case STATE_FLYING:
480
481
482
        {
            //qstr.append("Flying");
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
483
            ui->flying_state_label->clear();
484
485
486
            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);
487
            ui->flying_state_label->update();
488
            break;
489
490
        }

491
        case STATE_LAND:
492
493
494
        {
            //qstr.append("Land");
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
495
            ui->flying_state_label->clear();
496
497
498
            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);
499
            ui->flying_state_label->update();
500
            break;
501
502
        }

503
        default:
504
505
        {
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
506
            ui->flying_state_label->clear();
507
508
509
            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);
510
            ui->flying_state_label->update();
511
            break;
512
        }
513
    }
514
    //ui->flying_state_label->setText(qstr);
515
516
}

517
518
void MainWindow::batteryStateChangedCallback(const std_msgs::Int32& msg)
{
519
520
521
522
523
    // switch case with unabling buttons motors off, take off, etc... when battery is low
    QString qstr = "";
    switch(msg.data)
    {
        case BATTERY_STATE_LOW:
524
        {
525
            // DISABLE THE TAKE OFF AND LAND BUTTONS
526
527
528
529
            ui->take_off_button->setEnabled(false);
            ui->land_button->setEnabled(false);
            // ui->groupBox_4->setEnabled(false);

530
			// SET THE CLASS VARIABLE FOR TRACKING THE BATTERY STATE
roangel's avatar
roangel committed
531
            m_battery_state = BATTERY_STATE_LOW;
532
            break;
533
534
        }

535
        case BATTERY_STATE_NORMAL:
536
        {
537
538
539
540
            // ui->groupBox_4->setEnabled(true);
            ui->take_off_button->setEnabled(true);
            ui->land_button->setEnabled(true);

541
            // SET THE CLASS VARIABLE FOR TRACKING THE BATTERY STATE
roangel's avatar
roangel committed
542
            m_battery_state = BATTERY_STATE_NORMAL;
543
            break;
544
545
        }

546
547
548
        default:
            break;
    }
549
550
551
}


552
553
554
555
556
557
void MainWindow::setCrazyRadioStatus(int radio_status)
{
    // add more things whenever the status is changed
    switch(radio_status)
    {
        case CONNECTED:
558
559
        {
            // SET THE APPROPRIATE IMAGE FOR THE RADIOSTATUS LABEL
560
561
            rf_status_label_mutex.lock();
            //ui->rf_status_label->clear();
562
563
564
            QPixmap rf_connected_pixmap(":/images/rf_connected.png");
            ui->rf_status_label->setPixmap(rf_connected_pixmap);
            ui->rf_status_label->setScaledContents(true);
565
566
            //ui->rf_status_label->update();
            rf_status_label_mutex.unlock();
567
            // ENABLE THE REMAINDER OF THE GUI
568
            enableGUI();
569
            break;
570
571
        }

572
        case CONNECTING:
573
574
        {
            // SET THE APPROPRIATE IMAGE FOR THE RADIO STATUS LABEL
575
576
            rf_status_label_mutex.lock();
            //ui->rf_status_label->clear();
577
578
579
            QPixmap rf_connecting_pixmap(":/images/rf_connecting.png");
            ui->rf_status_label->setPixmap(rf_connecting_pixmap);
            ui->rf_status_label->setScaledContents(true);
580
581
            //ui->rf_status_label->update();
            rf_status_label_mutex.unlock();
582
            break;
583
584
        }

585
        case DISCONNECTED:
586
587
        {
            // SET THE APPROPRIATE IMAGE FOR THE RADIO STATUS LABEL
588
589
            rf_status_label_mutex.lock();
            //ui->rf_status_label->clear();
590
591
592
            QPixmap rf_disconnected_pixmap(":/images/rf_disconnected.png");
            ui->rf_status_label->setPixmap(rf_disconnected_pixmap);
            ui->rf_status_label->setScaledContents(true);
593
594
            //ui->rf_status_label->update();
            rf_status_label_mutex.unlock();
595
596
            // SET THE BATTERY VOLTAGE FIELD TO BE BLANK
            QString qstr = "-.-- V";
597
            voltage_field_mutex.lock();
598
            ui->voltage_field->setText(qstr);
599
            voltage_field_mutex.unlock();
600
            // SET THE APPROPRIATE IMAGE FOR THE BATTERY STATUS LABEL
601
            battery_status_label_mutex.lock();
602
603
            if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_UNKNOWN)
			{
604
				ui->battery_status_label->clear();
605
606
607
608
	            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;
609
	            ui->battery_status_label->update();
610
	        }
611
            battery_status_label_mutex.unlock();
612
            // DISABLE THE REMAINDER OF THE GUI
613
            disableGUI();
614
            break;
615
616
        }

617
        default:
618
        {
619
620
    		ROS_ERROR_STREAM("unexpected radio status: " << m_radio_status);
            break;
621
        }
622
623
624
625
    }
    this->m_radio_status = radio_status;
}

626
627
628



629
630
float MainWindow::fromVoltageToPercent(float voltage)
{
631
632
633
634
635
636
	// 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
637
	m_flying_state_mutex.lock();
638
639
640
641
642
643
644
645
646
647
	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;
	}
648
	m_flying_state_mutex.unlock();
649
650
	//voltage_when_empty = battery_voltage_empty_while_motors_off;
	//voltage_when_full  = battery_voltage_full_while_motors_off;
651

652
	// COMPUTE THE PERCENTAGE
653
	float percentage = 100.0f * (voltage-voltage_when_empty)/(voltage_when_full-voltage_when_empty);
654

655

656
657
	// CLIP THE PERCENTAGE TO BE BETWEEN [0,100]
    // > This should not happen to often
658
    if(percentage > 100.0f)
659
    {
660
        percentage = 100.0f;
661
    }
662
    if(percentage < 0.0f)
663
    {
664
        percentage = 0.0f;
665
    }
666

667
668
669
670
671
    return percentage;
}

void MainWindow::updateBatteryVoltage(float battery_voltage)
{
672
	// PUT THE VOLTAGE INTO THE CLASS VARIABLES
673
    m_battery_voltage = battery_voltage;
674

675
    // UPDATE THE BATTERY VOLTAGE FIELD
676
    voltage_field_mutex.lock();
roangel's avatar
roangel committed
677
    QString qstr = "";
roangel's avatar
roangel committed
678
    qstr.append(QString::number(battery_voltage, 'f', 2));
roangel's avatar
roangel committed
679
680
    qstr.append(" V");
    ui->voltage_field->setText(qstr);
681
    voltage_field_mutex.unlock();
682
683

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

686
	//ROS_INFO_STREAM("Battery percentage = " << battery_voltage_percentage );
687
688

	// UPDATE THE IMAGE DISPLAYED IN THE BATTERY VOLTAGE LABEL IMAGE
689
	battery_status_label_mutex.lock();
690
691
692
693
	switch(m_battery_state)
	{
		// WHEN THE BATTERY IS IN A LOW STATE
		case BATTERY_STATE_LOW:
694
        {
695
			// SET THE IMAGE FOR THE BATTERY STATUS LABEL
696
697
			if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_EMPTY)
			{
698
				ui->battery_status_label->clear();
699
700
701
702
				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;
703
				ui->battery_status_label->update();
704
			}
705
			break;
706
        }
707
708
709

		// WHEN THE BATTERY IS IN A NORMAL STATE
		case BATTERY_STATE_NORMAL:
710
        {
711
712
713
714
715
716

			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))
			)
717
			{
718
719
720
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_EMPTY)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
721
					ui->battery_status_label->clear();
722
723
724
725
					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;
726
					ui->battery_status_label->update();
727
				}
728
			}
729
730
731
732
733
			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))
			)
734
			{
735
736
737
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_20)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
738
					ui->battery_status_label->clear();
739
740
741
742
					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;
743
					ui->battery_status_label->update();
744
				}
745
			}
746
747
748
749
750
			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))
			)
751
			{
752
753
754
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_40)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
755
					ui->battery_status_label->clear();
756
757
758
759
					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;
760
					ui->battery_status_label->update();
761
				}
762
			}
763
764
765
766
767
			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))
			)
768
			{
769
770
771
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_60)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
772
					ui->battery_status_label->clear();
773
774
775
776
					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;
777
					ui->battery_status_label->update();
778
				}
779
			}
780
781
782
783
784
			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))
			)
785
			{
786
787
788
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_80)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
789
					ui->battery_status_label->clear();
790
791
792
793
					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;
794
					ui->battery_status_label->update();
795
				}
796
797
798
			}
			else
			{
799
800
801
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_FULL)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
802
					ui->battery_status_label->clear();
803
804
805
806
					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;
807
					ui->battery_status_label->update();
808
				}
809
810
			}
			break;
811
        }
812
813

		default:
814
        {
815
816
817
        	if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_UNKNOWN)
			{
	            // SET THE IMAGE FOR THE BATTERY STATUS LABEL
818
	            ui->battery_status_label->clear();
819
820
821
822
	            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;
823
				ui->battery_status_label->update();
824
	        }
825
			break;
826
        }
827
	}
828
	battery_status_label_mutex.unlock();
829
830
831
832
833
834
835
}

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

836
837
void MainWindow::crazyRadioStatusCallback(const std_msgs::Int32& msg)
{
838
    ROS_INFO("[Student GUI] Callback CrazyRadioStatus called");
839
840
841
    this->setCrazyRadioStatus(msg.data);
}

842
843
844
845
846
847
848
849
void MainWindow::loadCrazyflieContext()
{
	CMQuery contextCall;
	contextCall.request.studentID = m_student_id;
	ROS_INFO_STREAM("StudentID:" << m_student_id);

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