MainWindow.cpp 26.2 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>
roangel's avatar
roangel committed
36

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

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

#include "d_fall_pps/ViconData.h"

45
46
#include "d_fall_pps/CustomButton.h"

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

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

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

58
59
    setCrazyRadioStatus(DISCONNECTED);

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

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

66
    ros::NodeHandle nodeHandle(m_ros_namespace);
67

68
69
70
    customSetpointPublisher = nodeHandle.advertise<Setpoint>("CustomControllerService/Setpoint", 1);
    customSetpointSubscriber = nodeHandle.subscribe("CustomControllerService/Setpoint", 1, &MainWindow::customSetpointCallback, this);

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

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

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

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

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

82

83
    safeSetpointSubscriber = nodeHandle.subscribe("SafeControllerService/Setpoint", 1, &MainWindow::safeSetpointCallback, this);
84
    DBChangedSubscriber = nodeHandle.subscribe("/my_GUI/DBChanged", 1, &MainWindow::DBChangedCallback, this);
85

86
    ros::NodeHandle my_nodeHandle("~");
87
    controllerSetpointPublisher = my_nodeHandle.advertise<Setpoint>("ControllerSetpoint", 1);
88

89

90
    // communication with PPS Client, just to make it possible to communicate through terminal also we use PPSClient's name
91
92
    //ros::NodeHandle nh_PPSClient(m_ros_namespace + "/PPSClient");
    ros::NodeHandle nh_PPSClient("PPSClient");
93
    crazyRadioCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
94
    PPSClientCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("Command", 1);
95

96
97
    PPSClientStudentCustomButtonPublisher = nh_PPSClient.advertise<CustomButton>("StudentCustomButton", 1);

98

99
    // > For publishing a message that requests the
100
101
102
103
104
    //   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);

105
106
107

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

    // First get student ID
111
    if(!nh_PPSClient.getParam("agentID", m_student_id))
112
    {
113
		ROS_ERROR("Failed to get agentID");
114
115
	}

116
117
118
119
120
121
    // Then, Central manager
    centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
    loadCrazyflieContext();



122

123
124
125
126
    // 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");
127

128
    if(!nodeHandle_for_safeController.getParam("defaultSetpoint", default_setpoint))
roangel's avatar
roangel committed
129
    {
130
        ROS_ERROR_STREAM("The StudentGUI could not find parameter 'defaultSetpoint', as called from main(...)");
roangel's avatar
roangel committed
131
132
    }

133
    // Copy the default setpoint into respective text fields of the GUI
roangel's avatar
roangel committed
134
135
136
137
138
    ui->current_setpoint_x->setText(QString::number(default_setpoint[0]));
    ui->current_setpoint_y->setText(QString::number(default_setpoint[1]));
    ui->current_setpoint_z->setText(QString::number(default_setpoint[2]));
    ui->current_setpoint_yaw->setText(QString::number(default_setpoint[3]));

139

140
    disableGUI();
141
    highlightSafeControllerTab();
142
    ui->label_battery->setStyleSheet("QLabel { color : red; }");
roangel's avatar
roangel committed
143
    m_battery_state = BATTERY_STATE_NORMAL;
144

145
146
147
    ui->error_label->setStyleSheet("QLabel { color : red; }");
    ui->error_label->clear();

148
    initialize_custom_setpoint();
roangel's avatar
roangel committed
149
150
}

151

roangel's avatar
roangel committed
152
153
154
155
MainWindow::~MainWindow()
{
    delete ui;
}
156

157
158
void MainWindow::disableGUI()
{
159
160
161
    ui->motors_OFF_button->setEnabled(false);
    ui->take_off_button->setEnabled(false);
    ui->land_button->setEnabled(false);
162
163
164
165
}

void MainWindow::enableGUI()
{
166
    ui->motors_OFF_button->setEnabled(true);
roangel's avatar
roangel committed
167
168
    if(m_battery_state == BATTERY_STATE_NORMAL)
    {
169
170
        ui->take_off_button->setEnabled(true);
        ui->land_button->setEnabled(true);
roangel's avatar
roangel committed
171
    }
172
173
}

174
175
176
177
178
179
180
181
182
183
184
void MainWindow::highlightSafeControllerTab()
{
    ui->tabWidget->tabBar()->setTabTextColor(0, Qt::green);
    ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
}
void MainWindow::highlightCustomControllerTab()
{
    ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(1, Qt::green);
}

185
186
187
void MainWindow::DBChangedCallback(const std_msgs::Int32& msg)
{
    loadCrazyflieContext();
188
    ROS_INFO("context reloaded in student_GUI");
189
190
}

191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
void MainWindow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
{
    switch(msg.data)
    {
        case SAFE_CONTROLLER:
            highlightSafeControllerTab();
            break;
        case CUSTOM_CONTROLLER:
            highlightCustomControllerTab();
            break;
        default:
            break;
    }
}

206
void MainWindow::safeSetpointCallback(const Setpoint& newSetpoint)
207
{
208
    m_safe_setpoint = newSetpoint;
209
    // here we get the new setpoint, need to update it in GUI
210
211
212
213
    ui->current_setpoint_x->setText(QString::number(newSetpoint.x, 'f', 3));
    ui->current_setpoint_y->setText(QString::number(newSetpoint.y, 'f', 3));
    ui->current_setpoint_z->setText(QString::number(newSetpoint.z, 'f', 3));
    ui->current_setpoint_yaw->setText(QString::number(newSetpoint.yaw * RAD2DEG, 'f', 1));
214
215
}

216
217
218
219
220
221
222
223
224
225
void MainWindow::customSetpointCallback(const Setpoint& newSetpoint)
{
    m_custom_setpoint = newSetpoint;
    // here we get the new setpoint, need to update it in GUI
    ui->current_setpoint_x_2->setText(QString::number(newSetpoint.x, 'f', 3));
    ui->current_setpoint_y_2->setText(QString::number(newSetpoint.y, 'f', 3));
    ui->current_setpoint_z_2->setText(QString::number(newSetpoint.z, 'f', 3));
    ui->current_setpoint_yaw_2->setText(QString::number(newSetpoint.yaw * RAD2DEG, 'f', 1));
}

226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
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);
}

249
250
void MainWindow::batteryStateChangedCallback(const std_msgs::Int32& msg)
{
251
252
253
254
255
256
    // 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!");
257
258
259
260
            ui->take_off_button->setEnabled(false);
            ui->land_button->setEnabled(false);
            // ui->groupBox_4->setEnabled(false);

261
            ui->label_battery->setText(qstr);
roangel's avatar
roangel committed
262
            m_battery_state = BATTERY_STATE_LOW;
263
264
            break;
        case BATTERY_STATE_NORMAL:
265
266
267
268
            // ui->groupBox_4->setEnabled(true);
            ui->take_off_button->setEnabled(true);
            ui->land_button->setEnabled(true);

269
            ui->label_battery->clear();
roangel's avatar
roangel committed
270
            m_battery_state = BATTERY_STATE_NORMAL;
271
272
273
274
            break;
        default:
            break;
    }
275
276
277
}


278
279
280
281
282
283
284
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
285
            ui->connected_disconnected_label->setText("Crazyradio status: Connected!");
286
            enableGUI();
287
288
289
            break;
        case CONNECTING:
            // change icon
290
            ui->connected_disconnected_label->setText("Crazyradio status: Connecting...");
291
292
293
            break;
        case DISCONNECTED:
            // change icon, the rest of the GUI is disabled
294
            ui->connected_disconnected_label->setText("Crazyradio status: Disconnected");
295
            disableGUI();
296
297
298
299
300
301
302
303
            break;
        default:
    		ROS_ERROR_STREAM("unexpected radio status: " << m_radio_status);
            break;
    }
    this->m_radio_status = radio_status;
}

304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
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;
319
320
321
322
323
324
325

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

326
327
328
329
330
331
332
    return percentage;
}

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

roangel's avatar
roangel committed
335
    QString qstr = "";
roangel's avatar
roangel committed
336
    qstr.append(QString::number(battery_voltage, 'f', 2));
roangel's avatar
roangel committed
337
338
    qstr.append(" V");
    ui->voltage_field->setText(qstr);
339
340
341
342
343
344
345
}

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

346
347
void MainWindow::crazyRadioStatusCallback(const std_msgs::Int32& msg)
{
348
    ROS_INFO("Callback CrazyRadioStatus called");
349
350
351
    this->setCrazyRadioStatus(msg.data);
}

352
353
354
355
356
357
358
359
void MainWindow::loadCrazyflieContext()
{
	CMQuery contextCall;
	contextCall.request.studentID = m_student_id;
	ROS_INFO_STREAM("StudentID:" << m_student_id);

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

360
361
	if(centralManager.call(contextCall))
    {
362
363
		m_context = contextCall.response.crazyflieContext;
		ROS_INFO_STREAM("CrazyflieContext:\n" << m_context);
364
365
366
367
368
369
370
        // 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);
371
372
373
	}
    else
    {
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
		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;
}


396
397
void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to newViconData, from node
{
398
399
400
    for(std::vector<CrazyflieData>::const_iterator it = p_msg->crazyflies.begin(); it != p_msg->crazyflies.end(); ++it)
    {
		CrazyflieData global = *it;
401
402
403
404
405
406
        if(global.crazyflieName == m_context.crazyflieName)
        {
            CrazyflieData local = global;
            coordinatesToLocal(local);

            // now we have the local coordinates, put them in the labels
roangel's avatar
roangel committed
407
408
409
            ui->current_x->setText(QString::number(local.x, 'f', 3));
            ui->current_y->setText(QString::number(local.y, 'f', 3));
            ui->current_z->setText(QString::number(local.z, 'f', 3));
410
411
412
            ui->current_yaw->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
            ui->current_pitch->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
            ui->current_roll->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
413

414
415
416
417
418
419
420
            ui->current_x_2->setText(QString::number(local.x, 'f', 3));
            ui->current_y_2->setText(QString::number(local.y, 'f', 3));
            ui->current_z_2->setText(QString::number(local.z, 'f', 3));
            ui->current_yaw_2->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
            ui->current_pitch_2->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
            ui->current_roll_2->setText(QString::number(local.roll * RAD2DEG, 'f', 1));

421
            // also update diff
422
423
424
425
426
427
428
429
430
            ui->diff_x->setText(QString::number(m_safe_setpoint.x - local.x, 'f', 3));
            ui->diff_y->setText(QString::number(m_safe_setpoint.y - local.y, 'f', 3));
            ui->diff_z->setText(QString::number(m_safe_setpoint.z - local.z, 'f', 3));
            ui->diff_yaw->setText(QString::number((m_safe_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));

            ui->diff_x_2->setText(QString::number(m_custom_setpoint.x - local.x, 'f', 3));
            ui->diff_y_2->setText(QString::number(m_custom_setpoint.y - local.y, 'f', 3));
            ui->diff_z_2->setText(QString::number(m_custom_setpoint.z - local.z, 'f', 3));
            ui->diff_yaw_2->setText(QString::number((m_custom_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
431
        }
432
    }
433
}
434
435
436
437
438

void MainWindow::on_RF_Connect_button_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_RECONNECT;
439
    this->crazyRadioCommandPublisher.publish(msg);
roangel's avatar
roangel committed
440
    ROS_INFO("command reconnect published");
441
}
442

443
void MainWindow::on_take_off_button_clicked()
444
{
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
    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);
462
}
463
464
465
466

void MainWindow::on_set_setpoint_button_clicked()
{
    Setpoint msg_setpoint;
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485

    // initialize setpoint to previous one

    msg_setpoint.x = (ui->current_setpoint_x->text()).toFloat();
    msg_setpoint.y = (ui->current_setpoint_y->text()).toFloat();
    msg_setpoint.z = (ui->current_setpoint_z->text()).toFloat();
    msg_setpoint.yaw = (ui->current_setpoint_yaw->text()).toFloat();

    if(!ui->new_setpoint_x->text().isEmpty())
        msg_setpoint.x = (ui->new_setpoint_x->text()).toFloat();

    if(!ui->new_setpoint_y->text().isEmpty())
        msg_setpoint.y = (ui->new_setpoint_y->text()).toFloat();

    if(!ui->new_setpoint_z->text().isEmpty())
        msg_setpoint.z = (ui->new_setpoint_z->text()).toFloat();

    if(!ui->new_setpoint_yaw->text().isEmpty())
        msg_setpoint.yaw = (ui->new_setpoint_yaw->text()).toFloat() * DEG2RAD;
486

487
488
489
490
491
492
493
494
495
496
497
498
499
500

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

501
    this->controllerSetpointPublisher.publish(msg_setpoint);
502
503

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

506
507
508
509
510
511
512
513
514
515
516
void MainWindow::initialize_custom_setpoint()
{
    Setpoint msg_setpoint;
    msg_setpoint.x = 0;
    msg_setpoint.y = 0;
    msg_setpoint.z = 0.4;
    msg_setpoint.yaw = 0;

    this->customSetpointPublisher.publish(msg_setpoint);
}

517
518
519
void MainWindow::on_set_setpoint_button_2_clicked()
{
    Setpoint msg_setpoint;
520
521
522
523
524
525
526
527
528
529
530
531
532
533

    msg_setpoint.x = (ui->current_setpoint_x_2->text()).toFloat();
    msg_setpoint.y = (ui->current_setpoint_y_2->text()).toFloat();
    msg_setpoint.z = (ui->current_setpoint_z_2->text()).toFloat();
    msg_setpoint.yaw = (ui->current_setpoint_yaw_2->text()).toFloat();

    if(!ui->new_setpoint_x_2->text().isEmpty())
        msg_setpoint.x = (ui->new_setpoint_x_2->text()).toFloat();
    if(!ui->new_setpoint_y_2->text().isEmpty())
        msg_setpoint.y = (ui->new_setpoint_y_2->text()).toFloat();
    if(!ui->new_setpoint_z_2->text().isEmpty())
        msg_setpoint.z = (ui->new_setpoint_z_2->text()).toFloat();
    if(!ui->new_setpoint_yaw_2->text().isEmpty())
        msg_setpoint.yaw = (ui->new_setpoint_yaw_2->text()).toFloat() * DEG2RAD;
534
535

    this->customSetpointPublisher.publish(msg_setpoint);
536
537

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

540
void MainWindow::on_RF_disconnect_button_clicked()
541
542
543
544
545
546
{
    std_msgs::Int32 msg;
    msg.data = CMD_DISCONNECT;
    this->crazyRadioCommandPublisher.publish(msg);
    ROS_INFO("command disconnect published");
}
547

548
549


550
551

void MainWindow::on_load_safe_yaml_button_clicked()
552
{
553
    // Set the "load safe yaml" button to be disabled
554
    ui->load_safe_yaml_button->setEnabled(false);
555
556
557
558

    // Send a message requesting the parameters from the YAML
    // file to be reloaded for the safe controller
    std_msgs::Int32 msg;
559
    msg.data = LOAD_YAML_SAFE_CONTROLLER_AGENT;
560
561
562
563
564
565
566
567
568
    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.
569
    ros::NodeHandle nodeHandle("~");
beuchatp's avatar
beuchatp committed
570
    m_timer_yaml_file_for_safe_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::safeYamlFileTimerCallback, this, true);
571
}
roangel's avatar
roangel committed
572

573
574
575
576
577
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
578
579


580

581
582


583
void MainWindow::on_load_custom_yaml_button_clicked()
584
{
585
586
587
588
589
    // Set the "load custom yaml" button to be disabled
    ui->load_custom_yaml_button->setEnabled(false);

    // Send a message requesting the parameters from the YAML
    // file to be reloaded for the custom controller
590
    std_msgs::Int32 msg;
591
    msg.data = LOAD_YAML_CUSTOM_CONTROLLER_AGENT;
592
593
594
595
596
597
598
599
600
601
    this->requestLoadControllerYamlPublisher.publish(msg);
    ROS_INFO("Request load of custom 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("~");
beuchatp's avatar
beuchatp committed
602
    m_timer_yaml_file_for_custom_controlller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::customYamlFileTimerCallback, this, true);    
603
604
605
606
607
}

void MainWindow::customYamlFileTimerCallback(const ros::TimerEvent&)
{
    // Enble the "load custom yaml" button again
608
609
610
    ui->load_custom_yaml_button->setEnabled(true);
}

611
612
613
614




615
void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::Int32& msg)
616
{
617
618
    // Extract from the "msg" for which controller the YAML
    // parameters should be loaded
beuchatp's avatar
beuchatp committed
619
620
    int controller_to_load_yaml = msg.data;

beuchatp's avatar
beuchatp committed
621
    // Create the "nodeHandle" needed in the switch cases below
beuchatp's avatar
beuchatp committed
622
    ros::NodeHandle nodeHandle("~");
623

624
625
626
    // Switch between loading for the different controllers
    switch(controller_to_load_yaml)
    {
627
628
        case LOAD_YAML_SAFE_CONTROLLER_AGENT:
        case LOAD_YAML_SAFE_CONTROLLER_COORDINATOR:
629
630
631
632
633
634
635
636
637
            // 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
638
            m_timer_yaml_file_for_safe_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::safeYamlFileTimerCallback, this, true);
639

640
641
            break;

642
643
        case LOAD_YAML_CUSTOM_CONTROLLER_AGENT:
        case LOAD_YAML_CUSTOM_CONTROLLER_COORDINATOR:
644
645
646
647
648
649
650
651
652
            // Set the "load custom yaml" button to be disabled
            ui->load_custom_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
653
            m_timer_yaml_file_for_custom_controlller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::customYamlFileTimerCallback, this, true);    
654

655
656
657
658
659
660
            break;

        default:
            ROS_INFO("Unknown 'all controllers to load yaml' command, thus nothing will be disabled");
            break;
    }
661
662
}

663
664
665



666
667
668
669
void MainWindow::on_en_custom_controller_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_USE_CUSTOM_CONTROLLER;
roangel's avatar
roangel committed
670
    this->PPSClientCommandPublisher.publish(msg);
671
672
}

673

674
675
676
677
void MainWindow::on_en_safe_controller_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_USE_SAFE_CONTROLLER;
roangel's avatar
roangel committed
678
    this->PPSClientCommandPublisher.publish(msg);
679
}
680
681
682
683
684
685
686
687

void MainWindow::on_customButton_1_clicked()
{
    CustomButton msg_custom_button;
    msg_custom_button.button_index = 1;
    msg_custom_button.command_code = 0;
    this->PPSClientStudentCustomButtonPublisher.publish(msg_custom_button);

688
    ROS_INFO("Custom button 1 pressed in GUI");
689
690
691
692
693
694
695
696
}

void MainWindow::on_customButton_2_clicked()
{
    CustomButton msg_custom_button;
    msg_custom_button.button_index = 2;
    msg_custom_button.command_code = 0;
    this->PPSClientStudentCustomButtonPublisher.publish(msg_custom_button);
697
    ROS_INFO("Custom button 2 pressed in GUI");
698
699
700
701
702
703
704
705
}

void MainWindow::on_customButton_3_clicked()
{
    CustomButton msg_custom_button;
    msg_custom_button.button_index = 3;
    msg_custom_button.command_code = (ui->custom_command_3->text()).toFloat();
    this->PPSClientStudentCustomButtonPublisher.publish(msg_custom_button);
706
    ROS_INFO("Custom button 3 pressed in GUI");
707
}
708
709
710
711

Setpoint MainWindow::correctSetpointBox(Setpoint setpoint, CrazyflieContext context)
{
    Setpoint corrected_setpoint;
Paul Beuchat's avatar
Paul Beuchat committed
712
    corrected_setpoint =  setpoint;
713

Paul Beuchat's avatar
Paul Beuchat committed
714
    float x_size = context.localArea.xmax - context.localArea.xmin;
Paul Beuchat's avatar
Paul Beuchat committed
715
716
    float y_size = context.localArea.ymax - context.localArea.ymin;
    float z_size = context.localArea.zmax - context.localArea.zmin;
Paul Beuchat's avatar
Paul Beuchat committed
717
718
719
720
721
722
723
724
725
726
727
728
729
730

    if(setpoint.x > x_size/2)
        corrected_setpoint.x = x_size/2;
    if(setpoint.y > y_size/2)
        corrected_setpoint.y = y_size/2;
    if(setpoint.z > z_size)
        corrected_setpoint.z = z_size;

    if(setpoint.x < -x_size/2)
        corrected_setpoint.x = -x_size/2;
    if(setpoint.y < -y_size/2)
        corrected_setpoint.y = -y_size/2;
    if(setpoint.z < 0)
        corrected_setpoint.z = 0;
731

Paul Beuchat's avatar
Paul Beuchat committed
732
    return corrected_setpoint;
733
734
735
736
}

bool MainWindow::setpointInsideBox(Setpoint setpoint, CrazyflieContext context)
{
Paul Beuchat's avatar
Paul Beuchat committed
737
738

    float x_size = context.localArea.xmax - context.localArea.xmin;
Paul Beuchat's avatar
Paul Beuchat committed
739
740
    float y_size = context.localArea.ymax - context.localArea.ymin;
    float z_size = context.localArea.zmax - context.localArea.zmin;
741
    //position check
Paul Beuchat's avatar
Paul Beuchat committed
742
	if((setpoint.x < -x_size/2) or (setpoint.x > x_size/2)) {
743
744
745
		ROS_INFO_STREAM("x outside safety box");
		return false;
	}
Paul Beuchat's avatar
Paul Beuchat committed
746
	if((setpoint.y < -y_size/2) or (setpoint.y > y_size/2)) {
747
748
749
		ROS_INFO_STREAM("y outside safety box");
		return false;
	}
Paul Beuchat's avatar
Paul Beuchat committed
750
	if((setpoint.z < 0) or (setpoint.z > z_size)) {
751
752
753
754
755
756
		ROS_INFO_STREAM("z outside safety box");
		return false;
	}

	return true;
}