RosMsgTreeFkTest.cpp

Go to the documentation of this file.

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

#include <kdl/treefksolverpos_recursive.hpp>
#include <kdl/chainfksolvervel_recursive.hpp>
#include <ros/package.h>
#include <sensor_msgs/JointState.h>
#include <r2_msgs/PoseState.h>
#include <tf_conversions/tf_kdl.h>
#include <ros/init.h>

class RosMsgTreeFkTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        std::string packagePath = ros::package::getPath("robodyn_controllers");
        fk.loadFromFile(packagePath + "/test/urdf/BranchingTestRobot.xml");
        tree = fk.getTree();
        srand(time(NULL));
        ros::Time::init();
    }

    virtual void TearDown()
    {
    }

    RosMsgTreeFk                   fk;
    KDL::Tree                      tree;
    sensor_msgs::JointState        js;
    r2_msgs::PoseState ps;
};

TEST_F(RosMsgTreeFkTest, velTest)
{
    KDL::Chain chain;
    fk.getJointNames(js.name);
    js.position.resize(js.name.size(), 0.);
    js.velocity.resize(js.name.size(), 0.);
    js.effort.resize(js.name.size(), 0.);
    js.header.stamp = ros::Time::now();

    for (unsigned int i = 0; i < js.name.size(); ++i)
    {
        js.position[i] = ((double)(rand() % 314)) / 100. - 1.57;
        js.velocity[i] = ((double)(rand() % 200)) / 2000. - .1;
    }

    fk.getPoseState(js, ps);
    ASSERT_EQ(ps.name.size(), ps.positions.size());
    ASSERT_EQ(ps.name.size(), ps.velocities.size());
    ASSERT_EQ(ps.name.size(), ps.accelerations.size());

    KDL::FrameVel testFrame;
    std::map<std::string, KDL::Twist> vels;

    for (unsigned int i = 0; i < ps.name.size(); ++i)
    {
        // get chain
        fk.getChain(fk.getBaseName(), ps.name[i], chain);
        // make solver
        KDL::ChainFkSolverVel_recursive kdlSolver(chain);
        // get joints
        KDL::JntArrayVel chainJoints(chain.getNrOfJoints());
        std::vector<std::string> chainJointNames;
        fk.getChainJointNames(ps.name[i], chainJointNames);
        ASSERT_EQ(chainJoints.q.rows(), chainJointNames.size());

        for (unsigned int j = 0; j < chainJointNames.size(); ++j)
        {
            std::vector<std::string>::const_iterator nit = std::find(js.name.begin(), js.name.end(), chainJointNames[j]);

            if (nit != js.name.end())
            {
                chainJoints.q(j) = js.position[nit - js.name.begin()];
                chainJoints.qdot(j) = js.velocity[nit - js.name.begin()];
            }
        }

        kdlSolver.JntToCart(chainJoints, testFrame);
        KDL::Frame cmpFrame;
        tf::poseMsgToKDL(ps.positions[i], cmpFrame);
        EXPECT_EQ(testFrame.GetFrame(), cmpFrame);
        KDL::Twist cmpTwist;
        tf::twistMsgToKDL(ps.velocities[i], cmpTwist);
        EXPECT_EQ(testFrame.GetTwist(), cmpTwist);
        vels[ps.name[i]] = testFrame.GetTwist();
        tf::twistMsgToKDL(ps.accelerations[i], cmpTwist);
        EXPECT_EQ(KDL::Twist::Zero(), cmpTwist);
    }

    // do it again to check accelerations
    js.velocity[0]     += 0.05;
    ros::Time prevTime = js.header.stamp;
    js.header.stamp    = ros::Time::now();
    fk.getPoseState(js, ps);
    ASSERT_EQ(ps.name.size(), ps.positions.size());
    ASSERT_EQ(ps.name.size(), ps.velocities.size());
    ASSERT_EQ(ps.name.size(), ps.accelerations.size());

    ros::Duration elapsedTime = js.header.stamp - prevTime;

    for (unsigned int i = 0; i < ps.name.size(); ++i)
    {
        // get chain
        fk.getChain(fk.getBaseName(), ps.name[i], chain);
        // make solver
        KDL::ChainFkSolverVel_recursive kdlSolver(chain);
        // get joints
        KDL::JntArrayVel chainJoints(chain.getNrOfJoints());
        std::vector<std::string> chainJointNames;
        fk.getChainJointNames(ps.name[i], chainJointNames);
        ASSERT_EQ(chainJoints.q.rows(), chainJointNames.size());

        for (unsigned int j = 0; j < chainJointNames.size(); ++j)
        {
            std::vector<std::string>::const_iterator nit = std::find(js.name.begin(), js.name.end(), chainJointNames[j]);

            if (nit != js.name.end())
            {
                chainJoints.q(j)    = js.position[nit - js.name.begin()];
                chainJoints.qdot(j) = js.velocity[nit - js.name.begin()];
            }
        }

        kdlSolver.JntToCart(chainJoints, testFrame);
        KDL::Frame cmpFrame;
        tf::poseMsgToKDL(ps.positions[i], cmpFrame);
        EXPECT_EQ(testFrame.GetFrame(), cmpFrame);
        KDL::Twist cmpTwist;
        tf::twistMsgToKDL(ps.velocities[i], cmpTwist);
        EXPECT_EQ(testFrame.GetTwist(), cmpTwist);
        tf::twistMsgToKDL(ps.accelerations[i], cmpTwist);
        ASSERT_NE(0., elapsedTime.toSec());
        EXPECT_EQ((testFrame.GetTwist() - vels[ps.name[i]]) / elapsedTime.toSec(), cmpTwist);
    }
}

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