From b95dbf22956ed488d00474b36c823379378c9437 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Wed, 9 Dec 2015 16:15:46 -0800 Subject: [PATCH 1/4] Overhaul tests in urdf --- kdl_parser/src/kdl_parser.cpp | 8 +- kdl_parser/test/test_kdl_parser.cpp | 8 +- kdl_parser/test/test_kdl_parser.launch | 5 +- kdl_parser/test/test_robot.urdf | 425 +++++++++++++++++++++++ urdf/src/model.cpp | 3 +- urdf/test/one_link.urdf | 2 +- urdf/test/test_robot.urdf | 425 +++++++++++++++++++++++ urdf/test/test_robot_model_parser.cpp | 65 ++-- urdf/test/test_robot_model_parser.launch | 18 +- urdf/test/two_links_one_joint.urdf | 2 +- 10 files changed, 923 insertions(+), 38 deletions(-) create mode 100644 kdl_parser/test/test_robot.urdf create mode 100644 urdf/test/test_robot.urdf diff --git a/kdl_parser/src/kdl_parser.cpp b/kdl_parser/src/kdl_parser.cpp index 4b8c3ab..ee41acf 100644 --- a/kdl_parser/src/kdl_parser.cpp +++ b/kdl_parser/src/kdl_parser.cpp @@ -168,15 +168,19 @@ bool treeFromXml(TiXmlDocument *xml_doc, Tree& tree) bool treeFromUrdfModel(const urdf::ModelInterface& robot_model, Tree& tree) { tree = Tree(robot_model.getRoot()->name); + ROS_INFO("constructed tree"); // warn if root link has inertia. KDL does not support this if (robot_model.getRoot()->inertial) ROS_WARN("The root link %s has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.", robot_model.getRoot()->name.c_str()); // add all children - for (size_t i=0; ichild_links.size(); i++) - if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree)) + for (size_t i=0; ichild_links.size(); i++) { + ROS_INFO("Adding link to KDL tree"); + if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree)) { return false; + } + } return true; } diff --git a/kdl_parser/test/test_kdl_parser.cpp b/kdl_parser/test/test_kdl_parser.cpp index c43d75f..59ce8e1 100644 --- a/kdl_parser/test/test_kdl_parser.cpp +++ b/kdl_parser/test/test_kdl_parser.cpp @@ -63,8 +63,6 @@ protected: }; - - TEST_F(TestParser, test) { for (int i=1; isecond.children.size(), (unsigned int)1); ASSERT_TRUE(my_tree.getSegment("base_link")->second.parent == my_tree.getRootSegment()); ASSERT_EQ(my_tree.getSegment("base_link")->second.segment.getInertia().getMass(), 116.0); diff --git a/kdl_parser/test/test_kdl_parser.launch b/kdl_parser/test/test_kdl_parser.launch index dad91ee..a730458 100644 --- a/kdl_parser/test/test_kdl_parser.launch +++ b/kdl_parser/test/test_kdl_parser.launch @@ -1,6 +1,3 @@ - + diff --git a/kdl_parser/test/test_robot.urdf b/kdl_parser/test/test_robot.urdf new file mode 100644 index 0000000..467d133 --- /dev/null +++ b/kdl_parser/test/test_robot.urdf @@ -0,0 +1,425 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/src/model.cpp b/urdf/src/model.cpp index 7a6d4ff..a721056 100644 --- a/urdf/src/model.cpp +++ b/urdf/src/model.cpp @@ -183,8 +183,7 @@ bool Model::initString(const std::string& xml_string) this->root_link_ = model->root_link_; return true; } - else - return false; + return false; } diff --git a/urdf/test/one_link.urdf b/urdf/test/one_link.urdf index 15e64d5..4d99c96 100644 --- a/urdf/test/one_link.urdf +++ b/urdf/test/one_link.urdf @@ -1,5 +1,5 @@ - + diff --git a/urdf/test/test_robot.urdf b/urdf/test/test_robot.urdf new file mode 100644 index 0000000..467d133 --- /dev/null +++ b/urdf/test/test_robot.urdf @@ -0,0 +1,425 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/test/test_robot_model_parser.cpp b/urdf/test/test_robot_model_parser.cpp index d380f33..5e16980 100644 --- a/urdf/test/test_robot_model_parser.cpp +++ b/urdf/test/test_robot_model_parser.cpp @@ -50,26 +50,26 @@ char** g_argv; class TestParser : public testing::Test { public: - Model robot; - bool checkModel() + bool checkModel(urdf::Model & robot) { // get root link - boost::shared_ptr root_link=this->robot.getRoot(); + boost::shared_ptr root_link = robot.getRoot(); if (!root_link) { - ROS_ERROR("no root link %s",this->robot.getName().c_str()); + ROS_ERROR("no root link %s", robot.getName().c_str()); return false; } // go through entire tree - return this->traverse_tree(root_link); + return this->traverse_tree(root_link); }; protected: /// constructor - TestParser() + // num_links starts at 1 because traverse_tree doesn't count the root node + TestParser() : num_joints(0), num_links(1) { } @@ -81,11 +81,15 @@ protected: bool traverse_tree(boost::shared_ptr link,int level = 0) { + ROS_INFO("Traversing tree at level %d, link size %lu", level, link->child_links.size()); level+=2; + bool retval = true; for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) { - if (*child) + ++num_links; + if (*child && (*child)->parent_joint) { + ++num_joints; // check rpy double roll,pitch,yaw; (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(roll,pitch,yaw); @@ -96,7 +100,7 @@ protected: return false; } // recurse down the tree - return this->traverse_tree(*child,level); + retval &= this->traverse_tree(*child,level); } else { @@ -104,9 +108,12 @@ protected: return false; } } - // no children - return true; + // no more children + return retval; }; + + size_t num_joints; + size_t num_links; }; @@ -114,21 +121,37 @@ protected: TEST_F(TestParser, test) { + ASSERT_GE(g_argc, 3); std::string folder = std::string(g_argv[1]) + "/test/"; ROS_INFO("Folder %s",folder.c_str()); - for (int i=2; i root = robot.getRoot(); + ASSERT_TRUE(root); + EXPECT_EQ(root->name, root_name); + + ASSERT_TRUE(checkModel(robot)); + EXPECT_EQ(num_joints, expected_num_joints); + EXPECT_EQ(num_links, expected_num_links); + EXPECT_EQ(robot.joints_.size(), expected_num_joints); + EXPECT_EQ(robot.links_.size(), expected_num_links); + // test reading from parameter server ASSERT_TRUE(robot.initParam("robot_description")); ASSERT_FALSE(robot.initParam("robot_description_wim")); diff --git a/urdf/test/test_robot_model_parser.launch b/urdf/test/test_robot_model_parser.launch index 25c1844..9953b55 100644 --- a/urdf/test/test_robot_model_parser.launch +++ b/urdf/test/test_robot_model_parser.launch @@ -1,5 +1,19 @@ - + - + + + + + + + + + + + + + + + diff --git a/urdf/test/two_links_one_joint.urdf b/urdf/test/two_links_one_joint.urdf index 85e94a3..a4cf276 100644 --- a/urdf/test/two_links_one_joint.urdf +++ b/urdf/test/two_links_one_joint.urdf @@ -1,5 +1,5 @@ - + From 9869a67924989bc132a5a0873fc8d2e1e5db4141 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Wed, 9 Dec 2015 16:21:38 -0800 Subject: [PATCH 2/4] Revert debug statements in kdl_parser.cpp --- kdl_parser/src/kdl_parser.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/kdl_parser/src/kdl_parser.cpp b/kdl_parser/src/kdl_parser.cpp index ee41acf..4b8c3ab 100644 --- a/kdl_parser/src/kdl_parser.cpp +++ b/kdl_parser/src/kdl_parser.cpp @@ -168,19 +168,15 @@ bool treeFromXml(TiXmlDocument *xml_doc, Tree& tree) bool treeFromUrdfModel(const urdf::ModelInterface& robot_model, Tree& tree) { tree = Tree(robot_model.getRoot()->name); - ROS_INFO("constructed tree"); // warn if root link has inertia. KDL does not support this if (robot_model.getRoot()->inertial) ROS_WARN("The root link %s has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.", robot_model.getRoot()->name.c_str()); // add all children - for (size_t i=0; ichild_links.size(); i++) { - ROS_INFO("Adding link to KDL tree"); - if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree)) { + for (size_t i=0; ichild_links.size(); i++) + if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree)) return false; - } - } return true; } From 612d4339201696cc8066977f12f238be0774eb1a Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Wed, 9 Dec 2015 16:32:45 -0800 Subject: [PATCH 3/4] Fix values in kdl_parser test --- kdl_parser/test/test_kdl_parser.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/kdl_parser/test/test_kdl_parser.cpp b/kdl_parser/test/test_kdl_parser.cpp index 59ce8e1..ad0bb98 100644 --- a/kdl_parser/test/test_kdl_parser.cpp +++ b/kdl_parser/test/test_kdl_parser.cpp @@ -71,13 +71,13 @@ TEST_F(TestParser, test) } ASSERT_TRUE(treeFromFile(g_argv[g_argc-1], my_tree)); - ASSERT_EQ(my_tree.getNrOfJoints(), 3); - ASSERT_EQ(my_tree.getNrOfSegments(), 4); - ASSERT_TRUE(my_tree.getSegment("link1") == my_tree.getRootSegment()); + ASSERT_EQ(my_tree.getNrOfJoints(), 8); + ASSERT_EQ(my_tree.getNrOfSegments(), 16); + ASSERT_TRUE(my_tree.getSegment("dummy_link") == my_tree.getRootSegment()); ASSERT_EQ(my_tree.getRootSegment()->second.children.size(), (unsigned int)1); ASSERT_TRUE(my_tree.getSegment("base_link")->second.parent == my_tree.getRootSegment()); - ASSERT_EQ(my_tree.getSegment("base_link")->second.segment.getInertia().getMass(), 116.0); - ASSERT_NEAR(my_tree.getSegment("base_link")->second.segment.getInertia().getRotationalInertia().data[0], 15.6107, 0.001); + ASSERT_EQ(my_tree.getSegment("base_link")->second.segment.getInertia().getMass(), 10.0); + ASSERT_NEAR(my_tree.getSegment("base_link")->second.segment.getInertia().getRotationalInertia().data[0], 1.000, 0.001); SUCCEED(); } From d586847d6e747dffe5c8fa774ade19372db03c88 Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Mon, 14 Dec 2015 13:41:23 -0800 Subject: [PATCH 4/4] test_robot.urdf: fix indentation --- kdl_parser/test/test_robot.urdf | 2 +- urdf/test/test_robot.urdf | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/kdl_parser/test/test_robot.urdf b/kdl_parser/test/test_robot.urdf index 467d133..731e1f3 100644 --- a/kdl_parser/test/test_robot.urdf +++ b/kdl_parser/test/test_robot.urdf @@ -1,7 +1,7 @@ - + diff --git a/urdf/test/test_robot.urdf b/urdf/test/test_robot.urdf index 467d133..731e1f3 100644 --- a/urdf/test/test_robot.urdf +++ b/urdf/test/test_robot.urdf @@ -1,7 +1,7 @@ - +