ParameterService.cpp 12.7 KB
Newer Older
beuchatp's avatar
beuchatp committed
1
//    Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
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
//
//    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 service that manages the loading of YAML parameters
//
//    ----------------------------------------------------------------------------------





Paul Beuchat's avatar
Paul Beuchat committed
36
// INCLUDE THE HEADER
37
#include "nodes/ParameterService.h"
38
39
40
41
42
43
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
75
76





//    ----------------------------------------------------------------------------------
//    FFFFF  U   U  N   N   CCCC  TTTTT  III   OOO   N   N
//    F      U   U  NN  N  C        T     I   O   O  NN  N
//    FFF    U   U  N N N  C        T     I   O   O  N N N
//    F      U   U  N  NN  C        T     I   O   O  N  NN
//    F       UUU   N   N   CCCC    T    III   OOO   N   N
//
//    III M   M PPPP  L     EEEEE M   M EEEEE N   N TTTTT   A   TTTTT III  OOO  N   N
//     I  MM MM P   P L     E     MM MM E     NN  N   T    A A    T    I  O   O NN  N
//     I  M M M PPPP  L     EEE   M M M EEE   N N N   T   A   A   T    I  O   O N N N
//     I  M   M P     L     E     M   M E     N  NN   T   AAAAA   T    I  O   O N  NN
//    III M   M P     LLLLL EEEEE M   M EEEEE N   N   T   A   A   T   III  OOO  N   N
//    ----------------------------------------------------------------------------------





//    ----------------------------------------------------------------------------------
//    L       OOO     A    DDDD
//    L      O   O   A A   D   D
//    L      O   O  A   A  D   D
//    L      O   O  AAAAA  D   D
//    LLLLL   OOO   A   A  DDDD
//
//    PPPP     A    RRRR     A    M   M  EEEEE  TTTTT  EEEEE  RRRR    SSSS
//    P   P   A A   R   R   A A   MM MM  E        T    E      R   R  S
//    PPPP   A   A  RRRR   A   A  M M M  EEE      T    EEE    RRRR    SSS
//    P      AAAAA  R  R   AAAAA  M   M  E        T    E      R  R       S
//    P      A   A  R   R  A   A  M   M  EEEEE    T    EEEEE  R   R  SSSS
//    ----------------------------------------------------------------------------------



77
bool requestLoadYamlFilenameCallbackServiceCallback(LoadYamlFromFilename::Request &request, LoadYamlFromFilename::Response &response)
78
{
79
80
    // Put the flying state into the response variable
    requestLoadYamlFilenamePublisher.publish(request.stringWithHeader);
81

82
83
    // Put success into the response
    response.data = 1;
84

85
86
    // Return
    return true;
87
88
}

89
void requestLoadYamlFilenameCallback(const StringWithHeader& yaml_filename_to_load_with_header)
90
{
91
92
    // LOAD THE YAML FILE

93
    // Get the yaml file name requested
94
    std::string yaml_filename_to_load = yaml_filename_to_load_with_header.data;
95
    // Get the node handle to this parameter service
96
97
98
    ros::NodeHandle nodeHandle("~");
    // Instantiate a local variable for the command string that will be passed to the "system":
    std::string cmd;
99
100
    // Get the abolute path to "dfall_pkg":
    std::string dfall_pkg_path = ros::package::getPath("dfall_pkg");
101
    // Construct the system command string for (re-)loading the parameters:
102
    cmd = "rosparam load " + dfall_pkg_path + "/param" + "/" + yaml_filename_to_load + ".yaml " + m_base_namespace + "/" + yaml_filename_to_load;
103
    // Let the user know what is about to happen
104
    ROS_INFO_STREAM("[PARAMETER SERVICE] The following path will be used for locating the .yaml file: " << dfall_pkg_path  << " The comand line string sent to the 'system' is: " << cmd );
105
    // Send the "load yaml" command to the system
106
107
    system(cmd.c_str());

108

109

110
    // PUBLISH A MESSAGE THAT THE YAML FILE WAS LOADED
111

112
113
    // Create publisher as a local variable, using the filename
    // as the name of the message
114
    ros::Publisher yamlParametersReadyForFetchPublisher = nodeHandle.advertise<IntWithHeader>(yaml_filename_to_load, 1, true);
115
    //ros::spinOnce();
116
117
118
119
120
121
122
123
124
125
126
127
    // Create a local variable for the message
    IntWithHeader yaml_ready_msg;
    // Specify with the data the "type" of this parameter service
    switch (m_type)
    {
        case TYPE_AGENT:
        {
            yaml_ready_msg.data = LOAD_YAML_FROM_AGENT;
            break;
        }
        case TYPE_COORDINATOR:
        {
128
            yaml_ready_msg.data = LOAD_YAML_FROM_COORDINATOR;
129
130
131
132
133
134
135
136
137
138
139
140
            break;
        }
        default:
        {
            // Throw an error if the type is not recognised
            ROS_ERROR("[PARAMETER SERVICE] The 'm_type' variable was not recognised.");
            // Specify to load from the agent by default
            yaml_ready_msg.data = LOAD_YAML_FROM_AGENT;
            break;
        }
    }
    // Copy across the boolean field
141
    yaml_ready_msg.shouldCheckForAgentID = yaml_filename_to_load_with_header.shouldCheckForAgentID;
142
    // Copy across the vector of IDs
143
    if (yaml_filename_to_load_with_header.shouldCheckForAgentID)
144
    {
145
        for ( int i_ID=0 ; i_ID<yaml_filename_to_load_with_header.agentIDs.size() ; i_ID++ )
146
147
148
149
        {
            yaml_ready_msg.agentIDs.push_back(yaml_filename_to_load_with_header.agentIDs[i_ID]);
        }
    }
150
    // Sleep to make the publisher known to ROS (in seconds)
151
    ros::Duration(2.0).sleep();
152
153
    // Send the message
    yamlParametersReadyForFetchPublisher.publish(yaml_ready_msg);
154
155
156

    // Inform the user that this was published
    ROS_INFO_STREAM("[PARAMETER SERVICE] Published message that " << yaml_filename_to_load << " yaml parameters are ready." );
157
}
158

159
160
161
162




163
bool getTypeAndIDParameters()
164
{
165
166
    // Initialise the return variable as success
    bool return_was_successful = true;
167
168
169
170
171
172
173
174
175
176

    // 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
177
        ROS_ERROR("[PARAMETER SERVICE] Failed to get type");
178
179
    }

180
    // Set the "m_type" class variable based on this string loaded
181
182
    if ((!type_string.compare("coordinator")))
    {
183
        m_type = TYPE_COORDINATOR;
184
185
186
    }
    else if ((!type_string.compare("agent")))
    {
187
        m_type = TYPE_AGENT;
188
189
    }
    else
190
    {
191
192
193
        // Set "m_type" to the value indicating that it is invlid
        m_type = TYPE_INVALID;
        return_was_successful = false;
194
        ROS_ERROR("[PARAMETER SERVICE] The 'type' parameter retrieved was not recognised.");
195
196
197
    }


198
    // Construct the string to the namespace of this Paramater Service
199
    switch (m_type)
200
201
202
    {
        case TYPE_AGENT:
        {
203
204
205
206
207
208
209
210
211
212
213
214
            // 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("[PARAMETER SERVICE] Failed to get agentID");
            }
            else
            {
                // Inform the user about the type and ID
                ROS_INFO_STREAM("[PARAMETER SERVICE] Is of type AGENT with ID = " << m_ID);
            }
215
216
217
218
219
220
221
            break;
        }

        // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
        // > The master GUI
        case TYPE_COORDINATOR:
        {
222
223
224
225
226
227
228
229
230
231
232
233
            // 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("[PARAMETER SERVICE] Failed to get coordID");
            }
            else
            {
                // Inform the user about the type and ID
                ROS_INFO_STREAM("[PARAMETER SERVICE] Is of type COORDINATOR with ID = " << m_ID);
            }
234
235
236
237
238
            break;
        }

        default:
        {
239
240
241
            // Throw an error if the type is not recognised
            return_was_successful = false;
            ROS_ERROR("[PARAMETER SERVICE] The 'm_type' variable was not recognised.");
242
243
244
245
            break;
        }
    }

246
247
248
249
    // Return
    return return_was_successful;
}

250

251
252


253
254
255
256
257
258
259
260
261
262
263
264

bool constructNamespaces()
{
    // Initialise the return variable as success
    bool return_was_successful = true;

    // Get the namespace of this "ParameterService" node
    std::string this_node_namespace = ros::this_node::getNamespace();
    ROS_INFO_STREAM("[PARAMETER SERVICE] ros::this_node::getNamespace() =  " << this_node_namespace);

    // Construct the string to the namespace of this Paramater Service
    switch (m_type)
265
    {
266
        case TYPE_AGENT:
beuchatp's avatar
beuchatp committed
267
        {
268
269
270
271
            //m_base_namespace = ros::this_node::getNamespace();
            //m_base_namespace = "/agent" + m_Id + '/' + "ParameterService";
            m_base_namespace = this_node_namespace + '/' + "ParameterService";
            ROS_INFO_STREAM("[PARAMETER SERVICE] .yaml file parameters will be loaded into the 'base' namespace: " << m_base_namespace);
272
273
            break;
        }
274

275
276
277
278
        // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
        // > The master GUI
        case TYPE_COORDINATOR:
        {
279
280
281
282
            //m_base_namespace = ros::this_node::getNamespace();
            //m_base_namespace = "/ParameterService";
            m_base_namespace = this_node_namespace + '/' + "ParameterService";
            ROS_INFO_STREAM("[PARAMETER SERVICE] .yaml file parameters will be loaded into the 'base' namespace: " << m_base_namespace);
283
            break;
beuchatp's avatar
beuchatp committed
284
        }
285
286

        default:
beuchatp's avatar
beuchatp committed
287
        {
288
289
290
            // Throw an error if the type is not recognised
            return_was_successful = false;
            ROS_ERROR("[PARAMETER SERVICE] The 'm_type' type parameter was not recognised.");
291
            break;
beuchatp's avatar
beuchatp committed
292
        }
293
294
    }

295
296
297
298
299
300
301
    // Return
    return return_was_successful;
}




302

303
304
305
306
307
308
309
//    ----------------------------------------------------------------------------------
//    M   M    A    III  N   N
//    MM MM   A A    I   NN  N
//    M M M  A   A   I   N N N
//    M   M  AAAAA   I   N  NN
//    M   M  A   A  III  N   N
//    ----------------------------------------------------------------------------------
310

311
312
313
314
int main(int argc, char* argv[])
{
    // Starting the ROS-node
    ros::init(argc, argv, "ParameterService");
315

316
317
318
    // 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("~");
319

320
321
    // Get the type and ID of this parameter service
    bool isValid_type_and_ID = getTypeAndIDParameters();
322

323
324
325
    // Construct the namespace into which this parameter service
    // loads yaml parameters
    bool isValid_namespaces = constructNamespaces();
326

327
    // Stall if the TYPE and ID are not valid
328
329
330
331
332
    if ( !( isValid_type_and_ID && isValid_namespaces ) )
    {
        ROS_ERROR("[PARAMETER SERVICE] Service NOT FUNCTIONING :-)");
        ros::spin();
    }
333

334
    // Subscribe to the messages that request loading a yaml file
335
    ros::Subscriber requestLoadYamlFilenameSubscriber = nodeHandle.subscribe("requestLoadYamlFilename", 20, requestLoadYamlFilenameCallback);
336

337
338
339
340
    // Publisher for publishing "internally" to the subscriber above
    requestLoadYamlFilenamePublisher = nodeHandle.advertise <StringWithHeader>("requestLoadYamlFilename", 1);

    // Advertise the service for requests to load a yaml file
341
    ros::ServiceServer requestLoadYamlFilenameService = nodeHandle.advertiseService("requestLoadYamlFilename", requestLoadYamlFilenameCallbackServiceCallback);
342

343
    // Inform the user the this node is ready
344
    ROS_INFO("[PARAMETER SERVICE] Service ready :-)");
345
    // Spin as a single-thread node
346
347
348
349
    ros::spin();

    return 0;
}