Go to the documentation of this file.00001 #include <gtest/gtest.h>
00002 #include "robodyn_controllers/KdlTreeUtilities.h"
00003
00004 #include <ros/package.h>
00005
00006 class KdlTreeUtilitiesTest : public ::testing::Test
00007 {
00008 protected:
00009 virtual void SetUp()
00010 {
00011
00012 std::string packagePath = ros::package::getPath("robodyn_controllers");
00013 util.loadFromFile(packagePath + "/test/urdf/BranchingTestRobot.xml");
00014 }
00015
00016 virtual void TearDown()
00017 {
00018 }
00019
00020 KdlTreeUtilities util;
00021 };
00022
00023 TEST_F(KdlTreeUtilitiesTest, utilTest)
00024 {
00025 std::vector<std::string> jointNames;
00026 util.getJointNames( jointNames);
00027 ASSERT_EQ(4, jointNames.size());
00028 EXPECT_EQ("joint0", jointNames[0]);
00029 EXPECT_EQ("joint1", jointNames[1]);
00030 EXPECT_EQ("joint2", jointNames[2]);
00031 EXPECT_EQ("joint3", jointNames[3]);
00032
00033 EXPECT_EQ(4, util.getJointCount());
00034 util.getChainJointNames("link3", jointNames);
00035 ASSERT_EQ(2, jointNames.size());
00036 EXPECT_EQ("joint0", jointNames[0]);
00037 EXPECT_EQ("joint2", jointNames[1]);
00038
00039 util.removeSegment("link2");
00040 EXPECT_TRUE(util.hasSegment("linkb"));
00041 EXPECT_TRUE(util.hasSegment("link0"));
00042 EXPECT_TRUE(util.hasSegment("link1"));
00043 EXPECT_FALSE(util.hasSegment("link2"));
00044 EXPECT_TRUE(util.hasSegment("link3"));
00045 EXPECT_TRUE(util.hasSegment("link4"));
00046
00047 KDL::Segment s1("new_link");
00048 util.addSegment(s1, "link1");
00049 EXPECT_TRUE(util.hasSegment("linkb"));
00050 EXPECT_TRUE(util.hasSegment("link0"));
00051 EXPECT_TRUE(util.hasSegment("link1"));
00052 EXPECT_FALSE(util.hasSegment("link2"));
00053 EXPECT_TRUE(util.hasSegment("link3"));
00054 EXPECT_TRUE(util.hasSegment("link4"));
00055 EXPECT_TRUE(util.hasSegment("new_link"));
00056
00057 util.removeSegment("new_link");
00058 EXPECT_TRUE(util.hasSegment("linkb"));
00059 EXPECT_TRUE(util.hasSegment("link0"));
00060 EXPECT_TRUE(util.hasSegment("link1"));
00061 EXPECT_FALSE(util.hasSegment("link2"));
00062 EXPECT_TRUE(util.hasSegment("link3"));
00063 EXPECT_TRUE(util.hasSegment("link4"));
00064 EXPECT_FALSE(util.hasSegment("new_link"));
00065
00066 util.addSegment(s1, "link1");
00067 EXPECT_TRUE(util.hasSegment("linkb"));
00068 EXPECT_TRUE(util.hasSegment("link0"));
00069 EXPECT_TRUE(util.hasSegment("link1"));
00070 EXPECT_FALSE(util.hasSegment("link2"));
00071 EXPECT_TRUE(util.hasSegment("link3"));
00072 EXPECT_TRUE(util.hasSegment("link4"));
00073 EXPECT_TRUE(util.hasSegment("new_link"));
00074
00075 KDL::Segment s2("new_link2");
00076 util.addSegment(s2, "new_link");
00077 EXPECT_TRUE(util.hasSegment("linkb"));
00078 EXPECT_TRUE(util.hasSegment("link0"));
00079 EXPECT_TRUE(util.hasSegment("link1"));
00080 EXPECT_FALSE(util.hasSegment("link2"));
00081 EXPECT_TRUE(util.hasSegment("link3"));
00082 EXPECT_TRUE(util.hasSegment("link4"));
00083 EXPECT_TRUE(util.hasSegment("new_link"));
00084 EXPECT_TRUE(util.hasSegment("new_link2"));
00085
00086 util.removeSegment("new_link");
00087 EXPECT_TRUE(util.hasSegment("linkb"));
00088 EXPECT_TRUE(util.hasSegment("link0"));
00089 EXPECT_TRUE(util.hasSegment("link1"));
00090 EXPECT_FALSE(util.hasSegment("link2"));
00091 EXPECT_TRUE(util.hasSegment("link3"));
00092 EXPECT_TRUE(util.hasSegment("link4"));
00093 EXPECT_FALSE(util.hasSegment("new_link"));
00094 EXPECT_FALSE(util.hasSegment("new_link2"));
00095
00096 }
00097
00098 int main(int argc, char** argv)
00099 {
00100 ::testing::InitGoogleTest(&argc, argv);
00101 return RUN_ALL_TESTS();
00102 }