Line data    Source code

       1             : #include <gtest/gtest.h>
       2             : #include "testUtils.h"
       3             : 
       4             : #include <ros/ros.h>
       5             : #include <ros/console.h>
       6             : 
       7             : #include <sensor_msgs/JointState.h>
       8             : 
       9             : #include <ros_end_effector/Parser.h>
      10             : #include <ros_end_effector/EEInterface.h>
      11             : #include <ros_end_effector/Utils.h>
      12             : 
      13             : namespace {
      14             : 
      15             : class testEEInterface: public ::testing::Test {
      16             : 
      17             : 
      18             : protected:
      19             : 
      20          20 :     testEEInterface() {
      21          20 :     }
      22             : 
      23          20 :     virtual ~testEEInterface() {
      24          20 :     }
      25             : 
      26          20 :     virtual void SetUp() {
      27             : 
      28          40 :         ros::NodeHandle nh;
      29             : 
      30          40 :         ROSEE::Parser p ( nh );
      31          20 :         p.init ( ROSEE::Utils::getPackagePath() + "/configs/test_ee.yaml" );
      32          20 :         p.printEndEffectorFingerJointsMap();
      33             : 
      34          20 :         ee = std::make_shared<ROSEE::EEInterface>(p);
      35          20 :     }
      36             : 
      37          20 :     virtual void TearDown() {
      38          20 :     }
      39             : 
      40             :     ROSEE::EEInterface::Ptr ee;
      41             : };
      42             : 
      43             : 
      44          20 : TEST_F ( testEEInterface, checkFingers ) {
      45             : 
      46           8 :     std::vector<std::string> fingers;
      47           4 :     fingers = ee->getFingers();
      48             : 
      49           4 :     EXPECT_FALSE (fingers.empty());
      50             : 
      51           4 :     ROS_INFO_STREAM ( "Fingers in EEInterface: " );
      52          16 :     for ( auto& f : fingers ) {
      53          12 :         ROS_INFO_STREAM ( f );
      54             :     }
      55             : 
      56           4 :     EXPECT_TRUE ( ee->isFinger ( "finger_1" ) );
      57             : 
      58           4 :     EXPECT_FALSE ( ee->isFinger ( "finger_4" ) );
      59             : 
      60             : 
      61             : 
      62           4 : }
      63             : 
      64          20 : TEST_F ( testEEInterface, checkActuatedJointsNum ) {
      65             : 
      66           4 :     int joint_num = ee->getActuatedJointsNum();
      67           4 :     EXPECT_EQ ( 6, joint_num );
      68             : 
      69           4 :     EXPECT_FALSE ( joint_num < 0 );
      70             : 
      71           4 : }
      72             : 
      73          20 : TEST_F ( testEEInterface, checkEEFingerJoints ) {
      74             : 
      75           4 :     int joint_num_finger1 = ee->getActuatedJointsNumInFinger("finger_1");
      76             : 
      77           4 :     int joint_num = ee->getActuatedJointsNum();
      78             : 
      79           4 :     EXPECT_TRUE ( joint_num >= joint_num_finger1 );
      80             : 
      81           4 :     int joint_num_counter = 0;
      82           8 :     std::vector<std::string> fingers = ee->getFingers();
      83             : 
      84             : 
      85             : 
      86          16 :     for ( auto& f : fingers ) {
      87          12 :         joint_num_counter += ee->getActuatedJointsNumInFinger(f);
      88             :     }
      89             : 
      90           4 :     EXPECT_TRUE ( joint_num == joint_num_counter );
      91             : 
      92           4 : }
      93             : 
      94          20 : TEST_F ( testEEInterface, checkJointLimits) {
      95             : 
      96           8 :     Eigen::VectorXd upperLimits = ee->getUpperPositionLimits();
      97           8 :     Eigen::VectorXd lowerLimits = ee->getLowerPositionLimits();
      98             :     
      99           4 :     ASSERT_EQ (upperLimits.size(), lowerLimits.size()); //stop if it fails here
     100             :     
     101           4 :     EXPECT_TRUE (upperLimits.size() > 0);
     102             :     
     103          28 :     for (int i=0; i<upperLimits.size(); i++) {
     104             :     
     105          24 :         EXPECT_GE (upperLimits(i), lowerLimits(i)); //greater or equal than
     106          24 :         ROS_INFO_STREAM ( "Joint " << std::to_string(i) << " limits:  " <<
     107             :                           upperLimits(i) <<  ", " << lowerLimits(i) );
     108             : 
     109             :     }
     110             :     
     111             : }
     112             : 
     113          20 : TEST_F ( testEEInterface, checkIdJoints ) {
     114             : 
     115           8 :     std::vector<std::string> actJoints = ee->getActuatedJoints();
     116             :    // ASSERT_FALSE (actJoints.empty());  //a hand can have no actuated joints?
     117             :     
     118             :     // check if ids are unique
     119           4 :     int id = -1;
     120           4 :     int idPrevious = -1;
     121          28 :     for ( auto& j : actJoints ) {
     122          24 :         EXPECT_TRUE ( ee->getInternalIdForJoint (j, id) ); //return false if joint does not exist
     123          24 :         EXPECT_NE ( id, idPrevious );
     124          24 :         idPrevious = id;
     125             :     }
     126             :     
     127           4 : }
     128             : 
     129             : } //namespace
     130             : 
     131           4 : int main ( int argc, char **argv ) {
     132             : 
     133           4 :     if (argc < 2 ) {
     134             : 
     135           0 :         std::cout << "[TEST ERROR] Insert hand name as argument" << std::endl;
     136           0 :         return -1;
     137             :     }
     138             : 
     139             :     /* Run tests on an isolated roscore */
     140           4 :     if(setenv("ROS_MASTER_URI", "http://localhost:11322", 1) == -1)
     141             :     {
     142           0 :         perror("setenv");
     143           0 :         return 1;
     144             :     }
     145             : 
     146             :     //run roscore
     147           8 :     std::unique_ptr<ROSEE::TestUtils::Process> roscore;
     148           4 :     roscore.reset(new ROSEE::TestUtils::Process({"roscore", "-p", "11322"}));
     149             : 
     150           4 :     if ( ROSEE::TestUtils::prepareROSForTests ( argc, argv, "testEEInterface" ) != 0 ) {
     151             : 
     152           0 :         std::cout << "[TEST ERROR] Prepare Funcion failed" << std::endl;
     153           0 :         return -1;
     154             :     }
     155             : 
     156           4 :     ::testing::InitGoogleTest ( &argc, argv );
     157           4 :     return RUN_ALL_TESTS();
     158          12 : }