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/MapActionHandler.h>
18 :
19 41 : ROSEE::MapActionHandler::MapActionHandler() {}
20 :
21 182 : ROSEE::MapActionHandler::ActionPrimitiveMap ROSEE::MapActionHandler::getPrimitiveMap(std::string primitiveName) const{
22 :
23 182 : auto it = primitives.find (primitiveName);
24 182 : if (it == primitives.end() ){
25 38 : std::cerr << "[ERROR MapActionHandler::" << __func__ << "] Not found any primitive action with name " << primitiveName << std::endl;
26 38 : return ActionPrimitiveMap();
27 : }
28 :
29 144 : return it->second;
30 : }
31 :
32 131 : std::vector<ROSEE::MapActionHandler::ActionPrimitiveMap> ROSEE::MapActionHandler::getPrimitiveMap(ROSEE::ActionPrimitive::Type type) const {
33 :
34 131 : std::vector<ROSEE::MapActionHandler::ActionPrimitiveMap> vectRet;
35 :
36 629 : for (auto it : primitives) {
37 :
38 498 : if (it.second.begin()->second->getPrimitiveType() == type ){
39 55 : vectRet.push_back(it.second);
40 :
41 : }
42 : }
43 :
44 131 : return vectRet;
45 :
46 : }
47 :
48 0 : std::map<std::string, ROSEE::MapActionHandler::ActionPrimitiveMap> ROSEE::MapActionHandler::getAllPrimitiveMaps() const {
49 0 : return primitives;
50 : }
51 :
52 14 : ROSEE::ActionPrimitive::Ptr ROSEE::MapActionHandler::getPrimitive(std::string primitiveName, std::set<std::string> key) const {
53 :
54 28 : auto map = getPrimitiveMap(primitiveName);
55 :
56 14 : if (map.size() == 0 ) {
57 0 : return nullptr; //error message already printed in getPrimitiveMap
58 : }
59 :
60 14 : if (map.begin()->second->getKeyElements().size() != key.size()) {
61 : std::cerr << "[ERROR MapActionHandler::" << __func__ << "] The action '"
62 0 : << primitiveName << "' has as key a set of dimension " <<
63 0 : map.begin()->second->getKeyElements().size() <<
64 0 : " and not dimension of passed 2nd argument " << key.size() << std::endl;
65 0 : return nullptr;
66 : }
67 :
68 14 : auto it = map.find(key);
69 :
70 14 : if (it == map.end()) {
71 : std::cerr << "[ERROR MapActionHandler::" << __func__ << "] Not found any action '"
72 0 : << primitiveName << "' with key [ " ;
73 0 : for (auto keyEl : key) {
74 0 : std::cerr << keyEl << ", ";
75 : }
76 0 : std::cerr << "] " << std::endl;
77 0 : return nullptr;
78 : }
79 :
80 14 : return it->second;
81 : }
82 :
83 14 : ROSEE::ActionPrimitive::Ptr ROSEE::MapActionHandler::getPrimitive ( std::string primitiveName, std::vector<std::string> key) const {
84 :
85 28 : std::set <std::string> keySet (key.begin(), key.end());
86 28 : return getPrimitive(primitiveName, keySet);
87 : }
88 :
89 0 : ROSEE::ActionPrimitive::Ptr ROSEE::MapActionHandler::getPrimitive(std::string primitiveName, std::string key) const {
90 :
91 0 : std::set <std::string> keySet {key};
92 0 : return getPrimitive(primitiveName, keySet);
93 : }
94 :
95 0 : ROSEE::ActionPrimitive::Ptr ROSEE::MapActionHandler::getPrimitive(std::string primitiveName, std::pair<std::string,std::string> key) const {
96 :
97 0 : std::set <std::string> keySet {key.first, key.second};
98 0 : return getPrimitive(primitiveName, keySet);
99 : }
100 :
101 0 : std::vector<ROSEE::ActionPrimitive::Ptr> ROSEE::MapActionHandler::getPrimitive(ROSEE::ActionPrimitive::Type type, std::set<std::string> key) const {
102 :
103 0 : std::vector <ActionPrimitiveMap> maps = getPrimitiveMap(type);
104 :
105 : //now we look among the maps, for all the maps that has key size as the size of key passed
106 0 : std::vector <ActionPrimitiveMap> theMaps;
107 0 : for (int i =0; i<maps.size(); i++) {
108 0 : if (maps.at(i).begin()->second->getKeyElements().size() == key.size()) {
109 0 : theMaps.push_back(maps.at(i));
110 : }
111 : }
112 :
113 0 : if (theMaps.size() == 0 ) {
114 0 : std::cerr << "[ERROR MapActionHandler::" << __func__ << "] No primitive action of type '"
115 0 : << type << "' has as key a set of dimension " <<
116 0 : key.size() << " (passed 2nd argument)" << std::endl;
117 0 : return std::vector<ROSEE::ActionPrimitive::Ptr>();
118 : }
119 :
120 : //now we look, among all the themaps (where key size is the same as the passed arg key)
121 : // for an action that as effectively the wanted key. We can have more than one action
122 : // with the wanted key because in theMaps vector we have different primitives (altought
123 : // of same type). This is not possible now (because singleJointMultipleTips have as key a joint, so
124 : // a joint cant move X fingers and ALSO Y fingers) (and multiplePinch action have all
125 : // different key set size ( the number of fing used for multpinch). Anyway a vect is
126 : // returned because we do not know if in future we will have new type of primitives.
127 0 : std::vector<ROSEE::ActionPrimitive::Ptr> returnVect;
128 0 : for (auto action : theMaps) {
129 0 : auto it = action.find(key);
130 0 : if (it != action.end()) {
131 0 : returnVect.push_back(it->second);
132 : }
133 : }
134 :
135 0 : if (returnVect.size() == 0) {
136 0 : std::cerr << "[ERROR MapActionHandler::" << __func__ << "] Not found any primitive action of type '" << type << "' with key [ " ;
137 0 : for (auto keyEl : key) {
138 0 : std::cerr << keyEl << ", ";
139 : }
140 0 : std::cerr << "] " << std::endl;
141 0 : return std::vector<ROSEE::ActionPrimitive::Ptr>();
142 : }
143 :
144 0 : return returnVect;
145 :
146 : }
147 :
148 0 : std::vector<ROSEE::ActionPrimitive::Ptr> ROSEE::MapActionHandler::getPrimitive ( ROSEE::ActionPrimitive::Type type, std::vector<std::string> key) const {
149 :
150 0 : std::set <std::string> keySet (key.begin(), key.end());
151 0 : return getPrimitive(type, keySet);
152 : }
153 :
154 0 : std::vector<ROSEE::ActionPrimitive::Ptr> ROSEE::MapActionHandler::getPrimitive(ROSEE::ActionPrimitive::Type type, std::string key) const {
155 :
156 0 : std::set <std::string> keySet {key};
157 0 : return getPrimitive(type, keySet);
158 : }
159 :
160 0 : std::vector<ROSEE::ActionPrimitive::Ptr> ROSEE::MapActionHandler::getPrimitive(ROSEE::ActionPrimitive::Type type, std::pair<std::string,std::string> key) const {
161 :
162 0 : std::set <std::string> keySet {key.first, key.second};
163 0 : return getPrimitive(type, keySet);
164 : }
165 :
166 0 : std::map<std::string, std::set<std::string> > ROSEE::MapActionHandler::getPinchTightPairsMap() const {
167 0 : return pinchTightPairsMap;
168 : }
169 :
170 0 : std::map<std::string, std::set<std::string> > ROSEE::MapActionHandler::getPinchLoosePairsMap() const {
171 0 : return pinchLoosePairsMap;
172 : }
173 :
174 80 : std::shared_ptr<ROSEE::ActionGeneric> ROSEE::MapActionHandler::getGeneric(std::string name, bool verbose) const {
175 :
176 80 : auto it = generics.find(name);
177 80 : if (it == generics.end() ) {
178 24 : if (verbose) {
179 8 : std::cerr << "[ERROR MapActionHandler " << __func__ << "] No generic function named '" << name << "'" << std::endl;
180 : }
181 24 : return nullptr;
182 : }
183 :
184 56 : return it->second;
185 : }
186 :
187 0 : std::map<std::string, std::shared_ptr<ROSEE::ActionGeneric> > ROSEE::MapActionHandler::getAllGenerics() const {
188 0 : return generics;
189 : }
190 :
191 4 : std::shared_ptr<ROSEE::ActionTimed> ROSEE::MapActionHandler::getTimed(std::string name) const {
192 :
193 4 : auto it = timeds.find(name);
194 4 : if (it == timeds.end() ) {
195 0 : std::cerr << "[ERROR MapActionHandler " << __func__ << "] No timed function named '" << name << "'" << std::endl;
196 0 : return nullptr;
197 : }
198 :
199 4 : return it->second;
200 : }
201 :
202 0 : std::map<std::string, std::shared_ptr<ROSEE::ActionTimed>> ROSEE::MapActionHandler::getAllTimeds() const {
203 0 : return timeds;
204 : }
205 :
206 :
207 0 : std::map<std::string, ROSEE::ActionPrimitive::Ptr> ROSEE::MapActionHandler::getPrimitiveSingleJointMultipleTipsMap(unsigned int nFingers) const {
208 :
209 0 : std::map<std::string, ROSEE::ActionPrimitive::Ptr> ret;
210 :
211 0 : for (auto it : primitives) {
212 :
213 0 : if (it.second.begin()->second->getPrimitiveType() ==
214 0 : ROSEE::ActionPrimitive::Type::SingleJointMultipleTips &&
215 0 : it.second.begin()->second->getnFingersInvolved() == nFingers){
216 :
217 : //copy the map into one similar but with as key a strign and not a set
218 0 : for (auto itt : it.second) {
219 : //itt.first is the set of one element
220 0 : std::string key = *(itt.first.begin());
221 0 : ret.insert(std::make_pair(key, itt.second));
222 : }
223 :
224 : }
225 : }
226 :
227 0 : if (ret.size() == 0) {
228 0 : std::cerr << "[WARNING MapActionHandler::" << __func__ << "] Not found any singleJointMultipleTips action that moves " << nFingers << " fingers " << std::endl;
229 : }
230 :
231 0 : return ret;
232 : }
233 :
234 0 : ROSEE::Action::Ptr ROSEE::MapActionHandler::getGrasp(unsigned int nFingers, std::string graspName) {
235 :
236 0 : auto it = generics.find(graspName);
237 0 : if (it != generics.end()) {
238 0 : return it->second;
239 : }
240 :
241 0 : auto moreTip = getPrimitiveSingleJointMultipleTipsMap(nFingers);
242 0 : if (moreTip.size() == 1) { //if more than 1 I do not know how to choose the one that effectively "grasp"
243 0 : return moreTip.begin()->second;
244 : }
245 :
246 : std::cerr << "[WARNING MapActionHandler::" << __func__ << "] Not found any grasp named " << graspName << " neither a singleJointMultipleTips primitive "
247 0 : << "that move all fingers with a single joint, you should create one action for grasp before calling parseAllActions/parseAllGenerics()"
248 0 : << std::endl;
249 :
250 :
251 0 : return nullptr;
252 :
253 : }
254 :
255 :
256 0 : std::set<std::string> ROSEE::MapActionHandler::getFingertipsForPinch(std::string finger, ROSEE::ActionPrimitive::Type pinchType) const {
257 :
258 0 : std::set <std::string> pairedFinger;
259 :
260 0 : switch (pinchType) {
261 :
262 0 : case ROSEE::ActionPrimitive::Type::PinchTight : {
263 :
264 0 : auto it = pinchTightPairsMap.find(finger);
265 :
266 0 : if ( it != pinchTightPairsMap.end() ) {
267 0 : pairedFinger = it->second;
268 :
269 : } else {
270 0 : std::cerr << "[WARNING MapActionHandler " << __func__ << "] No companions found to make a tight pinch with " << finger << " finger"
271 0 : << std::endl;
272 : }
273 0 : break;
274 : }
275 :
276 0 : case ROSEE::ActionPrimitive::Type::PinchLoose : {
277 :
278 0 : auto it = pinchLoosePairsMap.find(finger);
279 :
280 0 : if ( it != pinchLoosePairsMap.end() ) {
281 0 : pairedFinger = it->second;
282 :
283 : } else {
284 0 : std::cerr << "[WARNING MapActionHandler " << __func__ << "] No companions found to make a loose pinch with " << finger << " finger"
285 0 : << std::endl;
286 : }
287 0 : break;
288 : }
289 :
290 0 : default: {
291 :
292 0 : std::cerr << "[WARNING MapActionHandler " << __func__ << "] Type " <<
293 0 : pinchType << " is not a type to look for companions " << std::endl;
294 : }
295 : }
296 :
297 0 : return pairedFinger;
298 : }
299 :
300 :
301 41 : bool ROSEE::MapActionHandler::parseAllActions(std::string pathFolder) {
302 :
303 41 : bool flag = true;
304 41 : if (! parseAllPrimitives(pathFolder + "/primitives/") ) {
305 0 : flag = false;
306 : }
307 41 : if (! parseAllGenerics(pathFolder + + "/generics/") ) {
308 0 : flag = false;
309 : }
310 41 : if (! parseAllTimeds(pathFolder + + "/timeds/") ) {
311 0 : flag = false;
312 : }
313 :
314 41 : return flag;
315 :
316 : }
317 :
318 41 : bool ROSEE::MapActionHandler::parseAllPrimitives(std::string pathFolder) {
319 :
320 82 : std::vector <std::string> filenames = ROSEE::Utils::getFilesInDir(pathFolder);
321 41 : YamlWorker yamlWorker;
322 :
323 199 : for (auto file : filenames) {
324 316 : ActionPrimitiveMap primitive = yamlWorker.parseYamlPrimitive(pathFolder+file);
325 158 : primitives.insert (std::make_pair( primitive.begin()->second->getName(), primitive) ) ;
326 : }
327 :
328 41 : findPinchPairsMap();
329 :
330 82 : return true;
331 :
332 : }
333 :
334 41 : bool ROSEE::MapActionHandler::parseAllGenerics(std::string pathFolder) {
335 :
336 82 : std::vector <std::string> filenames = ROSEE::Utils::getFilesInDir(pathFolder);
337 41 : YamlWorker yamlWorker;
338 316 : for (auto file : filenames) {
339 :
340 : ROSEE::ActionGeneric::Ptr genericPointer =
341 550 : yamlWorker.parseYamlGeneric(pathFolder+file);
342 275 : generics.insert (std::make_pair( genericPointer->getName(), genericPointer) ) ;
343 : }
344 :
345 :
346 82 : return true;
347 :
348 :
349 : }
350 :
351 41 : bool ROSEE::MapActionHandler::parseAllTimeds(std::string pathFolder) {
352 :
353 82 : std::vector <std::string> filenames = ROSEE::Utils::getFilesInDir(pathFolder);
354 41 : YamlWorker yamlWorker;
355 123 : for (auto file : filenames) {
356 :
357 164 : std::shared_ptr < ROSEE::ActionTimed > timed = yamlWorker.parseYamlTimed(pathFolder+file);
358 82 : timeds.insert (std::make_pair( timed->getName(), timed) ) ;
359 : }
360 :
361 82 : return true;
362 : }
363 :
364 :
365 12 : bool ROSEE::MapActionHandler::insertSingleGeneric(ROSEE::ActionGeneric::Ptr generic) {
366 :
367 12 : auto it = generics.find(generic->getName());
368 :
369 12 : if (it != generics.end()){
370 :
371 : std::cerr << "[ERROR MapActionHandler " << __func__ << "] Trying to insert generic action with name " <<
372 0 : generic->getName() << "which already exists" << std::endl;
373 :
374 0 : return false;
375 : }
376 :
377 : //it as hint beause we already did the lookup in the find above
378 12 : generics.insert(it, std::make_pair(generic->getName(), generic));
379 :
380 12 : return true;
381 :
382 : }
383 :
384 :
385 :
386 :
387 : //******************************** PRIVATE FUNCT ********************************************************************/
388 :
389 41 : void ROSEE::MapActionHandler::findPinchPairsMap() {
390 :
391 : //Assume only one pinch tight, it should be like this now.
392 82 : auto maps = getPrimitiveMap(ROSEE::ActionPrimitive::Type::PinchTight);
393 :
394 41 : if (maps.size() != 0 ){
395 62 : for (ActionPrimitiveMap map : maps) {
396 86 : for (auto mapEl : map) {
397 :
398 165 : for (auto fing : mapEl.first) { //.first is a set
399 :
400 : //we will insert all the set as value, this means that also will include the key itself,
401 : // we remove the key from the values later
402 110 : if (pinchTightPairsMap.count(fing) == 0 ) {
403 74 : pinchTightPairsMap.insert(std::make_pair(fing, mapEl.first));
404 : } else {
405 36 : pinchTightPairsMap.at(fing).insert (mapEl.first.begin(), mapEl.first.end());
406 : }
407 : }
408 : }
409 : }
410 : //remove the string key from the values.
411 105 : for (auto it : pinchTightPairsMap) {
412 74 : it.second.erase(it.first);
413 : }
414 : }
415 :
416 : //now do the same for the loose pinches
417 82 : auto mapsloose = getPrimitiveMap(ROSEE::ActionPrimitive::Type::PinchLoose);
418 :
419 41 : if (mapsloose.size() != 0 ) {
420 16 : for (ActionPrimitiveMap map : mapsloose) {
421 16 : for (auto mapEl : map) {
422 :
423 24 : for (auto fing : mapEl.first) { //.first is a set
424 :
425 : //we will insert all the set as value, this means that also will include the key itself,
426 : // we remove the key from the values later
427 16 : if (pinchLoosePairsMap.count(fing) == 0 ) {
428 16 : pinchLoosePairsMap.insert(std::make_pair(fing, mapEl.first));
429 : } else {
430 0 : pinchLoosePairsMap.at(fing).insert (mapEl.first.begin(), mapEl.first.end());
431 : }
432 : }
433 : }
434 : }
435 : //remove the string key from the values.
436 24 : for (auto it : pinchLoosePairsMap) {
437 16 : it.second.erase(it.first);
438 : }
439 : }
440 164 : }
441 :
442 :
|
