MainWindow.cpp 59.9 KB
Newer Older
1
//    Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero
roangel's avatar
roangel committed
2
//
3
4
5
//    This file is part of D-FaLL-System.
//    
//    D-FaLL-System is free software: you can redistribute it and/or modify
roangel's avatar
roangel committed
6
7
8
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
9
10
//    
//    D-FaLL-System is distributed in the hope that it will be useful,
roangel's avatar
roangel committed
11
12
13
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
14
//    
roangel's avatar
roangel committed
15
//    You should have received a copy of the GNU General Public License
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
//    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
//    
//
//    ----------------------------------------------------------------------------------
//    DDDD        FFFFF        L     L           SSSS  Y   Y   SSSS  TTTTT  EEEEE  M   M
//    D   D       F      aaa   L     L          S       Y Y   S        T    E      MM MM
//    D   D  ---  FFFF  a   a  L     L     ---   SSS     Y     SSS     T    EEE    M M M
//    D   D       F     a  aa  L     L              S    Y        S    T    E      M   M
//    DDDD        F      aa a  LLLL  LLLL       SSSS     Y    SSSS     T    EEEEE  M   M
//
//
//    DESCRIPTION:
//    Main window of the Student's GUI
//
//    ----------------------------------------------------------------------------------

roangel's avatar
roangel committed
32

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

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

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

#include "d_fall_pps/ViconData.h"

46
47
#include "d_fall_pps/CustomButton.h"

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

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

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

59
60
    setCrazyRadioStatus(DISCONNECTED);

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

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

67
    ros::NodeHandle nodeHandle(m_ros_namespace);
68

69

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

83
84
85
86
87
88
89
90
91
92
93

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


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



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

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

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

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

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

114

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

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

121

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

128

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

135
136
137

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

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

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



152

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

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

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

169

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

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

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

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

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


196
197
198
199
200
201

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


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

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


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

217

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

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

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

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

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

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

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

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

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

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

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

	// UPDATE THE LABEL TO DISPLAY THE FLYING STATE
374
    //QString qstr = "Flying State: ";
375
376
377
    switch(msg.data)
    {
        case STATE_MOTORS_OFF:
378
379
380
381
382
383
        {
            //qstr.append("Motors OFF");
            // SET THE APPROPRIATE 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);
384
            break;
385
386
        }

387
        case STATE_TAKE_OFF:
388
389
390
391
392
393
        {
            //qstr.append("Take OFF");
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
            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);
394
            break;
395
396
        }

397
        case STATE_FLYING:
398
399
400
401
402
403
        {
            //qstr.append("Flying");
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
            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);
404
            break;
405
406
        }

407
        case STATE_LAND:
408
409
410
411
412
413
        {
            //qstr.append("Land");
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
            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);
414
            break;
415
416
        }

417
        default:
418
419
420
421
422
        {
            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
            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);
423
            break;
424
        }
425
    }
426
    //ui->flying_state_label->setText(qstr);
427
428
}

429
430
void MainWindow::batteryStateChangedCallback(const std_msgs::Int32& msg)
{
431
432
433
434
435
    // switch case with unabling buttons motors off, take off, etc... when battery is low
    QString qstr = "";
    switch(msg.data)
    {
        case BATTERY_STATE_LOW:
436
        {
437
            // DISABLE THE TAKE OFF AND LAND BUTTONS
438
439
440
441
            ui->take_off_button->setEnabled(false);
            ui->land_button->setEnabled(false);
            // ui->groupBox_4->setEnabled(false);

442
			// SET THE CLASS VARIABLE FOR TRACKING THE BATTERY STATE
roangel's avatar
roangel committed
443
            m_battery_state = BATTERY_STATE_LOW;
444
            break;
445
446
        }

447
        case BATTERY_STATE_NORMAL:
448
        {
449
450
451
452
            // ui->groupBox_4->setEnabled(true);
            ui->take_off_button->setEnabled(true);
            ui->land_button->setEnabled(true);

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

458
459
460
        default:
            break;
    }
461
462
463
}


464
465
466
467
468
469
void MainWindow::setCrazyRadioStatus(int radio_status)
{
    // add more things whenever the status is changed
    switch(radio_status)
    {
        case CONNECTED:
470
471
472
473
474
475
        {
            // SET THE APPROPRIATE IMAGE FOR THE RADIOSTATUS LABEL
            QPixmap rf_connected_pixmap(":/images/rf_connected.png");
            ui->rf_status_label->setPixmap(rf_connected_pixmap);
            ui->rf_status_label->setScaledContents(true);
            // ENABLE THE REMAINDER OF THE GUI
476
            enableGUI();
477
            break;
478
479
        }

480
        case CONNECTING:
481
482
483
484
485
486
        {
            // SET THE APPROPRIATE IMAGE FOR THE RADIO STATUS LABEL
            QPixmap rf_connecting_pixmap(":/images/rf_connecting.png");
            ui->rf_status_label->setPixmap(rf_connecting_pixmap);
            ui->rf_status_label->setScaledContents(true);
            // SET THE BATTERY VOLTAGE FIELD TO BE BLANK
487
488
            // QString qstr = "-.-- V";
            // ui->voltage_field->setText(qstr);
489
            break;
490
491
        }

492
        case DISCONNECTED:
493
494
495
496
497
498
499
500
501
        {
            // SET THE APPROPRIATE IMAGE FOR THE RADIO STATUS LABEL
            QPixmap rf_disconnected_pixmap(":/images/rf_disconnected.png");
            ui->rf_status_label->setPixmap(rf_disconnected_pixmap);
            ui->rf_status_label->setScaledContents(true);
            // SET THE BATTERY VOLTAGE FIELD TO BE BLANK
            QString qstr = "-.-- V";
            ui->voltage_field->setText(qstr);
            // SET THE APPROPRIATE IMAGE FOR THE BATTERY STATUS LABEL
502
503
504
505
506
507
508
509
            if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_UNKNOWN)
			{
	            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;
	            //ui->battery_status_label->update();
	        }
510
            // DISABLE THE REMAINDER OF THE GUI
511
            disableGUI();
512
            break;
513
514
        }

515
        default:
516
        {
517
518
    		ROS_ERROR_STREAM("unexpected radio status: " << m_radio_status);
            break;
519
        }
520
521
522
523
    }
    this->m_radio_status = radio_status;
}

524
525
526



527
528
float MainWindow::fromVoltageToPercent(float voltage)
{
529
530
531
532
533
534
	// 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
535
536
537
538
539
540
541
542
543
544
545
546
	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;
	}
	//voltage_when_empty = battery_voltage_empty_while_motors_off;
	//voltage_when_full  = battery_voltage_full_while_motors_off;
547

548
	// COMPUTE THE PERCENTAGE
549
	float percentage = 100.0f * (voltage-voltage_when_empty)/(voltage_when_full-voltage_when_empty);
550

551

552
553
	// CLIP THE PERCENTAGE TO BE BETWEEN [0,100]
    // > This should not happen to often
554
    if(percentage > 100.0f)
555
    {
556
        percentage = 100.0f;
557
    }
558
    if(percentage < 0.0f)
559
    {
560
        percentage = 0.0f;
561
    }
562

563
564
565
566
567
    return percentage;
}

void MainWindow::updateBatteryVoltage(float battery_voltage)
{
568
	// PUT THE VOLTAGE INTO THE CLASS VARIABLES
569
    m_battery_voltage = battery_voltage;
570

571
    // UPDATE THE BATTERY VOLTAGE FIELD
roangel's avatar
roangel committed
572
    QString qstr = "";
roangel's avatar
roangel committed
573
    qstr.append(QString::number(battery_voltage, 'f', 2));
roangel's avatar
roangel committed
574
575
    qstr.append(" V");
    ui->voltage_field->setText(qstr);
576
577

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

580
	//ROS_INFO_STREAM("Battery percentage = " << battery_voltage_percentage );
581
582
583
584
585
586

	// UPDATE THE IMAGE DISPLAYED IN THE BATTERY VOLTAGE LABEL IMAGE
	switch(m_battery_state)
	{
		// WHEN THE BATTERY IS IN A LOW STATE
		case BATTERY_STATE_LOW:
587
        {
588
			// SET THE IMAGE FOR THE BATTERY STATUS LABEL
589
590
591
592
593
594
595
596
			if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_EMPTY)
			{
				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;
				//ui->battery_status_label->update();
			}
597
			break;
598
        }
599
600
601

		// WHEN THE BATTERY IS IN A NORMAL STATE
		case BATTERY_STATE_NORMAL:
602
        {
603
604
605
606
607
608

			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))
			)
609
			{
610
611
612
613
614
615
616
617
618
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_EMPTY)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
					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;
					//ui->battery_status_label->update();
				}
619
			}
620
621
622
623
624
			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))
			)
625
			{
626
627
628
629
630
631
632
633
634
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_20)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
					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;
					//ui->battery_status_label->update();
				}
635
			}
636
637
638
639
640
			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))
			)
641
			{
642
643
644
645
646
647
648
649
650
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_40)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
					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;
					//ui->battery_status_label->update();
				}
651
			}
652
653
654
655
656
			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))
			)
657
			{
658
659
660
661
662
663
664
665
666
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_60)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
					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;
					//ui->battery_status_label->update();
				}
667
			}
668
669
670
671
672
			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))
			)
673
			{
674
675
676
677
678
679
680
681
682
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_80)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
					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;
					//ui->battery_status_label->update();
				}
683
684
685
			}
			else
			{
686
687
688
689
690
691
692
693
694
				if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_FULL)
				{
					// SET THE IMAGE FOR THE BATTERY STATUS LABEL
					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;
					//ui->battery_status_label->update();
				}
695
696
			}
			break;
697
        }
698
699

		default:
700
        {
701
702
703
704
705
706
707
708
709
        	if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_UNKNOWN)
			{
	            // SET THE IMAGE FOR THE BATTERY STATUS LABEL
	            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;
				//ui->battery_status_label->update();
	        }
710
			break;
711
        }
712
	}
713
	//ui->battery_status_label->update();
714
715
716
717
718
719
720
}

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

721
722
void MainWindow::crazyRadioStatusCallback(const std_msgs::Int32& msg)
{
723
    ROS_INFO("[Student GUI] Callback CrazyRadioStatus called");
724
725
726
    this->setCrazyRadioStatus(msg.data);
}

727
728
729
730
731
732
733
734
void MainWindow::loadCrazyflieContext()
{
	CMQuery contextCall;
	contextCall.request.studentID = m_student_id;
	ROS_INFO_STREAM("StudentID:" << m_student_id);

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

735
736
	if(centralManager.call(contextCall))
    {
737
738
		m_context = contextCall.response.crazyflieContext;
		ROS_INFO_STREAM("CrazyflieContext:\n" << m_context);
739
740
741
742
743
744
745
        // 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);
746
747
748
	}
    else
    {
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
		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;
}


771
772
void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to newViconData, from node
{
773
774
775
    for(std::vector<CrazyflieData>::const_iterator it = p_msg->crazyflies.begin(); it != p_msg->crazyflies.end(); ++it)
    {
		CrazyflieData global = *it;
776
777
778
779
780
781
        if(global.crazyflieName == m_context.crazyflieName)
        {
            CrazyflieData local = global;
            coordinatesToLocal(local);

            // now we have the local coordinates, put them in the labels
782
783
784
785
786
787
788
789
790
791
792
793
794
            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));
795

796
            // also update diff
797
798
799
800
801
802
803
804
805
            ui->diff_x_safe->setText(QString::number(m_safe_setpoint.x - local.x, 'f', 3));
            ui->diff_y_safe->setText(QString::number(m_safe_setpoint.y - local.y, 'f', 3));
            ui->diff_z_safe->setText(QString::number(m_safe_setpoint.z - local.z, 'f', 3));
            ui->diff_yaw_safe->setText(QString::number((m_safe_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));

            ui->diff_x_demo->setText(QString::number(m_demo_setpoint.x - local.x, 'f', 3));
            ui->diff_y_demo->setText(QString::number(m_demo_setpoint.y - local.y, 'f', 3));
            ui->diff_z_demo->setText(QString::number(m_demo_setpoint.z - local.z, 'f', 3));
            ui->diff_yaw_demo->setText(QString::number((m_demo_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
806
        }
807
    }
808
}
809

810
811
812
813


//    ----------------------------------------------------------------------------------
// # RF Crazyradio Connect Disconnect
814
815
816
817
void MainWindow::on_RF_Connect_button_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_RECONNECT;
818
    this->crazyRadioCommandPublisher.publish(msg);
roangel's avatar
roangel committed
819
    ROS_INFO("command reconnect published");
820
}
821

822
823
824
825
826
827
828
829
830
831
832
833
void MainWindow::on_RF_disconnect_button_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_DISCONNECT;
    this->crazyRadioCommandPublisher.publish(msg);
    ROS_INFO("command disconnect published");
}



//    ----------------------------------------------------------------------------------
// # Take off, lanf, motors off
834
void MainWindow::on_take_off_button_clicked()
835
{
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
    std_msgs::Int32 msg;
    msg.data = CMD_CRAZYFLY_TAKE_OFF;
    this->PPSClientCommandPublisher.publish(msg);
}

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

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

855
856
857
858


//    ----------------------------------------------------------------------------------
// # Setpoint
859
void MainWindow::on_set_setpoint_button_safe_clicked()
860
861
{
    Setpoint msg_setpoint;
862
863
864

    // initialize setpoint to previous one

865
866
867
868
    msg_setpoint.x = (ui->current_setpoint_x_safe->text()).toFloat();
    msg_setpoint.y = (ui->current_setpoint_y_safe->text()).toFloat();
    msg_setpoint.z = (ui->current_setpoint_z_safe->text()).toFloat();
    msg_setpoint.yaw = (ui->current_setpoint_yaw_safe->text()).toFloat();
869

870
871
    if(!ui->new_setpoint_x_safe->text().isEmpty())
        msg_setpoint.x = (ui->new_setpoint_x_safe->text()).toFloat();
872

873
874
    if(!ui->new_setpoint_y_safe->text().isEmpty())
        msg_setpoint.y = (ui->new_setpoint_y_safe->text()).toFloat();
875

876
877
    if(!ui->new_setpoint_z_safe->text().isEmpty())
        msg_setpoint.z = (ui->new_setpoint_z_safe->text()).toFloat();
878

879
880
    if(!ui->new_setpoint_yaw_safe->text().isEmpty())
        msg_setpoint.yaw = (ui->new_setpoint_yaw_safe->text()).toFloat() * DEG2RAD;
881

882
883
884
885
886
887
888
889
890
891
892
893
894
895

    if(!setpointInsideBox(msg_setpoint, m_context))
    {
        ROS_INFO("Corrected setpoint, was out of bounds");

        // correct the setpoint given the box size
        msg_setpoint = correctSetpointBox(msg_setpoint, m_context);
        ui->error_label->setText("Setpoint is outside safety box");
    }
    else
    {
        ui->error_label->clear();
    }

896
    this->controllerSetpointPublisher.publish(msg_setpoint);
897
898

    ROS_INFO_STREAM("Setpoint change clicked with:" << msg_setpoint.x << ", "<< msg_setpoint.y << ", "<< msg_setpoint.z << ", "<< msg_setpoint.yaw);
899
}
900

901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
void MainWindow::initialize_demo_setpoint()
{
    Setpoint msg_setpoint;
    msg_setpoint.x = 0;
    msg_setpoint.y = 0;
    msg_setpoint.z = 0.4;
    msg_setpoint.yaw = 0;

    this->demoSetpointPublisher.publish(msg_setpoint);
}

void MainWindow::initialize_student_setpoint()
{
    Setpoint msg_setpoint;
    msg_setpoint.x = 0;
    msg_setpoint.y = 0;
    msg_setpoint.z = 0.4;
    msg_setpoint.yaw = 0;

    this->studentSetpointPublisher.publish(msg_setpoint);
}

void MainWindow::initialize_mpc_setpoint()
924
925
926
927
928
929
930
{
    Setpoint msg_setpoint;
    msg_setpoint.x = 0;
    msg_setpoint.y = 0;
    msg_setpoint.z = 0.4;
    msg_setpoint.yaw = 0;

931
    this->mpcSetpointPublisher.publish(msg_setpoint);
932
933
}

934
void MainWindow::on_set_setpoint_button_demo_clicked()
935
936
{
    Setpoint msg_setpoint;
937

938
939
940
941
    msg_setpoint.x = (ui->current_setpoint_x_demo->text()).toFloat();
    msg_setpoint.y = (ui->current_setpoint_y_demo->text()).toFloat();
    msg_setpoint.z = (ui->current_setpoint_z_demo->text()).toFloat();
    msg_setpoint.yaw = (ui->current_setpoint_yaw_demo->text()).toFloat();
942

943
944
945
946
947
948
949
950
    if(!ui->new_setpoint_x_demo->text().isEmpty())
        msg_setpoint.x = (ui->new_setpoint_x_demo->text()).toFloat();
    if(!ui->new_setpoint_y_demo->text().isEmpty())
        msg_setpoint.y = (ui->new_setpoint_y_demo->text()).toFloat();
    if(!ui->new_setpoint_z_demo->text().isEmpty())
        msg_setpoint.z = (ui->new_setpoint_z_demo->text()).toFloat();
    if(!ui->new_setpoint_yaw_demo->text().isEmpty())
        msg_setpoint.yaw = (ui->new_setpoint_yaw_demo->text()).toFloat() * DEG2RAD;
951

952
    this->demoSetpointPublisher.publish(msg_setpoint);
953
954

    ROS_INFO_STREAM("Setpoint change clicked with:" << msg_setpoint.x << ", "<< msg_setpoint.y << ", "<< msg_setpoint.z << ", "<< msg_setpoint.yaw);
955
956
}

957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
void MainWindow::on_set_setpoint_button_student_clicked()
{
    Setpoint msg_setpoint;

    msg_setpoint.x = (ui->current_setpoint_x_student->text()).toFloat();
    msg_setpoint.y = (ui->current_setpoint_y_student->text()).toFloat();
    msg_setpoint.z = (ui->current_setpoint_z_student->text()).toFloat();
    msg_setpoint.yaw = (ui->current_setpoint_yaw_student->text()).toFloat();

    if(!ui->new_setpoint_x_student->text().isEmpty())
        msg_setpoint.x = (ui->new_setpoint_x_student->text()).toFloat();
    if(!ui->new_setpoint_y_student->text().isEmpty())
        msg_setpoint.y = (ui->new_setpoint_y_student->text()).toFloat();
    if(!ui->new_setpoint_z_student->text().isEmpty())
        msg_setpoint.z = (ui->new_setpoint_z_student->text()).toFloat();
    if(!ui->new_setpoint_yaw_student->text().isEmpty())
        msg_setpoint.yaw = (ui->new_setpoint_yaw_student->text()).toFloat() * DEG2RAD;

    this->studentSetpointPublisher.publish(msg_setpoint);

    ROS_INFO_STREAM("Setpoint change clicked with:" << msg_setpoint.x << ", "<< msg_setpoint.y << ", "<< msg_setpoint.z << ", "<< msg_setpoint.yaw);
}

void MainWindow::on_set_setpoint_button_mpc_clicked()
{
    Setpoint msg_setpoint;

    msg_setpoint.x = (ui->current_setpoint_x_mpc->text()).toFloat();
    msg_setpoint.y = (ui->current_setpoint_y_mpc->text()).toFloat();
    msg_setpoint.z = (ui->current_setpoint_z_mpc->text()).toFloat();
    msg_setpoint.yaw = (ui->current_setpoint_yaw_mpc->text()).toFloat();
988

989
990
991
992
993
994
995
996
997
998
999
1000
    if(!ui->new_setpoint_x_mpc->text().isEmpty())
        msg_setpoint.x = (ui->new_setpoint_x_mpc->text()).toFloat();
    if(!ui->new_setpoint_y_mpc->text().isEmpty())
        msg_setpoint.y = (ui->new_setpoint_y_mpc->text()).toFloat();
    if(!ui->new_setpoint_z_mpc->text().isEmpty())
        msg_setpoint.z = (ui->new_setpoint_z_mpc->text()).toFloat();
    if(!ui->new_setpoint_yaw_mpc->text().isEmpty())
        msg_setpoint.yaw = (ui->new_setpoint_yaw_mpc->text()).toFloat() * DEG2RAD;

    this->mpcSetpointPublisher.publish(msg_setpoint);

    ROS_INFO_STREAM("Setpoint change clicked with:" << msg_setpoint.x << ", "<< msg_setpoint.y << ", "<< msg_setpoint.z << ", "<< msg_setpoint.yaw);
For faster browsing, not all history is shown. View entire blame