ParameterService.h 5.47 KB
Newer Older
Paul Beuchat's avatar
Paul Beuchat 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
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
//    Copyright (C) 2017, 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 service that manages the loading of YAML parameters
//
//    ----------------------------------------------------------------------------------





//    ----------------------------------------------------------------------------------
//    III  N   N   CCCC  L      U   U  DDDD   EEEEE   SSSS
//     I   NN  N  C      L      U   U  D   D  E      S
//     I   N N N  C      L      U   U  D   D  EEE     SSS
//     I   N  NN  C      L      U   U  D   D  E          S
//    III  N   N   CCCC  LLLLL   UUU   DDDD   EEEEE  SSSS
//    ----------------------------------------------------------------------------------

#include <stdlib.h>
#include <string>

#include <ros/ros.h>
#include <ros/package.h>
#include <ros/network.h>
#include "std_msgs/Int32.h"
//#include "std_msgs/Float32.h"
//#include <std_msgs/String.h>

#include "d_fall_pps/Controller.h"


//    ----------------------------------------------------------------------------------
//    DDDD   EEEEE  FFFFF  III  N   N  EEEEE   SSSS
//    D   D  E      F       I   NN  N  E      S
//    D   D  EEE    FFF     I   N N N  EEE     SSS
//    D   D  E      F       I   N  NN  E          S
//    DDDD   EEEEE  F      III  N   N  EEEEE  SSSS
//    ----------------------------------------------------------------------------------


66
67
68
69
70
// For which controller parameters to load from file
#define LOAD_YAML_SAFE_CONTROLLER_AGENT             1
#define LOAD_YAML_DEMO_CONTROLLER_AGENT             2
#define LOAD_YAML_STUDENT_CONTROLLER_AGENT          3
#define LOAD_YAML_MPC_CONTROLLER_AGENT              4
Paul Beuchat's avatar
Paul Beuchat committed
71

72
73
74
75
76
77
78
#define LOAD_YAML_SAFE_CONTROLLER_COORDINATOR       11
#define LOAD_YAML_DEMO_CONTROLLER_COORDINATOR       12
#define LOAD_YAML_STUDENT_CONTROLLER_COORDINATOR    13
#define LOAD_YAML_MPC_CONTROLLER_COORDINATOR        14


// For which controller parameters to fetch from  from file
Paul Beuchat's avatar
Paul Beuchat committed
79
#define FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT      1
80
81
82
#define FETCH_YAML_DEMO_CONTROLLER_FROM_OWN_AGENT      2


Paul Beuchat's avatar
Paul Beuchat committed
83
#define FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR    3
84
#define FETCH_YAML_DEMO_CONTROLLER_FROM_COORDINATOR    4
Paul Beuchat's avatar
Paul Beuchat committed
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111

#define TYPE_INVALID      -1
#define TYPE_COORDINATOR   1
#define TYPE_AGENT         2


// Namespacing the package
using namespace d_fall_pps;
//using namespace std;




//    ----------------------------------------------------------------------------------
//    V   V    A    RRRR   III    A    BBBB   L      EEEEE   SSSS
//    V   V   A A   R   R   I    A A   B   B  L      E      S
//    V   V  A   A  RRRR    I   A   A  BBBB   L      EEE     SSS
//     V V   AAAAA  R  R    I   AAAAA  B   B  L      E          S
//      V    A   A  R   R  III  A   A  BBBB   LLLLL  EEEEE  SSSS
//    ----------------------------------------------------------------------------------

// The type of this node, i.e., agent or a coordinator, specified as a parameter in the
// "*.launch" file
int my_type = 0;

// The ID of this agent, i.e., the ID of this computer in the case that this computer is
// and agent
112
std::string my_agentID = "000";
Paul Beuchat's avatar
Paul Beuchat committed
113
114
115

// 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
116
ros::Publisher controllerYamlReadyForFetchPublisher;
Paul Beuchat's avatar
Paul Beuchat committed
117
118


119
std::string m_base_namespace;
Paul Beuchat's avatar
Paul Beuchat committed
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138

ros::Subscriber requestLoadControllerYamlSubscriber_agent_to_self;


//    ----------------------------------------------------------------------------------
//    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
//
//    PPPP   RRRR    OOO   TTTTT   OOO   TTTTT  Y   Y  PPPP   EEEEE   SSSS
//    P   P  R   R  O   O    T    O   O    T     Y Y   P   P  E      S
//    PPPP   RRRR   O   O    T    O   O    T      Y    PPPP   EEE     SSS
//    P      R  R   O   O    T    O   O    T      Y    P      E          S
//    P      R   R   OOO     T     OOO     T      Y    P      EEEEE  SSSS
//    ----------------------------------------------------------------------------------

void requestLoadControllerYamlCallback(const std_msgs::Int32& msg);