KdlTreeParserTest.cpp

Go to the documentation of this file.

#include <gtest/gtest.h>
#include "robodyn_controllers/KdlTreeParser.h"

#include <ros/package.h>
#include <ros/init.h>

class KdlTreeParserTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
    }

    virtual void TearDown()
    {
    }

    class MyParser : public KdlTreeParser
    {
    public:
        MyParser() : KdlTreeParser(), initialized(false) {}
        void initialize()
        {
            initialized = true;
        }
        bool initialized;
    };

    MyParser parser;
    KDL::Tree tree;
};

TEST_F(KdlTreeParserTest, parserTest)
{
    // nothing yet
    EXPECT_FALSE(parser.initialized);
    tree = parser.getTree();
    EXPECT_EQ(0, tree.getNrOfSegments());

    // load
    std::string packagePath = ros::package::getPath("robodyn_controllers");
    parser.loadFromFile(packagePath + "/test/urdf/BranchingTestRobot.xml");

    // check initialize was called
    EXPECT_TRUE(parser.initialized);
    tree = parser.getTree();

    // make sure it was loaded properly though not an exhaustive check since it is a 3rd party library
    EXPECT_EQ(5, tree.getNrOfSegments());
    EXPECT_EQ(4, tree.getNrOfJoints());

    // verify init calle don setTree
    parser.initialized = false;
    parser.setTree(tree);
    EXPECT_TRUE(parser.initialized);

    // check throw on bad urdf
    EXPECT_ANY_THROW(parser.loadFromParam("bogusParam"));
}

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