update test to check for nan getRPY per #4247

urdf/test/singularity.urdf

@@ -0,0 +1,31 @@
+<?xml verison="1.0"?>
+<robot name="no_visual">
+  <link name="link1">
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1" iyy="1" izz="1" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+  <link name="link2">
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1" iyy="1" izz="1" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+  <joint name="joint12" type="fixed">
+    <parent link="link1"/>
+    <child link="link2"/>
+    <origin xyz="0 0 0" rpy="1.0 1.5707963267948966 0.5707963267948966"/>
+  </link>
+  <link name="link3">
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1" iyy="1" izz="1" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+  <joint name="joint13" type="fixed">
+    <parent link="link2"/>
+    <child link="link3"/>
+    <origin xyz="0 0 0" rpy="1.5707963267948966 1.5707963267948966 1.5707963267948966"/>
+  </link>
+</robot>

urdf/test/test_robot_model_parser.cpp

@@ -52,6 +52,21 @@ class TestParser : public testing::Test
 public:
   Model robot;
 
+  bool checkModel()
+  {
+    // get root link
+    boost::shared_ptr<const Link> root_link=this->robot.getRoot();
+    if (!root_link)
+    {
+      ROS_ERROR("no root link %s",this->robot.getName().c_str());
+      return false;
+    }
+
+    // go through entire tree
+    return this->traverse_tree(root_link); 
+
+  };
+
 protected:
   /// constructor
   TestParser()
@@ -63,6 +78,36 @@ protected:
   ~TestParser()
   {
   }
+
+  bool traverse_tree(boost::shared_ptr<const Link> link,int level = 0)
+  {
+    level+=2;
+    int count = 0;
+    for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
+    {
+      if (*child)
+      {
+        // check rpy
+        double roll,pitch,yaw;
+        (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(roll,pitch,yaw);
+
+        if (isnan(roll) || isnan(pitch) || isnan(yaw))
+        {
+          ROS_ERROR("getRPY() returned nan!");
+          return false;
+        }
+        // recurse down the tree
+        return this->traverse_tree(*child,level);
+      }
+      else
+      {
+        ROS_ERROR("root link: %s has a null child!",link->name.c_str());
+        return false;
+      }
+    }
+    // no children
+    return true;
+  };
 };
 
 
@@ -71,15 +116,18 @@ protected:
 TEST_F(TestParser, test)
 {
   std::string folder = std::string(g_argv[1]) + "/test/";
-  printf("Folder %s",folder.c_str());
+  ROS_INFO("Folder %s",folder.c_str());
   for (int i=2; i<g_argc; i++){
     std::string file = g_argv[i];
     bool expect_success = (file.substr(0,5)  != "fail_");
-    ROS_ERROR("Parsing file %d/%d called %s, expecting %d",(i-1), g_argc-1, (folder + file).c_str(), expect_success);
+    ROS_INFO("Parsing file %d/%d called %s, expecting %d",(i-1), g_argc-1, (folder + file).c_str(), expect_success);
     if (!expect_success)
       ASSERT_FALSE(robot.initFile(folder + file));
     else
+    {
       ASSERT_TRUE(robot.initFile(folder + file));
+      ASSERT_TRUE(checkModel());
+    }
   }
 
   // test reading from parameter server

urdf/test/test_robot_model_parser.launch

@@ -1,5 +1,5 @@
 <launch>
   <param name="robot_description" textfile="$(find urdf)/test/pr2_desc.urdf" />
 
-  <test test-name="robot_model_parser_test" pkg="urdf" type="test_parser" args="$(find urdf) fail_pr2_desc_bracket.urdf fail_pr2_desc_double_joint.urdf fail_pr2_desc_double.urdf fail_pr2_desc_loop.urdf fail_pr2_desc_no_filename_in_mesh.urdf fail_pr2_desc_no_joint2.urdf fail_pr2_desc_parent_itself.urdf fail_pr2_desc_two_trees.urdf fail_three_links_one_joint.urdf no_visual.urdf one_link.urdf pr2_desc_explicit_world.urdf pr2_desc_no_joint.urdf pr2_desc.urdf two_links_one_joint.urdf" />
+  <test test-name="robot_model_parser_test" pkg="urdf" type="test_parser" args="$(find urdf) fail_pr2_desc_bracket.urdf fail_pr2_desc_double_joint.urdf fail_pr2_desc_double.urdf fail_pr2_desc_loop.urdf fail_pr2_desc_no_filename_in_mesh.urdf fail_pr2_desc_no_joint2.urdf fail_pr2_desc_parent_itself.urdf fail_pr2_desc_two_trees.urdf fail_three_links_one_joint.urdf no_visual.urdf one_link.urdf pr2_desc_explicit_world.urdf pr2_desc_no_joint.urdf pr2_desc.urdf two_links_one_joint.urdf singularity.urdf" />
 </launch>