studentcontrollertab.cpp 32 KB
Newer Older
beuchatp's avatar
beuchatp committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
//    Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
//
//    This file is part of D-FaLL-System.
//
//    D-FaLL-System is free software: you can redistribute it and/or modify
//    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.
//
//    D-FaLL-System is distributed in the hope that it will be useful,
//    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.
//
//    You should have received a copy of the GNU General Public License
//    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:
//    The GUI for the Student Controller
//
//    ----------------------------------------------------------------------------------





36
37
38
39
40
41
42
43
#include "studentcontrollertab.h"
#include "ui_studentcontrollertab.h"

StudentControllerTab::StudentControllerTab(QWidget *parent) :
    QWidget(parent),
    ui(new Ui::StudentControllerTab)
{
    ui->setupUi(this);
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74

    // Hide the two red frames that are used to indcated
    // when pose data is occluded
    ui->red_frame_position_left->setVisible(false);
    ui->red_frame_position_right->setVisible(false);



#ifdef CATKIN_MAKE

    //ros::init();

    // Get the namespace of this node
    std::string this_namespace = ros::this_node::getNamespace();
    ROS_INFO_STREAM("[STUDENT CONTROLLER TAB GUI] ros::this_node::getNamespace() =  " << this_namespace);

    // Get the type and ID of this flying agent GUI
    bool isValid_type_and_ID = getTypeAndIDParameters();

    // Stall if the node IDs are not valid
    if ( !isValid_type_and_ID )
    {
        ROS_ERROR("[STUDENT CONTROLLER TAB GUI] Node NOT FUNCTIONING :-)");
        ros::spin();
    }


    // CREATE A NODE HANDLE TO THIS GUI
    ros::NodeHandle nodeHandle_for_this_gui(this_namespace);

    // CREATE THE REQUEST SETPOINT CHANGE PUBLISHER
75
    requestSetpointChangePublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::SetpointWithHeader>("StudentControllerService/RequestSetpointChange", 1);
76
77
78
79
80
81
82
83

    // SUBSCRIBE TO SETPOINT CHANGES
    // Only if this is an agent GUI
    if (m_type == TYPE_AGENT)
    {
        setpointChangedSubscriber = nodeHandle_for_this_gui.subscribe("StudentControllerService/SetpointChanged", 1, &StudentControllerTab::setpointChangedCallback, this);
    }

84
    // CREATE THE CUSTOM BUTTON PRESSED PUBLISHER
85
    customButtonPublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::CustomButtonWithHeader>("StudentControllerService/CustomButtonPressed", 1);
86

87
88
89
90
91
    // GET THE CURRENT SETPOINT
    // Only if this is an agent GUI
    if (m_type == TYPE_AGENT)
    {
        // > Request the current setpoint
92
93
        ros::ServiceClient getCurrentSetpointServiceClient = nodeHandle_for_this_gui.serviceClient<dfall_pkg::GetSetpointService>("StudentControllerService/GetCurrentSetpoint", false);
        dfall_pkg::GetSetpointService getSetpointCall;
94
95
96
97
98
99
100
101
102
103
104
105
106
        getSetpointCall.request.data = 0;
        getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
        if(getCurrentSetpointServiceClient.call(getSetpointCall))
        {
            setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
        }
        else
        {
            // Inform the user
            ROS_INFO("[STUDENT CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
        }
    }

107
108
#endif

109
110
111
112
113
114
}

StudentControllerTab::~StudentControllerTab()
{
    delete ui;
}
115

116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138




//    ----------------------------------------------------------------------------------
//     CCCC  U   U   SSSS  TTTTT   OOO   M   M
//    C      U   U  S        T    O   O  MM MM
//    C      U   U   SSS     T    O   O  M M M
//    C      U   U      S    T    O   O  M   M
//     CCCC   UUU   SSSS     T     OOO   M   M
//
//    BBBB   U   U  TTTTT  TTTTT   OOO   N   N   SSSS
//    B   B  U   U    T      T    O   O  NN  N  S
//    BBBB   U   U    T      T    O   O  N N N   SSS
//    B   B  U   U    T      T    O   O  N  NN      S
//    BBBB    UUU     T      T     OOO   N   N  SSSS
//    ----------------------------------------------------------------------------------


#ifdef CATKIN_MAKE
void StudentControllerTab::publish_custom_button_command(int button_index , QLineEdit * lineEdit_pointer)
{
    // Initialise the message as a local variable
139
    dfall_pkg::CustomButtonWithHeader msg;
140
141
142
143
144
    // Fill the header of the message
    fillCustomButtonMessageHeader( msg );
    // Fill in the button index
    msg.button_index = button_index;
    // Get the line edit data, as a float if possible
beuchatp's avatar
beuchatp committed
145
146
    bool isValidFloat = false;
    float lineEdit_as_float = (lineEdit_pointer->text()).toFloat(&isValidFloat);
147
    // Fill in the data
beuchatp's avatar
beuchatp committed
148
    if (isValidFloat)
149
150
151
152
153
154
155
156
157
158
159
160
161
162
        msg.float_data = lineEdit_as_float;
    else
        msg.string_data = (lineEdit_pointer->text()).toStdString();
    // Publish the setpoint
    this->customButtonPublisher.publish(msg);
    // Inform the user about the change
    ROS_INFO_STREAM("[STUDENT CONTROLLER TAB GUI] button " << button_index << " clicked.");
}
#endif


void StudentControllerTab::on_custom_button_1_clicked()
{
#ifdef CATKIN_MAKE
beuchatp's avatar
beuchatp committed
163
    publish_custom_button_command(1,ui->lineEdit_custom_1);
164
165
166
167
168
169
#endif
}

void StudentControllerTab::on_custom_button_2_clicked()
{
#ifdef CATKIN_MAKE
170
    publish_custom_button_command(2,ui->lineEdit_custom_2);
171
172
173
174
175
176
#endif
}

void StudentControllerTab::on_custom_button_3_clicked()
{
#ifdef CATKIN_MAKE
177
    publish_custom_button_command(3,ui->lineEdit_custom_3);
178
179
180
181
182
183
#endif
}

void StudentControllerTab::on_custom_button_4_clicked()
{
#ifdef CATKIN_MAKE
184
    publish_custom_button_command(4,ui->lineEdit_custom_4);
185
186
187
188
189
190
#endif
}

void StudentControllerTab::on_custom_button_5_clicked()
{
#ifdef CATKIN_MAKE
191
    publish_custom_button_command(5,ui->lineEdit_custom_5);
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
#endif
}





//    ----------------------------------------------------------------------------------
//    PPPP    OOO    SSSS  EEEEE     DDDD     A    TTTTT    A
//    P   P  O   O  S      E         D   D   A A     T     A A
//    PPPP   O   O   SSS   EEE       D   D  A   A    T    A   A
//    P      O   O      S  E         D   D  AAAAA    T    AAAAA
//    P       OOO   SSSS   EEEEE     DDDD   A   A    T    A   A
//    ----------------------------------------------------------------------------------


208
void StudentControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded)
209
{
210
211
    if (!occluded)
    {
212
213
        // INITIALISE A STRING VARIABLE FOR ADDING THE "+"
        QString qstr = "";
214
        // UPDATE THE MEASUREMENT COLUMN
215
216
217
218
219
220
221
222
223
224
225
226
227
        if (x < 0.0f) qstr = ""; else qstr = "+";
        ui->lineEdit_measured_x->setText(qstr + QString::number( x, 'f', 3));
        if (y < 0.0f) qstr = ""; else qstr = "+";
        ui->lineEdit_measured_y->setText(qstr + QString::number( y, 'f', 3));
        if (z < 0.0f) qstr = ""; else qstr = "+";
        ui->lineEdit_measured_z->setText(qstr + QString::number( z, 'f', 3));

        if (roll < 0.0f) qstr = ""; else qstr = "+";
        ui->lineEdit_measured_roll->setText(qstr + QString::number( roll  * RAD2DEG, 'f', 1));
        if (pitch < 0.0f) qstr = ""; else qstr = "+";
        ui->lineEdit_measured_pitch->setText(qstr + QString::number( pitch * RAD2DEG, 'f', 1));
        if (yaw < 0.0f) qstr = ""; else qstr = "+";
        ui->lineEdit_measured_yaw->setText(qstr + QString::number( yaw   * RAD2DEG, 'f', 1));
228

229
        // GET THE CURRENT SETPOINT
230
231
232
233
        float error_x   = x   - (ui->lineEdit_setpoint_current_x->text()  ).toFloat();
        float error_y   = y   - (ui->lineEdit_setpoint_current_y->text()  ).toFloat();
        float error_z   = z   - (ui->lineEdit_setpoint_current_z->text()  ).toFloat();
        float error_yaw = yaw - (ui->lineEdit_setpoint_current_yaw->text()).toFloat();
234

235
        // UPDATE THE ERROR COLUMN
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
        if (error_x < 0.0f) qstr = ""; else qstr = "+";
        ui->lineEdit_error_x->setText(qstr + QString::number( error_x, 'f', 3));
        if (error_y < 0.0f) qstr = ""; else qstr = "+";
        ui->lineEdit_error_y->setText(qstr + QString::number( error_y, 'f', 3));
        if (error_z < 0.0f) qstr = ""; else qstr = "+";
        ui->lineEdit_error_z->setText(qstr + QString::number( error_z, 'f', 3));

        if (error_yaw < 0.0f) qstr = ""; else qstr = "+";
        ui->lineEdit_error_yaw->setText(qstr + QString::number( error_yaw * RAD2DEG, 'f', 1));

        // Ensure the red frames are not visible
        if ( ui->red_frame_position_left->isVisible() )
            ui->red_frame_position_left->setVisible(false);
        if ( ui->red_frame_position_right->isVisible() )
            ui->red_frame_position_right->setVisible(false);
    }
    else
    {
        // Make visible the red frames to indicate occluded
        if ( !(ui->red_frame_position_left->isVisible()) )
            ui->red_frame_position_left->setVisible(true);
        if ( !(ui->red_frame_position_right->isVisible()) )
            ui->red_frame_position_right->setVisible(true);
259
    }
260
}
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311


void StudentControllerTab::poseDataUnavailableSlot()
{
    ui->lineEdit_measured_x->setText("xx.xx");
    ui->lineEdit_measured_y->setText("xx.xx");
    ui->lineEdit_measured_z->setText("xx.xx");

    ui->lineEdit_measured_roll->setText("xx.xx");
    ui->lineEdit_measured_pitch->setText("xx.xx");
    ui->lineEdit_measured_yaw->setText("xx.xx");

    ui->lineEdit_error_x->setText("xx.xx");
    ui->lineEdit_error_y->setText("xx.xx");
    ui->lineEdit_error_z->setText("xx.xx");
    ui->lineEdit_error_yaw->setText("xx.xx");


}










//    ----------------------------------------------------------------------------------
//     SSSS  EEEEE  TTTTT  PPPP    OOO   III  N   N  TTTTT
//    S      E        T    P   P  O   O   I   NN  N    T
//     SSS   EEE      T    PPPP   O   O   I   N N N    T
//        S  E        T    P      O   O   I   N  NN    T
//    SSSS   EEEEE    T    P       OOO   III  N   N    T
//
//     CCCC  H   H    A    N   N   GGGG  EEEEE  DDDD
//    C      H   H   A A   NN  N  G      E      D   D
//    C      HHHHH  A   A  N N N  G      EEE    D   D
//    C      H   H  AAAAA  N  NN  G   G  E      D   D
//     CCCC  H   H  A   A  N   N   GGGG  EEEEE  DDDD
//
//     CCCC    A    L      L      BBBB     A     CCCC  K   K
//    C       A A   L      L      B   B   A A   C      K  K
//    C      A   A  L      L      BBBB   A   A  C      KKK
//    C      AAAAA  L      L      B   B  AAAAA  C      K  K
//     CCCC  A   A  LLLLL  LLLLL  BBBB   A   A   CCCC  K   K
//    ----------------------------------------------------------------------------------


#ifdef CATKIN_MAKE
312
void StudentControllerTab::setpointChangedCallback(const dfall_pkg::SetpointWithHeader& newSetpoint)
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
{
    // INITIALISE A STRING VARIABLE FOR ADDING THE "+"
    QString qstr = "";

    // EXTRACT THE SETPOINT
    float x = newSetpoint.x;
    float y = newSetpoint.y;
    float z = newSetpoint.z;
    float yaw = newSetpoint.yaw;

    // UPDATE THE SETPOINT COLUMN
    if (x < 0.0f) qstr = ""; else qstr = "+";
    ui->lineEdit_setpoint_current_x->setText(qstr + QString::number( x, 'f', 3));
    if (y < 0.0f) qstr = ""; else qstr = "+";
    ui->lineEdit_setpoint_current_y->setText(qstr + QString::number( y, 'f', 3));
    if (z < 0.0f) qstr = ""; else qstr = "+";
    ui->lineEdit_setpoint_current_z->setText(qstr + QString::number( z, 'f', 3));

    if (yaw < 0.0f) qstr = ""; else qstr = "+";
332
    ui->lineEdit_setpoint_current_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', 1));
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
}
#endif










//    ----------------------------------------------------------------------------------
//    RRRR   EEEEE   QQQ   U   U  EEEEE   SSSS  TTTTT     N   N  EEEEE  W     W
//    R   R  E      Q   Q  U   U  E      S        T       NN  N  E      W     W
//    RRRR   EEE    Q   Q  U   U  EEE     SSS     T       N N N  EEE    W     W 
//    R   R  E      Q  Q   U   U  E          S    T       N  NN  E       W W W
//    R   R  EEEEE   QQ Q   UUU   EEEEE  SSSS     T       N   N  EEEEE    W W
//
//     SSSS  EEEEE  TTTTT  PPPP    OOO   III  N   N  TTTTT
//    S      E        T    P   P  O   O   I   NN  N    T
//     SSS   EEE      T    PPPP   O   O   I   N N N    T
//        S  E        T    P      O   O   I   N  NN    T
//    SSSS   EEEEE    T    P       OOO   III  N   N    T
//    ----------------------------------------------------------------------------------


360
void StudentControllerTab::publishSetpoint(float x, float y, float z, float yaw_degrees)
361
362
363
{
#ifdef CATKIN_MAKE
    // Initialise the message as a local variable
364
    dfall_pkg::SetpointWithHeader msg;
365
366
367
368
369
370
371
372

    // Fill the header of the message
    fillSetpointMessageHeader( msg );

    // Fill in the (x,y,z,yaw) values
    msg.x   = x;
    msg.y   = y;
    msg.z   = z;
373
    msg.yaw = yaw_degrees * DEG2RAD;
374
375
376
377
378

    // Publish the setpoint
    this->requestSetpointChangePublisher.publish(msg);

    // Inform the user about the change
379
    ROS_INFO_STREAM("[STUDENT CONTROLLER GUI] Published request for setpoint change to: [" << x << ", "<< y << ", "<< z << ", "<< yaw_degrees << "]");
380
381
#else
    // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
382
    QTextStream(stdout) << "[STUDENT CONTROLLER GUI] would publish request for: [" << x << ", "<< y << ", "<< z << ", "<< yaw_degrees << "]";
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
#endif
}



void StudentControllerTab::on_lineEdit_setpoint_new_x_returnPressed()
{
    ui->set_setpoint_button->animateClick();
}

void StudentControllerTab::on_lineEdit_setpoint_new_y_returnPressed()
{
    ui->set_setpoint_button->animateClick();
}

void StudentControllerTab::on_lineEdit_setpoint_new_z_returnPressed()
{
    ui->set_setpoint_button->animateClick();
}

void StudentControllerTab::on_lineEdit_setpoint_new_yaw_returnPressed()
{
    ui->set_setpoint_button->animateClick();
}

void StudentControllerTab::on_set_setpoint_button_clicked()
{

    // Initialise local variable for each of (x,y,z,yaw)
    float x = 0.0f, y = 0.0f, z = 0.0f, yaw = 0.0f;

    // Take the new value if available, otherwise use the old value
    // > For x
    if(!ui->lineEdit_setpoint_new_x->text().isEmpty())
        x = (ui->lineEdit_setpoint_new_x->text()).toFloat();
    else
        x = (ui->lineEdit_setpoint_current_x->text()).toFloat();
420
    // > For y
421
422
423
424
    if(!ui->lineEdit_setpoint_new_y->text().isEmpty())
        y = (ui->lineEdit_setpoint_new_y->text()).toFloat();
    else
        y = (ui->lineEdit_setpoint_current_y->text()).toFloat();
425
    // > For z
426
427
428
429
    if(!ui->lineEdit_setpoint_new_z->text().isEmpty())
        z = (ui->lineEdit_setpoint_new_z->text()).toFloat();
    else
        z = (ui->lineEdit_setpoint_current_z->text()).toFloat();
430
    // > For yaws
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
    if(!ui->lineEdit_setpoint_new_yaw->text().isEmpty())
        yaw = (ui->lineEdit_setpoint_new_yaw->text()).toFloat();
    else
        yaw = (ui->lineEdit_setpoint_current_yaw->text()).toFloat();

    // Call the function to publish the setpoint
    publishSetpoint(x,y,z,yaw);
}

void StudentControllerTab::on_default_setpoint_button_clicked()
{
#ifdef CATKIN_MAKE
    // Publish this as a blank setpoint with the 
    // "buttonID" field set appropriately

    // Initialise the message as a local variable
447
    dfall_pkg::SetpointWithHeader msg;
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467

    // Fill the header of the message
    fillSetpointMessageHeader( msg );

    // Fill in the (x,y,z,yaw) values
    msg.buttonID = REQUEST_DEFAULT_SETPOINT_BUTTON_ID;

    // Publish the default setpoint button press
    this->requestSetpointChangePublisher.publish(msg);

    // Inform the user about the change
    ROS_INFO_STREAM("[STUDENT CONTROLLER GUI] Published request for setpoint change to the default");
#endif
}

void StudentControllerTab::on_x_increment_plus_button_clicked()
{
    // Only need to do something if the field is not empty
    if(!ui->lineEdit_setpoint_increment_x->text().isEmpty())
    {
468
        increment_setpoint_by( (ui->lineEdit_setpoint_increment_x->text()).toFloat() ,0.0,0.0,0.0);
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
    }
    else
    {
        #ifdef CATKIN_MAKE
        // Inform the user that nothing can be done
        ROS_INFO_STREAM("[Student CONTROLLER GUI] Increment x setpoint clicked but field is empty");
        #endif

    }
}

void StudentControllerTab::on_x_increment_minus_button_clicked()
{

    // Only need to do something if the field is not empty
    if(!ui->lineEdit_setpoint_increment_x->text().isEmpty())
    {
486
        increment_setpoint_by( -(ui->lineEdit_setpoint_increment_x->text()).toFloat() ,0.0,0.0,0.0);
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
    }
    else
    {
        #ifdef CATKIN_MAKE
        // Inform the user that nothing can be done
        ROS_INFO_STREAM("[STUDENT CONTROLLER GUI] Increment x setpoint clicked but field is empty");
        #endif
    }
}

void StudentControllerTab::on_y_increment_plus_button_clicked()
{
    // Only need to do something if the field is not empty
    if(!ui->lineEdit_setpoint_increment_y->text().isEmpty())
    {
502
        increment_setpoint_by(0.0, (ui->lineEdit_setpoint_increment_y->text()).toFloat() ,0.0,0.0);
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
    }
    else
    {
        #ifdef CATKIN_MAKE
        // Inform the user that nothing can be done
        ROS_INFO_STREAM("[STUDENT CONTROLLER GUI] Increment x setpoint clicked but field is empty");
        #endif
    }
}

void StudentControllerTab::on_y_increment_minus_button_clicked()
{
    // Only need to do something if the field is not empty
    if(!ui->lineEdit_setpoint_increment_y->text().isEmpty())
    {
518
        increment_setpoint_by(0.0, -(ui->lineEdit_setpoint_increment_y->text()).toFloat() ,0.0,0.0);
519
520
521
522
523
    }
    else
    {
        #ifdef CATKIN_MAKE
        // Inform the user that nothing can be done
524
        ROS_INFO_STREAM("[STUDENT CONTROLLER GUI] Increment y setpoint clicked but field is empty");
525
526
527
528
529
530
531
532
533
        #endif
    }
}

void StudentControllerTab::on_z_increment_plus_button_clicked()
{
    // Only need to do something if the field is not empty
    if(!ui->lineEdit_setpoint_increment_z->text().isEmpty())
    {
534
        increment_setpoint_by(0.0,0.0, (ui->lineEdit_setpoint_increment_z->text()).toFloat() ,0.0);
535
536
537
538
539
    }
    else
    {
        #ifdef CATKIN_MAKE
        // Inform the user that nothing can be done
540
        ROS_INFO_STREAM("[STUDENT CONTROLLER GUI] Increment z setpoint clicked but field is empty");
541
542
543
544
545
546
547
548
549
        #endif
    }
}

void StudentControllerTab::on_z_increment_minus_button_clicked()
{
    // Only need to do something if the field is not empty
    if(!ui->lineEdit_setpoint_increment_z->text().isEmpty())
    {
550
551
        // Call the function to increment the setpoint
        increment_setpoint_by(0.0,0.0, -(ui->lineEdit_setpoint_increment_z->text()).toFloat() ,0.0);
552
553
554
555
556
    }
    else
    {
        #ifdef CATKIN_MAKE
        // Inform the user that nothing can be done
557
        ROS_INFO_STREAM("[STUDENT CONTROLLER GUI] Increment z setpoint clicked but field is empty");
558
559
560
561
562
563
564
565
566
        #endif
    }
}

void StudentControllerTab::on_yaw_increment_plus_button_clicked()
{
    // Only need to do something if the field is not empty
    if(!ui->lineEdit_setpoint_increment_yaw->text().isEmpty())
    {
567
568
        // Call the function to increment the setpoint
        increment_setpoint_by(0.0,0.0,0.0, (ui->lineEdit_setpoint_increment_yaw->text()).toFloat() );
569
570
571
572
573
    }
    else
    {
        #ifdef CATKIN_MAKE
        // Inform the user that nothing can be done
574
        ROS_INFO_STREAM("[STUDENT CONTROLLER GUI] Increment yaw setpoint clicked but field is empty");
575
576
577
578
579
580
581
582
        #endif
    }
}
void StudentControllerTab::on_yaw_increment_minus_button_clicked()
{
    // Only need to do something if the field is not empty
    if(!ui->lineEdit_setpoint_increment_yaw->text().isEmpty())
    {
583
584
585
586
587
588
589
590
591
592
593
594
595
        // Call the function to increment the setpoint
        increment_setpoint_by(0.0,0.0,0.0, -(ui->lineEdit_setpoint_increment_yaw->text()).toFloat() );
    }
    else
    {
        #ifdef CATKIN_MAKE
        // Inform the user that nothing can be done
        ROS_INFO_STREAM("[STUDENT CONTROLLER GUI] Increment yaw setpoint clicked but field is empty");
        #endif
    }
}


596
void StudentControllerTab::increment_setpoint_by(float x_inc, float y_inc, float z_inc, float yaw_inc_degrees)
597
598
599
600
601
602
603
{
	
    if (m_type == TYPE_AGENT)
    {
    	// WHEN GUI IS IN AGENT TYPE MODE:
    	// APPLY THE INCREMENT FROM THE CURRENT SETPOINT
		// Call the function to publish the setpoint
604
        publishSetpoint(
605
606
607
                (ui->lineEdit_setpoint_current_x->text()  ).toFloat() + x_inc,
                (ui->lineEdit_setpoint_current_y->text()  ).toFloat() + y_inc,
                (ui->lineEdit_setpoint_current_z->text()  ).toFloat() + z_inc,
608
                (ui->lineEdit_setpoint_current_yaw->text()).toFloat() + yaw_inc_degrees
609
610
            );
    }
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
    else if (m_type == TYPE_COORDINATOR)
    {
    	// WHEN GUI IS IN COORDINATOR TYPE MODE:
    	// APPLY THE INCREMENT FROM THE CURRENT SETPOINT

		// Initialise local variable for each of (x,y,z,yaw)
    	float x = 0.0f, y = 0.0f, z = 0.4f, yaw = 0.0f;

	    // Take the new value if available, otherwise use the old value
	    // > For x
	    if(!ui->lineEdit_setpoint_new_x->text().isEmpty())
	        x = (ui->lineEdit_setpoint_new_x->text()).toFloat();
	    // > For x
	    if(!ui->lineEdit_setpoint_new_y->text().isEmpty())
	        y = (ui->lineEdit_setpoint_new_y->text()).toFloat();
	    // > For x
	    if(!ui->lineEdit_setpoint_new_z->text().isEmpty())
	        z = (ui->lineEdit_setpoint_new_z->text()).toFloat();
	    // > For x
	    if(!ui->lineEdit_setpoint_new_yaw->text().isEmpty())
	        yaw = (ui->lineEdit_setpoint_new_yaw->text()).toFloat();

	    // Add the increment to this
	    float x_new   = x   + x_inc;
	    float y_new   = y   + y_inc;
	    float z_new   = z   + z_inc;
637
        float yaw_new = yaw + yaw_inc_degrees;
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656

        // INITIALISE A STRING VARIABLE FOR ADDING THE "+"
    	QString qstr = "";

	    // Put this back into the new fields
	    if (x_new < 0.0f) qstr = ""; else qstr = "+";
    		ui->lineEdit_setpoint_new_x->setText(qstr + QString::number( x_new, 'f', 3));
		if (y_new < 0.0f) qstr = ""; else qstr = "+";
    		ui->lineEdit_setpoint_new_y->setText(qstr + QString::number( y_new, 'f', 3));
	    if (z_new < 0.0f) qstr = ""; else qstr = "+";
    		ui->lineEdit_setpoint_new_z->setText(qstr + QString::number( z_new, 'f', 3));

    	if (yaw_new < 0.0f) qstr = ""; else qstr = "+";
    		ui->lineEdit_setpoint_new_yaw->setText(qstr + QString::number( yaw_new, 'f', 3));

		// Call the function to publish the setpoint
        publishSetpoint(x_new,y_new,z_new,yaw_new);

    }
657
658
659
660
    else
    {
        #ifdef CATKIN_MAKE
        // Inform the user that nothing can be done
661
        ROS_INFO_STREAM("[STUDENT CONTROLLER GUI] Increment setpoint not possible because m_type is not recognised.");
662
663
664
665
666
667
668
669
        #endif
    }
}





670

671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
//    ----------------------------------------------------------------------------------
//      A     GGGG  EEEEE  N   N  TTTTT     III  DDDD    SSSS
//     A A   G      E      NN  N    T        I   D   D  S
//    A   A  G      EEE    N N N    T        I   D   D   SSS
//    AAAAA  G   G  E      N  NN    T        I   D   D      S
//    A   A   GGGG  EEEEE  N   N    T       III  DDDD   SSSS
//    ----------------------------------------------------------------------------------


void StudentControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll)
{

    // Lock the mutex
    m_agentIDs_toCoordinate_mutex.lock();
    // Add the "coordinate all" flag
    m_shouldCoordinateAll = shouldCoordinateAll;
    // Clear the previous list of agent IDs
    m_vector_of_agentIDs_toCoordinate.clear();
    // Copy across the agent IDs, if necessary
    if (!shouldCoordinateAll)
    {
        for ( int irow = 0 ; irow < agentIDs.length() ; irow++ )
        {
            m_vector_of_agentIDs_toCoordinate.push_back( agentIDs[irow] );
        }
    }
    // Unlock the mutex
    m_agentIDs_toCoordinate_mutex.unlock();


#ifdef CATKIN_MAKE
    // If there is only one agent to coordinate,
    // then subscribe to the relevant data
    if (agentIDs.length() == 1)
    {

        // // > Create the appropriate node handle
        QString agent_base_namespace = "/dfall/agent" + QString::number(agentIDs[0]).rightJustified(3, '0');
        ros::NodeHandle agent_base_nodeHandle(agent_base_namespace.toStdString());

        // // > Request the current setpoint
712
713
        ros::ServiceClient getCurrentSetpointServiceClient = agent_base_nodeHandle.serviceClient<dfall_pkg::GetSetpointService>("StudentControllerService/GetCurrentSetpoint", false);
        dfall_pkg::GetSetpointService getSetpointCall;
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
        getSetpointCall.request.data = 0;
        getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
        if(getCurrentSetpointServiceClient.call(getSetpointCall))
        {
            setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
        }
        else
        {
            // Inform the user
            ROS_INFO("[STUDENT CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
        }

        // SUBSCRIBERS
        // > For receiving message that the setpoint was changed
        setpointChangedSubscriber = agent_base_nodeHandle.subscribe("StudentControllerService/SetpointChanged", 1, &StudentControllerTab::setpointChangedCallback, this);
    }
    else
    {
        // Unsubscribe
        setpointChangedSubscriber.shutdown();

        // Set information back to the default
        ui->lineEdit_setpoint_current_x->setText("xx.xx");
        ui->lineEdit_setpoint_current_y->setText("xx.xx");
        ui->lineEdit_setpoint_current_z->setText("xx.xx");
        ui->lineEdit_setpoint_current_yaw->setText("xx.xx");

    }
#endif
}






//    ----------------------------------------------------------------------------------
//    M   M   SSSS   GGG      H   H  EEEEE    A    DDDD   EEEEE  RRRR
//    MM MM  S      G   G     H   H  E       A A   D   D  E      R   R
//    M M M   SSS   G         HHHHH  EEE    A   A  D   D  EEE    RRRR
//    M   M      S  G   G     H   H  E      AAAAA  D   D  E      R   R
//    M   M  SSSS    GGGG     H   H  EEEEE  A   A  DDDD   EEEEE  R   R
//    ----------------------------------------------------------------------------------



#ifdef CATKIN_MAKE
// Fill the header for a message
762
void StudentControllerTab::fillSetpointMessageHeader( dfall_pkg::SetpointWithHeader & msg )
763
764
765
766
767
768
769
770
771
772
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
{
    switch (m_type)
    {
        case TYPE_AGENT:
        {
            msg.shouldCheckForAgentID = false;
            break;
        }
        case TYPE_COORDINATOR:
        {
            // Lock the mutex
            m_agentIDs_toCoordinate_mutex.lock();
            // Add the "coordinate all" flag
            msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
            // Add the agent IDs if necessary
            if (!m_shouldCoordinateAll)
            {
                for ( int irow = 0 ; irow < m_vector_of_agentIDs_toCoordinate.size() ; irow++ )
                {
                    msg.agentIDs.push_back( m_vector_of_agentIDs_toCoordinate[irow] );
                }
            }
            // Unlock the mutex
            m_agentIDs_toCoordinate_mutex.unlock();
            break;
        }

        default:
        {
            msg.shouldCheckForAgentID = true;
            ROS_ERROR("[STUDENT CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
            break;
        }
    } 
}
#endif



#ifdef CATKIN_MAKE
// Fill the header for a message
804
void StudentControllerTab::fillCustomButtonMessageHeader( dfall_pkg::CustomButtonWithHeader & msg )
805
806
807
808
809
810
811
812
813
814
815
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
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
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
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
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
{
    switch (m_type)
    {
        case TYPE_AGENT:
        {
            msg.shouldCheckForAgentID = false;
            break;
        }
        case TYPE_COORDINATOR:
        {
            // Lock the mutex
            m_agentIDs_toCoordinate_mutex.lock();
            // Add the "coordinate all" flag
            msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
            // Add the agent IDs if necessary
            if (!m_shouldCoordinateAll)
            {
                for ( int irow = 0 ; irow < m_vector_of_agentIDs_toCoordinate.size() ; irow++ )
                {
                    msg.agentIDs.push_back( m_vector_of_agentIDs_toCoordinate[irow] );
                }
            }
            // Unlock the mutex
            m_agentIDs_toCoordinate_mutex.unlock();
            break;
        }

        default:
        {
            msg.shouldCheckForAgentID = true;
            ROS_ERROR("[STUDENT CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
            break;
        }
    } 
}
#endif





//    ----------------------------------------------------------------------------------
//    III  DDDD       &&&      TTTTT  Y   Y  PPPP   EEEEE
//     I   D   D     &           T     Y Y   P   P  E
//     I   D   D      &          T      Y    PPPP   EEE
//     I   D   D     & & &       T      Y    P      E
//    III  DDDD       &&&        T      Y    P      EEEEE
//    ----------------------------------------------------------------------------------



#ifdef CATKIN_MAKE
bool StudentControllerTab::getTypeAndIDParameters()
{
    // Initialise the return variable as success
    bool return_was_successful = true;

    // Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
    // the "~" indcates that "self" is the node handle assigned to this variable.
    ros::NodeHandle nodeHandle("~");

    // Get the value of the "type" parameter into a local string variable
    std::string type_string;
    if(!nodeHandle.getParam("type", type_string))
    {
        // Throw an error if the agent ID parameter could not be obtained
        ROS_ERROR("[STUDENT CONTROLLER TAB GUI] Failed to get type");
    }

    // Set the "m_type" class variable based on this string loaded
    if ((!type_string.compare("coordinator")))
    {
        m_type = TYPE_COORDINATOR;
    }
    else if ((!type_string.compare("agent")))
    {
        m_type = TYPE_AGENT;
    }
    else
    {
        // Set "m_type" to the value indicating that it is invlid
        m_type = TYPE_INVALID;
        return_was_successful = false;
        ROS_ERROR("[STUDENT CONTROLLER TAB GUI] The 'type' parameter retrieved was not recognised.");
    }


    // Construct the string to the namespace of this Paramater Service
    switch (m_type)
    {
        case TYPE_AGENT:
        {
            // Get the value of the "agentID" parameter into the class variable "m_Id"
            if(!nodeHandle.getParam("agentID", m_ID))
            {
                // Throw an error if the agent ID parameter could not be obtained
                return_was_successful = false;
                ROS_ERROR("[STUDENT CONTROLLER TAB GUI] Failed to get agentID");
            }
            else
            {
                // Inform the user about the type and ID
                ROS_INFO_STREAM("[STUDENT CONTROLLER TAB GUI] Is of type AGENT with ID = " << m_ID);
            }
            break;
        }

        // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
        // > The master GUI
        case TYPE_COORDINATOR:
        {
            // Get the value of the "coordID" parameter into the class variable "m_Id"
            if(!nodeHandle.getParam("coordID", m_ID))
            {
                // Throw an error if the coord ID parameter could not be obtained
                return_was_successful = false;
                ROS_ERROR("[STUDENT CONTROLLER TAB GUI] Failed to get coordID");
            }
            else
            {
                // Inform the user about the type and ID
                ROS_INFO_STREAM("[STUDENT CONTROLLER TAB GUI] Is of type COORDINATOR with ID = " << m_ID);
            }
            break;
        }

        default:
        {
            // Throw an error if the type is not recognised
            return_was_successful = false;
            ROS_ERROR("[STUDENT CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
            break;
        }
    }

    // Return
    return return_was_successful;
}
#endif