Added Cone tests and pybinds.

Signed-off-by: Benjamin Perseghetti <[email protected]>

include/sdf/Visual.hh

@@ -22,6 +22,7 @@
 #include <gz/math/Pose3.hh>
 #include <gz/utils/ImplPtr.hh>
 #include "sdf/Box.hh"
+#include "sdf/Cone.hh"
 #include "sdf/Cylinder.hh"
 #include "sdf/Element.hh"
 #include "sdf/Material.hh"

python/CMakeLists.txt

@@ -36,6 +36,7 @@ pybind11_add_module(${BINDINGS_MODULE_NAME} MODULE
   src/sdf/pyCamera.cc
   src/sdf/pyCapsule.cc
   src/sdf/pyCollision.cc
+  src/sdf/pyCone.cc
   src/sdf/pyConvexDecomposition.cc
   src/sdf/pyCylinder.cc
   src/sdf/pyElement.cc
@@ -119,6 +120,7 @@ if (BUILD_TESTING AND NOT WIN32)
     pyCamera_TEST
     pyCapsule_TEST
     pyCollision_TEST
+    pyCone_TEST
     pyCylinder_TEST
     pyElement_TEST
     pyEllipsoid_TEST

python/src/sdf/_gz_sdformat_pybind11.cc

@@ -26,6 +26,7 @@
 #include "pyCamera.hh"
 #include "pyCapsule.hh"
 #include "pyCollision.hh"
+#include "pyCone.hh"
 #include "pyConvexDecomposition.hh"
 #include "pyCylinder.hh"
 #include "pyElement.hh"
@@ -86,6 +87,7 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) {
   sdf::python::defineCamera(m);
   sdf::python::defineCapsule(m);
   sdf::python::defineCollision(m);
+  sdf::python::defineCone(m);
   sdf::python::defineConvexDecomposition(m);
   sdf::python::defineContact(m);
   sdf::python::defineCylinder(m);

python/src/sdf/pyCone.cc

@@ -0,0 +1,60 @@
+/*
+ * Copyright 2024 CogniPilot Foundation
+ * Copyright 2024 Open Source Robotics Foundation
+ * Copyright 2024 Rudis Laboratories
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "pyCone.hh"
+
+#include <pybind11/pybind11.h>
+
+#include "sdf/Cone.hh"
+
+using namespace pybind11::literals;
+
+namespace sdf
+{
+// Inline bracket to help doxygen filtering.
+inline namespace SDF_VERSION_NAMESPACE {
+namespace python
+{
+/////////////////////////////////////////////////
+void defineCone(pybind11::object module)
+{
+  pybind11::class_<sdf::Cone>(module, "Cone")
+    .def(pybind11::init<>())
+    .def(pybind11::init<sdf::Cone>())
+    .def("radius", &sdf::Cone::Radius,
+         "Get the cone's radius in meters.")
+    .def("set_radius", &sdf::Cone::SetRadius,
+         "Set the cone's radius in meters.")
+    .def("length", &sdf::Cone::Length,
+         "Get the cone's length in meters.")
+    .def("set_length", &sdf::Cone::SetLength,
+         "Set the cone's length in meters.")
+    .def(
+        "shape",
+        pybind11::overload_cast<>(&sdf::Cone::Shape, pybind11::const_),
+        pybind11::return_value_policy::reference,
+        "Get a mutable Gazebo Math representation of this Cone.")
+    .def("__copy__", [](const sdf::Cone &self) {
+      return sdf::Cone(self);
+    })
+    .def("__deepcopy__", [](const sdf::Cone &self, pybind11::dict) {
+      return sdf::Cone(self);
+    }, "memo"_a);
+}
+}  // namespace python
+}  // namespace SDF_VERSION_NAMESPACE
+}  // namespace sdf

python/src/sdf/pyCone.hh

@@ -0,0 +1,43 @@
+/*
+ * Copyright 2024 CogniPilot Foundation
+ * Copyright 2024 Open Source Robotics Foundation
+ * Copyright 2024 Rudis Laboratories
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef SDFORMAT_PYTHON_CONE_HH_
+#define SDFORMAT_PYTHON_CONE_HH_
+
+#include <pybind11/pybind11.h>
+
+#include "sdf/Cone.hh"
+
+#include "sdf/config.hh"
+
+namespace sdf
+{
+// Inline bracket to help doxygen filtering.
+inline namespace SDF_VERSION_NAMESPACE {
+namespace python
+{
+/// Define a pybind11 wrapper for an sdf::Cone
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ */
+void defineCone(pybind11::object module);
+}  // namespace python
+}  // namespace SDF_VERSION_NAMESPACE
+}  // namespace sdf
+
+#endif  // SDFORMAT_PYTHON_CONE_HH_

python/src/sdf/pyGeometry.cc

@@ -22,6 +22,7 @@
 
 #include "sdf/Box.hh"
 #include "sdf/Capsule.hh"
+#include "sdf/Cone.hh"
 #include "sdf/Cylinder.hh"
 #include "sdf/Ellipsoid.hh"
 #include "sdf/Geometry.hh"
@@ -61,6 +62,12 @@ void defineGeometry(pybind11::object module)
          "geometry is not a capsule.")
     .def("set_capsule_shape", &sdf::Geometry::SetCapsuleShape,
          "Set the capsule shape.")
+    .def("cone_shape", &sdf::Geometry::ConeShape,
+         pybind11::return_value_policy::reference,
+         "Get the cone geometry, or None if the contained "
+         "geometry is not a cone.")
+    .def("set_cone_shape", &sdf::Geometry::SetConeShape,
+         "Set the cone shape.")
     .def("cylinder_shape", &sdf::Geometry::CylinderShape,
          pybind11::return_value_policy::reference,
          "Get the cylinder geometry, or None if the contained "
@@ -101,6 +108,7 @@ void defineGeometry(pybind11::object module)
     pybind11::enum_<sdf::GeometryType>(module, "GeometryType")
       .value("EMPTY", sdf::GeometryType::EMPTY)
       .value("BOX", sdf::GeometryType::BOX)
+      .value("CONE", sdf::GeometryType::CONE)
       .value("CYLINDER", sdf::GeometryType::CYLINDER)
       .value("PLANE", sdf::GeometryType::PLANE)
       .value("SPHERE", sdf::GeometryType::SPHERE)

python/src/sdf/pyParticleEmitter.cc

@@ -154,6 +154,7 @@ void defineParticleEmitter(pybind11::object module)
     pybind11::enum_<sdf::ParticleEmitterType>(particleEmitterModule, "ParticleEmitterType")
       .value("POINT", sdf::ParticleEmitterType::POINT)
       .value("BOX", sdf::ParticleEmitterType::BOX)
+      .value("CONE", sdf::ParticleEmitterType::CONE)
       .value("CYLINDER", sdf::ParticleEmitterType::CYLINDER)
       .value("ELLIPSOID", sdf::ParticleEmitterType::ELLIPSOID);
 }

python/test/pyCollision_TEST.py

@@ -14,7 +14,7 @@
 
 import copy
 from gz_test_deps.math import Pose3d
-from gz_test_deps.sdformat import (Box, Collision, Contact, Cylinder, Error,
+from gz_test_deps.sdformat import (Box, Collision, Cone, Contact, Cylinder, Error,
                                    Geometry, Plane, Surface, Sphere,
                                    SDFErrorsException)
 import gz_test_deps.sdformat as sdf
@@ -57,6 +57,7 @@ def test_default_construction(self):
         self.assertNotEqual(None, collision.geometry())
         self.assertEqual(sdf.GeometryType.EMPTY, collision.geometry().type())
         self.assertEqual(None, collision.geometry().box_shape())
+        self.assertEqual(None, collision.geometry().cone_shape())
         self.assertEqual(None, collision.geometry().cylinder_shape())
         self.assertEqual(None, collision.geometry().plane_shape())
         self.assertEqual(None, collision.geometry().sphere_shape())

python/test/pyCone_TEST.py

@@ -0,0 +1,114 @@
+# Copyright 2024 CogniPilot Foundation
+# Copyright 2024 Open Source Robotics Foundation
+# Copyright 2024 Rudis Laboratories
+
+# Licensed under the Apache License, Version 2.0 (the "License")
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+
+#       http://www.apache.org/licenses/LICENSE-2.0
+
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import copy
+
+import math
+
+from gz_test_deps.sdformat import Cone
+
+import unittest
+
+
+class ConeTEST(unittest.TestCase):
+
+  def test_default_construction(self):
+    cone = Cone()
+
+    self.assertEqual(math.pi * math.pow(0.5, 2) * 1.0,
+                     cone.shape().volume())
+
+    self.assertEqual(0.5, cone.radius())
+    self.assertEqual(1.0, cone.length())
+
+    cone.set_radius(0.5)
+    cone.set_length(2.3)
+
+    self.assertEqual(0.5, cone.radius())
+    self.assertEqual(2.3, cone.length())
+
+  def test_assignment(self):
+    cone = Cone()
+    cone.set_radius(0.2)
+    cone.set_length(3.0)
+    self.assertEqual(math.pi * math.pow(0.2, 2) * 3.0 / 3.0,
+                   cone.shape().volume())
+
+    cone2 = cone
+    self.assertEqual(0.2, cone2.radius())
+    self.assertEqual(3.0, cone2.length())
+
+    self.assertEqual(math.pi * math.pow(0.2, 2) * 3.0 / 3.0,
+                   cone2.shape().volume())
+    self.assertEqual(0.2, cone2.shape().radius())
+    self.assertEqual(3.0, cone2.shape().length())
+
+    cone.set_radius(2.0)
+    cone.set_length(0.3)
+
+    self.assertEqual(2.0, cone.radius())
+    self.assertEqual(0.3, cone.length())
+    self.assertEqual(2.0, cone2.radius())
+    self.assertEqual(0.3, cone2.length())
+
+
+  def test_copy_construction(self):
+    cone = Cone();
+    cone.set_radius(0.2)
+    cone.set_length(3.0)
+
+    cone2 = Cone(cone)
+    self.assertEqual(0.2, cone2.radius())
+    self.assertEqual(3.0, cone2.length())
+
+    cone.set_radius(2.)
+    cone.set_length(0.3)
+
+    self.assertEqual(2, cone.radius())
+    self.assertEqual(0.3, cone.length())
+    self.assertEqual(0.2, cone2.radius())
+    self.assertEqual(3.0, cone2.length())
+
+  def test_deepcopy(self):
+    cone = Cone();
+    cone.set_radius(0.2)
+    cone.set_length(3.0)
+
+    cone2 = copy.deepcopy(cone);
+    self.assertEqual(0.2, cone2.radius())
+    self.assertEqual(3.0, cone2.length())
+
+    cone.set_radius(2.)
+    cone.set_length(0.3)
+
+    self.assertEqual(2, cone.radius())
+    self.assertEqual(0.3, cone.length())
+    self.assertEqual(0.2, cone2.radius())
+    self.assertEqual(3.0, cone2.length())
+
+  def test_shape(self):
+    cone = Cone();
+    self.assertEqual(0.5, cone.radius())
+    self.assertEqual(1.0, cone.length())
+
+    cone.shape().set_radius(0.123)
+    cone.shape().set_length(0.456)
+    self.assertEqual(0.123, cone.radius())
+    self.assertEqual(0.456, cone.length())
+
+
+if __name__ == '__main__':
+    unittest.main()

python/test/pyGeometry_TEST.py

@@ -13,7 +13,7 @@
 # limitations under the License.
 
 import copy
-from gz_test_deps.sdformat import (Geometry, Box, Capsule, Cylinder, Ellipsoid,
+from gz_test_deps.sdformat import (Geometry, Box, Capsule, Cone, Cylinder, Ellipsoid,
                                    Mesh, Plane, Sphere)
 from gz_test_deps.math import Vector3d, Vector2d
 import gz_test_deps.sdformat as sdf
@@ -32,6 +32,9 @@ def test_default_construction(self):
     geom.set_type(sdf.GeometryType.CAPSULE)
     self.assertEqual(sdf.GeometryType.CAPSULE, geom.type())
 
+    geom.set_type(sdf.GeometryType.CONE)
+    self.assertEqual(sdf.GeometryType.CONE, geom.type())
+
     geom.set_type(sdf.GeometryType.CYLINDER)
     self.assertEqual(sdf.GeometryType.CYLINDER, geom.type())
 
@@ -121,6 +124,19 @@ def test_capsule(self):
     self.assertEqual(0.123, geom.capsule_shape().radius())
     self.assertEqual(4.56, geom.capsule_shape().length())
 
+  def test_cone(self):
+    geom = Geometry()
+    geom.set_type(sdf.GeometryType.CONE)
+
+    coneShape = Cone()
+    coneShape.set_radius(0.123)
+    coneShape.set_length(4.56)
+    geom.set_cone_shape(coneShape)
+
+    self.assertEqual(sdf.GeometryType.CONE, geom.type())
+    self.assertNotEqual(None, geom.cone_shape())
+    self.assertEqual(0.123, geom.cone_shape().radius())
+    self.assertEqual(4.56, geom.cone_shape().length())
 
   def test_cylinder(self):
     geom = Geometry()

python/test/pyParticleEmitter_TEST.py

@@ -34,6 +34,8 @@ def test_default_construction(self):
         self.assertTrue(emitter.set_type("box"))
         self.assertEqual("box", emitter.type_str())
         self.assertEqual(ParticleEmitter.ParticleEmitterType.BOX, emitter.type())
+        emitter.set_type(ParticleEmitter.ParticleEmitterType.CONE)
+        self.assertEqual("cone", emitter.type_str())
         emitter.set_type(ParticleEmitter.ParticleEmitterType.CYLINDER)
         self.assertEqual("cylinder", emitter.type_str())
 

python/test/pyVisual_TEST.py

@@ -65,6 +65,7 @@ def test_default_construction(self):
         self.assertNotEqual(None, visual.geometry())
         self.assertEqual(sdf.GeometryType.EMPTY, visual.geometry().type())
         self.assertEqual(None, visual.geometry().box_shape())
+        self.assertEqual(None, visual.geometry().cone_shape())
         self.assertEqual(None, visual.geometry().cylinder_shape())
         self.assertEqual(None, visual.geometry().plane_shape())
         self.assertEqual(None, visual.geometry().sphere_shape())

src/Collision_TEST.cc

@@ -84,6 +84,7 @@ TEST(DOMcollision, Construction)
   ASSERT_NE(nullptr, collision.Geom());
   EXPECT_EQ(sdf::GeometryType::EMPTY, collision.Geom()->Type());
   EXPECT_EQ(nullptr, collision.Geom()->BoxShape());
+  EXPECT_EQ(nullptr, collision.Geom()->ConeShape());
   EXPECT_EQ(nullptr, collision.Geom()->CylinderShape());
   EXPECT_EQ(nullptr, collision.Geom()->PlaneShape());
   EXPECT_EQ(nullptr, collision.Geom()->SphereShape());