38 #include <urdf_parser/urdf_parser.h> 40 #include <gtest/gtest.h> 41 #include <boost/filesystem/path.hpp> 43 #include <moveit_resources/config.h> 50 boost::filesystem::path res_path(MOVEIT_TEST_RESOURCES_DIR);
54 std::fstream xml_file((res_path /
"pr2_description/urdf/robot.xml").
string().c_str(), std::fstream::in);
55 if (xml_file.is_open())
57 while (xml_file.good())
60 std::getline(xml_file, line);
61 xml_string += (line +
"\n");
90 const std::vector<const moveit::core::JointModel*>& joints =
robot_model->getJointModels();
91 for (std::size_t i = 0; i < joints.size(); ++i)
93 ASSERT_EQ(joints[i]->getJointIndex(), i);
94 ASSERT_EQ(
robot_model->getJointModel(joints[i]->getName()), joints[i]);
96 const std::vector<const moveit::core::LinkModel*>& links =
robot_model->getLinkModels();
97 for (std::size_t i = 0; i < links.size(); ++i)
99 ASSERT_EQ(links[i]->getLinkIndex(), i);
105 TEST(SiblingAssociateLinks, SimpleYRobot)
110 const std::string MODEL =
"<?xml version=\"1.0\" ?>" 111 "<robot name=\"one_robot\">" 112 "<link name=\"base_link\"/>" 113 "<joint name=\"joint_a\" type=\"continuous\">" 114 " <parent link=\"base_link\"/>" 115 " <child link=\"link_a\"/>" 116 " <axis xyz=\"0 0 1\"/>" 117 " <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>" 119 "<link name=\"link_a\"/>" 120 "<joint name=\"joint_b\" type=\"fixed\">" 121 " <parent link=\"link_a\"/>" 122 " <child link=\"link_b\"/>" 123 " <origin rpy=\" 0.0 -0.42 0 \" xyz=\"0.0 0.5 0 \"/>" 125 "<link name=\"link_b\"/>" 126 "<joint name=\"joint_c\" type=\"fixed\">" 127 " <parent link=\"link_b\"/>" 128 " <child link=\"link_c\"/>" 129 " <origin rpy=\" 0.0 0.42 0.0 \" xyz=\"0.0 0.5 0 \"/>" 131 "<link name=\"link_c\"/>" 132 "<joint name=\"joint_d\" type=\"fixed\">" 133 " <parent link=\"link_a\"/>" 134 " <child link=\"link_d\"/>" 135 " <origin rpy=\" 0.0 -0.42 0 \" xyz=\"0.0 0.5 0 \"/>" 137 "<link name=\"link_d\"/>" 138 "<joint name=\"joint_e\" type=\"continuous\">" 139 " <parent link=\"link_d\"/>" 140 " <child link=\"link_e\"/>" 141 " <axis xyz=\"0 0 1\"/>" 142 " <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>" 144 "<link name=\"link_e\"/>" 147 const std::string SMODEL =
148 "<?xml version=\"1.0\" ?>" 149 "<robot name=\"one_robot\">" 150 "<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom\" type=\"planar\"/>" 151 "<group name=\"base_joint\"><joint name=\"base_joint\"/></group>" 153 urdf::ModelInterfaceSharedPtr
urdf_model = urdf::parseURDF(MODEL);
155 srdf_model->initString(*urdf_model, SMODEL);
159 const std::string a =
"link_a", b =
"link_b", c =
"link_c",
d =
"link_d";
160 auto connected = { a, b, c,
d };
163 for (
const std::string& root : connected)
165 SCOPED_TRACE(
"root: " + root);
166 std::set<std::string> expected_set(connected);
167 expected_set.erase(root);
168 std::set<std::string> actual_set;
169 for (
const auto& item : robot_model->getLinkModel(root)->getAssociatedFixedTransforms())
170 actual_set.insert(item.first->getName());
172 std::ostringstream expected, actual;
173 std::copy(expected_set.begin(), expected_set.end(), std::ostream_iterator<std::string>(expected,
" "));
174 std::copy(actual_set.begin(), actual_set.end(), std::ostream_iterator<std::string>(actual,
" "));
180 int main(
int argc,
char** argv)
182 testing::InitGoogleTest(&argc, argv);
183 return RUN_ALL_TESTS();
Core components of MoveIt!
TEST(SiblingAssociateLinks, SimpleYRobot)
std::map< const LinkModel *, Eigen::Affine3d, std::less< const LinkModel * >, Eigen::aligned_allocator< std::pair< const LinkModel *const, Eigen::Affine3d > > > LinkTransformMap
Map from link model instances to Eigen transforms.
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
urdf::ModelInterfaceSharedPtr urdf_model
def xml_string(rootXml, addHeader=True)
int main(int argc, char **argv)
srdf::ModelSharedPtr srdf_model
TEST_F(LoadPlanningModelsPr2, InitOK)
moveit::core::RobotModelConstPtr robot_model