Line data    Source code

       1             : /*
       2             :  * Copyright (C) 2020 IIT-HHCM
       3             :  * Author: Davide Torielli
       4             :  * email:  davide.torielli@iit.it
       5             :  *
       6             :  * Licensed under the Apache License, Version 2.0 (the "License");
       7             :  * you may not use this file except in compliance with the License.
       8             :  * You may obtain a copy of the License at
       9             :  *
      10             :  *     http://www.apache.org/licenses/LICENSE-2.0
      11             :  *
      12             :  * Unless required by applicable law or agreed to in writing, software
      13             :  * distributed under the License is distributed on an "AS IS" BASIS,
      14             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
      15             :  * See the License for the specific language governing permissions and
      16             :  * limitations under the License.
      17             :  */
      18             : 
      19             : #include <ros_end_effector/GraspingActions/ActionTrig.h>
      20             : 
      21         861 : ROSEE::ActionTrig::ActionTrig (std::string actionName, ActionPrimitive::Type actionType) :
      22         861 :     ActionPrimitive ( actionName, 1, 1, actionType ) { }
      23             : 
      24           0 : ROSEE::ActionTrig::ActionTrig (std::string actionName, ActionPrimitive::Type actionType, std::string tip, JointPos jp) :
      25           0 :     ActionPrimitive ( actionName, 1, 1, actionType ) {
      26             :         
      27           0 :     fingersInvolved.insert(tip);
      28           0 :     jointPos = jp;   
      29           0 : }
      30             : 
      31         693 : std::string ROSEE::ActionTrig::getFingerInvolved () const {
      32         693 :     return *( fingersInvolved.begin() );
      33             : }
      34             : 
      35        1231 : ROSEE::JointPos ROSEE::ActionTrig::getJointPos() const {
      36        1231 :     return jointPos;
      37             : }
      38             : 
      39         466 : std::set<std::string> ROSEE::ActionTrig::getKeyElements() const {
      40         466 :     return fingersInvolved;
      41             : }
      42             : 
      43         429 : void ROSEE::ActionTrig::setJointPos(ROSEE::JointPos jointPos) {
      44         429 :     this->jointPos = jointPos;
      45         429 : }
      46             : 
      47         543 : std::vector < ROSEE::JointPos > ROSEE::ActionTrig::getAllJointPos() const{
      48             :     
      49         543 :     std::vector < JointPos > retVect {jointPos};
      50         543 :     return retVect;
      51             : }
      52             : 
      53             : 
      54         429 : void ROSEE::ActionTrig::setFingerInvolved (std::string fingName ) {
      55         429 :     fingersInvolved.clear();
      56         429 :     fingersInvolved.insert(fingName);
      57         429 : }
      58             : 
      59         432 : bool ROSEE::ActionTrig::fillFromYaml (  YAML::const_iterator yamlIt  ) {
      60             : 
      61             : 
      62         864 :     std::vector <std::string> fingInvolvedVect = yamlIt->first.as <std::vector < std::string >> ();
      63         864 :     for (const auto &it : fingInvolvedVect) {
      64         432 :         fingersInvolved.insert(it);
      65             :     }
      66             : 
      67        2160 :     for ( YAML::const_iterator element = yamlIt->second.begin(); element != yamlIt->second.end(); ++element) {
      68             :         
      69        3456 :         std::string key = element->first.as<std::string>();
      70        1728 :         if ( key.compare ("ActionName") == 0 ){
      71         432 :             name = element->second.as < std::string > ();
      72             :             
      73        1296 :         } else  if (key.compare("JointsInvolvedCount") == 0) {
      74         432 :             jointsInvolvedCount = element->second.as < JointsInvolvedCount > ();
      75             :         
      76         864 :         } else if (key.compare ("PrimitiveType") == 0) {
      77             :             ROSEE::ActionPrimitive::Type parsedType = static_cast<ROSEE::ActionPrimitive::Type> ( 
      78         432 :                 element->second.as <unsigned int>() );
      79         432 :             if (parsedType != primitiveType ) {
      80           0 :                 std::cerr << "[ERROR ActionTrig::" << __func__ << " parsed a type " << parsedType << 
      81           0 :                     " but this object has primitive type " << primitiveType << std::endl; 
      82           0 :                 return false;
      83             :             }
      84             :             
      85         432 :         } else if (key.compare(0, 12, "ActionState_") == 0) { //compare 12 caracters from index 0 of key
      86         864 :             for(YAML::const_iterator asEl = element->second.begin(); asEl != element->second.end(); ++asEl) {
      87             :                 //asEl can be the map JointStates or the map Optional
      88             : 
      89         432 :                 if (asEl->first.as<std::string>().compare ("JointPos") == 0 ) {
      90         432 :                     jointPos =  asEl->second.as < JointPos >()  ;
      91             :                     
      92             :                 } else {
      93             :                     //ERRROr, only JointPos at this level (optional is not for trig)
      94             :                     std::cerr << "[ERROR ActionTrig::" << __func__ << " not know key " 
      95           0 :                         << asEl->first.as<std::string>() << 
      96           0 :                         " found in the yaml file at this level" << std::endl; 
      97           0 :                     return false;
      98             :                 }
      99             :             }
     100             :             
     101             :         } else {
     102             :             std::cerr << "[ERROR ActionTrig::" << __func__ << "not know key " << key << 
     103           0 :                 " found in the yaml file" << std::endl; 
     104             :         }
     105             :     }
     106         432 :     return true;
     107         183 : }