41 #include <gtest/gtest.h>
57 urdf::LinkConstSharedPtr root_link = robot.getRoot();
59 ROS_ERROR(
"no root link %s", robot.getName().c_str());
82 ROS_INFO(
"Traversing tree at level %d, link size %lu", level, link->child_links.size());
85 for (
const urdf::LinkSharedPtr & child : link->child_links)
88 if (child && child->parent_joint) {
91 double roll, pitch, yaw;
92 child->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(roll, pitch, yaw);
94 if (std::isnan(roll) || std::isnan(pitch) || std::isnan(yaw)) {
101 ROS_ERROR(
"root link: %s has a null child!", link->name.c_str());
115 std::string folder = std::string(
g_argv[1]) +
"/test/";
116 ROS_INFO(
"Folder %s", folder.c_str());
117 std::string file = std::string(
g_argv[2]);
118 bool expect_success = (file.substr(0, 5) !=
"fail_");
120 ROS_INFO(
"Parsing file %s, expecting %d", (folder + file).c_str(), expect_success);
121 if (!expect_success) {
122 ASSERT_FALSE(robot.
initFile(folder + file));
127 std::string robot_name = std::string(
g_argv[3]);
128 std::string root_name = std::string(
g_argv[4]);
129 size_t expected_num_joints = atoi(
g_argv[5]);
130 size_t expected_num_links = atoi(
g_argv[6]);
132 ASSERT_TRUE(robot.
initFile(folder + file));
134 EXPECT_EQ(robot.getName(), robot_name);
135 urdf::LinkConstSharedPtr root = robot.getRoot();
136 ASSERT_TRUE(
static_cast<bool>(root));
137 EXPECT_EQ(root->name, root_name);
139 ASSERT_TRUE(checkModel(robot));
140 EXPECT_EQ(num_joints, expected_num_joints);
141 EXPECT_EQ(num_links, expected_num_links);
142 EXPECT_EQ(robot.joints_.size(), expected_num_joints);
143 EXPECT_EQ(robot.links_.size(), expected_num_links);
146 ASSERT_TRUE(robot.
initParam(
"robot_description"));
147 ASSERT_FALSE(robot.
initParam(
"robot_description_wim"));
151 int main(
int argc,
char ** argv)
155 testing::InitGoogleTest(&argc, argv);
158 return RUN_ALL_TESTS();