LCOV - code coverage report
Current view: top level - include/ros_end_effector - MapActionHandler.h (source / functions) Hit Total Coverage
Test: main_coverage.info Lines: 1 1 100.0 %
Date: 2021-10-05 16:55:17 Functions: 1 1 100.0 %

          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             : #ifndef MAPACTIONHANDLER_H
      18             : #define MAPACTIONHANDLER_H
      19             : 
      20             : #include <string>
      21             : #include <map>
      22             : #include <set>
      23             : #include <memory>
      24             : 
      25             : #include <ros_end_effector/YamlWorker.h>
      26             : #include <ros_end_effector/Utils.h>
      27             : 
      28             : #include <ros_end_effector/GraspingActions/Action.h>
      29             : #include <ros_end_effector/GraspingActions/ActionPrimitive.h>
      30             : #include <ros_end_effector/GraspingActions/ActionTimed.h>
      31             : #include <ros_end_effector/GraspingActions/ActionGeneric.h>
      32             : 
      33             : 
      34             : 
      35             : namespace ROSEE {
      36             : /**
      37             :  * @todo write docs
      38             :  */
      39          41 : class MapActionHandler {
      40             : 
      41             : public:
      42             :     
      43             :     typedef std::shared_ptr<MapActionHandler> Ptr;
      44             : 
      45             :     MapActionHandler();
      46             : 
      47             :     typedef std::map < std::set < std::string >, ROSEE::ActionPrimitive::Ptr > ActionPrimitiveMap;
      48             : 
      49             :     /**
      50             :      * @param folder where the action are. the action will be look in (<pkg_path> + pathFolder + "/" + handName + "/") ;
      51             :      */
      52             :     bool parseAllPrimitives(std::string pathFolder);
      53             :     bool parseAllGenerics(std::string pathFolder);
      54             :     bool parseAllTimeds(std::string pathFolder);
      55             :     bool parseAllActions(std::string pathFolder);
      56             : 
      57             :     /************************************* Generic Functions to get actions *************************/
      58             :     /**
      59             :      * @brief getter to take all the primitives maps of one type (\param type) 
      60             :      *      A primitive map is a map where key is the element involved (joints or fingers) and values the action primitive itself
      61             :      *      It returns a vector (differently from version with string as argument) because we can have more primitives
      62             :      *      with same type (e.g. a singleJointMultipleTips primitive, which are differente between them because of the joint they move 
      63             :      *      (singleJointMultipleTips_5 , singleJointMultipleTips_4 ... ) , or also multipinch)). This consideration is valid for all the other getPrimitive
      64             :      * @param type the primitive type of the action that we want
      65             :      * @return vector of all the primitives that are of type \param type
      66             :      * @warning Be sure to call parsing functions (parseAll*** ) otherwise no actions are returned never
      67             :      */
      68             :     std::vector<ActionPrimitiveMap> getPrimitiveMap( ROSEE::ActionPrimitive::Type type ) const;
      69             :     
      70             :     ActionPrimitiveMap getPrimitiveMap( std::string primitiveName )  const;
      71             :     std::map <std::string, ActionPrimitiveMap> getAllPrimitiveMaps () const;
      72             :     
      73             :     /**
      74             :      * @brief getter to take a single primitive, identified by the name and by the key (e.g. finger involved or joint involved)
      75             :      * 
      76             :      * There are various version of this get, if you pass the type a vector is returned. For some type, can be  
      77             :      * easier to use pair or single string as second argument instead of the set of string, when the type is associated with these
      78             :      * particular keys (e.g. trig has a single string (the finger used) as key, pinch has a pair (the 2 fingers used for the pinch) ... )
      79             :      *
      80             :      * @param primitiveName the name of the primitive that we want
      81             :      * @param key a set of strings to identify the primitive that we want among all the primitives that are called \param primitiveName
      82             :      * @example to get a pinch with thumb and index, we will pass "pinch" and a set of two element, "thumb" and "index". Note that 
      83             :      *      for this case we can also pass a pair (and not a set) as second argument
      84             :      * 
      85             :      * @return pointer to the primitive that we were looking for. nullptr if action not found.
      86             :      * 
      87             :      */
      88             :     ROSEE::ActionPrimitive::Ptr getPrimitive (std::string primitiveName, std::set<std::string> key) const;
      89             :     std::vector<ROSEE::ActionPrimitive::Ptr> getPrimitive (ROSEE::ActionPrimitive::Type, std::set<std::string> key) const;
      90             :     ROSEE::ActionPrimitive::Ptr getPrimitive (std::string primitiveName, std::vector<std::string> key) const;
      91             :     std::vector<ROSEE::ActionPrimitive::Ptr> getPrimitive (ROSEE::ActionPrimitive::Type, std::vector<std::string> key) const;
      92             :     ROSEE::ActionPrimitive::Ptr getPrimitive (std::string primitiveName, std::pair<std::string, std::string> key) const;
      93             :     std::vector<ROSEE::ActionPrimitive::Ptr> getPrimitive (ROSEE::ActionPrimitive::Type, std::pair<std::string, std::string> key) const;
      94             :     ROSEE::ActionPrimitive::Ptr getPrimitive (std::string primitiveName, std::string key) const;
      95             :     std::vector<ROSEE::ActionPrimitive::Ptr> getPrimitive (ROSEE::ActionPrimitive::Type, std::string key) const;
      96             :     
      97             :     /**
      98             :      * @note these functions return generics and composed (because composed are a derived class of generic)
      99             :      */
     100             :     std::shared_ptr<ROSEE::ActionGeneric> getGeneric (std::string name, bool verbose = true) const;
     101             :     std::map <std::string, std::shared_ptr<ROSEE::ActionGeneric>> getAllGenerics () const;
     102             :     
     103             :     std::shared_ptr<ROSEE::ActionTimed> getTimed (std::string name) const;
     104             :     std::map <std::string, std::shared_ptr<ROSEE::ActionTimed>> getAllTimeds () const;
     105             :     
     106             :     /************************************* Specific functions for specific actions *************************/
     107             : 
     108             : 
     109             :     /**
     110             :      * @brief function to return the map that contains all the singleJointMultipleTips primitive with that moves the specific number
     111             :      *    of fingers \p nFingers . This is in practice, the file singleJointMultipleTips_*nFingers*.yaml
     112             :      * 
     113             :      * It return only one map because per definition we have only one ActionPrimitiveMap of type singleJointMultipleTips with a defined number of 
     114             :      * nFinger. (Then inside it we can have more primitives ( when we have a hand with more joints that moves more than 1 finger), but
     115             :      * always with the same number of nFinger)
     116             :      * 
     117             :      * @param nFingers the number of the finger that the singleJointMultipleTips primitives moves
     118             :      * @return A map with key the joint that moves the \p nFingers and as value the primitive with all the info to command it to the hand.
     119             :      *      Obvioulsy we can have more joints that move a certain number (\p nFingers) of fingers
     120             :      * @todo return with the key as set instead??? or leave as it is?
     121             :      */
     122             :     std::map <std::string, ROSEE::ActionPrimitive::Ptr> getPrimitiveSingleJointMultipleTipsMap ( unsigned int nFingers ) const;   
     123             :     
     124             :     /**
     125             :      * @brief This function try to get an action that should be a grasp
     126             :      * 
     127             :      * A real grasp, until now, can be two things: 
     128             :      *  - a generic/composed action, composed by trig with all the fingers
     129             :      *  - a SingleJointMultipleTips primitive action, where the number of fingers moved is obviously the number of finger of the hand
     130             :      * In the second case, if we have more joints that moves all the fingers, we return no action because we do not know which one is
     131             :      * "the grasp". If we have only one joint that move all fingers, we return the singleJointMultipleTips action associated with this joint, even if
     132             :      * there is the possibility that this is not a grasp (but which hand has only one joint that moves all fingers and it is not for a grasp?)
     133             :      * 
     134             :      * This function look first for a generic (or composed, that is a derived class of generic) action with name \p graspName , 
     135             :      * that should have been created and emitted before calling the \ref parseAllActions (or \ref parseAllGenerics) 
     136             :      * (e.g. in UniversalFindActions putting all the trigs togheter, but we can also define the grasp in another way).
     137             :      *   
     138             :      * @param nFingers the number of the finger of the hand, that is an information that this class hasn't
     139             :      * @param graspName name given to the action grasp, default is "grasp"
     140             :      * @return pointer to Action, because the pointed object can be a primitive or a generic action.
     141             :      * @todo If not found, it tries to create a composed action, done with all trigs, and store this new action??   
     142             :      * @warning Generic and singleJointMultipleTips are different ros msg: singleJointMultipleTips want a element to be set (the joint) so this can cause problems..
     143             :      *   so maybe this function should not exist
     144             :      */
     145             :     ROSEE::Action::Ptr getGrasp ( unsigned int nFingers, std::string graspName = "grasp" ) ;
     146             :     
     147             :     /************************************* Other functions not returning actions themselves *************************/
     148             :     std::set<std::string> getFingertipsForPinch ( std::string finger, ROSEE::ActionPrimitive::Type pinchType) const;
     149             :     std::map <std::string, std::set<std::string> > getPinchTightPairsMap ( ) const;
     150             :     std::map <std::string, std::set<std::string> > getPinchLoosePairsMap ( ) const;
     151             :     
     152             :     /***************************** Other *******************************************/
     153             :     
     154             :     /** 
     155             :      * @brief This is needed by rosservicehandler which has to include a new doable action, if received the service
     156             :      * 
     157             :      */
     158             :     bool insertSingleGeneric(ROSEE::ActionGeneric::Ptr generic);
     159             : 
     160             : private:
     161             :     
     162             :     void findPinchPairsMap();
     163             :     
     164             :     std::string handName;
     165             : 
     166             :     std::map <std::string, ActionPrimitiveMap> primitives;
     167             :     std::map <std::string, std::shared_ptr<ROSEE::ActionGeneric>> generics;
     168             :     std::map <std::string, std::shared_ptr<ROSEE::ActionTimed>> timeds;
     169             :     
     170             :     std::map <std::string, std::set<std::string> > pinchTightPairsMap;
     171             :     std::map <std::string, std::set<std::string> > pinchLoosePairsMap;
     172             : 
     173             : 
     174             : };
     175             : 
     176             : } //namespace rosee
     177             : 
     178             : #endif // MAPACTIONHANDLER_H

Generated by: LCOV version 1.13