Add python bindings and corresponding tests

include/sdf/Geometry.hh

@@ -241,11 +241,11 @@ namespace sdf
       double _density, sdf::ElementPtr _autoInertiaParams);
 
     /// \brief Calculate and return the AxisAlignedBox for the Geometry
-    /// @param _meshAabbCalculator The function to calculate the AABB of a mesh
-    /// @return std::optional with gz::math::AxisAlignedBox object or
+    /// \param _meshAabbCalculator The function to calculate the AABB of a mesh
+    /// \return std::optional with gz::math::AxisAlignedBox object or
     /// std::nullopt if the geometry type does not support AABB calculation
     public: std::optional<gz::math::AxisAlignedBox> AxisAlignedBox(
-      Mesh::AxisAlignedBoxCalculator _meshAabbCalculator) const;
+      const Mesh::AxisAlignedBoxCalculator &_meshAabbCalculator) const;
 
     /// \brief Get a pointer to the SDF element that was used during
     /// load.

include/sdf/Mesh.hh

@@ -211,12 +211,11 @@ namespace sdf
                               const ParserConfig &_config);
 
     /// \brief Get the Axis-aligned box for this Mesh.
-    /// \param[out] _errors A vector of Errors object. Each object
-    /// would contain an error code and an error message.
-    /// \param[in] _config Parser Configuration object.
+    /// \param[in] _meshAabbCalculator A custom function that calculates the
+    /// AxisAlignedBox for the Mesh.
     /// \return A gz::math::AxisAlignedBox object.
     public: std::optional<gz::math::AxisAlignedBox>
-            AxisAlignedBox(AxisAlignedBoxCalculator _meshAabbCalculator) const;
+            AxisAlignedBox(const AxisAlignedBoxCalculator &_meshAabbCalculator) const;
 
     /// \brief Get a pointer to the SDF element that was used during load.
     /// \return SDF element pointer. The value will be nullptr if Load has

python/src/sdf/pyBox.cc

@@ -45,6 +45,8 @@ void defineBox(pybind11::object module)
         "Get a mutable Gazebo Math representation of this Box.")
     .def("calculate_inertial", &sdf::Box::CalculateInertial,
          "Calculate and return the Inertial values for the Box.")
+    .def("axis_aligned_box", &sdf::Box::AxisAlignedBox,
+         "Get the axis-aligned box that contains this box.")
     .def("__copy__", [](const sdf::Box &self) {
       return sdf::Box(self);
     })

python/src/sdf/pyCapsule.cc

@@ -44,6 +44,8 @@ void defineCapsule(pybind11::object module)
          "Set the capsule's length in meters.")
     .def("calculate_inertial", &sdf::Capsule::CalculateInertial,
          "Calculate and return the Inertial values for the Capsule.")
+    .def("axis_aligned_box", &sdf::Capsule::AxisAlignedBox,
+         "Get the axis-aligned box that contains this Capsule.")
     .def(
         "shape",
         pybind11::overload_cast<>(&sdf::Capsule::Shape, pybind11::const_),

python/src/sdf/pyCone.cc

@@ -48,6 +48,8 @@ void defineCone(pybind11::object module)
         pybind11::overload_cast<>(&sdf::Cone::Shape, pybind11::const_),
         pybind11::return_value_policy::reference,
         "Get a mutable Gazebo Math representation of this Cone.")
+    .def("axis_aligned_box", &sdf::Cone::AxisAlignedBox,
+         "Get the axis-aligned box that contains this Cone.")
     .def("__copy__", [](const sdf::Cone &self) {
       return sdf::Cone(self);
     })

python/src/sdf/pyCylinder.cc

@@ -49,6 +49,8 @@ void defineCylinder(pybind11::object module)
         pybind11::overload_cast<>(&sdf::Cylinder::Shape, pybind11::const_),
         pybind11::return_value_policy::reference,
         "Get a mutable Gazebo Math representation of this Cylinder.")
+    .def("axis_aligned_box", &sdf::Cylinder::AxisAlignedBox,
+         "Get the axis-aligned box that contains this Cylinder.")
     .def("__copy__", [](const sdf::Cylinder &self) {
       return sdf::Cylinder(self);
     })

python/src/sdf/pyEllipsoid.cc

@@ -45,6 +45,8 @@ void defineEllipsoid(pybind11::object module)
         pybind11::overload_cast<>(&sdf::Ellipsoid::Shape, pybind11::const_),
         pybind11::return_value_policy::reference,
         "Get a mutable Gazebo Math representation of this Ellipsoid.")
+    .def("axis_aligned_box", &sdf::Ellipsoid::AxisAlignedBox,
+         "Get the axis-aligned box that contains this Ellipsoid.")
     .def("__copy__", [](const sdf::Ellipsoid &self) {
       return sdf::Ellipsoid(self);
     })

python/src/sdf/pyGeometry.cc

@@ -18,6 +18,7 @@
 
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
+#include <pybind11/functional.h>
 
 #include "sdf/ParserConfig.hh"
 
@@ -107,6 +108,8 @@ void defineGeometry(pybind11::object module)
     .def("heightmap_shape", &sdf::Geometry::HeightmapShape,
           pybind11::return_value_policy::reference,
           "Get the heightmap geometry.")
+    .def("axis_aligned_box", &sdf::Geometry::AxisAlignedBox,
+         "Get the axis-aligned box that contains the Geometry.")
     .def("__copy__", [](const sdf::Geometry &self) {
       return sdf::Geometry(self);
     })

python/src/sdf/pyMesh.cc

@@ -17,6 +17,8 @@
 #include "pyMesh.hh"
 
 #include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+#include <pybind11/functional.h>
 
 #include "sdf/ParserConfig.hh"
 #include "sdf/Mesh.hh"
@@ -78,6 +80,8 @@ void defineMesh(pybind11::object module)
     .def("set_center_submesh", &sdf::Mesh::SetCenterSubmesh,
          "Set whether the submesh should be centered. See CenterSubmesh() "
          "for more information.")
+    .def("axis_aligned_box", &sdf::Mesh::AxisAlignedBox,
+         "Get the axis-aligned box that contains this Mesh.")
     .def("__copy__", [](const sdf::Mesh &self) {
       return sdf::Mesh(self);
     })

python/src/sdf/pySphere.cc

@@ -45,6 +45,8 @@ void defineSphere(pybind11::object module)
         pybind11::overload_cast<>(&sdf::Sphere::Shape, pybind11::const_),
         pybind11::return_value_policy::reference,
         "Get a mutable Gazebo Math representation of this Sphere.")
+    .def("axis_aligned_box", &sdf::Sphere::AxisAlignedBox,
+         "Get the axis-aligned box that contains this Sphere.")
     .def("__copy__", [](const sdf::Sphere &self) {
       return sdf::Sphere(self);
     })

python/test/pyBox_TEST.py

@@ -12,7 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d
+from gz_test_deps.math import AxisAlignedBox, Inertiald, MassMatrix3d, Pose3d, Vector3d
 from gz_test_deps.sdformat import Box
 import unittest
 
@@ -89,5 +89,14 @@ def test_calculate_inertial(self):
       boxInertial.mass_matrix().mass())
     self.assertEqual(expectedInertial.pose(), boxInertial.pose())
 
+  def test_axis_aligned_box(self):
+    box = Box()
+    box.set_size(Vector3d(1, 2, 3))
+
+    self.assertEqual(
+      AxisAlignedBox(Vector3d(-0.5, -1, -1.5), Vector3d(0.5, 1, 1.5)),
+      box.axis_aligned_box())
+
+
 if __name__ == '__main__':
     unittest.main()

python/test/pyCapsule_TEST.py

@@ -16,7 +16,7 @@
 
 import math
 
-from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d
+from gz_test_deps.math import AxisAlignedBox, Inertiald, MassMatrix3d, Pose3d, Vector3d
 from gz_test_deps.sdformat import Capsule
 
 import unittest
@@ -107,6 +107,7 @@ def test_shape(self):
     self.assertEqual(0.123, capsule.radius())
     self.assertEqual(0.456, capsule.length())
 
+
   def test_calculate_inertial(self):
     capsule = Capsule()
 
@@ -145,5 +146,15 @@ def test_calculate_inertial(self):
       capsuleInertial.mass_matrix().mass())
     self.assertEqual(expectedInertial.pose(), capsuleInertial.pose())
 
+  def test_axis_aligned_box(self):
+    capsule = Capsule()
+    capsule.set_radius(0.5)
+    capsule.set_length(3.0)
+
+    self.assertEqual(
+      AxisAlignedBox(Vector3d(-0.5, -0.5, -1.5), Vector3d(0.5, 0.5, 1.5)),
+      capsule.axis_aligned_box())
+
+
 if __name__ == '__main__':
     unittest.main()

python/test/pyCone_TEST.py

@@ -18,6 +18,7 @@
 
 import math
 
+from gz_test_deps.math import AxisAlignedBox, Vector3d
 from gz_test_deps.sdformat import Cone
 
 import unittest
@@ -82,6 +83,7 @@ def test_copy_construction(self):
     self.assertEqual(0.2, cone2.radius())
     self.assertEqual(3.0, cone2.length())
 
+
   def test_deepcopy(self):
     cone = Cone();
     cone.set_radius(0.2)
@@ -109,6 +111,15 @@ def test_shape(self):
     self.assertEqual(0.123, cone.radius())
     self.assertEqual(0.456, cone.length())
 
+  def test_axis_aligned_box(self):
+    cone = Cone()
+    cone.set_radius(1.0)
+    cone.set_length(2.0)
+
+    self.assertEqual(
+      AxisAlignedBox(Vector3d(-1, -1, -1), Vector3d(1, 1, 1)),
+      cone.axis_aligned_box())
+
 
 if __name__ == '__main__':
     unittest.main()

python/test/pyCylinder_TEST.py

@@ -16,7 +16,7 @@
 
 import math
 
-from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d
+from gz_test_deps.math import AxisAlignedBox, Inertiald, MassMatrix3d, Pose3d, Vector3d
 from gz_test_deps.sdformat import Cylinder
 
 import unittest
@@ -147,5 +147,15 @@ def test_calculate_interial(self):
       cylinderInertial.mass_matrix().mass())
     self.assertEqual(expectedInertial.pose(), cylinderInertial.pose())
 
+  def test_axis_aligned_box(self):
+    cylinder = Cylinder()
+    cylinder.set_radius(0.8)
+    cylinder.set_length(1.8)
+
+    self.assertEqual(
+      AxisAlignedBox(Vector3d(-0.8, -0.8, -0.9), Vector3d(0.8, 0.8, 0.9)),
+      cylinder.axis_aligned_box())
+
+
 if __name__ == '__main__':
     unittest.main()

python/test/pyEllipsoid_TEST.py

@@ -13,7 +13,7 @@
 # limitations under the License.
 
 import copy
-from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d
+from gz_test_deps.math import AxisAlignedBox, Inertiald, MassMatrix3d, Pose3d, Vector3d
 import math
 from gz_test_deps.sdformat import Ellipsoid
 import unittest
@@ -77,6 +77,7 @@ def test_shape(self):
     ellipsoid.shape().set_radii(expectedradii)
     self.assertEqual(expectedradii, ellipsoid.radii())
 
+
   def test_calculate_inertial(self):
     ellipsoid = Ellipsoid()
 
@@ -116,5 +117,14 @@ def test_calculate_inertial(self):
       ellipsoidInertial.mass_matrix().mass())
     self.assertEqual(expectedInertial.pose(), ellipsoidInertial.pose())
 
+  def test_axis_aligned_box(self):
+    ellipsoid = Ellipsoid()
+    ellipsoid.set_radii(Vector3d(1.0, 2.0, 3.0))
+
+    self.assertEqual(
+      AxisAlignedBox(Vector3d(-1.0, -2.0, -3.0), Vector3d(1.0, 2.0, 3.0)),
+      ellipsoid.axis_aligned_box())
+
+
 if __name__ == '__main__':
     unittest.main()

python/test/pyGeometry_TEST.py

@@ -15,7 +15,7 @@
 import copy
 from gz_test_deps.sdformat import (Element, Error, Geometry, Box, Capsule, Cone, Cylinder, Ellipsoid,
                                    Heightmap, Mesh, ParserConfig, Plane, Sphere)
-from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d, Vector2d
+from gz_test_deps.math import AxisAlignedBox, Inertiald, MassMatrix3d, Pose3d, Vector3d, Vector2d
 import gz_test_deps.sdformat as sdf
 import math
 import unittest
@@ -388,5 +388,105 @@ def test_calculate_inertial(self):
     self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat)
     self.assertEqual(expectedInertial.pose(), sphereInertial.pose())
 
+  def test_axis_aligned_box(self):
+    geom = Geometry()
+
+    # Box
+    geom.set_type(sdf.GeometryType.BOX)
+    box = Box()
+    box.set_size(Vector3d(1, 2, 3))
+    geom.set_box_shape(box)
+
+    self.assertEqual(
+      AxisAlignedBox(Vector3d(-0.5, -1, -1.5), Vector3d(0.5, 1, 1.5)),
+      geom.axis_aligned_box(None))
+
+    # Capsule
+    geom.set_type(sdf.GeometryType.CAPSULE)
+    capsule = Capsule()
+    capsule.set_radius(0.5)
+    capsule.set_length(3.0)
+    geom.set_capsule_shape(capsule)
+
+    self.assertEqual(
+      AxisAlignedBox(Vector3d(-0.5, -0.5, -1.5), Vector3d(0.5, 0.5, 1.5)),
+      geom.axis_aligned_box(None))
+
+    # Cone
+    geom.set_type(sdf.GeometryType.CONE)
+    cone = Cone()
+    cone.set_radius(0.5)
+    cone.set_length(3.0)
+    geom.set_cone_shape(cone)
+
+    self.assertEqual(
+      AxisAlignedBox(Vector3d(-0.5, -0.5, -1.5), Vector3d(0.5, 0.5, 1.5)),
+      geom.axis_aligned_box(None))
+
+    # Cylinder
+    geom.set_type(sdf.GeometryType.CYLINDER)
+    cylinder = Cylinder()
+    cylinder.set_radius(0.5)
+    cylinder.set_length(3.0)
+    geom.set_cylinder_shape(cylinder)
+
+    self.assertEqual(
+      AxisAlignedBox(Vector3d(-0.5, -0.5, -1.5), Vector3d(0.5, 0.5, 1.5)),
+      geom.axis_aligned_box(None))
+
+    # Ellipsoid
+    geom.set_type(sdf.GeometryType.ELLIPSOID)
+    ellipsoid = Ellipsoid()
+    ellipsoid.set_radii(Vector3d(1, 2, 3))
+    geom.set_ellipsoid_shape(ellipsoid)
+
+    self.assertEqual(
+      AxisAlignedBox(Vector3d(-1, -2, -3), Vector3d(1, 2, 3)),
+      geom.axis_aligned_box(None))
+
+    # Sphere
+    geom.set_type(sdf.GeometryType.SPHERE)
+    sphere = Sphere()
+    sphere.set_radius(0.5)
+    geom.set_sphere_shape(sphere)
+
+    self.assertEqual(
+      AxisAlignedBox(Vector3d(-0.5, -0.5, -0.5), Vector3d(0.5, 0.5, 0.5)),
+      geom.axis_aligned_box(None))
+
+    # Mesh
+    geom.set_type(sdf.GeometryType.MESH)
+    mesh = Mesh()
+    geom.set_mesh_shape(mesh)
+
+    self.assertEqual(None, geom.axis_aligned_box(None))
+
+    def mesh_aabb_calulator(sdf_mesh: Mesh) -> AxisAlignedBox:
+      return AxisAlignedBox(Vector3d(-1, -1, -1), Vector3d(1, 1, 1))
+
+    self.assertEqual(
+      AxisAlignedBox(Vector3d(-1, -1, -1), Vector3d(1, 1, 1)),
+      geom.axis_aligned_box(mesh_aabb_calulator))
+
+    # Plane - Non volumetric geometry (no AABB)
+    geom.set_type(sdf.GeometryType.PLANE)
+    plane = Plane()
+    geom.set_plane_shape(plane)
+
+    self.assertEqual(None, geom.axis_aligned_box(None))
+
+    # Heightmap - Non volumetric geometry (no AABB)
+    geom.set_type(sdf.GeometryType.HEIGHTMAP)
+    heightmap = Heightmap()
+    geom.set_heightmap_shape(heightmap)
+
+    self.assertEqual(None, geom.axis_aligned_box(None))
+
+    # Empty - Non volumetric geometry (no AABB)
+    geom.set_type(sdf.GeometryType.EMPTY)
+
+    self.assertEqual(None, geom.axis_aligned_box(None))
+
+
 if __name__ == '__main__':
     unittest.main()