usd_parser: support cone shape

Signed-off-by: Steve Peters <[email protected]>
gazebosim · Oct 1, 2024 · 0907d62 · 0907d62
1 parent e9131bd
commit 0907d62
Showing 1 changed file with 43 additions and 0 deletions.
diff --git a/src/usd_parser/USDLinks.cc b/src/usd_parser/USDLinks.cc
@@ -27,6 +27,7 @@
 #undef __DEPRECATED
 #include <pxr/usd/kind/registry.h>
 #include <pxr/usd/usd/modelAPI.h>
+#include <pxr/usd/usdGeom/cone.h>
 #include <pxr/usd/usdGeom/cube.h>
 #include <pxr/usd/usdGeom/cylinder.h>
 #include <pxr/usd/usdGeom/gprim.h>
@@ -50,6 +51,7 @@
 
 #include "sdf/Box.hh"
 #include "sdf/Collision.hh"
+#include "sdf/Cone.hh"
 #include "sdf/Cylinder.hh"
 #include "sdf/Geometry.hh"
 #include "sdf/Link.hh"
@@ -512,6 +514,33 @@ void ParseSphere(const pxr::UsdPrim &_prim,
   _geom.SetSphereShape(s);
 }
 
+//////////////////////////////////////////////////
+/// \brief Parse USD cone
+/// \param[in] _prim Reference to the USD prim
+/// \param[in] _geom sdf geom
+/// \param[in] _scale scale mesh
+/// \param[in] _metersPerUnit meter per unit of the stage
+void ParseCone(
+  const pxr::UsdPrim &_prim,
+  sdf::Geometry &_geom,
+  const math::Vector3d &_scale,
+  double _metersPerUnit)
+{
+  auto variant_cone = pxr::UsdGeomCone(_prim);
+  double radius;
+  double height;
+  variant_cone.GetRadiusAttr().Get(&radius);
+  variant_cone.GetHeightAttr().Get(&height);
+
+  sdf::Cone c;
+  _geom.SetType(sdf::GeometryType::CONE);
+
+  c.SetRadius(radius * _metersPerUnit * _scale.X());
+  c.SetLength(height * _metersPerUnit * _scale.Z());
+
+  _geom.SetConeShape(c);
+}
+
 //////////////////////////////////////////////////
 /// \brief Parse USD cylinder
 /// \param[in] _prim Reference to the USD prim
@@ -621,6 +650,7 @@ gz::usd::UsdErrors ParseUSDLinks(
 
   sdf::Geometry geom;
   if (_prim.IsA<pxr::UsdGeomSphere>() ||
+      _prim.IsA<pxr::UsdGeomCone>() ||
       _prim.IsA<pxr::UsdGeomCylinder>() ||
       _prim.IsA<pxr::UsdGeomCube>() ||
       _prim.IsA<pxr::UsdGeomMesh>() ||
@@ -657,6 +687,13 @@ gz::usd::UsdErrors ParseUSDLinks(
         vis.SetGeom(geom);
         _link->AddVisual(vis);
       }
+      else if (_prim.IsA<pxr::UsdGeomCone>())
+      {
+        ParseCone(_prim, geom, _scale, metersPerUnit);
+        vis.SetName("visual_cone");
+        vis.SetGeom(geom);
+        _link->AddVisual(vis);
+      }
       else if (_prim.IsA<pxr::UsdGeomCylinder>())
       {
         ParseCylinder(_prim, geom, _scale, metersPerUnit);
@@ -726,6 +763,12 @@ gz::usd::UsdErrors ParseUSDLinks(
         col.SetGeom(colGeom);
         col.SetRawPose(poseCol);
       }
+      else if (_prim.IsA<pxr::UsdGeomCone>())
+      {
+        ParseCone(_prim, colGeom, scaleCol, metersPerUnit);
+        col.SetGeom(colGeom);
+        col.SetRawPose(poseCol);
+      }
       else if (_prim.IsA<pxr::UsdGeomCylinder>())
       {
         ParseCylinder(_prim, colGeom, scaleCol, metersPerUnit);