Line data    Source code

       1             : #include <gtest/gtest.h>
       2             : #include "testUtils.h"
       3             : 
       4             : #include <rclcpp/rclcpp.hpp>
       5             : 
       6             : #include <end_effector/FindActions.h>
       7             : #include <end_effector/ParserMoveIt.h>
       8             : #include <end_effector/GraspingActions/ActionPrimitive.h>
       9             : #include <end_effector/GraspingActions/ActionTrig.h>
      10             : #include <end_effector/GraspingActions/ActionComposed.h>
      11             : #include <end_effector/GraspingActions/ActionPinchGeneric.h>
      12             : 
      13             : namespace {
      14             : 
      15             : class testTimedAction: public ::testing::Test {
      16             : 
      17             : 
      18             : protected:
      19             : 
      20           8 :     testTimedAction() : timedAction("TestTimedAction") {
      21           8 :     }
      22             : 
      23           8 :     virtual ~testTimedAction() {
      24           8 :     }
      25             :     
      26           8 :     virtual void SetUp() override {
      27             :         
      28           8 :     }
      29             : 
      30           8 :     virtual void SetUp(int argc, char **argv) {
      31             :     
      32           8 :         node = ROSEE::TestUtils::prepareROSForTests ( argc, argv, "testTimedAction");
      33             :         
      34           8 :         ASSERT_NE(node, nullptr);
      35             :         
      36           8 :         std::shared_ptr <ROSEE::ParserMoveIt> parserMoveIt = std::make_shared <ROSEE::ParserMoveIt> (node);
      37             : 
      38             :         //if return false, models are not found and it is useless to continue the test
      39           8 :         ASSERT_TRUE(parserMoveIt->init ("robot_description", false)) ;
      40             : 
      41          16 :         ROSEE::FindActions actionsFinder (parserMoveIt);
      42             :                 
      43          16 :         std::string folderForActions = "ROSEE/actions/" + parserMoveIt->getHandName();
      44             :         
      45           8 :         trigMap = actionsFinder.findTrig(ROSEE::ActionPrimitive::Type::Trig, folderForActions + "/primitives/") ;  
      46          16 :         auto pinches = actionsFinder.findPinch(folderForActions + "/primitives/") ; 
      47             :         
      48           8 :         pinchTightMap = pinches.first;
      49           8 :         pinchLooseMap = pinches.second;
      50             :                 
      51           8 :         std::shared_ptr<ROSEE::ActionComposed> actionComposed = std::make_shared<ROSEE::ActionComposed> ("degrasp");
      52             : 
      53           8 :         if (trigMap.size() > 0){
      54          22 :             for (auto trig : trigMap) {
      55             :                 std::shared_ptr <ROSEE::ActionPrimitive> pointer = 
      56          16 :                     std::make_shared <ROSEE::ActionTrig> ( trig.second );
      57             :                     
      58          16 :                 actionComposed->sumAction(pointer, 0);
      59             :                 
      60             :                 //different name for each trig inserted
      61          16 :                 timedAction.insertAction(pointer, 0.54, 0.2, 0, 0.8, "trig_" + *(pointer->getKeyElements().begin()) );  
      62             :             }
      63             :         }
      64             :         
      65             :         //lets add a pinch
      66           8 :         if (pinchTightMap.size()>0) {
      67             :             
      68             :             std::shared_ptr <ROSEE::ActionPrimitive> pointer = 
      69           4 :                     std::make_shared <ROSEE::ActionPinchTight> ( pinchTightMap.begin()->second );
      70             :                     
      71           4 :             timedAction.insertAction(pointer, 0, 0.45);
      72             :             
      73           4 :         } else if (pinchLooseMap.size()>0) {
      74             :             
      75             :             std::shared_ptr <ROSEE::ActionPrimitive> pointer = 
      76           2 :                     std::make_shared <ROSEE::ActionPinchLoose> ( pinchLooseMap.begin()->second );
      77             :                     
      78           2 :             timedAction.insertAction(pointer, 0, 0.45);
      79             :         }
      80             :         
      81             :         //now lets add a reset grasp pos (trig all zero)
      82           8 :         if (actionComposed->numberOfInnerActions( ) > 0) {
      83           6 :             std::shared_ptr <ROSEE::Action> pointer =  actionComposed ;
      84           6 :             timedAction.insertAction(pointer);
      85             :         }
      86             :         
      87           8 :         ROSEE::YamlWorker yamlWorker;
      88           8 :         yamlWorker.createYamlFile (&timedAction, folderForActions + "/timeds/");
      89             :             
      90             :         //Parsing
      91           8 :         timedActionParsed = yamlWorker.parseYamlTimed (folderForActions + "/timeds/TestTimedAction.yaml");
      92             :         
      93             :     }
      94             : 
      95           8 :     virtual void TearDown() override{
      96           8 :     }
      97             :     
      98             :     rclcpp::Node::SharedPtr node;
      99             :     
     100             :     ROSEE::ActionTrig::Map trigMap;
     101             :     ROSEE::ActionPinchTight::Map pinchTightMap;
     102             :     ROSEE::ActionPinchLoose::Map pinchLooseMap;
     103             :     
     104             :     ROSEE::ActionTimed timedAction;
     105             :     std::shared_ptr<ROSEE::ActionTimed> timedActionParsed;
     106             : 
     107             :    
     108             : };
     109             : 
     110           8 : TEST_F ( testTimedAction, checkMembersSizeConsistency ) {
     111             :     
     112           4 :     SetUp(argc_g, argv_g);
     113             :     
     114           4 :     EXPECT_EQ ( timedAction.getInnerActionsNames().size(), timedAction.getAllActionMargins().size() );
     115           4 :     EXPECT_EQ ( timedAction.getAllActionMargins().size(), timedAction.getAllJointCountAction().size() );
     116           4 :     EXPECT_EQ ( timedAction.getAllJointCountAction().size(), timedAction.getAllJointPos().size() );
     117             :     
     118           4 :     EXPECT_EQ ( timedActionParsed->getInnerActionsNames().size(), timedActionParsed->getAllActionMargins().size() );
     119           4 :     EXPECT_EQ ( timedActionParsed->getAllActionMargins().size(), timedActionParsed->getAllJointCountAction().size() );
     120           4 :     EXPECT_EQ ( timedActionParsed->getAllJointCountAction().size(), timedActionParsed->getAllJointPos().size() );   
     121             :     
     122           4 : }
     123             : 
     124           8 : TEST_F ( testTimedAction, checkEmitParse ) {
     125             :     
     126           4 :     SetUp(argc_g, argv_g);
     127             :     
     128           4 :     if (timedAction.getInnerActionsNames().size() > 0) {
     129             :         
     130           8 :         EXPECT_EQ (timedAction.getName(), timedActionParsed->getName() );
     131           4 :         EXPECT_EQ (timedAction.getType(), timedActionParsed->getType() );
     132           8 :         EXPECT_EQ (timedAction.getFingersInvolved(), timedActionParsed->getFingersInvolved() );
     133             :         
     134          24 :         for (auto jointCount : timedAction.getJointsInvolvedCount()) {
     135          40 :             EXPECT_EQ ( jointCount.second, timedActionParsed->getJointsInvolvedCount().at(jointCount.first) ); 
     136             :         }
     137             :         
     138          24 :         for (auto joint: timedAction.getJointPos() ) {
     139             :             
     140             :             //compare size of joint (number of dofs)
     141          20 :             ASSERT_EQ (joint.second.size(), timedActionParsed->getJointPos().at(joint.first).size() );
     142             :             //loop the eventually multiple joint pos (when dofs > 1)
     143          40 :             for (int j = 0; j < joint.second.size(); ++j ){
     144          40 :                 EXPECT_DOUBLE_EQ ( joint.second.at(j), timedActionParsed->getJointPos().at(joint.first).at(j) ); 
     145             :             }     
     146             :         }
     147             :         
     148           4 :         unsigned int k = 0;
     149          18 :         for (auto innerActName : timedAction.getInnerActionsNames()) {
     150          28 :             EXPECT_EQ ( innerActName, timedActionParsed->getInnerActionsNames().at(k) );
     151          14 :             k++;
     152             :         }
     153             :         
     154           4 :         k = 0;
     155          18 :         for (auto timeMargins : timedAction.getAllActionMargins() ) {
     156             :             
     157          28 :             EXPECT_DOUBLE_EQ (timeMargins.first, timedActionParsed->getAllActionMargins().at(k).first);
     158          28 :             EXPECT_DOUBLE_EQ (timeMargins.second, timedActionParsed->getAllActionMargins().at(k).second);
     159          14 :             k++;
     160             :         }
     161             : 
     162           4 :         auto jpvector = timedAction.getAllJointPos();
     163          18 :         for (int i=0; i < jpvector.size(); i++) {
     164             :             
     165         100 :             for (auto joint: jpvector.at(i) ) {
     166             :                 
     167          86 :                 auto otherjointPos = timedActionParsed->getAllJointPos().at(i).at(joint.first);
     168             :                 //compare size of joint (number of dofs)
     169          86 :                 ASSERT_EQ (joint.second.size(), otherjointPos.size() );
     170             :                 
     171             :                 //loop the eventually multiple joint pos (when dofs > 1)
     172         172 :                 for (int j = 0; j < joint.second.size(); ++j ){
     173          86 :                     EXPECT_DOUBLE_EQ ( joint.second.at(j), otherjointPos.at(j) ); 
     174             :                 } 
     175             :             }
     176             :         }
     177             :         
     178           8 :         auto jpcvector = timedAction.getAllJointCountAction();
     179          18 :         for (int i=0; i < jpcvector.size(); i++) {
     180             :             
     181         100 :             for (auto joint: jpcvector.at(i) ) {
     182             :                 
     183         172 :                 EXPECT_EQ ( joint.second, timedActionParsed->getAllJointCountAction().at(i).at(joint.first) ); 
     184             :                 
     185             :             }
     186             :         }
     187             :     }
     188             : }
     189             : 
     190             : } //namespace
     191             : 
     192           4 : int main ( int argc, char **argv ) {
     193             :     
     194           4 :     if (argc < 2 ){
     195             :         
     196           0 :         std::cout << "[TEST ERROR] Insert hand name as argument" << std::endl;
     197           0 :         return -1;
     198             :     }
     199             :     
     200           4 :     rclcpp::init ( argc, argv );
     201             :     
     202           4 :     ::testing::InitGoogleTest ( &argc, argv );
     203           4 :     ::testing::AddGlobalTestEnvironment(new MyTestEnvironment(argc, argv));
     204             : 
     205           4 :     return RUN_ALL_TESTS();
     206             : }