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 Model robot;
00054
00055 bool checkModel()
00056 {
00057
00058 boost::shared_ptr<const Link> root_link=this->robot.getRoot();
00059 if (!root_link)
00060 {
00061 ROS_ERROR("no root link %s",this->robot.getName().c_str());
00062 return false;
00063 }
00064
00065
00066 return this->traverse_tree(root_link);
00067
00068 };
00069
00070 protected:
00072 TestParser()
00073 {
00074 }
00075
00076
00078 ~TestParser()
00079 {
00080 }
00081
00082 bool traverse_tree(boost::shared_ptr<const Link> link,int level = 0)
00083 {
00084 level+=2;
00085 int count = 0;
00086 for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
00087 {
00088 if (*child)
00089 {
00090
00091 double roll,pitch,yaw;
00092 (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(roll,pitch,yaw);
00093
00094 if (isnan(roll) || isnan(pitch) || isnan(yaw))
00095 {
00096 ROS_ERROR("getRPY() returned nan!");
00097 return false;
00098 }
00099
00100 return this->traverse_tree(*child,level);
00101 }
00102 else
00103 {
00104 ROS_ERROR("root link: %s has a null child!",link->name.c_str());
00105 return false;
00106 }
00107 }
00108
00109 return true;
00110 };
00111 };
00112
00113
00114
00115
00116 TEST_F(TestParser, test)
00117 {
00118 std::string folder = std::string(g_argv[1]) + "/test/";
00119 ROS_INFO("Folder %s",folder.c_str());
00120 for (int i=2; i<g_argc; i++){
00121 std::string file = g_argv[i];
00122 bool expect_success = (file.substr(0,5) != "fail_");
00123 ROS_INFO("Parsing file %d/%d called %s, expecting %d",(i-1), g_argc-1, (folder + file).c_str(), expect_success);
00124 if (!expect_success)
00125 ASSERT_FALSE(robot.initFile(folder + file));
00126 else
00127 {
00128 ASSERT_TRUE(robot.initFile(folder + file));
00129 ASSERT_TRUE(checkModel());
00130 }
00131 }
00132
00133
00134 ASSERT_TRUE(robot.initParam("robot_description"));
00135 ASSERT_FALSE(robot.initParam("robot_description_wim"));
00136 SUCCEED();
00137 }
00138
00139
00140
00141
00142 int main(int argc, char** argv)
00143 {
00144
00145 ros::init(argc, argv, "test", ros::init_options::AnonymousName);
00146 testing::InitGoogleTest(&argc, argv);
00147 g_argc = argc;
00148 g_argv = argv;
00149 return RUN_ALL_TESTS();
00150 }