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