MainWindow.cpp 53.7 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
    ui->label_battery->setStyleSheet("QLabel { color : red; }");
roangel's avatar
roangel committed
173
    m_battery_state = BATTERY_STATE_NORMAL;
174

175
176
177
178
179
180
181
182
183
184
    // SET THE IMAGE FOR THE BATTERY STATUS LABEL
    QPixmap battery_empty_pixmap(":/images/battery_empty.png");
    ui->label_battery->setPixmap(battery_empty_pixmap);
    ui->label_battery->setScaledContents(true);

    // SET THE IMAGE FOR THE BATTERY STATUS LABEL
    QPixmap rf_disconnected_pixmap(":/images/rf_disconnected.png");
    ui->rf_status_label->setPixmap(rf_disconnected_pixmap);
    ui->rf_status_label->setScaledContents(true);

185
186
187
    ui->error_label->setStyleSheet("QLabel { color : red; }");
    ui->error_label->clear();

188
189
190
191
192
193
194
    // 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()));


195
196
197
    initialize_demo_setpoint();
    initialize_student_setpoint();
    initialize_mpc_setpoint();
roangel's avatar
roangel committed
198
199
}

200

roangel's avatar
roangel committed
201
202
203
204
MainWindow::~MainWindow()
{
    delete ui;
}
205

206
207
void MainWindow::disableGUI()
{
208
209
210
    ui->motors_OFF_button->setEnabled(false);
    ui->take_off_button->setEnabled(false);
    ui->land_button->setEnabled(false);
211
212
213
214
}

void MainWindow::enableGUI()
{
215
    ui->motors_OFF_button->setEnabled(true);
roangel's avatar
roangel committed
216
217
    if(m_battery_state == BATTERY_STATE_NORMAL)
    {
218
219
        ui->take_off_button->setEnabled(true);
        ui->land_button->setEnabled(true);
roangel's avatar
roangel committed
220
    }
221
222
}

223
224
225
226
void MainWindow::highlightSafeControllerTab()
{
    ui->tabWidget->tabBar()->setTabTextColor(0, Qt::green);
    ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
227
228
    ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
229
    ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
230
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
231
}
232
void MainWindow::highlightDemoControllerTab()
233
234
235
{
    ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(1, Qt::green);
236
237
    ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
238
    ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
239
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
240
241
242
243
244
245
246
}
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);
247
    ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
248
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
249
250
251
252
253
254
255
}
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);
256
    ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
257
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
258
259
260
261
262
263
264
265
}
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);
266
267
268
269
270
271
272
273
274
275
    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);
276
277
}

278
279
280
void MainWindow::DBChangedCallback(const std_msgs::Int32& msg)
{
    loadCrazyflieContext();
281
    ROS_INFO("context reloaded in student_GUI");
282
283
}

284
285
286
287
288
289
290
void MainWindow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
{
    switch(msg.data)
    {
        case SAFE_CONTROLLER:
            highlightSafeControllerTab();
            break;
291
        case DEMO_CONTROLLER:
292
293
294
295
296
297
298
            highlightDemoControllerTab();
            break;
        case STUDENT_CONTROLLER:
            highlightStudentControllerTab();
            break;
        case MPC_CONTROLLER:
            highlightMpcControllerTab();
299
            break;
300
301
        case REMOTE_CONTROLLER:
            highlightRemoteControllerTab();
302
            break;
303
304
        case TUNING_CONTROLLER:
            highlightTuningControllerTab();
305
            break;
306
307
308
309
310
        default:
            break;
    }
}

311
void MainWindow::safeSetpointCallback(const Setpoint& newSetpoint)
312
{
313
    m_safe_setpoint = newSetpoint;
314
    // here we get the new setpoint, need to update it in GUI
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
    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));
339
340
}

341
void MainWindow::mpcSetpointCallback(const Setpoint& newSetpoint)
342
{
343
    m_mpc_setpoint = newSetpoint;
344
    // here we get the new setpoint, need to update it in GUI
345
346
347
348
    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));
349
350
}

351
352
void MainWindow::flyingStateChangedCallback(const std_msgs::Int32& msg)
{
353
354
355
356
	// PUT THE CURRENT STATE INTO THE CLASS VARIABLE
	m_flying_state = msg.data;

	// UPDATE THE LABEL TO DISPLAY THE FLYING STATE
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
    QString qstr = "Flying State: ";
    switch(msg.data)
    {
        case STATE_MOTORS_OFF:
            qstr.append("Motors OFF");
            break;
        case STATE_TAKE_OFF:
            qstr.append("Take OFF");
            break;
        case STATE_FLYING:
            qstr.append("Flying");
            break;
        case STATE_LAND:
            qstr.append("Land");
            break;
        default:
            break;
    }
    ui->flying_state_label->setText(qstr);
}

378
379
void MainWindow::batteryStateChangedCallback(const std_msgs::Int32& msg)
{
380
381
382
383
384
    // switch case with unabling buttons motors off, take off, etc... when battery is low
    QString qstr = "";
    switch(msg.data)
    {
        case BATTERY_STATE_LOW:
385
        {
386
            qstr.append("Low Battery!");
387
388
389
390
            ui->take_off_button->setEnabled(false);
            ui->land_button->setEnabled(false);
            // ui->groupBox_4->setEnabled(false);

391
            ui->label_battery->setText(qstr);
392
393
394
395
396
397
398

            // SET THE IMAGE FOR THE BATTERY STATUS LABEL
			QPixmap battery_empty_pixmap(":/images/battery_empty.png");
			ui->label_battery->setPixmap(battery_empty_pixmap);
			ui->label_battery->setScaledContents(true);

			// SET THE CLASS VARIABLE FOR TRACKING THE BATTERY STATE
roangel's avatar
roangel committed
399
            m_battery_state = BATTERY_STATE_LOW;
400
            break;
401
402
        }

403
        case BATTERY_STATE_NORMAL:
404
        {
405
406
407
408
            // ui->groupBox_4->setEnabled(true);
            ui->take_off_button->setEnabled(true);
            ui->land_button->setEnabled(true);

409
            ui->label_battery->clear();
410
411

            // SET THE CLASS VARIABLE FOR TRACKING THE BATTERY STATE
roangel's avatar
roangel committed
412
            m_battery_state = BATTERY_STATE_NORMAL;
413
            break;
414
415
        }

416
417
418
        default:
            break;
    }
419
420
421
}


422
423
424
425
426
427
428
void MainWindow::setCrazyRadioStatus(int radio_status)
{
    // add more things whenever the status is changed
    switch(radio_status)
    {
        case CONNECTED:
            // change icon, the rest of the GUI is available now
429
            ui->connected_disconnected_label->setText("Crazyradio status: Connected!");
430
            enableGUI();
431
432
433
            break;
        case CONNECTING:
            // change icon
434
            ui->connected_disconnected_label->setText("Crazyradio status: Connecting...");
435
436
437
            break;
        case DISCONNECTED:
            // change icon, the rest of the GUI is disabled
438
            ui->connected_disconnected_label->setText("Crazyradio status: Disconnected");
439
            disableGUI();
440
441
442
443
444
445
446
447
            break;
        default:
    		ROS_ERROR_STREAM("unexpected radio status: " << m_radio_status);
            break;
    }
    this->m_radio_status = radio_status;
}

448
449
float MainWindow::fromVoltageToPercent(float voltage)
{
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
    // int num_cutoffs = m_cutoff_voltages.size();
    // float hysteresis = 0.05;

    // while(m_battery_level < num_cutoffs && voltage >= m_cutoff_voltages[m_battery_level])
    // {
    //     ++m_battery_level;
    // }
    // while(m_battery_level > 0 && voltage < m_cutoff_voltages[m_battery_level - 1] - hysteresis)
    // {
    //     --m_battery_level;
    // }

    // float percentage = 100.0 * m_battery_level/num_cutoffs;

	// 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
    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;
	}
480

481
	// COMPUTE THE PERCENTAGE
482
	float percentage = 100 * (voltage-voltage_when_empty)/(voltage_when_full-voltage_when_empty);
483

484

485
486
	// CLIP THE PERCENTAGE TO BE BETWEEN [0,100]
    // > This should not happen to often
487
488
489
490
491
    if(percentage > 100)
        percentage = 100;
    if(percentage < 0)
        percentage = 0;

492
493
494
495
496
    return percentage;
}

void MainWindow::updateBatteryVoltage(float battery_voltage)
{
497
	// PUT THE VOLTAGE INTO THE CLASS VARIABLES
498
    m_battery_voltage = battery_voltage;
499

500
    // UPDATE THE BATTERY VOLTAGE FIELD
roangel's avatar
roangel committed
501
    QString qstr = "";
roangel's avatar
roangel committed
502
    qstr.append(QString::number(battery_voltage, 'f', 2));
roangel's avatar
roangel committed
503
504
    qstr.append(" V");
    ui->voltage_field->setText(qstr);
505
506
507
508
509
510
511
512
513

	// COMPUTE THE BATTERY VOLTAGE AS A PERCENTAGE
	int battery_voltage_percentage = (int) fromVoltageToPercent(m_battery_voltage);

	// 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:
514
        {
515
516
517
518
519
			// SET THE IMAGE FOR THE BATTERY STATUS LABEL
			QPixmap battery_empty_pixmap(":/images/battery_empty.png");
			ui->label_battery->setPixmap(battery_empty_pixmap);
			ui->label_battery->setScaledContents(true);
			break;
520
        }
521
522
523

		// WHEN THE BATTERY IS IN A NORMAL STATE
		case BATTERY_STATE_NORMAL:
524
        {
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
			if (battery_voltage_percentage <= 0)
			{
				// SET THE IMAGE FOR THE BATTERY STATUS LABEL
				QPixmap battery_empty_pixmap(":/images/battery_empty.png");
				ui->label_battery->setPixmap(battery_empty_pixmap);
				ui->label_battery->setScaledContents(true);
			}
			else if (battery_voltage_percentage <= 20)
			{
				// SET THE IMAGE FOR THE BATTERY STATUS LABEL
				QPixmap battery_20_pixmap(":/images/battery_20.png");
				ui->label_battery->setPixmap(battery_20_pixmap);
				ui->label_battery->setScaledContents(true);
			}
			else if (battery_voltage_percentage <= 40)
			{
				// SET THE IMAGE FOR THE BATTERY STATUS LABEL
				QPixmap battery_40_pixmap(":/images/battery_40.png");
				ui->label_battery->setPixmap(battery_40_pixmap);
				ui->label_battery->setScaledContents(true);
			}
			else if (battery_voltage_percentage <= 60)
			{
				// SET THE IMAGE FOR THE BATTERY STATUS LABEL
				QPixmap battery_60_pixmap(":/images/battery_60.png");
				ui->label_battery->setPixmap(battery_60_pixmap);
				ui->label_battery->setScaledContents(true);
			}
			else if (battery_voltage_percentage <= 80)
			{
				// SET THE IMAGE FOR THE BATTERY STATUS LABEL
				QPixmap battery_80_pixmap(":/images/battery_80.png");
				ui->label_battery->setPixmap(battery_80_pixmap);
				ui->label_battery->setScaledContents(true);
			}
			else
			{
				// SET THE IMAGE FOR THE BATTERY STATUS LABEL
				QPixmap battery_full_pixmap(":/images/battery_full.png");
				ui->label_battery->setPixmap(battery_full_pixmap);
				ui->label_battery->setScaledContents(true);
			}
			break;
568
        }
569
570
571
572
573
574

		default:
			break;
	}


575
576
577
578
579
580
581
}

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

582
583
void MainWindow::crazyRadioStatusCallback(const std_msgs::Int32& msg)
{
584
    ROS_INFO("Callback CrazyRadioStatus called");
585
586
587
    this->setCrazyRadioStatus(msg.data);
}

588
589
590
591
592
593
594
595
void MainWindow::loadCrazyflieContext()
{
	CMQuery contextCall;
	contextCall.request.studentID = m_student_id;
	ROS_INFO_STREAM("StudentID:" << m_student_id);

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

596
597
	if(centralManager.call(contextCall))
    {
598
599
		m_context = contextCall.response.crazyflieContext;
		ROS_INFO_STREAM("CrazyflieContext:\n" << m_context);
600
601
602
603
604
605
606
        // 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);
607
608
609
	}
    else
    {
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
		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;
}


632
633
void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to newViconData, from node
{
634
635
636
    for(std::vector<CrazyflieData>::const_iterator it = p_msg->crazyflies.begin(); it != p_msg->crazyflies.end(); ++it)
    {
		CrazyflieData global = *it;
637
638
639
640
641
642
        if(global.crazyflieName == m_context.crazyflieName)
        {
            CrazyflieData local = global;
            coordinatesToLocal(local);

            // now we have the local coordinates, put them in the labels
643
644
645
646
647
648
649
650
651
652
653
654
655
            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));
656

657
            // also update diff
658
659
660
661
662
663
664
665
666
            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));
667
        }
668
    }
669
}
670

671
672
673
674


//    ----------------------------------------------------------------------------------
// # RF Crazyradio Connect Disconnect
675
676
677
678
void MainWindow::on_RF_Connect_button_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_RECONNECT;
679
    this->crazyRadioCommandPublisher.publish(msg);
roangel's avatar
roangel committed
680
    ROS_INFO("command reconnect published");
681
}
682

683
684
685
686
687
688
689
690
691
692
693
694
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
695
void MainWindow::on_take_off_button_clicked()
696
{
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
    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);
714
}
715

716
717
718
719


//    ----------------------------------------------------------------------------------
// # Setpoint
720
void MainWindow::on_set_setpoint_button_safe_clicked()
721
722
{
    Setpoint msg_setpoint;
723
724
725

    // initialize setpoint to previous one

726
727
728
729
    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();
730

731
732
    if(!ui->new_setpoint_x_safe->text().isEmpty())
        msg_setpoint.x = (ui->new_setpoint_x_safe->text()).toFloat();
733

734
735
    if(!ui->new_setpoint_y_safe->text().isEmpty())
        msg_setpoint.y = (ui->new_setpoint_y_safe->text()).toFloat();
736

737
738
    if(!ui->new_setpoint_z_safe->text().isEmpty())
        msg_setpoint.z = (ui->new_setpoint_z_safe->text()).toFloat();
739

740
741
    if(!ui->new_setpoint_yaw_safe->text().isEmpty())
        msg_setpoint.yaw = (ui->new_setpoint_yaw_safe->text()).toFloat() * DEG2RAD;
742

743
744
745
746
747
748
749
750
751
752
753
754
755
756

    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();
    }

757
    this->controllerSetpointPublisher.publish(msg_setpoint);
758
759

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

762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
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()
785
786
787
788
789
790
791
{
    Setpoint msg_setpoint;
    msg_setpoint.x = 0;
    msg_setpoint.y = 0;
    msg_setpoint.z = 0.4;
    msg_setpoint.yaw = 0;

792
    this->mpcSetpointPublisher.publish(msg_setpoint);
793
794
}

795
void MainWindow::on_set_setpoint_button_demo_clicked()
796
797
{
    Setpoint msg_setpoint;
798

799
800
801
802
    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();
803

804
805
806
807
808
809
810
811
    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;
812

813
    this->demoSetpointPublisher.publish(msg_setpoint);
814
815

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

818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
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();
849

850
851
852
853
854
855
856
857
858
859
860
861
862
    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);
}
863

864

865
866
//    ----------------------------------------------------------------------------------
// # Load Yaml when acting as the GUI for an Agent
867
void MainWindow::on_load_safe_yaml_button_clicked()
868
{
869
    // Set the "load safe yaml" button to be disabled
870
    ui->load_safe_yaml_button->setEnabled(false);
871
872
873
874

    // Send a message requesting the parameters from the YAML
    // file to be reloaded for the safe controller
    std_msgs::Int32 msg;
875
    msg.data = LOAD_YAML_SAFE_CONTROLLER_AGENT;
876
877
878
879
880
881
882
883
884
    this->requestLoadControllerYamlPublisher.publish(msg);
    ROS_INFO("Request load of safe controller YAML published");

    // Start a timer which will enable the button in its callback
    // > This is required because the agent node waits some time between
    //   re-loading the values from the YAML file and then assigning then
    //   to the local variable of the agent.
    // > Thus we use this timer to prevent the user from clicking the
    //   button in the GUI repeatedly.
885
    ros::NodeHandle nodeHandle("~");
beuchatp's avatar
beuchatp committed
886
    m_timer_yaml_file_for_safe_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::safeYamlFileTimerCallback, this, true);
887
}
roangel's avatar
roangel committed
888

889
890
891
892
893
void MainWindow::safeYamlFileTimerCallback(const ros::TimerEvent&)
{
    // Enble the "load safe yaml" button again
    ui->load_safe_yaml_button->setEnabled(true);
}
roangel's avatar
roangel committed
894
895


896

897
898
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
924
void MainWindow::on_load_demo_yaml_button_clicked()
{
    // Set the "load demo yaml" button to be disabled
    ui->load_demo_yaml_button->setEnabled(false);

    // Send a message requesting the parameters from the YAML
    // file to be reloaded for the demo controller
    std_msgs::Int32 msg;
    msg.data = LOAD_YAML_DEMO_CONTROLLER_AGENT;
    this->requestLoadControllerYamlPublisher.publish(msg);
    ROS_INFO("Request load of demo controller YAML published");

    // Start a timer which will enable the button in its callback
    // > This is required because the agent node waits some time between
    //   re-loading the values from the YAML file and then assigning then
    //   to the local variable of the agent.
    // > Thus we use this timer to prevent the user from clicking the
    //   button in the GUI repeatedly.
    ros::NodeHandle nodeHandle("~");
    m_timer_yaml_file_for_demo_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::demoYamlFileTimerCallback, this, true);
}

void MainWindow::demoYamlFileTimerCallback(const ros::TimerEvent&)
{
    // Enble the "load demo yaml" button again
    ui->load_demo_yaml_button->setEnabled(true);
}

925
926


927
void MainWindow::on_load_student_yaml_button_clicked()
928
{
929
930
    // Set the "load student yaml" button to be disabled
    ui->load_student_yaml_button->setEnabled(false);
931
932

    // Send a message requesting the parameters from the YAML
933
    // file to be reloaded for the student controller
934
    std_msgs::Int32 msg;
935
    msg.data = LOAD_YAML_STUDENT_CONTROLLER_AGENT;
936
    this->requestLoadControllerYamlPublisher.publish(msg);
937
    ROS_INFO("Request load of student controller YAML published");
938
939
940
941
942
943
944
945

    // Start a timer which will enable the button in its callback
    // > This is required because the agent node waits some time between
    //   re-loading the values from the YAML file and then assigning then
    //   to the local variable of the agent.
    // > Thus we use this timer to prevent the user from clicking the
    //   button in the GUI repeatedly.
    ros::NodeHandle nodeHandle("~");
946
    m_timer_yaml_file_for_student_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::studentYamlFileTimerCallback, this, true);    
947
948
}

949
void MainWindow::studentYamlFileTimerCallback(const ros::TimerEvent&)
950
{
951
952
    // Enble the "load student yaml" button again
    ui->load_student_yaml_button->setEnabled(true);
953
954
}

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
void MainWindow::on_load_mpc_yaml_button_clicked()
{
    // Set the "load mpc yaml" button to be disabled
    ui->load_mpc_yaml_button->setEnabled(false);

    // Send a message requesting the parameters from the YAML
    // file to be reloaded for the mpc controller
    std_msgs::Int32 msg;
    msg.data = LOAD_YAML_MPC_CONTROLLER_AGENT;
    this->requestLoadControllerYamlPublisher.publish(msg);
    ROS_INFO("Request load of mpc controller YAML published");

    // Start a timer which will enable the button in its callback
    // > This is required because the agent node waits some time between
    //   re-loading the values from the YAML file and then assigning then
    //   to the local variable of the agent.
    // > Thus we use this timer to prevent the user from clicking the
    //   button in the GUI repeatedly.
    ros::NodeHandle nodeHandle("~");
    m_timer_yaml_file_for_mpc_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::mpcYamlFileTimerCallback, this, true);
}

void MainWindow::mpcYamlFileTimerCallback(const ros::TimerEvent&)
{
    // Enble the "load mpc yaml" button again
    ui->load_mpc_yaml_button->setEnabled(true);
}

985
986


987
988
989
990
991
992
993
994
995
996
997
998
999
1000

void MainWindow::on_load_remote_yaml_button_clicked()
{
    // Set the "load remote yaml" button to be disabled
    ui->load_remote_yaml_button->setEnabled(false);

    // Send a message requesting the parameters from the YAML
    // file to be reloaded for the remote controller
    std_msgs::Int32 msg;
    msg.data = LOAD_YAML_REMOTE_CONTROLLER_AGENT;
    this->requestLoadControllerYamlPublisher.publish(msg);
    ROS_INFO("[STUDENT GUI] Request load of remote controller YAML published");

    // Start a timer which will enable the button in its callback
For faster browsing, not all history is shown. View entire blame