diff --git a/urdf_parser/include/urdf_parser/urdf_parser.h b/urdf_parser/include/urdf_parser/urdf_parser.h index 70d5f543..918ba654 100644 --- a/urdf_parser/include/urdf_parser/urdf_parser.h +++ b/urdf_parser/include/urdf_parser/urdf_parser.h @@ -174,6 +174,7 @@ class URDFVersion final namespace urdf{ URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDF(const std::string &xml_string); URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDFFile(const std::string &path); + URDFDOM_DLLAPI std::string exportURDF(const ModelInterface &model); } #endif diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index e7cbdf18..23b9529e 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -584,6 +584,15 @@ bool exportCylinder(Cylinder &y, tinyxml2::XMLElement *xml) return true; } +bool exportCapsule(Capsule &c, tinyxml2::XMLElement *xml) +{ + tinyxml2::XMLElement *capsule_xml = xml->GetDocument()->NewElement("capsule"); + capsule_xml->SetAttribute("radius", urdf_export_helpers::values2str(c.radius).c_str()); + capsule_xml->SetAttribute("length", urdf_export_helpers::values2str(c.length).c_str()); + xml->LinkEndChild(capsule_xml); + return true; +} + bool exportMesh(Mesh &m, tinyxml2::XMLElement *xml) { // e.g. add @@ -614,6 +623,10 @@ bool exportGeometry(GeometrySharedPtr &geom, tinyxml2::XMLElement *xml) { exportMesh((*(urdf::dynamic_pointer_cast(geom).get())), geometry_xml); } + else if (urdf::dynamic_pointer_cast(geom)) + { + exportCapsule((*(urdf::dynamic_pointer_cast(geom).get())), geometry_xml); + } else { CONSOLE_BRIDGE_logError("geometry not specified, I'll make one up for you!"); diff --git a/urdf_parser/src/model.cpp b/urdf_parser/src/model.cpp index 2998827f..63d2b616 100644 --- a/urdf_parser/src/model.cpp +++ b/urdf_parser/src/model.cpp @@ -289,6 +289,7 @@ tinyxml2::XMLDocument* exportURDFInternal(const ModelInterface &model) tinyxml2::XMLElement* robot = doc->NewElement("robot"); robot->SetAttribute("name", model.name_.c_str()); + robot->SetAttribute("version", "1.2"); doc->LinkEndChild(robot); @@ -312,4 +313,13 @@ tinyxml2::XMLDocument* exportURDFInternal(const ModelInterface &model) return doc; } + +URDFDOM_DLLAPI std::string exportURDF(const ModelInterface &model) +{ + tinyxml2::XMLDocument *doc = exportURDFInternal(model); + tinyxml2::XMLPrinter printer; + doc->Print(&printer); + delete doc; + return printer.CStr(); +} } diff --git a/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp b/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp index b586a5d0..173b3be1 100644 --- a/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp +++ b/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include "urdf_model/link.h" #include "urdf_parser/urdf_parser.h" @@ -266,6 +267,51 @@ TEST(URDF_UNIT_TEST, parse_multiple_capsules_in_same_link) EXPECT_DOUBLE_EQ(2.0, collision_capsule_1->length); } +TEST(URDF_UNIT_TEST, export_capsule_geometry_round_trip) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr model = urdf::parseURDF(urdf_str); + ASSERT_NE(nullptr, model); + + std::string exported = urdf::exportURDF(*model); + ASSERT_FALSE(exported.empty()); + + urdf::ModelInterfaceSharedPtr reparsed = urdf::parseURDF(exported); + ASSERT_NE(nullptr, reparsed); + + urdf::LinkConstSharedPtr link = reparsed->getLink("link1"); + ASSERT_NE(nullptr, link); + + ASSERT_EQ(1u, link->visual_array.size()); + auto visual_capsule = std::dynamic_pointer_cast(link->visual_array[0]->geometry); + ASSERT_NE(nullptr, visual_capsule); + EXPECT_DOUBLE_EQ(0.05, visual_capsule->radius); + EXPECT_DOUBLE_EQ(0.5, visual_capsule->length); + + ASSERT_EQ(1u, link->collision_array.size()); + auto collision_capsule = std::dynamic_pointer_cast(link->collision_array[0]->geometry); + ASSERT_NE(nullptr, collision_capsule); + EXPECT_DOUBLE_EQ(0.1, collision_capsule->radius); + EXPECT_DOUBLE_EQ(1.0, collision_capsule->length); +} + int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv);