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             : #include <end_effector/YamlWorker.h>
      18             : 
      19         293 : ROSEE::YamlWorker::YamlWorker ( ) {}
      20             : 
      21          36 : std::string ROSEE::YamlWorker::createYamlFile( const ROSEE::Action* action, std::string pathFolder) {
      22             :     
      23          36 :     ROSEE::Utils::create_directory ( pathFolder );
      24          36 :     std::string output = emitYaml ( action );
      25          36 :     ROSEE::Utils::out2file(pathFolder + action->getName() + ".yaml", output);
      26         108 :     return (pathFolder + action->getName() + ".yaml");
      27             :     
      28             : }
      29             : 
      30           0 : std::string ROSEE::YamlWorker::createYamlFile( const ROSEE::ActionGeneric::Ptr action, std::string pathFolder) {
      31             :     
      32           0 :     ROSEE::Utils::create_directory ( pathFolder );
      33           0 :     std::string output = emitYaml ( action );
      34           0 :     ROSEE::Utils::out2file(pathFolder + action->getName() + ".yaml", output);
      35           0 :     return (pathFolder + action->getName() + ".yaml");
      36             :     
      37             : }
      38             : 
      39         152 : std::string ROSEE::YamlWorker::createYamlFile(
      40             :     const std::map < std::set <std::string> , ActionPrimitive* > mapOfActions,
      41             :     const std::string actionName, std::string pathFolder) {
      42             :     
      43         152 :     ROSEE::Utils::create_directory ( pathFolder );
      44         152 :     std::string output = emitYaml ( mapOfActions );
      45         152 :     ROSEE::Utils::out2file( pathFolder + actionName + ".yaml", output);
      46         456 :     return (pathFolder + actionName + ".yaml");
      47             :     
      48             :     
      49             : }
      50             : 
      51             : 
      52         152 : std::string ROSEE::YamlWorker::emitYaml ( 
      53             :     const std::map < std::set <std::string> , ActionPrimitive* > mapOfActions ) {
      54             :     
      55         152 :     YAML::Emitter out;
      56         152 :     out << YAML::BeginMap;
      57         503 :     for (const auto & mapEl : mapOfActions) {
      58         351 :         mapEl.second->emitYaml(out);
      59         351 :         out << YAML::Newline << YAML::Newline; //double to insert a blanck line between tips pair
      60             :     }
      61         152 :     out << YAML::EndMap;
      62         304 :     return out.c_str();
      63             :     
      64             : }
      65             : 
      66          36 : std::string ROSEE::YamlWorker::emitYaml  ( const ROSEE::Action* action ) {
      67             :     
      68          36 :     YAML::Emitter out;
      69          36 :     action->emitYaml(out);
      70          36 :     out << YAML::Newline << YAML::Newline; //double to insert a blanck line
      71          72 :     return out.c_str();
      72             :     
      73             : }
      74             : 
      75           0 : std::string ROSEE::YamlWorker::emitYaml  ( const ROSEE::ActionGeneric::Ptr action ) {
      76             :     
      77           0 :     YAML::Emitter out;
      78           0 :     action->emitYaml(out);
      79           0 :     out << YAML::Newline << YAML::Newline; //double to insert a blanck line
      80           0 :     return out.c_str();
      81             :     
      82             : }
      83             : 
      84           0 : std::map<std::set<std::string>, ROSEE::ActionPrimitive::Ptr> ROSEE::YamlWorker::parseYamlPrimitive(
      85             :         std::string fileWithPath, ROSEE::ActionPrimitive::Type actionType) {
      86             :     
      87           0 :     std::map < std::set < std::string>, ROSEE::ActionPrimitive::Ptr > parsedMap; 
      88             : 
      89             :     //TODO check elsewhere if file exist or not?
      90           0 :     std::ifstream ifile(fileWithPath);
      91           0 :     if (! ifile) {
      92           0 :         std::cout << "[ERROR YAMLPARSER:: " << __func__ << "]: file " << fileWithPath << " not found. "  << std::endl;
      93           0 :             return parsedMap;
      94             :     }
      95             :     
      96           0 :     YAML::Node node = YAML::LoadFile(fileWithPath);
      97             :     
      98           0 :     for(YAML::const_iterator it4Action = node.begin(); it4Action != node.end(); ++it4Action) {
      99           0 :         ActionPrimitive::Ptr ptr;
     100           0 :         switch (actionType) {
     101           0 :         case ActionPrimitive::Type::PinchTight: {
     102           0 :             ptr = std::make_shared <ActionPinchTight>();
     103           0 :             break;
     104             :         }
     105           0 :         case ActionPrimitive::Type::PinchLoose: {
     106           0 :             ptr = std::make_shared <ActionPinchLoose> ();
     107           0 :             break;
     108             :         }
     109           0 :         case ActionPrimitive::Type::Trig: {
     110           0 :             ptr = std::make_shared <ActionTrig>("trig", ActionPrimitive::Type::Trig);
     111           0 :             break;
     112             :         }
     113           0 :         case ActionPrimitive::Type::TipFlex: {
     114           0 :             ptr = std::make_shared <ActionTrig>("tipFlex", ActionPrimitive::Type::TipFlex);
     115           0 :             break;
     116             :         }
     117           0 :         case ActionPrimitive::Type::FingFlex: {
     118           0 :             ptr = std::make_shared <ActionTrig>("fingFlex", ActionPrimitive::Type::FingFlex);
     119           0 :             break;
     120             :         }
     121           0 :         case ActionPrimitive::Type::SingleJointMultipleTips: {
     122           0 :             ptr = std::make_shared <ActionSingleJointMultipleTips>();
     123           0 :             break;
     124             :         }
     125           0 :         case ActionPrimitive::Type::MultiplePinchTight: {
     126           0 :             ptr = std::make_shared <ActionMultiplePinchTight> ();
     127           0 :             break;
     128             :         }
     129             : 
     130           0 :         default : {
     131           0 :             std::cout << "[ERROR YAMLPARSER]: " << actionType << " : type not found" << std::endl;
     132             :         }
     133             :         }
     134             :         
     135           0 :         ptr->fillFromYaml ( it4Action );
     136             :         
     137           0 :         parsedMap.insert ( std::make_pair ( ptr->getKeyElements(), ptr) );
     138             :     }
     139             :         
     140           0 :     return parsedMap;
     141             : }
     142             : 
     143             : 
     144          96 : std::map < std::set < std::string>, ROSEE::ActionPrimitive::Ptr > ROSEE::YamlWorker::parseYamlPrimitive (std::string fileWithPath) {
     145             :     
     146          96 :     std::map < std::set < std::string>, ROSEE::ActionPrimitive::Ptr > parsedMap; 
     147             : 
     148             :     //TODO check elsewhere if file exist or not?
     149         192 :     std::ifstream ifile(fileWithPath);
     150          96 :     if (! ifile) {
     151           0 :         std::cout << "[ERROR YAMLPARSER:: " << __func__ << "]: file " << fileWithPath << " not found. "  << std::endl;
     152           0 :             return parsedMap;
     153             :     }
     154             :     
     155         192 :     YAML::Node node = YAML::LoadFile(fileWithPath);
     156             : 
     157         415 :     for(YAML::const_iterator it4Action = node.begin(); it4Action != node.end(); ++it4Action) {
     158             :         
     159         446 :         std::string actionName;
     160         223 :         ROSEE::ActionPrimitive::Type type = ROSEE::ActionPrimitive::Type::None;
     161             :         
     162             :         //now look for the type of the action and for the name
     163         669 :         for ( YAML::const_iterator actionState = it4Action->second.begin(); actionState != it4Action->second.end(); ++actionState) {        
     164             :         
     165         223 :             if (actionState->first.as<std::string>().compare("PrimitiveType") == 0) {
     166         223 :                 type = static_cast<ROSEE::ActionPrimitive::Type> ( actionState->second.as < int > () );
     167         223 :                 break; //we only need the type
     168             :             }
     169             :         }
     170             :         
     171             :         //now use emit of specific action
     172           0 :         ActionPrimitive::Ptr ptr;
     173         223 :         switch (type) {
     174          44 :         case ActionPrimitive::Type::PinchTight: {
     175          44 :             ptr = std::make_shared <ActionPinchTight>();
     176          44 :             break;
     177             :         }
     178          11 :         case ActionPrimitive::Type::PinchLoose: {
     179          11 :             ptr = std::make_shared <ActionPinchLoose> ();
     180          11 :             break;
     181             :         }
     182          56 :         case ActionPrimitive::Type::Trig: {
     183          56 :             ptr = std::make_shared <ActionTrig>("trig", ActionPrimitive::Type::Trig);
     184          56 :             break;
     185             :         }
     186          56 :         case ActionPrimitive::Type::TipFlex: {
     187          56 :             ptr = std::make_shared <ActionTrig>("tipFlex", ActionPrimitive::Type::TipFlex);
     188          56 :             break;
     189             :         }
     190          56 :         case ActionPrimitive::Type::FingFlex: {
     191          56 :             ptr = std::make_shared <ActionTrig>("fingFlex", ActionPrimitive::Type::FingFlex);
     192          56 :             break;
     193             :         }
     194           0 :         case ActionPrimitive::Type::SingleJointMultipleTips: {
     195           0 :             ptr = std::make_shared <ActionSingleJointMultipleTips>();
     196           0 :             break;
     197             :         }
     198           0 :         case ActionPrimitive::Type::MultiplePinchTight: {
     199           0 :             ptr = std::make_shared <ActionMultiplePinchTight> ();
     200           0 :             break;
     201             :         }
     202             : 
     203           0 :         default : {
     204           0 :             std::cout << "[ERROR YAMLPARSER]: " << type << " : type not found" << std::endl;
     205             :         }
     206             :         }
     207             :         
     208         223 :         ptr->fillFromYaml ( it4Action );
     209             :         
     210         223 :         parsedMap.insert ( std::make_pair ( ptr->getKeyElements(), ptr) );
     211             :     }
     212             :         
     213          96 :     return parsedMap;
     214             :     
     215             : }
     216             : 
     217             : 
     218           9 : ROSEE::ActionComposed ROSEE::YamlWorker::parseYamlComposed (std::string fileWithPath){
     219             :     
     220           9 :     ROSEE::ActionComposed parsedAction; 
     221             : 
     222             :     //TODO check elsewhere if file exist or not?
     223          18 :     std::ifstream ifile(fileWithPath);
     224           9 :     if (! ifile) {
     225           0 :         std::cout << "[ERROR YAMLPARSER:: " << __func__ << "]: file " << fileWithPath << " not found. "  << std::endl;
     226           0 :             return parsedAction;
     227             :     }
     228             :     
     229          18 :     YAML::Node node = YAML::LoadFile(fileWithPath);
     230          18 :     YAML::const_iterator yamlIt = node.begin();
     231             : 
     232           9 :     parsedAction.fillFromYaml ( yamlIt );
     233             :     
     234           9 :     return parsedAction;
     235             :     
     236             : }
     237             : 
     238           0 : ROSEE::ActionGeneric::Ptr ROSEE::YamlWorker::parseYamlGeneric(std::string fileWithPath) {
     239             :     
     240           0 :     ActionGeneric::Ptr ptrAction;
     241             :     
     242             :     //TODO check elsewhere if file exist or not?
     243           0 :     std::ifstream ifile(fileWithPath);
     244           0 :     if (! ifile) {
     245           0 :         std::cout << "[ERROR YAMLPARSER:: " << __func__ << "]: file " << fileWithPath << " not found. "  << std::endl;
     246           0 :         return ptrAction;
     247             :     }
     248             :     
     249           0 :     YAML::Node node = YAML::LoadFile(fileWithPath);
     250             :     
     251           0 :     YAML::const_iterator it4Action = node.begin(); //for not primitive, only one action is inside the file
     252             :         
     253           0 :     ROSEE::Action::Type type = ROSEE::Action::Type::None;
     254             :     
     255             :     //now look for the type of the action
     256           0 :     for ( YAML::const_iterator el = it4Action->second.begin(); el != it4Action->second.end(); ++el) {        
     257             :     
     258           0 :         if (el->first.as<std::string>().compare("Type") == 0) {
     259           0 :             type = static_cast<ROSEE::Action::Type> ( el->second.as < int > () );
     260           0 :             break; //we only need the type
     261             :         }
     262             :     }
     263             :     
     264           0 :     switch (type) {
     265           0 :     case Action::Type::Primitive : {
     266             :         std::cout << "[ERROR YAMLPARSER:: " << __func__ << "]: file " << fileWithPath << " contains a primitive action, "
     267           0 :         << " please use parseYamlPrimitive to parse it " << std::endl;
     268           0 :         return ptrAction;
     269             :         break;
     270             :     }
     271           0 :     case Action::Type::Generic : {
     272           0 :         ptrAction = std::make_shared <ActionGeneric> ();
     273           0 :         break;
     274             :     }
     275           0 :     case Action::Type::Composed : {
     276           0 :         ptrAction = std::make_shared <ActionComposed> ();
     277           0 :         break;
     278             :     }
     279           0 :     case Action::Type::Timed : {
     280             :         std::cout << "[ERROR YAMLPARSER:: " << __func__ << "]: file " << fileWithPath << " contains a timed action, "
     281           0 :         << " please use parseYamlTimed to parse it " << std::endl;
     282           0 :         return ptrAction;
     283             :         break;
     284             :     }
     285           0 :     default : {
     286           0 :         std::cout << "[ERROR YAMLPARSER:: " << __func__ << "]: file " << fileWithPath << " contains an action of not know type "
     287           0 :         << type << std::endl;
     288           0 :         return ptrAction; 
     289             :     }
     290             :     }
     291             : 
     292           0 :     ptrAction->fillFromYaml(it4Action);
     293             :     
     294           0 :     return ptrAction;
     295             :     
     296             :     
     297             : }
     298             : 
     299             : 
     300           8 : std::shared_ptr < ROSEE::ActionTimed > ROSEE::YamlWorker::parseYamlTimed (std::string fileWithPath){
     301             :     
     302           8 :     std::shared_ptr < ROSEE::ActionTimed > ptrAction; 
     303             : 
     304             :     //TODO check elsewhere if file exist or not?
     305          16 :     std::ifstream ifile(fileWithPath);
     306           8 :     if (! ifile) {
     307           0 :         std::cout << "[ERROR YAMLPARSER:: " << __func__ << "]: file " << fileWithPath << " not found. "  << std::endl;
     308           0 :             return nullptr;
     309             :     }
     310             :     
     311          16 :     YAML::Node node = YAML::LoadFile(fileWithPath);
     312          16 :     YAML::const_iterator yamlIt = node.begin();
     313           8 :     ptrAction = std::make_shared <ActionTimed> ();
     314             : 
     315           8 :     ptrAction->fillFromYaml ( yamlIt );
     316             :     
     317           8 :     return ptrAction;
     318             :     
     319             : }