test_ee_interface.cpp

Go to the documentation of this file.

#include <gtest/gtest.h>
#include "testUtils.h"

#include <ros/ros.h>
#include <ros/console.h>

#include <sensor_msgs/JointState.h>

#include <end_effector/Parser.h>
#include <end_effector/EEInterface.h>
#include <end_effector/Utils.h>

namespace {

class testEEInterface: public ::testing::Test {


protected:

    testEEInterface() {
    }

    virtual ~testEEInterface() {
    }

    virtual void SetUp() {

        ros::NodeHandle nh;

        ROSEE::Parser p ( nh );
        p.init ( ROSEE::Utils::getPackagePath() + "/configs/urdf/test_ee.urdf",
                 ROSEE::Utils::getPackagePath() + "/configs/srdf/test_ee.srdf",
                 "ROSEE/actions/test_ee"
        );
        p.printEndEffectorFingerJointsMap();

        ee = std::make_shared<ROSEE::EEInterface>(p);
    }

    virtual void TearDown() {
    }

    ROSEE::EEInterface::Ptr ee;
};


TEST_F ( testEEInterface, checkFingers ) {

    std::vector<std::string> fingers;
    fingers = ee->getFingers();

    EXPECT_FALSE (fingers.empty());

    ROS_INFO_STREAM ( "Fingers in EEInterface: " );
    for ( auto& f : fingers ) {
        ROS_INFO_STREAM ( f );
    }

    EXPECT_TRUE ( ee->isFinger ( "finger_1" ) );

    EXPECT_FALSE ( ee->isFinger ( "finger_4" ) );



}

TEST_F ( testEEInterface, checkActuatedJointsNum ) {

    int joint_num = ee->getActuatedJointsNum();
    EXPECT_EQ ( 6, joint_num );

    EXPECT_FALSE ( joint_num < 0 );

}

TEST_F ( testEEInterface, checkEEFingerJoints ) {

    int joint_num_finger1 = ee->getActuatedJointsNumInFinger("finger_1");

    int joint_num = ee->getActuatedJointsNum();

    EXPECT_TRUE ( joint_num >= joint_num_finger1 );

    int joint_num_counter = 0;
    std::vector<std::string> fingers = ee->getFingers();



    for ( auto& f : fingers ) {
        joint_num_counter += ee->getActuatedJointsNumInFinger(f);
    }

    EXPECT_TRUE ( joint_num == joint_num_counter );

}

TEST_F ( testEEInterface, checkJointLimits) {

    Eigen::VectorXd upperLimits = ee->getUpperPositionLimits();
    Eigen::VectorXd lowerLimits = ee->getLowerPositionLimits();
    
    ASSERT_EQ (upperLimits.size(), lowerLimits.size()); //stop if it fails here
    
    EXPECT_TRUE (upperLimits.size() > 0);
    
    for (int i=0; i<upperLimits.size(); i++) {
    
        EXPECT_GE (upperLimits(i), lowerLimits(i)); //greater or equal than
        ROS_INFO_STREAM ( "Joint " << std::to_string(i) << " limits:  " <<
                          upperLimits(i) <<  ", " << lowerLimits(i) );

    }
    
}

TEST_F ( testEEInterface, checkIdJoints ) {

    std::vector<std::string> actJoints = ee->getActuatedJoints();
   // ASSERT_FALSE (actJoints.empty());  //a hand can have no actuated joints?
    
    // check if ids are unique
    int id = -1;
    int idPrevious = -1;
    for ( auto& j : actJoints ) {
        EXPECT_TRUE ( ee->getInternalIdForJoint (j, id) ); //return false if joint does not exist
        EXPECT_NE ( id, idPrevious );
        idPrevious = id;
    }
    
}

} //namespace

int main ( int argc, char **argv ) {

    if (argc < 2 ) {

        std::cout << "[TEST ERROR] Insert hand name as argument" << std::endl;
        return -1;
    }

    /* Run tests on an isolated roscore */
    if(setenv("ROS_MASTER_URI", "http://localhost:11322", 1) == -1)
    {
        perror("setenv");
        return 1;
    }

    //run roscore
    std::unique_ptr<ROSEE::TestUtils::Process> roscore;
    roscore.reset(new ROSEE::TestUtils::Process({"roscore", "-p", "11322"}));

    if ( ROSEE::TestUtils::prepareROSForTests ( argc, argv, "testEEInterface" ) != 0 ) {

        std::cout << "[TEST ERROR] Prepare Funcion failed" << std::endl;
        return -1;
    }

    ::testing::InitGoogleTest ( &argc, argv );
    return RUN_ALL_TESTS();
}