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