MainWindow.cpp 77.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



mastefan's avatar
mastefan committed
115
    // Publisher and Subscriber for DroneX
pragash1's avatar
pragash1 committed
116
117
118
119
    dronexButtonPressedPublisher = nodeHandle.advertise<std_msgs::Int32>("DroneXControllerService/ButtonPressed", 1);
    dronexSetpointPublisher       =  nodeHandle.advertise<Setpoint>("DroneXControllerService/Setpoint", 1);
    dronexSetpointSubscriber      =  nodeHandle.subscribe("DroneXControllerService/Setpoint", 1, &MainWindow::dronexSetpointCallback, this);
    dronexSetpointToGUISubscriber =  nodeHandle.subscribe("DroneXControllerService/SetpointToGUI", 1, &MainWindow::dronexSetpointCallback, this);
mastefan's avatar
mastefan committed
120
121
122
123
    droneXIntegrParamPublisher       =  nodeHandle.advertise<Setpoint>("DroneXControllerService/IntegrParams", 1);
    droneXWeightParamPublisher       =  nodeHandle.advertise<Setpoint>("DroneXControllerService/WeightParam", 1);
    droneXWeightParamPublisher       =  nodeHandle.advertise<Setpoint>("DroneXControllerService/WeightParam", 1);
    droneXPitchBaselineParamPublisher =  nodeHandle.advertise<Setpoint>("DroneXControllerService/PitchBaselineParam", 1);
maruggv's avatar
maruggv committed
124

125
126
127
128
129
130
131
    // 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 );

132

133

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

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

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

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

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

145

146
    safeSetpointSubscriber = nodeHandle.subscribe("SafeControllerService/Setpoint", 1, &MainWindow::safeSetpointCallback, this);
147
    DBChangedSubscriber = nodeHandle.subscribe("/my_GUI/DBChanged", 1, &MainWindow::DBChangedCallback, this);
148

149
    ros::NodeHandle my_nodeHandle("~");
150
    controllerSetpointPublisher = my_nodeHandle.advertise<Setpoint>("ControllerSetpoint", 1);
151

152

153
    // communication with PPS Client, just to make it possible to communicate through terminal also we use PPSClient's name
154
155
    //ros::NodeHandle nh_PPSClient(m_ros_namespace + "/PPSClient");
    ros::NodeHandle nh_PPSClient("PPSClient");
156
    crazyRadioCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
pragash1's avatar
pragash1 committed
157
    PPSClientCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("Command", 1);
158

159

160
    // > For publishing a message that requests the
161
162
163
164
165
    //   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);

166
167
168

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

    // First get student ID
172
    if(!nh_PPSClient.getParam("agentID", m_student_id))
173
    {
174
		ROS_ERROR("Failed to get agentID");
175
176
	}

177
178
179
180
181
182
    // Then, Central manager
    centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
    loadCrazyflieContext();



183

184
185
186
187
    // 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");
188

189
    if(!nodeHandle_for_safeController.getParam("defaultSetpoint", default_setpoint))
roangel's avatar
roangel committed
190
    {
191
        ROS_ERROR_STREAM("The StudentGUI could not find parameter 'defaultSetpoint', as called from main(...)");
roangel's avatar
roangel committed
192
193
    }

194
    // Copy the default setpoint into respective text fields of the GUI
195
196
197
198
    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
199

200

201
    disableGUI();
202
    highlightSafeControllerTab();
203
204

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

207
208
209
    // SET THE BATTERY VOLTAGE FIELD TO BE BLANK
    QString qstr = "-.-- V";
    ui->voltage_field->setText(qstr);
210
    // SET THE IMAGE FOR THE BATTERY STATUS LABEL
211
212
213
    QPixmap battery_unknown_pixmap(":/images/battery_unknown.png");
    ui->battery_status_label->setPixmap(battery_unknown_pixmap);
    ui->battery_status_label->setScaledContents(true);
214
    m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
215

216
    // SET THE IMAGE FOR THE CRAZY RADIO STATUS LABEL
217
218
219
220
    QPixmap rf_disconnected_pixmap(":/images/rf_disconnected.png");
    ui->rf_status_label->setPixmap(rf_disconnected_pixmap);
    ui->rf_status_label->setScaledContents(true);

221
222
223
224
225
226
    // 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);


227
228
229
230
231
232

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


233
234
235
    ui->error_label->setStyleSheet("QLabel { color : red; }");
    ui->error_label->clear();

236
237
238
239
    // 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
240
    m_close_GUI_shortcut = new QShortcut(QKeySequence(tr("CTRL+C")), this, SLOT(close()));
241
242


243
244
245
    initialize_demo_setpoint();
    initialize_student_setpoint();
    initialize_mpc_setpoint();
246
    initialize_picker_setpoint();
pragash1's avatar
pragash1 committed
247
248
    //TODO add initialize_dronex_setpoint();

roangel's avatar
roangel committed
249
250
}

251

roangel's avatar
roangel committed
252
253
254
255
MainWindow::~MainWindow()
{
    delete ui;
}
256

257
258
void MainWindow::disableGUI()
{
259
260
261
    ui->motors_OFF_button->setEnabled(false);
    ui->take_off_button->setEnabled(false);
    ui->land_button->setEnabled(false);
262
263
264
265
}

void MainWindow::enableGUI()
{
266
    ui->motors_OFF_button->setEnabled(true);
roangel's avatar
roangel committed
267
268
    if(m_battery_state == BATTERY_STATE_NORMAL)
    {
269
270
        ui->take_off_button->setEnabled(true);
        ui->land_button->setEnabled(true);
roangel's avatar
roangel committed
271
    }
272
273
}

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

363
364
365
void MainWindow::DBChangedCallback(const std_msgs::Int32& msg)
{
    loadCrazyflieContext();
366
    ROS_INFO("context reloaded in student_GUI");
367
368
}

369
370
371
372
373
374
375
void MainWindow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
{
    switch(msg.data)
    {
        case SAFE_CONTROLLER:
            highlightSafeControllerTab();
            break;
376
        case DEMO_CONTROLLER:
377
378
379
380
381
382
383
            highlightDemoControllerTab();
            break;
        case STUDENT_CONTROLLER:
            highlightStudentControllerTab();
            break;
        case MPC_CONTROLLER:
            highlightMpcControllerTab();
384
            break;
385
386
        case REMOTE_CONTROLLER:
            highlightRemoteControllerTab();
387
            break;
388
389
        case TUNING_CONTROLLER:
            highlightTuningControllerTab();
390
            break;
391
392
393
        case PICKER_CONTROLLER:
            highlightPickerControllerTab();
            break;
pragash1's avatar
pragash1 committed
394
395
        case DRONEX_CONTROLLER:
            highlightDroneXControllerTab();
pragash1's avatar
pragash1 committed
396
            break;
397
        default:
pragash1's avatar
pragash1 committed
398
          ROS_INFO_STREAM("controllerUsedChangedCallback-Controller Flag not recognised----------------" << msg.data);
399
400
401
402
            break;
    }
}

403
void MainWindow::safeSetpointCallback(const Setpoint& newSetpoint)
404
{
405
    m_safe_setpoint = newSetpoint;
406
    // here we get the new setpoint, need to update it in GUI
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
    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));
431
432
}

433
void MainWindow::mpcSetpointCallback(const Setpoint& newSetpoint)
434
{
435
    m_mpc_setpoint = newSetpoint;
436
    // here we get the new setpoint, need to update it in GUI
437
438
439
440
    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));
441
442
}

443
444
445
446
void MainWindow::pickerSetpointCallback(const Setpoint& newSetpoint)
{
    m_picker_setpoint = newSetpoint;
    // here we get the new setpoint, need to update it in GUI
447
    ui->picker_z_slider->setValue( int(newSetpoint.z*100.0) );
448
    ui->picker_yaw_dial->setValue( int(newSetpoint.yaw * RAD2DEG) );
pragash1's avatar
pragash1 committed
449

450
451
}

pragash1's avatar
pragash1 committed
452
453
454
455
456
457
458
459
460
461
void MainWindow::dronexSetpointCallback(const Setpoint& newSetpoint)
{
    m_picker_setpoint = newSetpoint;
    ROS_INFO("dronexSetpointCallback");
    // here we get the new setpoint, need to update it in GUI
    //ui->picker_z_slider->setValue( int(newSetpoint.z*100.0) );
    //ui->picker_yaw_dial->setValue( int(newSetpoint.yaw * RAD2DEG) );

}

462
463
void MainWindow::flyingStateChangedCallback(const std_msgs::Int32& msg)
{
464
	// PUT THE CURRENT STATE INTO THE CLASS VARIABLE
465
	m_flying_state_mutex.lock();
466
	m_flying_state = msg.data;
467
	m_flying_state_mutex.unlock();
468
469

	// UPDATE THE LABEL TO DISPLAY THE FLYING STATE
470
    //QString qstr = "Flying State: ";
471
472
473
    switch(msg.data)
    {
        case STATE_MOTORS_OFF:
474
475
476
        {
            //qstr.append("Motors OFF");
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
477
            ui->flying_state_label->clear();
478
479
480
            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);
481
            ui->flying_state_label->update();
482
            break;
483
484
        }

485
        case STATE_TAKE_OFF:
486
487
488
        {
            //qstr.append("Take OFF");
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
489
            ui->flying_state_label->clear();
490
491
492
            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);
493
            ui->flying_state_label->update();
494
            break;
495
496
        }

497
        case STATE_FLYING:
498
499
500
        {
            //qstr.append("Flying");
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
501
            ui->flying_state_label->clear();
502
503
504
            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);
505
            ui->flying_state_label->update();
506
            break;
507
508
        }

509
        case STATE_LAND:
510
511
512
        {
            //qstr.append("Land");
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
513
            ui->flying_state_label->clear();
514
515
516
            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);
517
            ui->flying_state_label->update();
518
            break;
519
520
        }

521
        default:
522
523
        {
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
524
            ui->flying_state_label->clear();
525
526
527
            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);
528
            ui->flying_state_label->update();
529
            break;
530
        }
531
    }
532
    //ui->flying_state_label->setText(qstr);
533
534
}

535
536
void MainWindow::batteryStateChangedCallback(const std_msgs::Int32& msg)
{
537
538
539
540
541
    // switch case with unabling buttons motors off, take off, etc... when battery is low
    QString qstr = "";
    switch(msg.data)
    {
        case BATTERY_STATE_LOW:
542
        {
543
            // DISABLE THE TAKE OFF AND LAND BUTTONS
544
545
546
547
            ui->take_off_button->setEnabled(false);
            ui->land_button->setEnabled(false);
            // ui->groupBox_4->setEnabled(false);

548
			// SET THE CLASS VARIABLE FOR TRACKING THE BATTERY STATE
roangel's avatar
roangel committed
549
            m_battery_state = BATTERY_STATE_LOW;
550
            break;
551
552
        }

553
        case BATTERY_STATE_NORMAL:
554
        {
555
556
557
558
            // ui->groupBox_4->setEnabled(true);
            ui->take_off_button->setEnabled(true);
            ui->land_button->setEnabled(true);

559
            // SET THE CLASS VARIABLE FOR TRACKING THE BATTERY STATE
roangel's avatar
roangel committed
560
            m_battery_state = BATTERY_STATE_NORMAL;
561
            break;
562
563
        }

564
565
566
        default:
            break;
    }
567
568
569
}


570
571
572
573
574
575
void MainWindow::setCrazyRadioStatus(int radio_status)
{
    // add more things whenever the status is changed
    switch(radio_status)
    {
        case CONNECTED:
576
577
        {
            // SET THE APPROPRIATE IMAGE FOR THE RADIOSTATUS LABEL
578
579
            rf_status_label_mutex.lock();
            //ui->rf_status_label->clear();
580
581
582
            QPixmap rf_connected_pixmap(":/images/rf_connected.png");
            ui->rf_status_label->setPixmap(rf_connected_pixmap);
            ui->rf_status_label->setScaledContents(true);
583
584
            //ui->rf_status_label->update();
            rf_status_label_mutex.unlock();
585
            // ENABLE THE REMAINDER OF THE GUI
586
            enableGUI();
587
            break;
588
589
        }

590
        case CONNECTING:
591
592
        {
            // SET THE APPROPRIATE IMAGE FOR THE RADIO STATUS LABEL
593
594
            rf_status_label_mutex.lock();
            //ui->rf_status_label->clear();
595
596
597
            QPixmap rf_connecting_pixmap(":/images/rf_connecting.png");
            ui->rf_status_label->setPixmap(rf_connecting_pixmap);
            ui->rf_status_label->setScaledContents(true);
598
599
            //ui->rf_status_label->update();
            rf_status_label_mutex.unlock();
600
            break;
601
602
        }

603
        case DISCONNECTED:
604
605
        {
            // SET THE APPROPRIATE IMAGE FOR THE RADIO STATUS LABEL
606
607
            rf_status_label_mutex.lock();
            //ui->rf_status_label->clear();
608
609
610
            QPixmap rf_disconnected_pixmap(":/images/rf_disconnected.png");
            ui->rf_status_label->setPixmap(rf_disconnected_pixmap);
            ui->rf_status_label->setScaledContents(true);
611
612
            //ui->rf_status_label->update();
            rf_status_label_mutex.unlock();
613
614
            // SET THE BATTERY VOLTAGE FIELD TO BE BLANK
            QString qstr = "-.-- V";
615
            voltage_field_mutex.lock();
616
            ui->voltage_field->setText(qstr);
617
            voltage_field_mutex.unlock();
618
            // SET THE APPROPRIATE IMAGE FOR THE BATTERY STATUS LABEL
619
            battery_status_label_mutex.lock();
620
621
            if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_UNKNOWN)
			{
622
				ui->battery_status_label->clear();
623
624
625
626
	            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;
627
	            ui->battery_status_label->update();
628
	        }
629
            battery_status_label_mutex.unlock();
630
            // DISABLE THE REMAINDER OF THE GUI
631
            disableGUI();
632
            break;
633
634
        }

635
        default:
636
        {
637
638
    		ROS_ERROR_STREAM("unexpected radio status: " << m_radio_status);
            break;
639
        }
640
641
642
643
    }
    this->m_radio_status = radio_status;
}

644
645
646



647
648
float MainWindow::fromVoltageToPercent(float voltage)
{
649
650
651
652
653
654
	// 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
655
	m_flying_state_mutex.lock();
656
657
658
659
660
661
662
663
664
665
	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;
	}
666
	m_flying_state_mutex.unlock();
667
668
	//voltage_when_empty = battery_voltage_empty_while_motors_off;
	//voltage_when_full  = battery_voltage_full_while_motors_off;
669

670
	// COMPUTE THE PERCENTAGE
671
	float percentage = 100.0f * (voltage-voltage_when_empty)/(voltage_when_full-voltage_when_empty);
672

673

674
675
	// CLIP THE PERCENTAGE TO BE BETWEEN [0,100]
    // > This should not happen to often
676
    if(percentage > 100.0f)
677
    {
678
        percentage = 100.0f;
679
    }
680
    if(percentage < 0.0f)
681
    {
682
        percentage = 0.0f;
683
    }
684

685
686
687
688
689
    return percentage;
}

void MainWindow::updateBatteryVoltage(float battery_voltage)
{
690
	// PUT THE VOLTAGE INTO THE CLASS VARIABLES
691
    m_battery_voltage = battery_voltage;
692

693
    // UPDATE THE BATTERY VOLTAGE FIELD
694
    voltage_field_mutex.lock();
roangel's avatar
roangel committed
695
    QString qstr = "";
roangel's avatar
roangel committed
696
    qstr.append(QString::number(battery_voltage, 'f', 2));
roangel's avatar
roangel committed
697
698
    qstr.append(" V");
    ui->voltage_field->setText(qstr);
699
    voltage_field_mutex.unlock();
700
701

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

704
	//ROS_INFO_STREAM("Battery percentage = " << battery_voltage_percentage );
705
706

	// UPDATE THE IMAGE DISPLAYED IN THE BATTERY VOLTAGE LABEL IMAGE
707
	battery_status_label_mutex.lock();
708
709
710
711
	switch(m_battery_state)
	{
		// WHEN THE BATTERY IS IN A LOW STATE
		case BATTERY_STATE_LOW:
712
        {
713
			// SET THE IMAGE FOR THE BATTERY STATUS LABEL
714
715
			if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_EMPTY)
			{
716
				ui->battery_status_label->clear();
717
718
719
720
				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;
721
				ui->battery_status_label->update();
722
			}
723
			break;
724
        }
725
726
727

		// WHEN THE BATTERY IS IN A NORMAL STATE
		case BATTERY_STATE_NORMAL:
728
        {
729
730
731
732
733
734

			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))
			)
735
			{
736
737
738
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_EMPTY)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
739
					ui->battery_status_label->clear();
740
741
742
743
					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;
744
					ui->battery_status_label->update();
745
				}
746
			}
747
748
749
750
751
			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))
			)
752
			{
753
754
755
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_20)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
756
					ui->battery_status_label->clear();
757
758
759
760
					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;
761
					ui->battery_status_label->update();
762
				}
763
			}
764
765
766
767
768
			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))
			)
769
			{
770
771
772
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_40)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
773
					ui->battery_status_label->clear();
774
775
776
777
					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;
778
					ui->battery_status_label->update();
779
				}
780
			}
781
782
783
784
785
			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))
			)
786
			{
787
788
789
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_60)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
790
					ui->battery_status_label->clear();
791
792
793
794
					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;
795
					ui->battery_status_label->update();
796
				}
797
			}
798
799
800
801
802
			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))
			)
803
			{
804
805
806
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_80)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
807
					ui->battery_status_label->clear();
808
809
810
811
					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;
812
					ui->battery_status_label->update();
813
				}
814
815
816
			}
			else
			{
817
818
819
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_FULL)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
820
					ui->battery_status_label->clear();
821
822
823
824
					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;
825
					ui->battery_status_label->update();
826
				}
827
828
			}
			break;
829
        }
830
831

		default:
832
        {
833
834
835
        	if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_UNKNOWN)
			{
	            // SET THE IMAGE FOR THE BATTERY STATUS LABEL