Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <string>
00038 #include <gtest/gtest.h>
00039 #include <ros/ros.h>
00040 #include "kdl_parser/kdl_parser.hpp"
00041
00042 using namespace kdl_parser;
00043
00044 int g_argc;
00045 char** g_argv;
00046
00047 class TestParser : public testing::Test
00048 {
00049 public:
00050 KDL::Tree my_tree;
00051
00052 protected:
00054 TestParser()
00055 {
00056 }
00057
00058
00060 ~TestParser()
00061 {
00062 }
00063 };
00064
00065
00066
00067
00068 TEST_F(TestParser, test)
00069 {
00070 for (int i=1; i<g_argc-2; i++){
00071 ROS_ERROR("Testing file %s", g_argv[i]);
00072 ASSERT_FALSE(treeFromFile(g_argv[i], my_tree));
00073 }
00074
00075 ASSERT_TRUE(treeFromFile(g_argv[g_argc-1], my_tree));
00076 ASSERT_EQ(my_tree.getNrOfJoints(), (unsigned int)44);
00077 ASSERT_EQ(my_tree.getNrOfSegments(), (unsigned int)81);
00078 ASSERT_TRUE(my_tree.getSegment("base_footprint") == my_tree.getRootSegment());
00079 ASSERT_EQ(my_tree.getRootSegment()->second.children.size(), (unsigned int)1);
00080 ASSERT_TRUE(my_tree.getSegment("base_link")->second.parent == my_tree.getRootSegment());
00081 ASSERT_EQ(my_tree.getSegment("base_link")->second.segment.getInertia().getMass(), 116.0);
00082 ASSERT_NEAR(my_tree.getSegment("base_link")->second.segment.getInertia().getRotationalInertia().data[0], 15.6107, 0.001);
00083 SUCCEED();
00084 }
00085
00086
00087
00088
00089 int main(int argc, char** argv)
00090 {
00091 testing::InitGoogleTest(&argc, argv);
00092 ros::init(argc, argv, "test_kdl_parser");
00093 g_argc = argc;
00094 g_argv = argv;
00095 return RUN_ALL_TESTS();
00096 }