Line data    Source code

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