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);