BaseFrameVerifierTest.cpp

Go to the documentation of this file.

#include <gtest/gtest.h>
#include "nasa_controllers_core/BaseFrameVerifier.h"
#include "nasa_controllers_core/RosMsgBFVerifier.h"
#include <ros/package.h>

class BaseFrameVerifierTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        std::string packagePath = ros::package::getPath("nasa_controllers_core");
        std::string urdfFile    = packagePath + "/test/urdf/SimpleTestRobot.xml";
        rmv                     = new RosMsgBFVerifier(urdfFile);
        tree                    = rmv->getTree();

        refFrame = "link1";
        dist     = 0.03;
        err      = 0.01;

        bfv = new BaseFrameVerifier(refFrame, dist, err);

        ros::Time::init();

        // set up PoseState message
        eefPoseState.name.push_back("link1");
        eefPoseState.name.push_back("link2");
        eefPoseState.name.push_back("link3");
        eefPoseState.name.push_back("link4");

        tf::quaternionTFToMsg(tf::createQuaternionFromRPY(0.0, 0.0, 0.0), pos.orientation);
        pos.position.x = 0.0;
        pos.position.y = 0.0;
        pos.position.z = 0.0;

        eefPoseState.positions.push_back(pos);

        pos.position.x = 5.0;
        pos.position.y = 3.0;
        eefPoseState.positions.push_back(pos);

        pos.position.x = -2.0;
        pos.position.y = 5.0;
        tf::quaternionTFToMsg(tf::createQuaternionFromRPY(0.0, 1.57, 0.0), pos.orientation);
        eefPoseState.positions.push_back(pos);

        pos.position.x = 0.0;
        pos.position.y = 5.0;
        tf::quaternionTFToMsg(tf::createQuaternionFromRPY(0.0, -1.57, 0.0), pos.orientation);
        eefPoseState.positions.push_back(pos);

    }

    virtual void TearDown()
    {
    }

    BaseFrameVerifier*      bfv;
    RosMsgBFVerifier*       rmv;
    std::string             refFrame;
    double                  dist, err;
    KDL::Tree               tree;
    r2_msgs::PoseState      eefPoseState;
    geometry_msgs::Pose     pos;
    r2_msgs::StringArray    baseFrames;
    r2_msgs::PoseTrajectory trajMsg;
};

TEST_F(BaseFrameVerifierTest, InitializeMapTest)
{
    // Initialize maps with test tree
    bfv->InitializeMap(tree);
    KDL::Frame zeroData;

    ASSERT_FALSE(        bfv->eefFrameMap.empty());
    EXPECT_EQ (0,        bfv->eefFrameMap.count("joint1"));
    EXPECT_EQ (0,        bfv->eefFrameMap.count("link1"));
    EXPECT_EQ (1,        bfv->eefFrameMap.count("link2"));
    EXPECT_EQ (zeroData, bfv->eefFrameMap["link2"]);
    EXPECT_EQ (0,        bfv->eefFrameMap.count("link3"));
    EXPECT_EQ (1,        bfv->eefFrameMap.count("link4"));
}

TEST_F(BaseFrameVerifierTest, UpdateCurrentEEFPositionsTest)
{
    bfv->InitializeMap(tree);

    bfv->UpdateCurrentEEFPositions(eefPoseState);

    KDL::Frame frame;

    try
    {
        RosMsgConverter::PoseStateToFrame(eefPoseState, refFrame, "link2", frame);
    }
    catch (std::exception& e)
    {
        NasaCommonLogging::Logger::log("gov.nasa.controllers.BaseFrameVerifierTest", log4cpp::Priority::ERROR, e.what());
    }

    EXPECT_EQ(frame, bfv->eefFrameMap["link2"]);

}


TEST_F(BaseFrameVerifierTest, CreatePullCommandTest)
{
    bfv->InitializeMap(tree);
    bfv->UpdateCurrentEEFPositions(eefPoseState);

    ASSERT_EQ(-1, bfv->CreatePullCommand(baseFrames, "link2", trajMsg));

    baseFrames.data.push_back("link2");
    ASSERT_EQ(0,                       bfv->CreatePullCommand(baseFrames, "link4", trajMsg));
    EXPECT_EQ(2,                       trajMsg.nodes.size());
    EXPECT_EQ("link2",                 trajMsg.nodes[0]);
    EXPECT_EQ("link4",                 trajMsg.nodes[1]);
    EXPECT_EQ(refFrame,                trajMsg.refFrames[0]);
    EXPECT_EQ("link4",                 trajMsg.refFrames[1]);
    EXPECT_EQ(5.0,                     trajMsg.points[0].positions[0].position.x);
    EXPECT_EQ(ros::Duration(100 * dist), trajMsg.points[0].time_from_start);
    EXPECT_EQ(0.0,                     trajMsg.points[1].positions[0].position.x);
    EXPECT_EQ(-dist,                   trajMsg.points[1].positions[0].position.z);

}

TEST_F(BaseFrameVerifierTest, CheckPositionAgainstOutcomesTest)
{
    bfv->InitializeMap(tree);
    bfv->UpdateCurrentEEFPositions(eefPoseState);
    baseFrames.data.push_back("link2");
    ASSERT_EQ(0, bfv->CreatePullCommand(baseFrames, "link4", trajMsg));

    EXPECT_EQ(0, bfv->CheckPositionAgainstOutcomes("link3"));
    EXPECT_EQ(1, bfv->CheckPositionAgainstOutcomes("link4"));

}

TEST_F(BaseFrameVerifierTest, ClearCurrentTrajectoryTest)
{
    bfv->InitializeMap(tree);
    bfv->UpdateCurrentEEFPositions(eefPoseState);
    bfv->ClearCurrentTrajectory(trajMsg);

    EXPECT_EQ(2, trajMsg.nodes.size());
    EXPECT_EQ(refFrame, trajMsg.refFrames[0]);
    EXPECT_EQ(ros::Duration(1.0), trajMsg.points[0].time_from_start);

}

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