MainWindow.cpp 48.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
71
    // SUBSCRIBERS AND PUBLISHERS:
    // > For the Demo Controller SETPOINTS
72
73
    demoSetpointPublisher  = nodeHandle.advertise<Setpoint>("DemoControllerService/Setpoint", 1);
    demoSetpointSubscriber = nodeHandle.subscribe("DemoControllerService/Setpoint", 1, &MainWindow::demoSetpointCallback, this);
74
    // > For the Student Controller SETPOINTS
75
76
    studentSetpointPublisher  = nodeHandle.advertise<Setpoint>("StudentControllerService/Setpoint", 1);
    studentSetpointSubscriber = nodeHandle.subscribe("StudentControllerService/Setpoint", 1, &MainWindow::studentSetpointCallback, this);
77
    // > For the MPC Controller SETPOINTS
78
79
    mpcSetpointPublisher  = nodeHandle.advertise<Setpoint>("MpcControllerService/Setpoint", 1);
    mpcSetpointSubscriber = nodeHandle.subscribe("MpcControllerService/Setpoint", 1, &MainWindow::mpcSetpointCallback, this);
80

81
82
83
84
85
86
87
88
89
90
91

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


92
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
    // subscribers
roangel's avatar
roangel committed
102
    crazyRadioStatusSubscriber = nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &MainWindow::crazyRadioStatusCallback, this);
103

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

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

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

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

112

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

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

119

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

126
127
    PPSClientStudentCustomButtonPublisher = nh_PPSClient.advertise<CustomButton>("StudentCustomButton", 1);

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
    ui->error_label->setStyleSheet("QLabel { color : red; }");
    ui->error_label->clear();

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


185
186
187
    initialize_demo_setpoint();
    initialize_student_setpoint();
    initialize_mpc_setpoint();
roangel's avatar
roangel committed
188
189
}

190

roangel's avatar
roangel committed
191
192
193
194
MainWindow::~MainWindow()
{
    delete ui;
}
195

196
197
void MainWindow::disableGUI()
{
198
199
200
    ui->motors_OFF_button->setEnabled(false);
    ui->take_off_button->setEnabled(false);
    ui->land_button->setEnabled(false);
201
202
203
204
}

void MainWindow::enableGUI()
{
205
    ui->motors_OFF_button->setEnabled(true);
roangel's avatar
roangel committed
206
207
    if(m_battery_state == BATTERY_STATE_NORMAL)
    {
208
209
        ui->take_off_button->setEnabled(true);
        ui->land_button->setEnabled(true);
roangel's avatar
roangel committed
210
    }
211
212
}

213
214
215
216
void MainWindow::highlightSafeControllerTab()
{
    ui->tabWidget->tabBar()->setTabTextColor(0, Qt::green);
    ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
217
218
    ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
219
    ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
220
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
221
}
222
void MainWindow::highlightDemoControllerTab()
223
224
225
{
    ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(1, Qt::green);
226
227
    ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
228
    ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
229
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
230
231
232
233
234
235
236
}
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);
237
    ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
238
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
239
240
241
242
243
244
245
}
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);
246
    ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
247
    ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
248
249
250
251
252
253
254
255
}
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);
256
257
258
259
260
261
262
263
264
265
    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);
266
267
}

268
269
270
void MainWindow::DBChangedCallback(const std_msgs::Int32& msg)
{
    loadCrazyflieContext();
271
    ROS_INFO("context reloaded in student_GUI");
272
273
}

274
275
276
277
278
279
280
void MainWindow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
{
    switch(msg.data)
    {
        case SAFE_CONTROLLER:
            highlightSafeControllerTab();
            break;
281
        case DEMO_CONTROLLER:
282
283
284
285
286
287
288
            highlightDemoControllerTab();
            break;
        case STUDENT_CONTROLLER:
            highlightStudentControllerTab();
            break;
        case MPC_CONTROLLER:
            highlightMpcControllerTab();
289
            break;
290
291
        case REMOTE_CONTROLLER:
            highlightRemoteControllerTab();
292
            break;
293
294
        case TUNING_CONTROLLER:
            highlightTuningControllerTab();
295
            break;
296
297
298
299
300
        default:
            break;
    }
}

301
void MainWindow::safeSetpointCallback(const Setpoint& newSetpoint)
302
{
303
    m_safe_setpoint = newSetpoint;
304
    // here we get the new setpoint, need to update it in GUI
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
    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));
329
330
}

331
void MainWindow::mpcSetpointCallback(const Setpoint& newSetpoint)
332
{
333
    m_mpc_setpoint = newSetpoint;
334
    // here we get the new setpoint, need to update it in GUI
335
336
337
338
    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));
339
340
}

341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
void MainWindow::flyingStateChangedCallback(const std_msgs::Int32& msg)
{
    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);
}

364
365
void MainWindow::batteryStateChangedCallback(const std_msgs::Int32& msg)
{
366
367
368
369
370
371
    // switch case with unabling buttons motors off, take off, etc... when battery is low
    QString qstr = "";
    switch(msg.data)
    {
        case BATTERY_STATE_LOW:
            qstr.append("Low Battery!");
372
373
374
375
            ui->take_off_button->setEnabled(false);
            ui->land_button->setEnabled(false);
            // ui->groupBox_4->setEnabled(false);

376
            ui->label_battery->setText(qstr);
roangel's avatar
roangel committed
377
            m_battery_state = BATTERY_STATE_LOW;
378
379
            break;
        case BATTERY_STATE_NORMAL:
380
381
382
383
            // ui->groupBox_4->setEnabled(true);
            ui->take_off_button->setEnabled(true);
            ui->land_button->setEnabled(true);

384
            ui->label_battery->clear();
roangel's avatar
roangel committed
385
            m_battery_state = BATTERY_STATE_NORMAL;
386
387
388
389
            break;
        default:
            break;
    }
390
391
392
}


393
394
395
396
397
398
399
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
400
            ui->connected_disconnected_label->setText("Crazyradio status: Connected!");
401
            enableGUI();
402
403
404
            break;
        case CONNECTING:
            // change icon
405
            ui->connected_disconnected_label->setText("Crazyradio status: Connecting...");
406
407
408
            break;
        case DISCONNECTED:
            // change icon, the rest of the GUI is disabled
409
            ui->connected_disconnected_label->setText("Crazyradio status: Disconnected");
410
            disableGUI();
411
412
413
414
415
416
417
418
            break;
        default:
    		ROS_ERROR_STREAM("unexpected radio status: " << m_radio_status);
            break;
    }
    this->m_radio_status = radio_status;
}

419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
float MainWindow::fromVoltageToPercent(float voltage)
{
    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;
434
435
436
437
438
439
440

    // should not hapen, but just in case...
    if(percentage > 100)
        percentage = 100;
    if(percentage < 0)
        percentage = 0;

441
442
443
444
445
446
447
    return percentage;
}

void MainWindow::updateBatteryVoltage(float battery_voltage)
{
    m_battery_voltage = battery_voltage;
    // Need to take voltage, display it and transform it to percentage
448
    // int percentage = (int) fromVoltageToPercent(m_battery_voltage);
449

roangel's avatar
roangel committed
450
    QString qstr = "";
roangel's avatar
roangel committed
451
    qstr.append(QString::number(battery_voltage, 'f', 2));
roangel's avatar
roangel committed
452
453
    qstr.append(" V");
    ui->voltage_field->setText(qstr);
454
455
456
457
458
459
460
}

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

461
462
void MainWindow::crazyRadioStatusCallback(const std_msgs::Int32& msg)
{
463
    ROS_INFO("Callback CrazyRadioStatus called");
464
465
466
    this->setCrazyRadioStatus(msg.data);
}

467
468
469
470
471
472
473
474
void MainWindow::loadCrazyflieContext()
{
	CMQuery contextCall;
	contextCall.request.studentID = m_student_id;
	ROS_INFO_STREAM("StudentID:" << m_student_id);

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

475
476
	if(centralManager.call(contextCall))
    {
477
478
		m_context = contextCall.response.crazyflieContext;
		ROS_INFO_STREAM("CrazyflieContext:\n" << m_context);
479
480
481
482
483
484
485
        // 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);
486
487
488
	}
    else
    {
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
		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;
}


511
512
void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to newViconData, from node
{
513
514
515
    for(std::vector<CrazyflieData>::const_iterator it = p_msg->crazyflies.begin(); it != p_msg->crazyflies.end(); ++it)
    {
		CrazyflieData global = *it;
516
517
518
519
520
521
        if(global.crazyflieName == m_context.crazyflieName)
        {
            CrazyflieData local = global;
            coordinatesToLocal(local);

            // now we have the local coordinates, put them in the labels
522
523
524
525
526
527
528
529
530
531
532
533
534
            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));
535

536
            // also update diff
537
538
539
540
541
542
543
544
545
            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));
546
        }
547
    }
548
}
549

550
551
552
553


//    ----------------------------------------------------------------------------------
// # RF Crazyradio Connect Disconnect
554
555
556
557
void MainWindow::on_RF_Connect_button_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_RECONNECT;
558
    this->crazyRadioCommandPublisher.publish(msg);
roangel's avatar
roangel committed
559
    ROS_INFO("command reconnect published");
560
}
561

562
563
564
565
566
567
568
569
570
571
572
573
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
574
void MainWindow::on_take_off_button_clicked()
575
{
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
    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);
593
}
594

595
596
597
598


//    ----------------------------------------------------------------------------------
// # Setpoint
599
void MainWindow::on_set_setpoint_button_safe_clicked()
600
601
{
    Setpoint msg_setpoint;
602
603
604

    // initialize setpoint to previous one

605
606
607
608
    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();
609

610
611
    if(!ui->new_setpoint_x_safe->text().isEmpty())
        msg_setpoint.x = (ui->new_setpoint_x_safe->text()).toFloat();
612

613
614
    if(!ui->new_setpoint_y_safe->text().isEmpty())
        msg_setpoint.y = (ui->new_setpoint_y_safe->text()).toFloat();
615

616
617
    if(!ui->new_setpoint_z_safe->text().isEmpty())
        msg_setpoint.z = (ui->new_setpoint_z_safe->text()).toFloat();
618

619
620
    if(!ui->new_setpoint_yaw_safe->text().isEmpty())
        msg_setpoint.yaw = (ui->new_setpoint_yaw_safe->text()).toFloat() * DEG2RAD;
621

622
623
624
625
626
627
628
629
630
631
632
633
634
635

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

636
    this->controllerSetpointPublisher.publish(msg_setpoint);
637
638

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

641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
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()
664
665
666
667
668
669
670
{
    Setpoint msg_setpoint;
    msg_setpoint.x = 0;
    msg_setpoint.y = 0;
    msg_setpoint.z = 0.4;
    msg_setpoint.yaw = 0;

671
    this->mpcSetpointPublisher.publish(msg_setpoint);
672
673
}

674
void MainWindow::on_set_setpoint_button_demo_clicked()
675
676
{
    Setpoint msg_setpoint;
677

678
679
680
681
    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();
682

683
684
685
686
687
688
689
690
    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;
691

692
    this->demoSetpointPublisher.publish(msg_setpoint);
693
694

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

697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
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();
728

729
730
731
732
733
734
735
736
737
738
739
740
741
    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);
}
742

743

744
745
//    ----------------------------------------------------------------------------------
// # Load Yaml when acting as the GUI for an Agent
746
void MainWindow::on_load_safe_yaml_button_clicked()
747
{
748
    // Set the "load safe yaml" button to be disabled
749
    ui->load_safe_yaml_button->setEnabled(false);
750
751
752
753

    // Send a message requesting the parameters from the YAML
    // file to be reloaded for the safe controller
    std_msgs::Int32 msg;
754
    msg.data = LOAD_YAML_SAFE_CONTROLLER_AGENT;
755
756
757
758
759
760
761
762
763
    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.
764
    ros::NodeHandle nodeHandle("~");
beuchatp's avatar
beuchatp committed
765
    m_timer_yaml_file_for_safe_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::safeYamlFileTimerCallback, this, true);
766
}
roangel's avatar
roangel committed
767

768
769
770
771
772
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
773
774


775

776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
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);
}

804
805


806
void MainWindow::on_load_student_yaml_button_clicked()
807
{
808
809
    // Set the "load student yaml" button to be disabled
    ui->load_student_yaml_button->setEnabled(false);
810
811

    // Send a message requesting the parameters from the YAML
812
    // file to be reloaded for the student controller
813
    std_msgs::Int32 msg;
814
    msg.data = LOAD_YAML_STUDENT_CONTROLLER_AGENT;
815
    this->requestLoadControllerYamlPublisher.publish(msg);
816
    ROS_INFO("Request load of student controller YAML published");
817
818
819
820
821
822
823
824

    // 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("~");
825
    m_timer_yaml_file_for_student_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::studentYamlFileTimerCallback, this, true);    
826
827
}

828
void MainWindow::studentYamlFileTimerCallback(const ros::TimerEvent&)
829
{
830
831
    // Enble the "load student yaml" button again
    ui->load_student_yaml_button->setEnabled(true);
832
833
}

834
835


836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
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);
}

864
865


866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896

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
    // > 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_remote_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::remoteYamlFileTimerCallback, this, true);
}

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



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
925
926
void MainWindow::on_load_tuning_yaml_button_clicked()
{
    // Set the "load tuning yaml" button to be disabled
    ui->load_tuning_yaml_button->setEnabled(false);

    // Send a message requesting the parameters from the YAML
    // file to be reloaded for the tuning controller
    std_msgs::Int32 msg;
    msg.data = LOAD_YAML_TUNING_CONTROLLER_AGENT;
    this->requestLoadControllerYamlPublisher.publish(msg);
    ROS_INFO("[STUDENT GUI] Request load of tuning 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_tuning_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::tuningYamlFileTimerCallback, this, true);
}

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



927
void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::Int32& msg)
928
{
929
930
    // Extract from the "msg" for which controller the YAML
    // parameters should be loaded
beuchatp's avatar
beuchatp committed
931
932
    int controller_to_load_yaml = msg.data;

beuchatp's avatar
beuchatp committed
933
    // Create the "nodeHandle" needed in the switch cases below
beuchatp's avatar
beuchatp committed
934
    ros::NodeHandle nodeHandle("~");
935

936
937
938
    // Switch between loading for the different controllers
    switch(controller_to_load_yaml)
    {
939
940
        case LOAD_YAML_SAFE_CONTROLLER_AGENT:
        case LOAD_YAML_SAFE_CONTROLLER_COORDINATOR:
941
942
943
944
945
946
947
948
949
            // Set the "load safe yaml" button to be disabled
            ui->load_safe_yaml_button->setEnabled(false);

            // 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.
beuchatp's avatar
beuchatp committed
950
            m_timer_yaml_file_for_safe_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::safeYamlFileTimerCallback, this, true);
951

952
953
            break;

954
955
        case LOAD_YAML_DEMO_CONTROLLER_AGENT:
        case LOAD_YAML_DEMO_CONTROLLER_COORDINATOR:
956
            // Set the "load demo yaml" button to be disabled
957
            ui->load_demo_yaml_button->setEnabled(false);
958
959
960
961
962
963
964

            // 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.
965
            m_timer_yaml_file_for_demo_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::demoYamlFileTimerCallback, this, true);    
966

967
968
            break;

969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
        case LOAD_YAML_STUDENT_CONTROLLER_AGENT:
        case LOAD_YAML_STUDENT_CONTROLLER_COORDINATOR:
            // Set the "load student yaml" button to be disabled
            ui->load_student_yaml_button->setEnabled(false);

            // 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.
            m_timer_yaml_file_for_student_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::studentYamlFileTimerCallback, this, true);    

            break;

        case LOAD_YAML_MPC_CONTROLLER_AGENT:
        case LOAD_YAML_MPC_CONTROLLER_COORDINATOR:
            // Set the "load mpc yaml" button to be disabled
            ui->load_mpc_yaml_button->setEnabled(false);

            // 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.
            m_timer_yaml_file_for_mpc_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::mpcYamlFileTimerCallback, this, true);    

            break;

999
1000
        case LOAD_YAML_REMOTE_CONTROLLER_AGENT:
        case LOAD_YAML_REMOTE_CONTROLLER_COORDINATOR:
For faster browsing, not all history is shown. View entire blame