JointTorqueLimiterTest.cpp

Go to the documentation of this file.

#include <gtest/gtest.h>
#include "robodyn_controllers/JointTorqueLimiter.h"
#include <ros/package.h>
#include <sensor_msgs/JointState.h>

class JointTorqueLimiterTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        std::string packagePath = ros::package::getPath("robodyn_controllers");
        std::string urdfFile    = packagePath + "/test/urdf/SimpleTestRobot.xml";
        jtl                     = new JointTorqueLimiter(urdfFile);
        ros::Time::init();
    }

    virtual void TearDown()
    {
    }

    JointTorqueLimiter* jtl;
};

TEST_F(JointTorqueLimiterTest, InitializeMapsTest)
{

    // Initialize maps with test tree
    jtl->initializeMaps();

    ASSERT_FALSE(jtl->jointTorqueLimMap.empty());
    EXPECT_EQ (1, jtl->jointTorqueLimMap.count("joint1"));
    EXPECT_EQ (0, jtl->jointTorqueLimMap.count("link3"));

}

TEST_F(JointTorqueLimiterTest, PopulateTorqueMsgTest)
{
    jtl->initializeMaps();

    sensor_msgs::JointState tLim;
    EXPECT_EQ(0, tLim.name.size());

    ASSERT_EQ(0,        jtl->populateTorqueMsg(tLim));
    ASSERT_EQ(3,        jtl->jointTorqueLimMap.size());
    ASSERT_EQ(3,        tLim.name.size());
    EXPECT_EQ("joint1", tLim.name[0]);
    EXPECT_EQ(0.0,      tLim.effort[1]);

    jtl->jointTorqueLimMap["joint2"] = 2;
    ASSERT_EQ(0, jtl->populateTorqueMsg(tLim));
    EXPECT_EQ(2, tLim.effort[1]);
}

TEST_F(JointTorqueLimiterTest, StoreJointTorques1Test)
{
    jtl->initializeMaps();

    double yLim = 5.0;
    jtl->setYankLimit(yLim);

    EXPECT_EQ(0,   jtl->jointTorqueLimMap["joint1"]);
    ASSERT_EQ(0,   jtl->storeJointTorques(2.0));
    EXPECT_EQ(2.0, jtl->jointTorqueLimMap["joint1"]);
    EXPECT_EQ(2.0, jtl->jointTorqueLimMap["joint3"]);

    ASSERT_EQ(0,   jtl->storeJointTorques(10.0));
    EXPECT_EQ(7.0, jtl->jointTorqueLimMap["joint1"]);
    EXPECT_EQ(7.0, jtl->jointTorqueLimMap["joint2"]);

}

TEST_F(JointTorqueLimiterTest, StoreJointTorques2Test)
{
    jtl->initializeMaps();

    double yLim = 5.0;
    jtl->setYankLimit(yLim);

    sensor_msgs::JointState tLim;
    tLim.name.push_back("joint1");
    tLim.name.push_back("joint2");
    tLim.effort.push_back(1.0);
    tLim.effort.push_back(2.0);

    ASSERT_EQ(0,   jtl->storeJointTorques(tLim));
    EXPECT_EQ(1.0, jtl->jointTorqueLimMap["joint1"]);
    EXPECT_EQ(2.0, jtl->jointTorqueLimMap["joint2"]);
    EXPECT_EQ(0.0, jtl->jointTorqueLimMap["joint3"]);

    tLim.name.push_back("joint3");
    tLim.effort.push_back(6.0);

    ASSERT_EQ(0,   jtl->storeJointTorques(tLim));
    EXPECT_EQ(5.0, jtl->jointTorqueLimMap["joint3"]);
}

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