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 "urdf/model.h"
00040
00041
00042
00043 #include <ros/ros.h>
00044
00045 using namespace urdf;
00046
00047 int g_argc;
00048 char** g_argv;
00049
00050 class TestParser : public testing::Test
00051 {
00052 public:
00053
00054 bool checkModel(urdf::Model & robot)
00055 {
00056
00057 urdf::LinkConstSharedPtr root_link = robot.getRoot();
00058 if (!root_link)
00059 {
00060 ROS_ERROR("no root link %s", robot.getName().c_str());
00061 return false;
00062 }
00063
00064
00065 return this->traverse_tree(root_link);
00066
00067 };
00068
00069 protected:
00071
00072 TestParser() : num_joints(0), num_links(1)
00073 {
00074 }
00075
00076
00078 ~TestParser()
00079 {
00080 }
00081
00082 bool traverse_tree(urdf::LinkConstSharedPtr link,int level = 0)
00083 {
00084 ROS_INFO("Traversing tree at level %d, link size %lu", level, link->child_links.size());
00085 level+=2;
00086 bool retval = true;
00087 for (std::vector<urdf::LinkSharedPtr>::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
00088 {
00089 ++num_links;
00090 if (*child && (*child)->parent_joint)
00091 {
00092 ++num_joints;
00093
00094 double roll,pitch,yaw;
00095 (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(roll,pitch,yaw);
00096
00097 if (isnan(roll) || isnan(pitch) || isnan(yaw))
00098 {
00099 ROS_ERROR("getRPY() returned nan!");
00100 return false;
00101 }
00102
00103 retval &= this->traverse_tree(*child,level);
00104 }
00105 else
00106 {
00107 ROS_ERROR("root link: %s has a null child!",link->name.c_str());
00108 return false;
00109 }
00110 }
00111
00112 return retval;
00113 };
00114
00115 size_t num_joints;
00116 size_t num_links;
00117 };
00118
00119
00120
00121
00122 TEST_F(TestParser, test)
00123 {
00124 ASSERT_GE(g_argc, 3);
00125 std::string folder = std::string(g_argv[1]) + "/test/";
00126 ROS_INFO("Folder %s",folder.c_str());
00127 std::string file = std::string(g_argv[2]);
00128 bool expect_success = (file.substr(0,5) != "fail_");
00129 urdf::Model robot;
00130 ROS_INFO("Parsing file %s, expecting %d",(folder + file).c_str(), expect_success);
00131 if (!expect_success) {
00132 ASSERT_FALSE(robot.initFile(folder + file));
00133 return;
00134 }
00135
00136 ASSERT_EQ(g_argc, 7);
00137 std::string robot_name = std::string(g_argv[3]);
00138 std::string root_name = std::string(g_argv[4]);
00139 size_t expected_num_joints = atoi(g_argv[5]);
00140 size_t expected_num_links = atoi(g_argv[6]);
00141
00142 ASSERT_TRUE(robot.initFile(folder + file));
00143
00144 EXPECT_EQ(robot.getName(), robot_name);
00145 urdf::LinkConstSharedPtr root = robot.getRoot();
00146 ASSERT_TRUE(static_cast<bool>(root));
00147 EXPECT_EQ(root->name, root_name);
00148
00149 ASSERT_TRUE(checkModel(robot));
00150 EXPECT_EQ(num_joints, expected_num_joints);
00151 EXPECT_EQ(num_links, expected_num_links);
00152 EXPECT_EQ(robot.joints_.size(), expected_num_joints);
00153 EXPECT_EQ(robot.links_.size(), expected_num_links);
00154
00155
00156 ASSERT_TRUE(robot.initParam("robot_description"));
00157 ASSERT_FALSE(robot.initParam("robot_description_wim"));
00158 SUCCEED();
00159 }
00160
00161
00162
00163
00164 int main(int argc, char** argv)
00165 {
00166
00167 ros::init(argc, argv, "test", ros::init_options::AnonymousName);
00168 testing::InitGoogleTest(&argc, argv);
00169 g_argc = argc;
00170 g_argv = argv;
00171 return RUN_ALL_TESTS();
00172 }