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 ROSEE_ACTIONTIMED_H
      18             : #define ROSEE_ACTIONTIMED_H
      19             : 
      20             : #include <vector>
      21             : #include <string>
      22             : #include <map>
      23             : #include <end_effector/GraspingActions/Action.h>
      24             : #include <end_effector/Utils.h>
      25             : #include <yaml-cpp/yaml.h>
      26             : 
      27             : namespace ROSEE {
      28             : 
      29             : /**
      30             :  * @brief An action composed by other ones that must be executed one after other with some wait time (also 0) in between. 
      31             :  * E.G. 0.000000second -----> Grasp -----> 0.1 + 0.1 second -----> OpenLid -----> 0.5second ---- and so on
      32             :  * This class contains all the joint position of each action (\ref actionsJointPosMap ) and some time margins which indicates 
      33             :  * wait time before and after the action (\ref actionsTimeMarginsMap). 
      34             :  * Each action inside is identified by its name, so no two action with same name can exist (see \ref insertAction )
      35             :  * After create the ActionTimed object, we can add actions with \ref insertAction().
      36             :  * As all other Action classes, it implements also functions to emit and parse in a yaml file
      37             :  * @todo identify inner actions with integer instead of given name?
      38             :  */
      39             : class ActionTimed : public Action
      40             : {
      41             : public:
      42             :     typedef std::shared_ptr<ActionTimed> Ptr;
      43             :     typedef std::shared_ptr<const ActionTimed> ConstPtr;
      44             :     /**
      45             :      * @brief Default constructor, used when parsing action from yaml file
      46             :      */
      47             :     ActionTimed();
      48             :     
      49             :     /**
      50             :      * @brief Costructor
      51             :      * @param actionName name of the action that will be created
      52             :      */
      53             :     ActionTimed(std::string actionName);
      54             : 
      55             :     /**
      56             :      * @brief Destructor
      57             :      */
      58          24 :     ~ActionTimed() {};
      59             :     
      60             :     /**
      61             :      * @brief Override this function is necessary because it is pure virtual in father class \ref Action. 
      62             :      * @return JointPos the joints positions of the last inserted action (the last one in the time line)
      63             :      */
      64             :     JointPos getJointPos () const override;
      65             :     
      66             :     /**
      67             :      * @brief Override function from father \ref Action. For this class, it returns the jointPos of all the 
      68             :      * inner actions that are inside this ActionTimed.
      69             :      * @return vector of JointPos of all the inner actions, in order of execution
      70             :      */
      71             :     std::vector < ROSEE::JointPos > getAllJointPos () const override;
      72             : 
      73             :     /**
      74             :      * @brief Get the JointsInvolvedCount maps of all the inner actions, in order
      75             :      * @return vector of JointsInvolvedCount map of inner actions
      76             :      */ 
      77             :     std::vector < ROSEE::JointsInvolvedCount > getAllJointCountAction() const;
      78             :     
      79             :     /**
      80             :      * @brief get all the time margins of all inner action
      81             :      * @return vector of pair : the first element of pair is the time to wait before
      82             :      *        executing the action, the second element the time to wait after.
      83             :      */
      84             :     std::vector < std::pair <double, double> > getAllActionMargins() const;
      85             :     
      86             :     /**
      87             :      * @brief get for joint positions
      88             :      * @param actionName the name of the inner action of which we want to know the JointPos
      89             :      * @return JointPos position of joints of \p actionName
      90             :      *         Return empty JointPos if \p actionName is not present in this ActionTimed
      91             :      */
      92             :     ROSEE::JointPos getJointPosAction ( std::string actionName ) const ;
      93             :     
      94             :     /**
      95             :      * @brief get for time margins
      96             :      * @param actionName the name of the inner action of which we want to know the time margins
      97             :      * @return a pair of positive double. The first is the time needed BEFORE the action, the secont the time AFTER
      98             :      *         return -1 -1 if the \p actionName was not present in this ActionTimed.
      99             :      */
     100             :     std::pair <double, double> getActionMargins ( std::string actionName ) const ;
     101             :     
     102             :     /**
     103             :      * @brief get for JointsInvolvedCount of the inner actions
     104             :      * @param actionName the name of the inner action of which we want to know the JointCount
     105             :      * @return JointsInvolvedCount of joints of \p actionName
     106             :      *         Return empty JointsInvolvedCount if \p actionName is not present in this
     107             :      *         ActionTimed
     108             :      */
     109             :     ROSEE::JointsInvolvedCount getJointCountAction ( std::string actionName ) const;
     110             : 
     111             :     /**
     112             :      * @brief getter for action that composed this one
     113             :      * @return vector of string of inner actions, ordered by time execution
     114             :      */
     115             :     std::vector <std::string> getInnerActionsNames() const ;
     116             :     /**
     117             :      * @brief Print info about this action
     118             :      */
     119             :     void print () const override;
     120             :     
     121             :     /**
     122             :      * @brief Emit info in a file with yaml format
     123             :      * @param out a YAML::Emitter& object to emit the infos
     124             :      */    
     125             :     void emitYaml ( YAML::Emitter& out) const override;
     126             :     
     127             :     /**
     128             :      * @brief Fill the internal data with infos taken from yaml file. 
     129             :      * @param yamlIt a yamlt iterator to a node which has loaded the file
     130             :      * @return false if some error happened
     131             :      */
     132             :     bool fillFromYaml(  YAML::const_iterator yamlIt ) override;
     133             :     
     134             :     /** 
     135             :      * @brief Insert an action as last one in the time line 
     136             :      * @param action pointer to the action to be inserted
     137             :      * @param marginBefore the time margin to wait before executing the \p action 
     138             :      * @param marginAfter the time margin to wait after executing the \p action 
     139             :      * @param jointPosIndex (default == 0) the wanted jointPos of \p action to insert. Error the index is greater than the number
     140             :      *      of joint pos in the \p action. First element has index 0. 
     141             :      * @param percentJointPos (default == 1) OPTIONAL argument to scale all the joint position of the \p action that is being inserted
     142             :      * @param newActionName (default == "") OPTIONAL argument if we want to store the \p action with a different name
     143             :      * @return False if some error happened
     144             :      * @warning We can't have inned actions with same name. So, if \p action name (or \p newActionName) is already present, 
     145             :      * the action is not inserted and the function returns false. Being \ref Action names not changeable, to solve this 
     146             :      * we can pass the \p newActionName argument to this function. If it will be inserted, it will be referenced with this new name
     147             :      * @note We take only necessary infos from \p action and store them in the members of ActionTimed. There is not way to go
     148             :      * back to the original inserted action from an ActionTimed
     149             :      */
     150             :     bool insertAction ( ROSEE::Action::Ptr action, double marginBefore = 0.0, double marginAfter = 0.0, 
     151             :                         unsigned int jointPosIndex = 0, double percentJointPos = 1, std::string newActionName = "");
     152             :     
     153             : private:
     154             :     std::map <std::string, std::pair<double, double> > actionsTimeMarginsMap;
     155             :     std::map <std::string, ROSEE::JointPos> actionsJointPosMap;
     156             :     std::map <std::string, ROSEE::JointsInvolvedCount> actionsJointCountMap;
     157             :     
     158             :     /**
     159             :      * Here is contained the wanted final position of the timed action.
     160             :      * So, it is the sum of all the wanted joint position of all the inner actions
     161             :      */
     162             :     ROSEE::JointPos jointPosFinal;
     163             :     
     164             :     /**
     165             :      * This vector is used to take count of the order of the actions inserted
     166             :      */
     167             :     std::vector < std::string > actionsNamesOrdered;
     168             : 
     169             : };
     170             : 
     171             : }
     172             : 
     173             : #endif // ROSEE_ACTIONTIMED_H