00001 #include <gtest/gtest.h>
00002 #include "nasa_controllers_core/BaseFrameVerifier.h"
00003 #include "nasa_controllers_core/RosMsgBFVerifier.h"
00004 #include <ros/package.h>
00005
00006 class BaseFrameVerifierTest : public ::testing::Test
00007 {
00008 protected:
00009 virtual void SetUp()
00010 {
00011 std::string packagePath = ros::package::getPath("nasa_controllers_core");
00012 std::string urdfFile = packagePath + "/test/urdf/SimpleTestRobot.xml";
00013 rmv = new RosMsgBFVerifier(urdfFile);
00014 tree = rmv->getTree();
00015
00016 refFrame = "link1";
00017 dist = 0.03;
00018 err = 0.01;
00019
00020 bfv = new BaseFrameVerifier(refFrame, dist, err);
00021
00022 ros::Time::init();
00023
00024
00025 eefPoseState.name.push_back("link1");
00026 eefPoseState.name.push_back("link2");
00027 eefPoseState.name.push_back("link3");
00028 eefPoseState.name.push_back("link4");
00029
00030 tf::quaternionTFToMsg(tf::createQuaternionFromRPY(0.0, 0.0, 0.0), pos.orientation);
00031 pos.position.x = 0.0;
00032 pos.position.y = 0.0;
00033 pos.position.z = 0.0;
00034
00035 eefPoseState.positions.push_back(pos);
00036
00037 pos.position.x = 5.0;
00038 pos.position.y = 3.0;
00039 eefPoseState.positions.push_back(pos);
00040
00041 pos.position.x = -2.0;
00042 pos.position.y = 5.0;
00043 tf::quaternionTFToMsg(tf::createQuaternionFromRPY(0.0, 1.57, 0.0), pos.orientation);
00044 eefPoseState.positions.push_back(pos);
00045
00046 pos.position.x = 0.0;
00047 pos.position.y = 5.0;
00048 tf::quaternionTFToMsg(tf::createQuaternionFromRPY(0.0, -1.57, 0.0), pos.orientation);
00049 eefPoseState.positions.push_back(pos);
00050
00051 }
00052
00053 virtual void TearDown()
00054 {
00055 }
00056
00057 BaseFrameVerifier* bfv;
00058 RosMsgBFVerifier* rmv;
00059 std::string refFrame;
00060 double dist, err;
00061 KDL::Tree tree;
00062 r2_msgs::PoseState eefPoseState;
00063 geometry_msgs::Pose pos;
00064 r2_msgs::StringArray baseFrames;
00065 r2_msgs::PoseTrajectory trajMsg;
00066 };
00067
00068 TEST_F(BaseFrameVerifierTest, InitializeMapTest)
00069 {
00070
00071 bfv->InitializeMap(tree);
00072 KDL::Frame zeroData;
00073
00074 ASSERT_FALSE( bfv->eefFrameMap.empty());
00075 EXPECT_EQ (0, bfv->eefFrameMap.count("joint1"));
00076 EXPECT_EQ (0, bfv->eefFrameMap.count("link1"));
00077 EXPECT_EQ (1, bfv->eefFrameMap.count("link2"));
00078 EXPECT_EQ (zeroData, bfv->eefFrameMap["link2"]);
00079 EXPECT_EQ (0, bfv->eefFrameMap.count("link3"));
00080 EXPECT_EQ (1, bfv->eefFrameMap.count("link4"));
00081 }
00082
00083 TEST_F(BaseFrameVerifierTest, UpdateCurrentEEFPositionsTest)
00084 {
00085 bfv->InitializeMap(tree);
00086
00087 bfv->UpdateCurrentEEFPositions(eefPoseState);
00088
00089 KDL::Frame frame;
00090
00091 try
00092 {
00093 RosMsgConverter::PoseStateToFrame(eefPoseState, refFrame, "link2", frame);
00094 }
00095 catch (std::exception& e)
00096 {
00097 NasaCommonLogging::Logger::log("gov.nasa.controllers.BaseFrameVerifierTest", log4cpp::Priority::ERROR, e.what());
00098 }
00099
00100 EXPECT_EQ(frame, bfv->eefFrameMap["link2"]);
00101
00102 }
00103
00104
00105 TEST_F(BaseFrameVerifierTest, CreatePullCommandTest)
00106 {
00107 bfv->InitializeMap(tree);
00108 bfv->UpdateCurrentEEFPositions(eefPoseState);
00109
00110 ASSERT_EQ(-1, bfv->CreatePullCommand(baseFrames, "link2", trajMsg));
00111
00112 baseFrames.data.push_back("link2");
00113 ASSERT_EQ(0, bfv->CreatePullCommand(baseFrames, "link4", trajMsg));
00114 EXPECT_EQ(2, trajMsg.nodes.size());
00115 EXPECT_EQ("link2", trajMsg.nodes[0]);
00116 EXPECT_EQ("link4", trajMsg.nodes[1]);
00117 EXPECT_EQ(refFrame, trajMsg.refFrames[0]);
00118 EXPECT_EQ("link4", trajMsg.refFrames[1]);
00119 EXPECT_EQ(5.0, trajMsg.points[0].positions[0].position.x);
00120 EXPECT_EQ(ros::Duration(100 * dist), trajMsg.points[0].time_from_start);
00121 EXPECT_EQ(0.0, trajMsg.points[1].positions[0].position.x);
00122 EXPECT_EQ(-dist, trajMsg.points[1].positions[0].position.z);
00123
00124 }
00125
00126 TEST_F(BaseFrameVerifierTest, CheckPositionAgainstOutcomesTest)
00127 {
00128 bfv->InitializeMap(tree);
00129 bfv->UpdateCurrentEEFPositions(eefPoseState);
00130 baseFrames.data.push_back("link2");
00131 ASSERT_EQ(0, bfv->CreatePullCommand(baseFrames, "link4", trajMsg));
00132
00133 EXPECT_EQ(0, bfv->CheckPositionAgainstOutcomes("link3"));
00134 EXPECT_EQ(1, bfv->CheckPositionAgainstOutcomes("link4"));
00135
00136 }
00137
00138 TEST_F(BaseFrameVerifierTest, ClearCurrentTrajectoryTest)
00139 {
00140 bfv->InitializeMap(tree);
00141 bfv->UpdateCurrentEEFPositions(eefPoseState);
00142 bfv->ClearCurrentTrajectory(trajMsg);
00143
00144 EXPECT_EQ(2, trajMsg.nodes.size());
00145 EXPECT_EQ(refFrame, trajMsg.refFrames[0]);
00146 EXPECT_EQ(ros::Duration(1.0), trajMsg.points[0].time_from_start);
00147
00148 }
00149
00150 int main(int argc, char** argv)
00151 {
00152 ::testing::InitGoogleTest(&argc, argv);
00153 return RUN_ALL_TESTS();
00154 }