RosMsgVelocityMonitorTest.cpp

Go to the documentation of this file.

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

class RosMsgSafetyVelocityMonitorTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        std::string packagePath = ros::package::getPath("robodyn_controllers");
        std::stringstream ss;
        ss << packagePath << "/test/urdf/BranchingTestRobot.xml";
        urdfFile = ss.str();
        //rmt = new RosMsgSafetyVelocityMonitorTest(ss.str());//.loadFromFile(ss.str());
        ros::Time::init();
    }

    virtual void TearDown()
    {
    }

    std::string urdfFile;


};

TEST_F(RosMsgSafetyVelocityMonitorTest, isVelocityBelowLimsTest)
{
    RosMsgSafetyVelocityMonitor*      rmt = new RosMsgSafetyVelocityMonitor(urdfFile);
    diagnostic_msgs::DiagnosticStatus errorMsg;
    r2_msgs::PoseState    poseState;

    rmt->getTree().getSegments().begin();

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

    for (int i = 0; i < poseState.name.size(); i++)
    {
        geometry_msgs::Pose p;
        p.position.x = 1.0;
        p.position.y = 1.0;
        p.position.z = 1.0;
        poseState.positions.push_back(p);

        geometry_msgs::Twist lv;
        lv.linear.x = 5.0;
        lv.linear.y = 5.0;
        lv.linear.z = 5.0;
        lv.angular.x = 5.0;
        lv.angular.y = 5.0;
        lv.angular.z = 5.0;
        poseState.velocities.push_back(lv);
    }

    rmt->setAngularVelocityLimit(2.0);
    rmt->setLiniearVelocityLimit(2.0);

    EXPECT_FALSE(rmt->isVelocityBelowLims(poseState, errorMsg));

    rmt->setAngularVelocityLimit(9.0);
    EXPECT_FALSE(rmt->isVelocityBelowLims(poseState, errorMsg));

    rmt->setLiniearVelocityLimit(9.0);
    EXPECT_TRUE(rmt->isVelocityBelowLims(poseState, errorMsg));

}



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