Line data    Source code

       1             : #include <gtest/gtest.h>
       2             : #include "testUtils.h"
       3             : 
       4             : #include <ros/ros.h>
       5             : 
       6             : #include <ros_end_effector/FindActions.h>
       7             : #include <ros_end_effector/ParserMoveIt.h>
       8             : #include <ros_end_effector/GraspingActions/ActionComposed.h>
       9             : #include <ros_end_effector/GraspingActions/ActionPrimitive.h>
      10             : #include <ros_end_effector/GraspingActions/ActionTrig.h>
      11             : 
      12             : namespace {
      13             : 
      14             : class testComposedAction: public ::testing::Test {
      15             : 
      16             : 
      17             : protected:
      18             : 
      19          12 :     testComposedAction() : grasp("grasp", true) {
      20          12 :     }
      21             : 
      22          12 :     virtual ~testComposedAction() {
      23          12 :     }
      24             : 
      25          12 :     virtual void SetUp() override {
      26             :         
      27          24 :         std::shared_ptr <ROSEE::ParserMoveIt> parserMoveIt = std::make_shared <ROSEE::ParserMoveIt> ();
      28             : 
      29             :         //if return false, models are not found and it is useless to continue the test
      30          12 :         ASSERT_TRUE(parserMoveIt->init ("robot_description", false)) ;
      31             : 
      32          24 :         ROSEE::FindActions actionsFinder (parserMoveIt);
      33             :         
      34          24 :         std::string folderForActions = ROSEE::Utils::getPackagePath() + "/configs/actions/tests/" + parserMoveIt->getHandName();
      35             :         
      36          12 :         trigMap = actionsFinder.findTrig(ROSEE::ActionPrimitive::Type::Trig, folderForActions + "/primitives/") ;  
      37             : 
      38          12 :         if (trigMap.size() > 0){
      39          33 :             for (auto trig : trigMap) {
      40             :                 std::shared_ptr <ROSEE::ActionPrimitive> pointer = 
      41          48 :                     std::make_shared <ROSEE::ActionTrig> ( trig.second );
      42          24 :                 grasp.sumAction ( pointer );  
      43             :             }
      44             : 
      45           9 :             ROSEE::YamlWorker yamlWorker;
      46           9 :             yamlWorker.createYamlFile (&grasp, folderForActions + "/generics/");
      47             :             
      48             :             //Parsing
      49           9 :             graspParsed = yamlWorker.parseYamlComposed (folderForActions + "/generics/grasp.yaml");
      50             :             
      51             :         } 
      52             :     }
      53             : 
      54          12 :     virtual void TearDown() {
      55          12 :     }
      56             :     
      57             :     std::map < std::string , ROSEE::ActionTrig > trigMap;
      58             :     ROSEE::ActionComposed grasp;
      59             :     ROSEE::ActionComposed graspParsed;
      60             : 
      61             :    
      62             : };
      63             : 
      64          20 : TEST_F ( testComposedAction, checkNumberPrimitives ) {
      65             :     
      66           4 :     EXPECT_EQ ( grasp.numberOfInnerActions(), grasp.getInnerActionsNames().size() );
      67             :     
      68           4 :     EXPECT_EQ ( graspParsed.numberOfInnerActions(), graspParsed.getInnerActionsNames().size() );    
      69             :     
      70           4 : }
      71             : 
      72          20 : TEST_F ( testComposedAction, checkEmitParse ) {
      73             :     
      74           4 :     if (trigMap.size() > 0) { //if empty, no grasp is defined in the setup so test without meaning
      75             :         
      76           3 :         EXPECT_EQ (grasp.getName(), graspParsed.getName() );
      77           3 :         EXPECT_EQ (grasp.getType(), graspParsed.getType() );
      78           3 :         EXPECT_EQ (grasp.isIndependent(), graspParsed.isIndependent() );
      79           3 :         EXPECT_EQ (grasp.numberOfInnerActions(), graspParsed.numberOfInnerActions() );
      80           3 :         EXPECT_EQ (grasp.getFingersInvolved(), graspParsed.getFingersInvolved() );
      81             :         
      82          22 :         for (auto joint: grasp.getJointPos() ) {
      83             :             
      84             :             //compare size of joint (number of dofs)
      85          19 :             ASSERT_EQ (joint.second.size(), graspParsed.getJointPos().at(joint.first).size() );
      86             :             //loop the eventually multiple joint pos (when dofs > 1)
      87          38 :             for (int j = 0; j < joint.second.size(); ++j ){
      88          19 :                 EXPECT_DOUBLE_EQ ( joint.second.at(j), graspParsed.getJointPos().at(joint.first).at(j) ); 
      89             :             }     
      90             :         }
      91             :         
      92          22 :         for (auto jointCount : grasp.getJointsInvolvedCount()) {
      93             : 
      94          19 :             EXPECT_EQ ( jointCount.second, graspParsed.getJointsInvolvedCount().at(jointCount.first) ); 
      95             :                
      96             :         }
      97             :     }
      98             : }
      99             : 
     100             : // if independent, at maximum only one primitive can influence each joint
     101          20 : TEST_F ( testComposedAction, checkIndependence ) { 
     102           4 :     if (grasp.isIndependent()) {
     103          23 :         for (auto it : grasp.getJointsInvolvedCount() ) {
     104          19 :             EXPECT_LE ( it.second, 1 );
     105             :         }
     106             :     }
     107           4 : }
     108             : 
     109             : } //namespace
     110             : 
     111           4 : int main ( int argc, char **argv ) {
     112             :     
     113           4 :     if (argc < 2 ){
     114             :         
     115           0 :         std::cout << "[TEST ERROR] Insert hand name as argument" << std::endl;
     116           0 :         return -1;
     117             :     }
     118             :     
     119             :     /* Run tests on an isolated roscore */
     120           4 :     if(setenv("ROS_MASTER_URI", "http://localhost:11322", 1) == -1)
     121             :     {
     122           0 :         perror("setenv");
     123           0 :         return 1;
     124             :     }
     125             :     
     126             :     //run roscore
     127           8 :     std::unique_ptr<ROSEE::TestUtils::Process> roscore;
     128           4 :     roscore.reset(new ROSEE::TestUtils::Process({"roscore", "-p", "11322"}));
     129             :     
     130           4 :     if ( ROSEE::TestUtils::prepareROSForTests ( argc, argv, "testComposedAction" ) != 0 ) {
     131             :         
     132           0 :         std::cout << "[TEST ERROR] Prepare Funcion failed" << std::endl;
     133           0 :         return -1;
     134             :     }
     135             :     
     136             :     
     137           4 :     ::testing::InitGoogleTest ( &argc, argv );
     138           4 :     return RUN_ALL_TESTS();
     139          12 : }