RosMsgFkMonitorTest.cpp

Go to the documentation of this file.

#include <gtest/gtest.h>
#include "robodyn_controllers/KdlTreeFk.h"
#include "robodyn_controllers/RosMsgFkMonitor.h"
#include "robodyn_utilities/RosMsgConverter.h"
#include <ros/package.h>

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

    virtual void TearDown()
    {
    }

    RosMsgFkMonitor rmt;
};

TEST_F(RosMsgFkMonitorTest, getNodeFramesTest)
{
    r2_msgs::StringArray     nodes;
    std::map<std::string, KDL::FrameVel> nodeFrameResult;

    EXPECT_FALSE(rmt.getNodeFrames(nodes, nodeFrameResult )) << "getNodeFrames should return false when sent an empty node list";
    EXPECT_EQ(0, nodeFrameResult.size()) << "nodeFrameResult should be empty";


    nodes.data.push_back("joint1");
    EXPECT_TRUE(rmt.getNodeFrames(nodes, nodeFrameResult)) << "getNodeFrames should return true when asked to get a node in its tree";
    EXPECT_EQ(1,                         nodeFrameResult.size()) << "nodeFrameResult should have 1 value in it";
    EXPECT_EQ("joint1",                  nodeFrameResult.begin()->first) << "nodeFrameResult's first element is joint1";

    nodes.data.push_back("joint2");
    nodes.data.push_back("joint3");
    EXPECT_TRUE(rmt.getNodeFrames(nodes, nodeFrameResult)) << "getNodeFrames should return true when asked for a list of nodes in tree";
    EXPECT_EQ(3,                         nodeFrameResult.size()) << "nodeFrameResult should have 3 values in it";
    EXPECT_EQ("joint1",                  nodeFrameResult.begin()->first) << "nodeFrameResult's first element is joint1";
    EXPECT_EQ("joint3",                  nodeFrameResult.rbegin()->first) << "nodeFrameResut's last element is joint3";

    nodes.data.push_back("joint4");
    EXPECT_FALSE(rmt.getNodeFrames(nodes, nodeFrameResult)) << "getNodeFrames should return false when asked to retreive a node not in the tree";
    EXPECT_EQ(3,                          nodeFrameResult.size()) << "nodeFrameResult should still be populated with valid nodes in tree";


}

TEST_F(RosMsgFkMonitorTest, getPoseStateTest)
{
    sensor_msgs::JointState actualState;
    r2_msgs::PoseState poseState;

    actualState.name.push_back("joint0");
    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.position.push_back(5.0);

    actualState.velocity.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);
    actualState.effort.push_back(5.0);

    std::vector<std::string> jointNames;
    rmt.getJointNames(jointNames);
    KDL::JntArrayVel joints;

    try
    {
        RosMsgConverter::JointStateToJntArrayVel(actualState, jointNames, joints);
    }
    catch (std::exception& e)
    {
        NasaCommonLogging::Logger::log("gov.nasa.controllers.RosMsgFkMonitorTest", log4cpp::Priority::ERROR, e.what());
    }

    EXPECT_TRUE(rmt.getPoseState(actualState, poseState));
    EXPECT_EQ(4, poseState.positions.size()) << "we should have 4 position messages, one for each joint";
    EXPECT_EQ(4, poseState.velocities.size()) << "we should have 4 twist velocity messages, one for each joint velocity";
    EXPECT_EQ(4, poseState.accelerations.size()) << "we should have 4 twist messages, one for each joint acceleration";
//    geometry_msgs::Pose pose = poseState.positions[0];
//    geometry_msgs::Twist vel = poseState.velocities[0];
//    geometry_msgs::Twist acc = poseState.accelerations[0];

}



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