Merge pull request #116 from ros/fix_tests
Overhaul tests in urdf and kdl_parser
This commit is contained in:
Jackie Kay
commit
1e74e643e8
|
|
@ -63,8 +63,6 @@ protected:
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
TEST_F(TestParser, test)
|
TEST_F(TestParser, test)
|
||||||
{
|
{
|
||||||
for (int i=1; i<g_argc-2; i++){
|
for (int i=1; i<g_argc-2; i++){
|
||||||
|
|
@ -73,13 +71,13 @@ TEST_F(TestParser, test)
|
||||||
}
|
}
|
||||||
|
|
||||||
ASSERT_TRUE(treeFromFile(g_argv[g_argc-1], my_tree));
|
ASSERT_TRUE(treeFromFile(g_argv[g_argc-1], my_tree));
|
||||||
ASSERT_EQ(my_tree.getNrOfJoints(), (unsigned int)44);
|
ASSERT_EQ(my_tree.getNrOfJoints(), 8);
|
||||||
ASSERT_EQ(my_tree.getNrOfSegments(), (unsigned int)81);
|
ASSERT_EQ(my_tree.getNrOfSegments(), 16);
|
||||||
ASSERT_TRUE(my_tree.getSegment("base_footprint") == my_tree.getRootSegment());
|
ASSERT_TRUE(my_tree.getSegment("dummy_link") == my_tree.getRootSegment());
|
||||||
ASSERT_EQ(my_tree.getRootSegment()->second.children.size(), (unsigned int)1);
|
ASSERT_EQ(my_tree.getRootSegment()->second.children.size(), (unsigned int)1);
|
||||||
ASSERT_TRUE(my_tree.getSegment("base_link")->second.parent == my_tree.getRootSegment());
|
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_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], 15.6107, 0.001);
|
ASSERT_NEAR(my_tree.getSegment("base_link")->second.segment.getInertia().getRotationalInertia().data[0], 1.000, 0.001);
|
||||||
SUCCEED();
|
SUCCEED();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,6 +1,3 @@
|
||||||
<launch>
|
<launch>
|
||||||
<test test-name="test_kdl_parser" pkg="kdl_parser" type="test_kdl_parser" args="$(find kdl_parser)/test/pr2_desc_vector.xml \
|
<test test-name="test_kdl_parser" pkg="kdl_parser" type="test_kdl_parser" args="$(find kdl_parser)/test/test_robot.urdf"/>
|
||||||
$(find kdl_parser)/test/pr2_desc_bracket.xml \
|
|
||||||
$(find kdl_parser)/test/pr2_desc_bracket2.xml \
|
|
||||||
$(find kdl_parser)/test/pr2_desc.xml" />
|
|
||||||
</launch>
|
</launch>
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,425 @@
|
||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot name="r2d2">
|
||||||
|
|
||||||
|
<material name="blue">
|
||||||
|
<color rgba="0 0 .8 1"/>
|
||||||
|
</material>
|
||||||
|
<material name="black">
|
||||||
|
<color rgba="0 0 0 1"/>
|
||||||
|
</material>
|
||||||
|
<material name="white">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
|
||||||
|
<link name="dummy_link"/>
|
||||||
|
|
||||||
|
<link name="base_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.6" radius="0.2"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="blue"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.6" radius="0.2"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="dummy_to_base" type="fixed">
|
||||||
|
<parent link="dummy_link"/>
|
||||||
|
<child link="base_link"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="right_leg">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.6 .1 .2"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.6 .1 .2"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="base_to_right_leg" type="fixed">
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="right_leg"/>
|
||||||
|
<origin xyz="0 -0.22 .25"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="right_base">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.4 .1 .1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.4 .1 .1"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="right_base_joint" type="fixed">
|
||||||
|
<parent link="right_leg"/>
|
||||||
|
<child link="right_base"/>
|
||||||
|
<origin xyz="0 0 -0.6"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="right_front_wheel">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.1" radius="0.035"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.1" radius="0.035"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="1"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="right_front_wheel_joint" type="continuous">
|
||||||
|
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||||
|
<parent link="right_base"/>
|
||||||
|
<child link="right_front_wheel"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="right_back_wheel">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.1" radius="0.035"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.1" radius="0.035"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="1"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="right_back_wheel_joint" type="continuous">
|
||||||
|
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||||
|
<parent link="right_base"/>
|
||||||
|
<child link="right_back_wheel"/>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="left_leg">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.6 .1 .2"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.6 .1 .2"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="base_to_left_leg" type="fixed">
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="left_leg"/>
|
||||||
|
<origin xyz="0 0.22 .25"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="left_base">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.4 .1 .1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.4 .1 .1"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="left_base_joint" type="fixed">
|
||||||
|
<parent link="left_leg"/>
|
||||||
|
<child link="left_base"/>
|
||||||
|
<origin xyz="0 0 -0.6"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="left_front_wheel">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.1" radius="0.035"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.1" radius="0.035"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="1"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="left_front_wheel_joint" type="continuous">
|
||||||
|
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||||
|
<parent link="left_base"/>
|
||||||
|
<child link="left_front_wheel"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="left_back_wheel">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.1" radius="0.035"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.1" radius="0.035"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="1"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="left_back_wheel_joint" type="continuous">
|
||||||
|
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||||
|
<parent link="left_base"/>
|
||||||
|
<child link="left_back_wheel"/>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="gripper_extension" type="prismatic">
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="gripper_pole"/>
|
||||||
|
<limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0.19 0 .2"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="gripper_pole">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.2" radius=".01"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.2" radius=".01"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.05"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="left_gripper_joint" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0.2 0.01 0"/>
|
||||||
|
<parent link="gripper_pole"/>
|
||||||
|
<child link="left_gripper"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="left_gripper">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.05"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="left_tip_joint" type="fixed">
|
||||||
|
<parent link="left_gripper"/>
|
||||||
|
<child link="left_tip"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="left_tip">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.05"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="right_gripper_joint" type="revolute">
|
||||||
|
<axis xyz="0 0 -1"/>
|
||||||
|
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
|
||||||
|
<parent link="gripper_pole"/>
|
||||||
|
<child link="right_gripper"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="right_gripper">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.05"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="right_tip_joint" type="fixed">
|
||||||
|
<parent link="right_gripper"/>
|
||||||
|
<child link="right_tip"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="right_tip">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.05"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="head">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.2"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.2"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="2"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="head_swivel" type="continuous">
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="head"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<origin xyz="0 0 0.3"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="box">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<box size=".08 .08 .08"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="blue"/>
|
||||||
|
<origin xyz="-0.04 0 0"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<box size=".08 .08 .08"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="1"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="tobox" type="fixed">
|
||||||
|
<parent link="head"/>
|
||||||
|
<child link="box"/>
|
||||||
|
<origin xyz="0.1814 0 0.1414"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
</robot>
|
||||||
|
|
||||||
|
|
@ -183,8 +183,7 @@ bool Model::initString(const std::string& xml_string)
|
||||||
this->root_link_ = model->root_link_;
|
this->root_link_ = model->root_link_;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
else
|
return false;
|
||||||
return false;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,5 +1,5 @@
|
||||||
<?xml verison="1.0"?>
|
<?xml verison="1.0"?>
|
||||||
<robot name="no_visual">
|
<robot name="one_link">
|
||||||
<link name="link1">
|
<link name="link1">
|
||||||
<inertial>
|
<inertial>
|
||||||
<mass value="1"/>
|
<mass value="1"/>
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,425 @@
|
||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot name="r2d2">
|
||||||
|
|
||||||
|
<material name="blue">
|
||||||
|
<color rgba="0 0 .8 1"/>
|
||||||
|
</material>
|
||||||
|
<material name="black">
|
||||||
|
<color rgba="0 0 0 1"/>
|
||||||
|
</material>
|
||||||
|
<material name="white">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
|
||||||
|
<link name="dummy_link"/>
|
||||||
|
|
||||||
|
<link name="base_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.6" radius="0.2"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="blue"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.6" radius="0.2"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="dummy_to_base" type="fixed">
|
||||||
|
<parent link="dummy_link"/>
|
||||||
|
<child link="base_link"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="right_leg">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.6 .1 .2"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.6 .1 .2"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="base_to_right_leg" type="fixed">
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="right_leg"/>
|
||||||
|
<origin xyz="0 -0.22 .25"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="right_base">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.4 .1 .1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.4 .1 .1"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="right_base_joint" type="fixed">
|
||||||
|
<parent link="right_leg"/>
|
||||||
|
<child link="right_base"/>
|
||||||
|
<origin xyz="0 0 -0.6"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="right_front_wheel">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.1" radius="0.035"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.1" radius="0.035"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="1"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="right_front_wheel_joint" type="continuous">
|
||||||
|
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||||
|
<parent link="right_base"/>
|
||||||
|
<child link="right_front_wheel"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="right_back_wheel">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.1" radius="0.035"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.1" radius="0.035"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="1"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="right_back_wheel_joint" type="continuous">
|
||||||
|
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||||
|
<parent link="right_base"/>
|
||||||
|
<child link="right_back_wheel"/>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="left_leg">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.6 .1 .2"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.6 .1 .2"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="base_to_left_leg" type="fixed">
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="left_leg"/>
|
||||||
|
<origin xyz="0 0.22 .25"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="left_base">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.4 .1 .1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.4 .1 .1"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="10"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="left_base_joint" type="fixed">
|
||||||
|
<parent link="left_leg"/>
|
||||||
|
<child link="left_base"/>
|
||||||
|
<origin xyz="0 0 -0.6"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="left_front_wheel">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.1" radius="0.035"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.1" radius="0.035"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="1"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="left_front_wheel_joint" type="continuous">
|
||||||
|
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||||
|
<parent link="left_base"/>
|
||||||
|
<child link="left_front_wheel"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="left_back_wheel">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.1" radius="0.035"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.1" radius="0.035"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="1"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="left_back_wheel_joint" type="continuous">
|
||||||
|
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||||
|
<parent link="left_base"/>
|
||||||
|
<child link="left_back_wheel"/>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="gripper_extension" type="prismatic">
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="gripper_pole"/>
|
||||||
|
<limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0.19 0 .2"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="gripper_pole">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.2" radius=".01"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.2" radius=".01"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.05"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="left_gripper_joint" type="revolute">
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0.2 0.01 0"/>
|
||||||
|
<parent link="gripper_pole"/>
|
||||||
|
<child link="left_gripper"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="left_gripper">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.05"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="left_tip_joint" type="fixed">
|
||||||
|
<parent link="left_gripper"/>
|
||||||
|
<child link="left_tip"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="left_tip">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.05"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="right_gripper_joint" type="revolute">
|
||||||
|
<axis xyz="0 0 -1"/>
|
||||||
|
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
|
||||||
|
<parent link="gripper_pole"/>
|
||||||
|
<child link="right_gripper"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="right_gripper">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.05"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="right_tip_joint" type="fixed">
|
||||||
|
<parent link="right_gripper"/>
|
||||||
|
<child link="right_tip"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="right_tip">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.05"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="head">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.2"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.2"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="2"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="head_swivel" type="continuous">
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="head"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<origin xyz="0 0 0.3"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="box">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<box size=".08 .08 .08"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="blue"/>
|
||||||
|
<origin xyz="-0.04 0 0"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<box size=".08 .08 .08"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="1"/>
|
||||||
|
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="tobox" type="fixed">
|
||||||
|
<parent link="head"/>
|
||||||
|
<child link="box"/>
|
||||||
|
<origin xyz="0.1814 0 0.1414"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
</robot>
|
||||||
|
|
||||||
|
|
@ -50,26 +50,26 @@ char** g_argv;
|
||||||
class TestParser : public testing::Test
|
class TestParser : public testing::Test
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
Model robot;
|
|
||||||
|
|
||||||
bool checkModel()
|
bool checkModel(urdf::Model & robot)
|
||||||
{
|
{
|
||||||
// get root link
|
// get root link
|
||||||
boost::shared_ptr<const Link> root_link=this->robot.getRoot();
|
boost::shared_ptr<const Link> root_link = robot.getRoot();
|
||||||
if (!root_link)
|
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;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// go through entire tree
|
// go through entire tree
|
||||||
return this->traverse_tree(root_link);
|
return this->traverse_tree(root_link);
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
/// constructor
|
/// 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<const Link> link,int level = 0)
|
bool traverse_tree(boost::shared_ptr<const Link> link,int level = 0)
|
||||||
{
|
{
|
||||||
|
ROS_INFO("Traversing tree at level %d, link size %lu", level, link->child_links.size());
|
||||||
level+=2;
|
level+=2;
|
||||||
|
bool retval = true;
|
||||||
for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
|
for (std::vector<boost::shared_ptr<Link> >::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
|
// check rpy
|
||||||
double roll,pitch,yaw;
|
double roll,pitch,yaw;
|
||||||
(*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(roll,pitch,yaw);
|
(*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(roll,pitch,yaw);
|
||||||
|
|
@ -96,7 +100,7 @@ protected:
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
// recurse down the tree
|
// recurse down the tree
|
||||||
return this->traverse_tree(*child,level);
|
retval &= this->traverse_tree(*child,level);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
|
@ -104,9 +108,12 @@ protected:
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
// no children
|
// no more children
|
||||||
return true;
|
return retval;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
size_t num_joints;
|
||||||
|
size_t num_links;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -114,21 +121,37 @@ protected:
|
||||||
|
|
||||||
TEST_F(TestParser, test)
|
TEST_F(TestParser, test)
|
||||||
{
|
{
|
||||||
|
ASSERT_GE(g_argc, 3);
|
||||||
std::string folder = std::string(g_argv[1]) + "/test/";
|
std::string folder = std::string(g_argv[1]) + "/test/";
|
||||||
ROS_INFO("Folder %s",folder.c_str());
|
ROS_INFO("Folder %s",folder.c_str());
|
||||||
for (int i=2; i<g_argc; i++){
|
std::string file = std::string(g_argv[2]);
|
||||||
std::string file = g_argv[i];
|
bool expect_success = (file.substr(0,5) != "fail_");
|
||||||
bool expect_success = (file.substr(0,5) != "fail_");
|
urdf::Model robot;
|
||||||
ROS_INFO("Parsing file %d/%d called %s, expecting %d",(i-1), g_argc-1, (folder + file).c_str(), expect_success);
|
ROS_INFO("Parsing file %s, expecting %d",(folder + file).c_str(), expect_success);
|
||||||
if (!expect_success)
|
if (!expect_success) {
|
||||||
ASSERT_FALSE(robot.initFile(folder + file));
|
ASSERT_FALSE(robot.initFile(folder + file));
|
||||||
else
|
return;
|
||||||
{
|
|
||||||
ASSERT_TRUE(robot.initFile(folder + file));
|
|
||||||
ASSERT_TRUE(checkModel());
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
ASSERT_EQ(g_argc, 7);
|
||||||
|
std::string robot_name = std::string(g_argv[3]);
|
||||||
|
std::string root_name = std::string(g_argv[4]);
|
||||||
|
size_t expected_num_joints = atoi(g_argv[5]);
|
||||||
|
size_t expected_num_links = atoi(g_argv[6]);
|
||||||
|
|
||||||
|
ASSERT_TRUE(robot.initFile(folder + file));
|
||||||
|
|
||||||
|
EXPECT_EQ(robot.getName(), robot_name);
|
||||||
|
boost::shared_ptr<const urdf::Link> 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
|
// test reading from parameter server
|
||||||
ASSERT_TRUE(robot.initParam("robot_description"));
|
ASSERT_TRUE(robot.initParam("robot_description"));
|
||||||
ASSERT_FALSE(robot.initParam("robot_description_wim"));
|
ASSERT_FALSE(robot.initParam("robot_description_wim"));
|
||||||
|
|
|
||||||
|
|
@ -1,5 +1,19 @@
|
||||||
<launch>
|
<launch>
|
||||||
<param name="robot_description" textfile="$(find urdf)/test/pr2_desc.urdf" />
|
<param name="robot_description" textfile="$(find urdf)/test/test_robot.urdf" />
|
||||||
|
|
||||||
<test test-name="robot_model_parser_test" pkg="urdf" type="test_urdf_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_no_joint.urdf pr2_desc.urdf two_links_one_joint.urdf singularity.urdf" />
|
<!-- args: path, urdf file, robot name, root name, #joints, #links-->
|
||||||
|
<test test-name="robot_model_parser_test" pkg="urdf" type="test_urdf_parser" args="$(find urdf) test_robot.urdf r2d2 dummy_link 16 17" />
|
||||||
|
|
||||||
|
<test test-name="robot_model_parser_test_no_visual" pkg="urdf" type="test_urdf_parser" args="$(find urdf) no_visual.urdf no_visual link1 0 1" />
|
||||||
|
<test test-name="robot_model_parser_test_one_link" pkg="urdf" type="test_urdf_parser" args="$(find urdf) one_link.urdf one_link link1 0 1" />
|
||||||
|
<test test-name="robot_model_parser_test_two_links" pkg="urdf" type="test_urdf_parser" args="$(find urdf) two_links_one_joint.urdf two_links_one_joint link1 1 2" />
|
||||||
|
|
||||||
|
<!-- Cases expected not to parse correctly only get filename information (path, urdf file) -->
|
||||||
|
<test test-name="robot_model_parser_test_fail_bracket" pkg="urdf" type="test_urdf_parser" args="$(find urdf) fail_pr2_desc_bracket.urdf" />
|
||||||
|
<test test-name="robot_model_parser_test_fail_joint_mismatch" pkg="urdf" type="test_urdf_parser" args="$(find urdf) fail_three_links_one_joint.urdf" />
|
||||||
|
<test test-name="robot_model_parser_test_fail_double_joint" pkg="urdf" type="test_urdf_parser" args="$(find urdf) fail_pr2_desc_double_joint.urdf" />
|
||||||
|
<test test-name="robot_model_parser_test_fail_loop" pkg="urdf" type="test_urdf_parser" args="$(find urdf) fail_pr2_desc_loop.urdf" />
|
||||||
|
<test test-name="robot_model_parser_test_fail_no_filename" pkg="urdf" type="test_urdf_parser" args="$(find urdf) fail_pr2_desc_no_filename_in_mesh.urdf" />
|
||||||
|
<test test-name="robot_model_parser_test_fail_no_joint" pkg="urdf" type="test_urdf_parser" args="$(find urdf) fail_pr2_desc_no_joint2.urdf" />
|
||||||
|
<test test-name="robot_model_parser_test_fail_two_trees" pkg="urdf" type="test_urdf_parser" args="$(find urdf) fail_pr2_desc_two_trees.urdf" />
|
||||||
</launch>
|
</launch>
|
||||||
|
|
|
||||||
|
|
@ -1,5 +1,5 @@
|
||||||
<?xml verison="1.0"?>
|
<?xml verison="1.0"?>
|
||||||
<robot name="no_visual">
|
<robot name="two_links_one_joint">
|
||||||
<link name="link1">
|
<link name="link1">
|
||||||
<inertial>
|
<inertial>
|
||||||
<mass value="1"/>
|
<mass value="1"/>
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue