JointTrajectoryManager_Test.cpp

Go to the documentation of this file.

/*
 * JointTrajectoryManager_Test.cpp
 *
 *  Created on: Sept 05, 2012
 *      Author: Ross Taylor
 */

#include <iostream>
#include <gtest/gtest.h>
#include "robodyn_controllers/JointTrajectoryManager.h"
#include <tf/tf.h>

class JointTrajectoryManagerTest : public ::testing::Test
{
protected:
    class derivedManager : public JointTrajectoryManager
    {
    public:
        struct GoalIdCompare : public std::binary_function<actionlib_msgs::GoalID, actionlib_msgs::GoalID, bool>
        {
            bool operator()(const actionlib_msgs::GoalID& left, const actionlib_msgs::GoalID& right) const
            {
                return (left.stamp < right.stamp || (left.stamp == right.stamp && left.id < right.id));
            }
        };

        std::map<actionlib_msgs::GoalID, bool, GoalIdCompare> activeGoals;

    protected:
        bool isActive(const actionlib_msgs::GoalID& goalId) const
        {
            if (activeGoals.find(goalId) == activeGoals.end())
            {
                return false;
            }
            else
            {
                return activeGoals.at(goalId);
            }
        }
    };

    virtual void SetUp()
    {
        manager = new derivedManager();
    }

    virtual void TearDown()
    {
        // delete stuff, etc.
        delete manager;
    }

    derivedManager* manager; // pointer for easy destruction between tests
};

// TEST_F uses SetUp/TearDown in Test above
// second argument describes the test (make descriptive)
TEST_F(JointTrajectoryManagerTest, managerTest)
{
    // EXPECT_EQ (test equality)
    // EXPECT_FLOAT_EQ (test equality, with roundoff allowed)
    // EXPECT_TRUE
    // EXPECT_FALSE
    // EXPECT_THROW
    // google google test for more
    trajectory_msgs::JointTrajectory trajectory;
    trajectory.header.frame_id = "testGoal1";
    ros::Time::init();
    trajectory.header.stamp = ros::Time::now();
    trajectory.joint_names.push_back("joint0");
    trajectory.joint_names.push_back("joint1");
    trajectory.joint_names.push_back("joint2");

    trajectory_msgs::JointTrajectoryPoint point;
    point.positions.push_back(1.0);
    point.positions.push_back(2.0);
    point.positions.push_back(3.0);
    point.time_from_start = ros::Duration(1.1);
    trajectory.points.push_back(point);

    actionlib_msgs::GoalID goalId;
    goalId.id = trajectory.header.frame_id;
    goalId.stamp = trajectory.header.stamp;
    manager->addToJointGoalMap(trajectory.joint_names, goalId);
    manager->activeGoals[goalId] = true;

    EXPECT_THROW(manager->removeFromJointGoalMap(goalId), std::runtime_error);
    manager->activeGoals.erase(goalId);
    EXPECT_NO_THROW(manager->removeFromJointGoalMap(goalId));
    EXPECT_NO_THROW(manager->addToJointGoalMap(trajectory.joint_names, goalId));
    goalId.stamp = ros::Time::now();
    EXPECT_NO_THROW(manager->addToJointGoalMap(trajectory.joint_names, goalId));
}

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