Line data Source code
1 : /*
2 : * Copyright 2020 <copyright holder> <email>
3 : *
4 : * Licensed under the Apache License, Version 2.0 (the "License");
5 : * you may not use this file except in compliance with the License.
6 : * You may obtain a copy of the License at
7 : *
8 : * http://www.apache.org/licenses/LICENSE-2.0
9 : *
10 : * Unless required by applicable law or agreed to in writing, software
11 : * distributed under the License is distributed on an "AS IS" BASIS,
12 : * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 : * See the License for the specific language governing permissions and
14 : * limitations under the License.
15 : */
16 :
17 : #include <ros_end_effector/YamlWorker.h>
18 :
19 420 : ROSEE::YamlWorker::YamlWorker ( ) {}
20 :
21 36 : std::string ROSEE::YamlWorker::createYamlFile( const ROSEE::Action* action, std::string pathFolder) {
22 :
23 36 : ROSEE::Utils::create_directory ( pathFolder );
24 72 : std::string output = emitYaml ( action );
25 36 : ROSEE::Utils::out2file(pathFolder + action->getName() + ".yaml", output);
26 72 : return (pathFolder + action->getName() + ".yaml");
27 :
28 : }
29 :
30 4 : std::string ROSEE::YamlWorker::createYamlFile( const ROSEE::ActionGeneric::Ptr action, std::string pathFolder) {
31 :
32 4 : ROSEE::Utils::create_directory ( pathFolder );
33 8 : std::string output = emitYaml ( action );
34 4 : ROSEE::Utils::out2file(pathFolder + action->getName() + ".yaml", output);
35 8 : return (pathFolder + action->getName() + ".yaml");
36 :
37 : }
38 :
39 152 : std::string ROSEE::YamlWorker::createYamlFile(
40 : const std::map < std::set <std::string> , ActionPrimitive* > mapOfActions,
41 : const std::string actionName, std::string pathFolder) {
42 :
43 152 : ROSEE::Utils::create_directory ( pathFolder );
44 304 : std::string output = emitYaml ( mapOfActions );
45 152 : ROSEE::Utils::out2file( pathFolder + actionName + ".yaml", output);
46 304 : return (pathFolder + actionName + ".yaml");
47 :
48 :
49 : }
50 :
51 :
52 152 : std::string ROSEE::YamlWorker::emitYaml (
53 : const std::map < std::set <std::string> , ActionPrimitive* > mapOfActions ) {
54 :
55 304 : YAML::Emitter out;
56 152 : out << YAML::BeginMap;
57 503 : for (const auto & mapEl : mapOfActions) {
58 351 : mapEl.second->emitYaml(out);
59 351 : out << YAML::Newline << YAML::Newline; //double to insert a blanck line between tips pair
60 : }
61 152 : out << YAML::EndMap;
62 304 : return out.c_str();
63 :
64 : }
65 :
66 36 : std::string ROSEE::YamlWorker::emitYaml ( const ROSEE::Action* action ) {
67 :
68 72 : YAML::Emitter out;
69 36 : action->emitYaml(out);
70 36 : out << YAML::Newline << YAML::Newline; //double to insert a blanck line
71 72 : return out.c_str();
72 :
73 : }
74 :
75 4 : std::string ROSEE::YamlWorker::emitYaml ( const ROSEE::ActionGeneric::Ptr action ) {
76 :
77 8 : YAML::Emitter out;
78 4 : action->emitYaml(out);
79 4 : out << YAML::Newline << YAML::Newline; //double to insert a blanck line
80 8 : return out.c_str();
81 :
82 : }
83 :
84 0 : std::map<std::set<std::string>, ROSEE::ActionPrimitive::Ptr> ROSEE::YamlWorker::parseYamlPrimitive(
85 : std::string fileWithPath, ROSEE::ActionPrimitive::Type actionType) {
86 :
87 0 : std::map < std::set < std::string>, ROSEE::ActionPrimitive::Ptr > parsedMap;
88 :
89 : //TODO check elsewhere if file exist or not?
90 0 : std::ifstream ifile(fileWithPath);
91 0 : if (! ifile) {
92 0 : std::cout << "[ERROR YAMLPARSER:: " << __func__ << "]: file " << fileWithPath << " not found. " << std::endl;
93 0 : return parsedMap;
94 : }
95 :
96 0 : YAML::Node node = YAML::LoadFile(fileWithPath);
97 :
98 0 : for(YAML::const_iterator it4Action = node.begin(); it4Action != node.end(); ++it4Action) {
99 0 : ActionPrimitive::Ptr ptr;
100 0 : switch (actionType) {
101 0 : case ActionPrimitive::Type::PinchTight: {
102 0 : ptr = std::make_shared <ActionPinchTight>();
103 0 : break;
104 : }
105 0 : case ActionPrimitive::Type::PinchLoose: {
106 0 : ptr = std::make_shared <ActionPinchLoose> ();
107 0 : break;
108 : }
109 0 : case ActionPrimitive::Type::Trig: {
110 0 : ptr = std::make_shared <ActionTrig>("trig", ActionPrimitive::Type::Trig);
111 0 : break;
112 : }
113 0 : case ActionPrimitive::Type::TipFlex: {
114 0 : ptr = std::make_shared <ActionTrig>("tipFlex", ActionPrimitive::Type::TipFlex);
115 0 : break;
116 : }
117 0 : case ActionPrimitive::Type::FingFlex: {
118 0 : ptr = std::make_shared <ActionTrig>("fingFlex", ActionPrimitive::Type::FingFlex);
119 0 : break;
120 : }
121 0 : case ActionPrimitive::Type::SingleJointMultipleTips: {
122 0 : ptr = std::make_shared <ActionSingleJointMultipleTips>();
123 0 : break;
124 : }
125 0 : case ActionPrimitive::Type::MultiplePinchTight: {
126 0 : ptr = std::make_shared <ActionMultiplePinchTight> ();
127 0 : break;
128 : }
129 :
130 0 : default : {
131 0 : std::cout << "[ERROR YAMLPARSER]: " << actionType << " : type not found" << std::endl;
132 : }
133 : }
134 :
135 0 : ptr->fillFromYaml ( it4Action );
136 :
137 0 : parsedMap.insert ( std::make_pair ( ptr->getKeyElements(), ptr) );
138 : }
139 :
140 0 : return parsedMap;
141 : }
142 :
143 :
144 254 : std::map < std::set < std::string>, ROSEE::ActionPrimitive::Ptr > ROSEE::YamlWorker::parseYamlPrimitive (std::string fileWithPath) {
145 :
146 254 : std::map < std::set < std::string>, ROSEE::ActionPrimitive::Ptr > parsedMap;
147 :
148 : //TODO check elsewhere if file exist or not?
149 508 : std::ifstream ifile(fileWithPath);
150 254 : if (! ifile) {
151 0 : std::cout << "[ERROR YAMLPARSER:: " << __func__ << "]: file " << fileWithPath << " not found. " << std::endl;
152 0 : return parsedMap;
153 : }
154 :
155 508 : YAML::Node node = YAML::LoadFile(fileWithPath);
156 :
157 824 : for(YAML::const_iterator it4Action = node.begin(); it4Action != node.end(); ++it4Action) {
158 :
159 1140 : std::string actionName;
160 570 : ROSEE::ActionPrimitive::Type type = ROSEE::ActionPrimitive::Type::None;
161 :
162 : //now look for the type of the action and for the name
163 1523 : for ( YAML::const_iterator actionState = it4Action->second.begin(); actionState != it4Action->second.end(); ++actionState) {
164 :
165 1523 : if (actionState->first.as<std::string>().compare("PrimitiveType") == 0) {
166 570 : type = static_cast<ROSEE::ActionPrimitive::Type> ( actionState->second.as < int > () );
167 570 : break; //we only need the type
168 : }
169 : }
170 :
171 : //now use emit of specific action
172 1140 : ActionPrimitive::Ptr ptr;
173 570 : switch (type) {
174 99 : case ActionPrimitive::Type::PinchTight: {
175 99 : ptr = std::make_shared <ActionPinchTight>();
176 99 : break;
177 : }
178 19 : case ActionPrimitive::Type::PinchLoose: {
179 19 : ptr = std::make_shared <ActionPinchLoose> ();
180 19 : break;
181 : }
182 144 : case ActionPrimitive::Type::Trig: {
183 144 : ptr = std::make_shared <ActionTrig>("trig", ActionPrimitive::Type::Trig);
184 144 : break;
185 : }
186 144 : case ActionPrimitive::Type::TipFlex: {
187 144 : ptr = std::make_shared <ActionTrig>("tipFlex", ActionPrimitive::Type::TipFlex);
188 144 : break;
189 : }
190 144 : case ActionPrimitive::Type::FingFlex: {
191 144 : ptr = std::make_shared <ActionTrig>("fingFlex", ActionPrimitive::Type::FingFlex);
192 144 : break;
193 : }
194 8 : case ActionPrimitive::Type::SingleJointMultipleTips: {
195 8 : ptr = std::make_shared <ActionSingleJointMultipleTips>();
196 8 : break;
197 : }
198 12 : case ActionPrimitive::Type::MultiplePinchTight: {
199 12 : ptr = std::make_shared <ActionMultiplePinchTight> ();
200 12 : break;
201 : }
202 :
203 0 : default : {
204 0 : std::cout << "[ERROR YAMLPARSER]: " << type << " : type not found" << std::endl;
205 : }
206 : }
207 :
208 570 : ptr->fillFromYaml ( it4Action );
209 :
210 570 : parsedMap.insert ( std::make_pair ( ptr->getKeyElements(), ptr) );
211 : }
212 :
213 254 : return parsedMap;
214 :
215 : }
216 :
217 :
218 9 : ROSEE::ActionComposed ROSEE::YamlWorker::parseYamlComposed (std::string fileWithPath){
219 :
220 9 : ROSEE::ActionComposed parsedAction;
221 :
222 : //TODO check elsewhere if file exist or not?
223 18 : std::ifstream ifile(fileWithPath);
224 9 : if (! ifile) {
225 0 : std::cout << "[ERROR YAMLPARSER:: " << __func__ << "]: file " << fileWithPath << " not found. " << std::endl;
226 0 : return parsedAction;
227 : }
228 :
229 18 : YAML::Node node = YAML::LoadFile(fileWithPath);
230 18 : YAML::const_iterator yamlIt = node.begin();
231 :
232 9 : parsedAction.fillFromYaml ( yamlIt );
233 :
234 9 : return parsedAction;
235 :
236 : }
237 :
238 275 : ROSEE::ActionGeneric::Ptr ROSEE::YamlWorker::parseYamlGeneric(std::string fileWithPath) {
239 :
240 275 : ActionGeneric::Ptr ptrAction;
241 :
242 : //TODO check elsewhere if file exist or not?
243 550 : std::ifstream ifile(fileWithPath);
244 275 : if (! ifile) {
245 0 : std::cout << "[ERROR YAMLPARSER:: " << __func__ << "]: file " << fileWithPath << " not found. " << std::endl;
246 0 : return ptrAction;
247 : }
248 :
249 550 : YAML::Node node = YAML::LoadFile(fileWithPath);
250 :
251 550 : YAML::const_iterator it4Action = node.begin(); //for not primitive, only one action is inside the file
252 :
253 275 : ROSEE::Action::Type type = ROSEE::Action::Type::None;
254 :
255 : //now look for the type of the action
256 707 : for ( YAML::const_iterator el = it4Action->second.begin(); el != it4Action->second.end(); ++el) {
257 :
258 707 : if (el->first.as<std::string>().compare("Type") == 0) {
259 275 : type = static_cast<ROSEE::Action::Type> ( el->second.as < int > () );
260 275 : break; //we only need the type
261 : }
262 : }
263 :
264 275 : switch (type) {
265 0 : case Action::Type::Primitive : {
266 : std::cout << "[ERROR YAMLPARSER:: " << __func__ << "]: file " << fileWithPath << " contains a primitive action, "
267 0 : << " please use parseYamlPrimitive to parse it " << std::endl;
268 0 : return ptrAction;
269 : break;
270 : }
271 209 : case Action::Type::Generic : {
272 209 : ptrAction = std::make_shared <ActionGeneric> ();
273 209 : break;
274 : }
275 66 : case Action::Type::Composed : {
276 66 : ptrAction = std::make_shared <ActionComposed> ();
277 66 : break;
278 : }
279 0 : case Action::Type::Timed : {
280 : std::cout << "[ERROR YAMLPARSER:: " << __func__ << "]: file " << fileWithPath << " contains a timed action, "
281 0 : << " please use parseYamlTimed to parse it " << std::endl;
282 0 : return ptrAction;
283 : break;
284 : }
285 0 : default : {
286 0 : std::cout << "[ERROR YAMLPARSER:: " << __func__ << "]: file " << fileWithPath << " contains an action of not know type "
287 0 : << type << std::endl;
288 0 : return ptrAction;
289 : }
290 : }
291 :
292 275 : ptrAction->fillFromYaml(it4Action);
293 :
294 275 : return ptrAction;
295 :
296 :
297 : }
298 :
299 :
300 90 : std::shared_ptr < ROSEE::ActionTimed > ROSEE::YamlWorker::parseYamlTimed (std::string fileWithPath){
301 :
302 180 : std::shared_ptr < ROSEE::ActionTimed > ptrAction;
303 :
304 : //TODO check elsewhere if file exist or not?
305 180 : std::ifstream ifile(fileWithPath);
306 90 : if (! ifile) {
307 0 : std::cout << "[ERROR YAMLPARSER:: " << __func__ << "]: file " << fileWithPath << " not found. " << std::endl;
308 0 : return nullptr;
309 : }
310 :
311 180 : YAML::Node node = YAML::LoadFile(fileWithPath);
312 180 : YAML::const_iterator yamlIt = node.begin();
313 90 : ptrAction = std::make_shared <ActionTimed> ();
314 :
315 90 : ptrAction->fillFromYaml ( yamlIt );
316 :
317 90 : return ptrAction;
318 :
319 183 : }
|
