RosMsgTreeIdTest.cpp

Go to the documentation of this file.

#include <gtest/gtest.h>
#include "robodyn_controllers/KdlTreeId.h"
#include "robodyn_controllers/RosMsgTreeId.h"
#include <ros/package.h>

class RosMsgTreeIdTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        std::string packagePath = ros::package::getPath("robodyn_controllers");
        id.loadFromFile(packagePath + "/test/urdf/SimpleTestRobot.xml");
        tree = id.getTree();
        jd.InitializeMaps(tree);
        ros::Time::init();
    }

    virtual void TearDown()
    {
    }

    KdlTreeId         id;
    KDL::Tree         tree;
    JointDynamicsData jd;
    JntDynData        data;
    RosMsgTreeId      rmt;
};

TEST_F(RosMsgTreeIdTest, SetJointDataTest)
{
    sensor_msgs::JointState          actualState;
    sensor_msgs::JointState          traj1;
    r2_msgs::WrenchState wrenchState;

    actualState.name.push_back("joint1");
    actualState.name.push_back("joint2");
    actualState.name.push_back("joint3");

    actualState.position.push_back(5.0);
    actualState.position.push_back(5.0);
    actualState.position.push_back(5.0);

    actualState.velocity.push_back(5.0);
    actualState.velocity.push_back(5.0);
    actualState.velocity.push_back(5.0);

    actualState.effort.push_back(5.0);
    actualState.effort.push_back(5.0);
    actualState.effort.push_back(5.0);

    // Empty traj message
    rmt.setJointData(actualState, traj1, wrenchState, jd);
    data = jd.jointDataMap["joint1"];
    EXPECT_EQ(5.0, data.pos);
    EXPECT_EQ(0.0, data.vel);
    EXPECT_EQ(0.0, data.acc);

    traj1.name.push_back("joint1");
    traj1.position.push_back(1.0);
    traj1.velocity.push_back(1.0);
    traj1.effort.push_back(1.0);

    traj1.name.push_back("joint2");
    traj1.name.push_back("joint3");

    traj1.position.push_back(2.0);
    traj1.position.push_back(2.0);

    traj1.velocity.push_back(2.0);
    traj1.velocity.push_back(2.0);

    traj1.effort.push_back(2.0);
    traj1.effort.push_back(2.0);

    // Finally start the test
    // Actual state populated only
    rmt.setJointData(actualState, traj1, wrenchState, jd);

    data = jd.jointDataMap["joint1"];
    EXPECT_EQ(5.0, data.pos);
    EXPECT_EQ(1.0, data.vel);
    EXPECT_EQ(1.0, data.acc);

    data = jd.jointDataMap["joint3"];
    EXPECT_EQ(5.0, data.pos);

    EXPECT_EQ(0.0, jd.extForceMap["link2"].force(0));

    data = jd.jointDataMap["joint2"];
    EXPECT_EQ(5.0, data.pos);
    EXPECT_EQ(2.0, data.vel);
    EXPECT_EQ(2.0, data.acc);

    // trajectory and force
    wrenchState.name.push_back("link2");
    geometry_msgs::Vector3 init;
    init.x = 3.0;
    init.y = 3.0;
    init.z = 3.0;
    geometry_msgs::Wrench initW;
    initW.force  = init;
    initW.torque = init;
    wrenchState.wrench.push_back(initW);

    rmt.setJointData(actualState, traj1, wrenchState, jd);

    data = jd.jointDataMap["joint1"];
    EXPECT_EQ(5.0, data.pos);
    EXPECT_EQ(1.0, data.vel);
    EXPECT_EQ(1.0, data.acc);

    data = jd.jointDataMap["joint2"];
    EXPECT_EQ(5.0, data.pos);
    EXPECT_EQ(2.0, data.vel);
    EXPECT_EQ(2.0, data.acc);

    data = jd.jointDataMap["joint3"];
    EXPECT_EQ(5.0, data.pos);
    EXPECT_EQ(2.0, data.vel);
    EXPECT_EQ(2.0, data.acc);

    EXPECT_EQ(0.0,  jd.extForceMap["link1"].force(0));
    EXPECT_EQ(-3.0, jd.extForceMap["link2"].force(0));
    EXPECT_EQ(-3.0, jd.extForceMap["link2"].force(1));
    EXPECT_EQ(-3.0, jd.extForceMap["link2"].force(2));
    EXPECT_EQ(-3.0, jd.extForceMap["link2"].torque(0));
    EXPECT_EQ(-3.0, jd.extForceMap["link2"].torque(1));
    EXPECT_EQ(-3.0, jd.extForceMap["link2"].torque(2));
}

TEST_F(RosMsgTreeIdTest, GetJointCommandTest)
{
    // Set up map
    jd.jointTorqueCommandMap["joint1"] = 1.0;
    jd.jointTorqueCommandMap["joint2"] = 2.0;
    jd.jointTorqueCommandMap["joint3"] = 3.0;

    // Set up message
    sensor_msgs::JointState tauFF;

    rmt.getJointCommand(jd, tauFF);

    ASSERT_EQ(3,        tauFF.name.size());
    EXPECT_EQ(-1.0,     tauFF.effort[0]);
    EXPECT_EQ("joint2", tauFF.name[1]);
    EXPECT_EQ(-3.0,     tauFF.effort[2]);
    EXPECT_EQ(0,        tauFF.position.size());

}

TEST_F(RosMsgTreeIdTest, GetSegmentForcesTest)
{
    KDL::Vector force(1.0, 0.0, 0.0), torque(0.0, 2.0, 1.0);
    KDL::Wrench testW(force, torque);
    jd.segForceMap["link1"] =  testW;
    jd.segForceMap["link2"] = KDL::Wrench::Zero();
    KDL::Wrench testW2 = testW ;
    testW2.force.y(17);
    jd.segForceMap["link3"] = testW2;

    r2_msgs::WrenchState segF;

    rmt.getSegmentForces(jd, segF);

    ASSERT_EQ(4,   segF.name.size());
    EXPECT_EQ(1.0, segF.wrench[0].force.x);
    EXPECT_EQ(0.0, segF.wrench[1].force.y);
    EXPECT_EQ(1.0, segF.wrench[2].torque.z);

}

TEST_F(RosMsgTreeIdTest, SetEmptyTorqueMsgTest)
{
    // Set up map
    jd.jointTorqueCommandMap["joint1"] = 1.0;
    jd.jointTorqueCommandMap["joint2"] = 2.0;
    jd.jointTorqueCommandMap["joint3"] = 3.0;

    // Set up message
    sensor_msgs::JointState tauFF;

    rmt.setEmptyTorqueMsg(jd, tauFF);

    ASSERT_EQ(3,        tauFF.name.size());
    EXPECT_EQ(0.0,      tauFF.effort[0]);
    EXPECT_EQ("joint2", tauFF.name[1]);
    EXPECT_EQ(0.0,      tauFF.effort[2]);
    EXPECT_EQ(0,        tauFF.position.size());

}

TEST_F(RosMsgTreeIdTest, GetJointInertiasTest)
{
    // Set up map
    jd.jointInertiaMap["joint1"] = 1.0;
    jd.jointInertiaMap["joint2"] = 2.0;
    jd.jointInertiaMap["joint3"] = 3.0;

    // Set up message
    sensor_msgs::JointState Hv;

    rmt.getJointInertias(jd, Hv);

    ASSERT_EQ(3,        Hv.name.size());
    EXPECT_EQ(1.0,      Hv.position[0]);
    EXPECT_EQ("joint2", Hv.name[1]);
    EXPECT_EQ(3.0,      Hv.position[2]);
    EXPECT_EQ(0,        Hv.velocity.size());

}

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