diff --git a/moveit_core/robot_model/test/test.cpp b/moveit_core/robot_model/test/test.cpp index 84744f28c61..77cb22dc5e3 100644 --- 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 = + "" + "" + "" + " " + " " + " " + " " + " " + " " + "" + "" + " " + " " + " " + " " + "" + "" + " " + " " + " " + " " + " " + " " + "" + "" + " " + " " + " " + "" + "" + " " + " " + " " + " " + " " + " " + "" + " " + " " + " " + " " + " " + "" + " " + " " + " " + " " + " " + " " + "" + ""; + + static const std::string SMODEL2 = + "" + "" + "" + "" + "" + "" + "" + "" + ""; + 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);