bool exportGeometry(GeometryPtr &geom, TiXmlElement *xml) { TiXmlElement *geometry_xml = new TiXmlElement("geometry"); if( geom->type == Geometry::SPHERE ) { exportSphere(*((Sphere*)toPlainGeometryPtr(geom)), geometry_xml); } else if ( geom->type == Geometry::BOX ) { exportBox(*((Box*)toPlainGeometryPtr(geom)), geometry_xml); } else if (geom->type == Geometry::CYLINDER) { exportCylinder(*((Cylinder*)toPlainGeometryPtr(geom)), geometry_xml); } else if (geom->type == Geometry::MESH) { exportMesh(*((Mesh*)toPlainGeometryPtr(geom)), geometry_xml); } else { logError("geometry not specified, I'll make one up for you!"); Sphere *s = new Sphere(); s->radius = 0.03; resetPtr(geom,s); exportSphere(*s, geometry_xml); } xml->LinkEndChild(geometry_xml); return true; }
bool exportGeometry(GeometrySharedPtr &geom, TiXmlElement *xml) { TiXmlElement *geometry_xml = new TiXmlElement("geometry"); if (urdf::dynamic_pointer_cast<Sphere>(geom)) { exportSphere((*(urdf::dynamic_pointer_cast<Sphere>(geom).get())), geometry_xml); } else if (urdf::dynamic_pointer_cast<Box>(geom)) { exportBox((*(urdf::dynamic_pointer_cast<Box>(geom).get())), geometry_xml); } else if (urdf::dynamic_pointer_cast<Cylinder>(geom)) { exportCylinder((*(urdf::dynamic_pointer_cast<Cylinder>(geom).get())), geometry_xml); } else if (urdf::dynamic_pointer_cast<Mesh>(geom)) { exportMesh((*(urdf::dynamic_pointer_cast<Mesh>(geom).get())), geometry_xml); } else { CONSOLE_BRIDGE_logError("geometry not specified, I'll make one up for you!"); Sphere *s = new Sphere(); s->radius = 0.03; geom.reset(s); exportSphere((*(urdf::dynamic_pointer_cast<Sphere>(geom).get())), geometry_xml); } xml->LinkEndChild(geometry_xml); return true; }