RosMsgBFVerifierTest.cpp

Go to the documentation of this file.

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

class RosMsgBFVerifierTest : 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);

        ros::Time::init();

        eefState.name.push_back("link2");
        eefState.name.push_back("link4");
        eefState.locked.push_back(1);
        eefState.loaded.push_back(1);
        eefState.locked.push_back(1);
        eefState.loaded.push_back(1);
    }

    virtual void TearDown()
    {
    }

    RosMsgBFVerifier* rmv;
    r2_msgs::EndEffectorState eefState;
};

TEST_F(RosMsgBFVerifierTest, InitializeMapTest)
{
    // Initialize maps with test tree
    rmv->InitializeMap();

    ASSERT_FALSE( rmv->endEffectorMap.empty());
    EXPECT_EQ (0, rmv->endEffectorMap.count("joint1"));
    EXPECT_EQ (0, rmv->endEffectorMap.count("link1"));
    EXPECT_EQ (1, rmv->endEffectorMap.count("link2"));
    EXPECT_EQ (0, rmv->endEffectorMap["link2"].locked);
    EXPECT_EQ (0, rmv->endEffectorMap["link2"].loaded);
    EXPECT_EQ (0, rmv->endEffectorMap["link2"].verified);
    EXPECT_EQ (0, rmv->endEffectorMap.count("link3"));
    EXPECT_EQ (1, rmv->endEffectorMap.count("link4"));
}

TEST_F(RosMsgBFVerifierTest, UpdateEEFStateTest)
{
    rmv->InitializeMap();

    rmv->UpdateEEFState(eefState);

    EXPECT_EQ(1, rmv->endEffectorMap["link2"].locked);
    EXPECT_EQ(1, rmv->endEffectorMap["link4"].loaded);

    rmv->SetBaseAsVerif("link4");
    eefState.locked[1] = 0;
    rmv->UpdateEEFState(eefState);
    EXPECT_EQ(0, rmv->endEffectorMap["link4"].locked);
    EXPECT_EQ(0, rmv->endEffectorMap["link4"].verified);

}

TEST_F(RosMsgBFVerifierTest, SetBaseAsVerifTest)
{
    std::vector<std::string> bases;

    rmv->InitializeMap();
    rmv->UpdateEEFState(eefState);
    rmv->SetBaseAsVerif("link2");

    EXPECT_EQ(1, rmv->endEffectorMap["link2"].verified);
    EXPECT_EQ(0, rmv->endEffectorMap["link4"].verified);

    eefState.locked[1] = 0;
    rmv->UpdateEEFState(eefState);
    rmv->SetBaseAsVerif("link4");

    EXPECT_EQ(1, rmv->endEffectorMap["link2"].verified);
    EXPECT_EQ(0, rmv->endEffectorMap["link4"].verified);

    eefState.loaded[0] = 0;
    rmv->UpdateEEFState(eefState);

    EXPECT_EQ(0, rmv->endEffectorMap["link2"].verified);
    EXPECT_EQ(0, rmv->endEffectorMap["link4"].verified);

    eefState.locked[1] = 1;
    eefState.loaded[0] = 1;
    rmv->UpdateEEFState(eefState);
    bases.push_back("link2");
    bases.push_back("link4");
    rmv->SetBaseAsVerif(bases);

    EXPECT_EQ(1, rmv->endEffectorMap["link2"].verified);
    EXPECT_EQ(1, rmv->endEffectorMap["link4"].verified);
}

TEST_F(RosMsgBFVerifierTest, isLegalToVerifTest)
{
    std::string          name = "link2";
    r2_msgs::StringArray baseFrameMsg;

    rmv->InitializeMap();
    rmv->UpdateEEFState(eefState);

    EXPECT_EQ(0, rmv->isLegalToVerif(name, baseFrameMsg));

    name = "link3";
    rmv->SetBaseAsVerif("link2");
    baseFrameMsg.data.push_back("link2");
    EXPECT_EQ(0, rmv->isLegalToVerif(name, baseFrameMsg));

    name = "link4";
    EXPECT_EQ(1, rmv->isLegalToVerif(name, baseFrameMsg));

    name = "link2";
    EXPECT_EQ(0, rmv->isLegalToVerif(name, baseFrameMsg));

}

TEST_F(RosMsgBFVerifierTest, PopulateBaseVerifMessageTest)
{
    r2_msgs::EndEffectorState bfVerif;

    rmv->InitializeMap();
    rmv->UpdateEEFState(eefState);

    rmv->PopulateBaseVerifMessage(bfVerif);
    ASSERT_EQ("link2", bfVerif.name[0]);
    EXPECT_EQ(0, bfVerif.verified[0]);

    rmv->SetBaseAsVerif("link2");
    rmv->PopulateBaseVerifMessage(bfVerif);
    ASSERT_EQ("link2", bfVerif.name[0]);
    EXPECT_EQ(1, bfVerif.verified[0]);
    ASSERT_EQ("link4", bfVerif.name[1]);
    EXPECT_EQ(0, bfVerif.verified[1]);
}

TEST_F(RosMsgBFVerifierTest, PopulateStatusMessageTest)
{
    std::string          modeName, qualifier;
    r2_msgs::StringArray statusMsg;

    modeName  = "idle";
    qualifier = "poop";

    rmv->PopulateStatusMessage(modeName, qualifier, statusMsg);
    ASSERT_EQ(2, statusMsg.data.size());
    EXPECT_EQ("idle", statusMsg.data[0]);
    EXPECT_EQ("poop", statusMsg.data[1]);
}

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