GripperSetpointFactory_Test.cpp

Go to the documentation of this file.

#include <iostream>
#include <gtest/gtest.h>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/lexical_cast.hpp>
#include "robot_instance/RobotInstanceFactory.h"
#include "robot_instance/GripperSetpointFactory.h"
#include <ros/package.h>

using namespace std;

class GripperSetpointFactoryTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        packagePath      = ros::package::getPath("robot_instance");
        configPath       = packagePath + "/test/config";
        configSafetyPath = packagePath + "/test/configSafety";
        fileName         = "TestRobotInstance.xml";
        instance         = RobotInstanceFactory::createRobotInstanceFromFile(configPath, configSafetyPath, fileName);

        gripperSetpointMap = boost::make_shared<GripperSetpointMap>();
    }

    virtual void TearDown()
    {
    }

    string packagePath, configPath, configSafetyPath, fileName;
    RobotInstancePtr instance;
    GripperSetpointMapPtr gripperSetpointMap;
};

TEST_F(GripperSetpointFactoryTest, FromFile)
{
    string setpointFile = instance->configurationSafetyBasePath + instance->COMMAND_PATH + instance->mechanismSetpointMap["r2/right_leg/gripper/joint0"];

    GripperSetpointFactory::fromFile(setpointFile, gripperSetpointMap);

    EXPECT_EQ(3, gripperSetpointMap->size());

    GripperSetpoint setpoint = gripperSetpointMap->operator[]("handrail");
    EXPECT_FLOAT_EQ(0.50, setpoint.jawPosition);
    EXPECT_FLOAT_EQ(0.03, setpoint.jawPositionDelta);
    EXPECT_FLOAT_EQ(111,  setpoint.expectedLoad);
    EXPECT_FLOAT_EQ(10,   setpoint.expectedLoadDelta);

    setpoint = gripperSetpointMap->operator[]("seattrack");
    EXPECT_FLOAT_EQ(0.22, setpoint.jawPosition);
    EXPECT_FLOAT_EQ(0.03, setpoint.jawPositionDelta);
    EXPECT_FLOAT_EQ(111,  setpoint.expectedLoad);
    EXPECT_FLOAT_EQ(10,   setpoint.expectedLoadDelta);

    setpoint = gripperSetpointMap->operator[]("closed");
    EXPECT_FLOAT_EQ(0.0,  setpoint.jawPosition);
    EXPECT_FLOAT_EQ(0.03, setpoint.jawPositionDelta);
    EXPECT_FLOAT_EQ(289,  setpoint.expectedLoad);
    EXPECT_FLOAT_EQ(10,   setpoint.expectedLoadDelta);
}

int main(int argc, char** argv)
{
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}