ParameterService.cpp 14.1 KB
Newer Older
1
//    Copyright (C) 2017, 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
77
78
79
80
81
82
83





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




void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
{
    // Extract from the "msg" for which controller the YAML
    // parameters should be loaded
    int controller_to_load_yaml = msg.data;

84
85
    ROS_INFO_STREAM("The Parameter Service node received the message to load YAML parameters from file into cache");

86

beuchatp's avatar
beuchatp committed
87
88
    // Instantiate a local varaible to confirm that something can actually be loaded
    // from a YAML file
89
    bool isValidToAttemptLoad = true;
90

91
92
93
94
95
96
    // Instantiate a local variable for the string that will be passed to the "system"
    std::string cmd;

    // Get the abolute path to "d_fall_pps"
    std::string d_fall_pps_path = ros::package::getPath("d_fall_pps");

97
    // Switch between loading for the different controllers
98
    if ( (controller_to_load_yaml==LOAD_YAML_SAFE_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR) )
99
    {
beuchatp's avatar
beuchatp committed
100
        // Re-load the parameters of the safe controller:
101
        cmd = "rosparam load " + d_fall_pps_path + "/param/SafeController.yaml " + m_base_namespace + "/SafeController";
102
103
104
    }
    else if ( (controller_to_load_yaml==LOAD_YAML_SAFE_CONTROLLER_AGENT) && (my_type==TYPE_AGENT) )
    {
beuchatp's avatar
beuchatp committed
105
        // Re-load the parameters of the safe controller:
106
        cmd = "rosparam load " + d_fall_pps_path + "/param/SafeController.yaml " + m_base_namespace + "/SafeController";
107
    }
Paul Beuchat's avatar
Paul Beuchat committed
108
    else if ( (controller_to_load_yaml==LOAD_YAML_DEMO_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR) )
109
    {
Paul Beuchat's avatar
Paul Beuchat committed
110
        // Re-load the parameters of the demo controller:
111
        cmd = "rosparam load " + d_fall_pps_path + "/param/DemoController.yaml " + m_base_namespace + "/DemoController";
112
    }
Paul Beuchat's avatar
Paul Beuchat committed
113
    else if ( (controller_to_load_yaml==LOAD_YAML_DEMO_CONTROLLER_AGENT) && (my_type==TYPE_AGENT) )
114
    {
Paul Beuchat's avatar
Paul Beuchat committed
115
        // Re-load the parameters of the demo controller:
116
        cmd = "rosparam load " + d_fall_pps_path + "/param/DemoController.yaml " + m_base_namespace + "/DemoController";
117
118
119
    }
    else
    {
beuchatp's avatar
beuchatp committed
120
        // Let the user know that the command was not recognised
121
122
123
        ROS_INFO_STREAM("> Nothing to load for this parameter service with.");
        ROS_INFO_STREAM("> The message received has 'controller_to_load_yaml'   =  " << controller_to_load_yaml);
        ROS_INFO_STREAM("> And the type of this Parameter Service is 'my_type'  =  " << my_type);
beuchatp's avatar
beuchatp committed
124
        // Set the boolean that prevents the fetch message from being sent
125
        isValidToAttemptLoad = false;
126
127
128
    }


129
130
131
    // Only bother with ttempting to loaded the .yaml file, and subseuently send the "ready for fetch"
    // message if something can actually be loaded from a YAML file
    if (isValidToAttemptLoad)
132
    {
133
        // Let the user know what is about to happen
beuchatp's avatar
beuchatp committed
134
        ROS_INFO_STREAM("> The following path will be used for locating the .yaml file: " << d_fall_pps_path  << " The comand line string sent to the 'system' is: " << cmd );
135
136
137
138
139
140

        // Re-load the parameters by pass the command line string via a "system" call
        // > i.e., this replicates pasting this string in a new terminal window and pressing enter
        system(cmd.c_str());

        // Pause breifly to ensure that the yaml file is fully loaded
141
        ros::Duration(0.5).sleep();
beuchatp's avatar
beuchatp committed
142
143
144
145

        // Instantiate a local varaible to confirm that something was actually loaded from
        // a YAML file
        bool isReadyForFetch = true;
146
    
147
148
149
150
151
152
153
154
        // Instantiate a local variable for the fetch message
        std_msgs::Int32 fetch_msg;
        // Fill in the data of the fetch message
        switch(controller_to_load_yaml)
        {
            case (LOAD_YAML_SAFE_CONTROLLER_COORDINATOR):
                fetch_msg.data = FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR;
                break;
Paul Beuchat's avatar
Paul Beuchat committed
155
156
            case (LOAD_YAML_DEMO_CONTROLLER_COORDINATOR):
                fetch_msg.data = FETCH_YAML_DEMO_CONTROLLER_FROM_COORDINATOR;
157
158
159
160
                break;
            case (LOAD_YAML_SAFE_CONTROLLER_AGENT):
                fetch_msg.data = FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT;
                break;
Paul Beuchat's avatar
Paul Beuchat committed
161
162
            case (LOAD_YAML_DEMO_CONTROLLER_AGENT):
                fetch_msg.data = FETCH_YAML_DEMO_CONTROLLER_FROM_OWN_AGENT;
163
164
165
                break;
            default:
                // Let the user know that the command was not recognised
beuchatp's avatar
beuchatp committed
166
                ROS_INFO("Unknown 'controller to load yaml' command, thus a 'ready to fetch' message will NOT be published.");
167
168
169
170
171
172
                // Set the boolean that prevents the fetch message from being sent
                isReadyForFetch = false;
                break;
        }
        // Send a message that the YAML parameter have been loaded and hence are
        // ready to be fetched (i.e., using getparam())
beuchatp's avatar
beuchatp committed
173
174
        if (isReadyForFetch)
        {
175
            controllerYamlReadyForFetchPublisher.publish(fetch_msg);
beuchatp's avatar
beuchatp committed
176
        }
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
    }
}







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

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

    // 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("~");

203
204
205
206
207
    // Get the namespace of this "ParameterService" node
    std::string m_namespace = ros::this_node::getNamespace();
    ROS_INFO_STREAM("For ParameterService, ros::this_node::getNamespace() =  " << m_namespace);


208

209
210
211
212
213
214
215
216
217
    // 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("Failed to get type from ParameterService");
    }

    // Set the "my_type" instance variable based on this string loaded
218
219
    if ((!type_string.compare("coordinator")))
    {
beuchatp's avatar
beuchatp committed
220
        my_type = TYPE_COORDINATOR;
221
222
223
    }
    else if ((!type_string.compare("agent")))
    {
beuchatp's avatar
beuchatp committed
224
        my_type = TYPE_AGENT;
225
226
    }
    else
227
    {
beuchatp's avatar
beuchatp committed
228
229
        // Set "my_type" to the value indicating that it is invlid
        my_type = TYPE_INVALID;
230
        ROS_ERROR("The 'type' parameter retrieved was not recognised.");
231
232
233
234
235
236
237
238
239
240
    }


    // Get the value of the "agentID" parameter into the instance variable "my_agentID"
    if(!nodeHandle.getParam("agentID", my_agentID))
    {
        // Throw an error if the agent ID parameter could not be obtained
        ROS_ERROR("Failed to get agentID from ParameterService");
    }

241

242
243
    // Publisher that notifies the relevant nodes when the YAML paramters have been loaded
    // from file into ram/cache, and hence are ready to be fetched
244
    controllerYamlReadyForFetchPublisher = nodeHandle.advertise<std_msgs::Int32>("controllerYamlReadyForFetch", 1);
245
246
    

247
248
249
250
251
    // Construct the string to the namespace of this Paramater Service
    switch (my_type)
    {
        case TYPE_AGENT:
        {
252
253
254
255
            //m_base_namespace = ros::this_node::getNamespace();
            //m_base_namespace = "/agent" + my_agentID + '/' + "ParameterService";
            m_base_namespace = m_namespace + '/' + "ParameterService";
            ROS_INFO_STREAM("This Paramter Sercice will load .yaml file parameters into the 'base' namespace: " << m_base_namespace);
256
257
258
259
260
261
262
            break;
        }

        // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
        // > The master GUI
        case TYPE_COORDINATOR:
        {
263
264
265
266
            //m_base_namespace = ros::this_node::getNamespace();
            //m_base_namespace = "/ParameterService";
            m_base_namespace = m_namespace + '/' + "ParameterService";
            ROS_INFO_STREAM("This Paramter Sercice will load .yaml file parameters into the 'base' namespace: " << m_base_namespace);
267
268
269
270
271
272
273
274
275
276
277
278
            break;
        }

        default:
        {
            ROS_ERROR("The 'my_type' type parameter was not recognised.");
            break;
        }
    }

    

279

280
    // SUBSCRIBE TO THE APPROPRIATE "request" MESSAGES DEPENDING ON THE "my_type"
281
282
283
284
285
286
287
    // Delare the subscribers as local variables here so that they persist for the life
    // time of this main() function. The varaibles will be assigned in the switch case below
    // > Subscribers for when this Parameter Service node is: TYPE_AGENT
    ros::Subscriber requestLoadControllerYamlSubscriber_agent_to_self;
    ros::Subscriber requestLoadControllerYamlSubscriber_agent_to_coordinator;
    // > Subscribers for when this Parameter Service node is: TYPE_COORDINATOR
    ros::Subscriber requestLoadControllerYamlSubscriber_coordinator_to_self;
288
289

    // SUBSCRIBE TO THE APPROPRIATE "request" MESSAGES DEPENDING ON THE "my_type"
290
    switch (my_type)
291
292
293
294
    {
        // AN AGENT TYPE PARAMETER SERVICE IS REQUESTED FROM:
        // > The master GUI
        // > The agent's own "PPSClient" node
295
        case TYPE_AGENT:
beuchatp's avatar
beuchatp committed
296
        {
297
298
299
300
            // Subscribing to the agent's own PPSclient
            // > First: Construct a node handle to the PPSclient
            ros::NodeHandle nh_PPSClient_for_this_agent("PPSClient");
            // > Second: Subscribe to the "requestLoadControllerYaml" topic
beuchatp's avatar
beuchatp committed
301
            requestLoadControllerYamlSubscriber_agent_to_self = nh_PPSClient_for_this_agent.subscribe("requestLoadControllerYaml", 1, requestLoadControllerYamlCallback);
302
303
304
305
306

            // Subscribing to the coordinator
            // > First: construct a node handle to the coordinator
            ros::NodeHandle nh_coordinator_for_this_agent = ros::NodeHandle();
            // > Second: Subscribe to the "requestLoadControllerYaml" topic
307
            requestLoadControllerYamlSubscriber_agent_to_coordinator = nh_coordinator_for_this_agent.subscribe("/my_GUI/requestLoadControllerYaml", 1, requestLoadControllerYamlCallback);            
308
309

            // Inform the user what was subscribed to:
310
            ROS_INFO_STREAM("This Parameter Service has subscribed to 'requestLoadControllerYaml' messages from both the 'my_GUI' and the 'PPSClient'");
311
312
            break;
        }
313

314
315
316
317
318
319
        // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
        // > The master GUI
        case TYPE_COORDINATOR:
        {
            // Subscribing to the coordinator's own "my_GUI" 
            // > First: Get the node handle required
320
            ros::NodeHandle nh_coordinator_for_this_coordinator = ros::NodeHandle();
321
            // > Second: Subscribe to requests from: the master GUI
322
            requestLoadControllerYamlSubscriber_coordinator_to_self = nh_coordinator_for_this_coordinator.subscribe("/my_GUI/requestLoadControllerYaml", 1, requestLoadControllerYamlCallback);
323
324
325

            // Inform the user what was subscribed to:
            ROS_INFO_STREAM("This Parameter Service has subscribed to 'requestLoadControllerYaml' messages from 'my_GUI'");
326
            break;
beuchatp's avatar
beuchatp committed
327
        }
328
329

        default:
beuchatp's avatar
beuchatp committed
330
        {
331
            ROS_ERROR("The 'my_type' type parameter was not recognised.");
332
            break;
beuchatp's avatar
beuchatp committed
333
        }
334
335
336
337
338
339
340
341
    }


    ROS_INFO("CentralManagerService ready");
    ros::spin();

    return 0;
}