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/ActionPrimitive.h>
      20             : 
      21             : // std::ostream& operator<<(std::ostream& out, const ROSEE::ActionPrimitive::Type type){
      22             : //     const char* s = 0;
      23             : // #define PROCESS_VAL(p) case(p): s = #p; break;
      24             : //     switch(type){
      25             : //         PROCESS_VAL(ROSEE::ActionPrimitive::Type::PinchTight);     
      26             : //         PROCESS_VAL(ROSEE::ActionPrimitive::Type::PinchLoose);     
      27             : //         PROCESS_VAL(ROSEE::ActionPrimitive::Type::MultiplePinchTight);
      28             : //         PROCESS_VAL(ROSEE::ActionPrimitive::Type::Trig);
      29             : //         PROCESS_VAL(ROSEE::ActionPrimitive::Type::TipFlex);
      30             : //         PROCESS_VAL(ROSEE::ActionPrimitive::Type::FingFlex);
      31             : //         PROCESS_VAL(ROSEE::ActionPrimitive::Type::SingleJointMultipleTips);
      32             : //         PROCESS_VAL(ROSEE::ActionPrimitive::Type::None);
      33             : //     }
      34             : // #undef PROCESS_VAL
      35             : // 
      36             : //     return out << s;
      37             : // }
      38             : 
      39          20 : ROSEE::ActionPrimitive::ActionPrimitive(std::string name,  unsigned int maxStoredActionStates, ActionPrimitive::Type primitiveType) : 
      40          20 :     Action(name, Action::Type::Primitive), maxStoredActionStates(maxStoredActionStates), primitiveType(primitiveType) {};
      41             : 
      42       15683 : ROSEE::ActionPrimitive::ActionPrimitive(
      43             :     std::string name, unsigned int nFingersInvolved, unsigned int maxStoredActionStates,
      44       15683 :         ActionPrimitive::Type primitiveType) :
      45             :         Action(name, Action::Type::Primitive), nFingersInvolved(nFingersInvolved), maxStoredActionStates(maxStoredActionStates),
      46       15683 :         primitiveType(primitiveType) {}
      47             : 
      48             : 
      49         802 : ROSEE::ActionPrimitive::Type ROSEE::ActionPrimitive::getPrimitiveType() const {
      50         802 :     return primitiveType;
      51             : }
      52             : 
      53         116 : unsigned int ROSEE::ActionPrimitive::getMaxStoredActionStates() const {
      54         116 :     return maxStoredActionStates;
      55             : }
      56             : 
      57         116 : unsigned int ROSEE::ActionPrimitive::getnFingersInvolved() const {
      58         116 :     return nFingersInvolved;
      59             : }
      60             : 
      61       99695 : void ROSEE::ActionPrimitive::setJointsInvolvedCount(ROSEE::JointsInvolvedCount jointsInvolvedCount) {
      62       99695 :     this->jointsInvolvedCount = jointsInvolvedCount;
      63       99695 : }
      64             : 
      65             : 
      66             : 
      67         264 : void ROSEE::ActionPrimitive::emitYaml ( YAML::Emitter& out ) const {
      68             : 
      69             :     // key: set of string (eg two tip names)
      70         264 :     out << YAML::Key << YAML::Flow << fingersInvolved;
      71             : 
      72         264 :     unsigned int nCont = 1;
      73         264 :     out << YAML::Value << YAML::BeginMap;
      74         264 :     out << YAML::Key << "PrimitiveType" << YAML::Value << primitiveType;
      75         264 :     out << YAML::Key << "ActionName" << YAML::Value << name;
      76         264 :     out << YAML::Key << "JointsInvolvedCount" << YAML::Value << YAML::BeginMap;
      77        2013 :     for (const auto &jointCount : jointsInvolvedCount ) {
      78        1749 :         out << YAML::Key << jointCount.first;
      79        1749 :         out << YAML::Value << jointCount.second;
      80             :     }
      81         264 :     out << YAML::EndMap;
      82             : 
      83             : 
      84         528 :     for (const auto & jointPos : getAllJointPos()) {
      85             : 
      86         528 :         std::string contSeq = "ActionState_" + std::to_string(nCont);
      87         264 :         out << YAML::Key << contSeq;
      88             : 
      89         264 :         out << YAML::Value << YAML::BeginMap;
      90             :             //actionState.first, the jointstates map
      91         264 :             out << YAML::Key << "JointPos" << YAML::Value << YAML::BeginMap;
      92        2013 :             for (const auto &joint : jointPos) {
      93        1749 :                 out << YAML::Key << joint.first;
      94        1749 :                 out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
      95             :             }
      96         264 :             out << YAML::EndMap;
      97             : 
      98         264 :         out << YAML::EndMap;
      99         264 :         nCont++;
     100             :     }
     101         264 :     out << YAML::EndMap;
     102         447 : }
     103             : 
     104             : 
     105             :