Added simple test for associated fixed transforms of siblings.

BryceStevenWilley · Sep 29, 2018 · 76366f1 · 76366f1
1 parent ebe2372
commit 76366f1
Showing 1 changed file with 79 additions and 0 deletions.
diff --git a/moveit_core/robot_model/test/test.cpp b/moveit_core/robot_model/test/test.cpp
@@ -102,6 +102,85 @@ TEST_F(LoadPlanningModelsPr2, Model)
   moveit::tools::Profiler::Status();
 }
 
+TEST(SiblingAssociateLinks, SimpleYRobot)
+{
+  static const std::string MODEL2 =
+        "<?xml version=\"1.0\" ?>"
+        "<robot name=\"one_robot\">"
+        "<link name=\"base_link\">"
+        "  <collision name=\"my_collision\">"
+        "    <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
+        "    <geometry>"
+        "      <box size=\"1 2 1\" />"
+        "    </geometry>"
+        "  </collision>"
+        "</link>"
+        "<joint name=\"joint_a\" type=\"continuous\">"
+        "   <axis xyz=\"0 0 1\"/>"
+        "   <parent link=\"base_link\"/>"
+        "   <child link=\"link_a\"/>"
+        "   <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>"
+        "</joint>"
+        "<link name=\"link_a\">"
+        "  <collision>"
+        "    <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
+        "    <geometry>"
+        "      <box size=\"1 2 1\" />"
+        "    </geometry>"
+        "  </collision>"
+        "</link>"
+        "<joint name=\"joint_b\" type=\"fixed\">"
+        "  <parent link=\"link_a\"/>"
+        "  <child link=\"link_b\"/>"
+        "  <origin rpy=\" 0.0 -0.42 0 \" xyz=\"0.0 0.5 0 \"/>"
+        "</joint>"
+        "<link name=\"link_b\">"
+        "  <collision>"
+        "    <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
+        "    <geometry>"
+        "      <box size=\"1 2 1\" />"
+        "    </geometry>"
+        "  </collision>"
+        "</link>"
+        "  <joint name=\"joint_c\" type=\"fixed\">"
+        "    <parent link=\"link_a\"/>"
+        "    <child link=\"link_c\"/>"
+        "    <origin rpy=\" 0.0 0.42 0.0 \" xyz=\"0.0 0.5 0 \"/>"
+        "  </joint>"
+        "<link name=\"link_c\">"
+        "  <collision>"
+        "    <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
+        "    <geometry>"
+        "      <box size=\"1 2 1\" />"
+        "    </geometry>"
+        "  </collision>"
+        "</link>"
+        "</robot>";
+
+    static const std::string SMODEL2 =
+        "<?xml version=\"1.0\" ?>"
+        "<robot name=\"one_robot\">"
+        "<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"planar\"/>"
+        "<group name=\"base_from_joints\">"
+        "<joint name=\"base_joint\"/>"
+        "<joint name=\"joint_a\"/>"
+        "<joint name=\"joint_c\"/>"
+        "</group>"
+        "</robot>";
+    urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(MODEL2);
+    srdf::ModelSharedPtr srdf_model(new srdf::Model());
+    srdf_model->initString(*urdf_model, SMODEL2);
+    moveit::core::RobotModelConstPtr robot_model;
+    robot_model.reset(new moveit::core::RobotModel(urdf_model, srdf_model));
+
+    const moveit::core::LinkTransformMap map = robot_model->getLinkModel("link_c")->getAssociatedFixedTransforms();
+    // Should contain both link_a and link_b.
+    ASSERT_EQ(map.size(), 2);
+    auto it = map.find(robot_model->getLinkModel("link_b"));
+    // If it correctly finds the sibling link, it will not equal the end iterator.
+    ASSERT_NE(it, map.end());
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);