enablecontrollerloadyamlbar.cpp 16.3 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
36
37
//    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 bar for selecting which controller is active, and
//    for request that paramters are loaded from the respective
//    YAML files.
//
//    ----------------------------------------------------------------------------------





38
39
40
41
42
43
44
45
#include "enablecontrollerloadyamlbar.h"
#include "ui_enablecontrollerloadyamlbar.h"

EnableControllerLoadYamlBar::EnableControllerLoadYamlBar(QWidget *parent) :
    QWidget(parent),
    ui(new Ui::EnableControllerLoadYamlBar)
{
    ui->setupUi(this);
46
47
48
49
50


#ifdef CATKIN_MAKE
    //ros::init();

51
52
    // Get the namespace of this node
    std::string this_namespace = ros::this_node::getNamespace();
53
    ROS_INFO_STREAM("[ENABLE CONTROLLER LOAD YAML GUI BAR] ros::this_node::getNamespace() =  " << this_namespace);
54
55
56
57
58
59
60

    // 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 )
    {
61
        ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] Node NOT FUNCTIONING :-)");
62
63
64
65
66
67
68
69
        ros::spin();
    }


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

    // CREATE THE COMMAND PUBLISHER
70
    commandPublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::IntWithHeader>("FlyingAgentClient/Command", 1);
71
72
73

    // CREATE THE REQUEST LOAD YAML FILE PUBLISHER
    // Get the node handle to this parameter service
74
    m_requestLoadYamlFilenamePublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::StringWithHeader>("ParameterService/requestLoadYamlFilename", 1);
75
76
#endif

77
78
79
80
81
82
83
84
85
86
}

EnableControllerLoadYamlBar::~EnableControllerLoadYamlBar()
{
    delete ui;
}




87
88
89
90
91
92
93
94
95
96
97
98
99
100
void EnableControllerLoadYamlBar::showHideController_default_changed()
{
    ui->enable_default_button   ->setHidden( !(ui->enable_default_button->isHidden()) );
    ui->load_yaml_default_button->setHidden( !(ui->load_yaml_default_button->isHidden()) );
}

void EnableControllerLoadYamlBar::showHideController_student_changed()
{
    ui->enable_student_button   ->setHidden( !(ui->enable_student_button->isHidden()) );
    ui->load_yaml_student_button->setHidden( !(ui->load_yaml_student_button->isHidden()) );
}

void EnableControllerLoadYamlBar::showHideController_picker_changed()
{
101
    ui->enable_picker_button   ->setHidden( !(ui->enable_picker_button->isHidden()) );
102
    ui->load_yaml_picker_button->setHidden( !(ui->load_yaml_picker_button->isHidden()) );
103
104
}

105
106
107
108
109
110
void EnableControllerLoadYamlBar::showHideController_tuning_changed()
{
    ui->enable_tuning_button   ->setHidden( !(ui->enable_tuning_button->isHidden()) );
    ui->load_yaml_tuning_button->setHidden( !(ui->load_yaml_tuning_button->isHidden()) );
}

111
void EnableControllerLoadYamlBar::showHideController_template_changed()
112
{
113
114
    ui->enable_template_button   ->setHidden( !(ui->enable_template_button->isHidden()) );
    ui->load_yaml_template_button->setHidden( !(ui->load_yaml_template_button->isHidden()) );
115
116
117
118
119
120
}





121
122
// ENABLE CONTROLLER BUTTONS ON-CLICK CALLBACK

123
void EnableControllerLoadYamlBar::on_enable_default_button_clicked()
124
{
125
#ifdef CATKIN_MAKE
126
    dfall_pkg::IntWithHeader msg;
127
    fillIntMessageHeader(msg);
128
    msg.data = CMD_USE_SAFE_CONTROLLER;
129
    this->commandPublisher.publish(msg);
130
    ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable Safe Controller");
131
#endif
132
133
}

134
void EnableControllerLoadYamlBar::on_enable_student_button_clicked()
135
136
{
#ifdef CATKIN_MAKE
137
    dfall_pkg::IntWithHeader msg;
138
    fillIntMessageHeader(msg);
139
    msg.data = CMD_USE_STUDENT_CONTROLLER;
140
    this->commandPublisher.publish(msg);
141
    ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable Student Controller");
142
143
144
#endif
}

145
void EnableControllerLoadYamlBar::on_enable_picker_button_clicked()
146
{
147
#ifdef CATKIN_MAKE
148
    dfall_pkg::IntWithHeader msg;
149
    fillIntMessageHeader(msg);
150
    msg.data = CMD_USE_PICKER_CONTROLLER;
151
    this->commandPublisher.publish(msg);
152
    ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable Picker Controller");
153
#endif
154
155
}

156
void EnableControllerLoadYamlBar::on_enable_tuning_button_clicked()
157
{
158
#ifdef CATKIN_MAKE
159
    dfall_pkg::IntWithHeader msg;
160
    fillIntMessageHeader(msg);
161
    msg.data = CMD_USE_TUNING_CONTROLLER;
162
    this->commandPublisher.publish(msg);
163
    ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable Tuning Controller");
164
#endif
165
166
}

167
void EnableControllerLoadYamlBar::on_enable_template_button_clicked()
168
{
beuchatp's avatar
beuchatp committed
169
#ifdef CATKIN_MAKE
170
    dfall_pkg::IntWithHeader msg;
171
    fillIntMessageHeader(msg);
172
    msg.data = CMD_USE_TEMPLATE_CONTROLLER;
173
    this->commandPublisher.publish(msg);
174
    ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable Template Controller");
beuchatp's avatar
beuchatp committed
175
#endif
176
177
178
}


179
180
181
182
183





184
185
// LOAD YAML BUTTONS ON-CLICK CALLBACK

186
void EnableControllerLoadYamlBar::on_load_yaml_default_button_clicked()
187
{
188
189
190
191
192
193
194
195
196
197
198
    #ifdef CATKIN_MAKE
    // Create a local variable for the message
    dfall_pkg::StringWithHeader yaml_filename_msg;
    // Set for whom this applies to
    fillStringMessageHeader(yaml_filename_msg);
    // Specify the data
    yaml_filename_msg.data = "DefaultController";
    // Send the message
    m_requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
    // Inform the user that the menu item was selected
    ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Load Default Controller YAML was clicked.");
199
#endif
200
201
}

202
void EnableControllerLoadYamlBar::on_load_yaml_student_button_clicked()
203
204
205
{
#ifdef CATKIN_MAKE
    // Create a local variable for the message
206
    dfall_pkg::StringWithHeader yaml_filename_msg;
207
208
209
    // Set for whom this applies to
    fillStringMessageHeader(yaml_filename_msg);
    // Specify the data
210
    yaml_filename_msg.data = "StudentController";
211
212
213
    // Send the message
    m_requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
    // Inform the user that the menu item was selected
214
    ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Load Student Controller YAML was clicked.");
215
216
217
#endif
}

218
void EnableControllerLoadYamlBar::on_load_yaml_picker_button_clicked()
219
{
220
221
#ifdef CATKIN_MAKE
    // Create a local variable for the message
222
    dfall_pkg::StringWithHeader yaml_filename_msg;
223
224
225
    // Set for whom this applies to
    fillStringMessageHeader(yaml_filename_msg);
    // Specify the data
226
    yaml_filename_msg.data = "PickerController";
227
228
229
    // Send the message
    m_requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
    // Inform the user that the menu item was selected
230
    ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Load Picker Controller YAML was clicked.");
231
#endif
232
233
}

234
void EnableControllerLoadYamlBar::on_load_yaml_tuning_button_clicked()
235
{
236
237
#ifdef CATKIN_MAKE
    // Create a local variable for the message
238
    dfall_pkg::StringWithHeader yaml_filename_msg;
239
240
241
    // Set for whom this applies to
    fillStringMessageHeader(yaml_filename_msg);
    // Specify the data
242
    yaml_filename_msg.data = "TuningController";
243
244
245
    // Send the message
    m_requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
    // Inform the user that the menu item was selected
246
    ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Load Tuning Controller YAML was clicked.");
247
#endif
248
249
}

250
void EnableControllerLoadYamlBar::on_load_yaml_template_button_clicked()
251
{
252
#ifdef CATKIN_MAKE
253
    // Create a local variable for the message
254
    dfall_pkg::StringWithHeader yaml_filename_msg;
255
256
257
    // Set for whom this applies to
    fillStringMessageHeader(yaml_filename_msg);
    // Specify the data
258
    yaml_filename_msg.data = "TemplateController";
259
260
261
    // Send the message
    m_requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
    // Inform the user that the menu item was selected
262
    ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Load Template Controller YAML was clicked.");
263
#endif
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
//    ----------------------------------------------------------------------------------
//      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 EnableControllerLoadYamlBar::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();
}





305
306
307
308
309
310
311
312
313
314
//    ----------------------------------------------------------------------------------
//    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
//    ----------------------------------------------------------------------------------



315
316
#ifdef CATKIN_MAKE
// Fill the head for a message
317
void EnableControllerLoadYamlBar::fillIntMessageHeader( dfall_pkg::IntWithHeader & msg )
318
319
320
321
322
{
    switch (m_type)
    {
        case TYPE_AGENT:
        {
323
            msg.shouldCheckForAgentID = false;
324
325
326
327
328
329
330
            break;
        }
        case TYPE_COORDINATOR:
        {
            // Lock the mutex
            m_agentIDs_toCoordinate_mutex.lock();
            // Add the "coordinate all" flag
331
            msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
            // 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:
        {
347
            msg.shouldCheckForAgentID = true;
348
349
350
351
352
353
354
355
356
357
358
359
360
            ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] The 'm_type' variable was not recognised.");
            break;
        }
    } 
}
#endif





#ifdef CATKIN_MAKE
// Fill the head for a message
361
void EnableControllerLoadYamlBar::fillStringMessageHeader( dfall_pkg::StringWithHeader & msg )
362
363
364
365
366
{
    switch (m_type)
    {
        case TYPE_AGENT:
        {
367
            msg.shouldCheckForAgentID = false;
368
369
370
371
            break;
        }
        case TYPE_COORDINATOR:
        {
372
373
374
            // Lock the mutex
            m_agentIDs_toCoordinate_mutex.lock();
            // Add the "coordinate all" flag
375
            msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
376
377
378
379
380
381
382
383
384
385
            // 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();
386
387
388
389
390
            break;
        }

        default:
        {
391
            msg.shouldCheckForAgentID = true;
392
            ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] The 'm_type' variable was not recognised.");
393
394
395
396
397
398
399
            break;
        }
    } 
}
#endif


400
401
402
403
404
405
406
407
408
409
410
411
412
413




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



414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
#ifdef CATKIN_MAKE
bool EnableControllerLoadYamlBar::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
429
        ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] Failed to get type");
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
    }

    // 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;
446
        ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] The 'type' parameter retrieved was not recognised.");
447
448
449
450
451
452
453
454
455
456
457
458
459
    }


    // 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;
460
                ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] Failed to get agentID");
461
462
463
464
            }
            else
            {
                // Inform the user about the type and ID
465
                ROS_INFO_STREAM("[ENABLE CONTROLLER LOAD YAML GUI BAR] Is of type AGENT with ID = " << m_ID);
466
467
468
469
470
471
472
473
474
475
476
477
478
            }
            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;
479
                ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] Failed to get coordID");
480
481
482
483
            }
            else
            {
                // Inform the user about the type and ID
484
                ROS_INFO_STREAM("[ENABLE CONTROLLER LOAD YAML GUI BAR] Is of type COORDINATOR with ID = " << m_ID);
485
486
487
488
489
490
491
492
            }
            break;
        }

        default:
        {
            // Throw an error if the type is not recognised
            return_was_successful = false;
493
            ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] The 'm_type' variable was not recognised.");
494
495
496
497
498
499
500
            break;
        }
    }

    // Return
    return return_was_successful;
}
501
#endif