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