Added SphericalCoordinates pybind11 interface (#357)

* Added SphericalCoordinates pybind11 interface

Signed-off-by: Alejandro Hernández <[email protected]>

src/python/CMakeLists.txt

@@ -88,7 +88,6 @@ if (PYTHONLIBS_FOUND)
     # Add the Python tests
     set(python_tests
       python_TEST
-      SphericalCoordinates_TEST
     )
 
     foreach (test ${python_tests})

src/python_pybind11/CMakeLists.txt

@@ -29,6 +29,7 @@ if (${pybind11_FOUND})
     src/RotationSpline.cc
     src/SemanticVersion.cc
     src/SignalStats.cc
+    src/SphericalCoordinates.cc
     src/Spline.cc
     src/StopWatch.cc
     src/Temperature.cc
@@ -115,6 +116,7 @@ if (${pybind11_FOUND})
       SemanticVersion_TEST
       SignalStats_TEST
       Sphere_TEST
+      SphericalCoordinates_TEST
       Spline_TEST
       StopWatch_TEST
       Temperature_TEST

src/python_pybind11/src/SphericalCoordinates.cc

@@ -0,0 +1,132 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * 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 <pybind11/operators.h>
+
+#include "SphericalCoordinates.hh"
+#include <ignition/math/Angle.hh>
+#include <ignition/math/SphericalCoordinates.hh>
+
+namespace ignition
+{
+namespace math
+{
+namespace python
+{
+void defineMathSphericalCoordinates(py::module &m, const std::string &typestr)
+{
+  using Class = ignition::math::SphericalCoordinates;
+  std::string pyclass_name = typestr;
+
+  py::class_<Class> sphericalCoordinates(m,
+                    pyclass_name.c_str(),
+                    py::buffer_protocol(),
+                    py::dynamic_attr());
+  sphericalCoordinates
+    .def(py::init<>())
+    .def(py::init<const Class&>())
+    .def(py::init<const Class::SurfaceType>())
+    .def(py::init<const Class::SurfaceType, const ignition::math::Angle &,
+                  const ignition::math::Angle &, const double,
+                  const ignition::math::Angle &>())
+    .def(py::self != py::self)
+    .def(py::self == py::self)
+    .def("spherical_from_local_position",
+         &Class::SphericalFromLocalPosition,
+         "Convert a Cartesian position vector to geodetic coordinates.")
+    .def("global_from_local_velocity",
+         &Class::GlobalFromLocalVelocity,
+         "Convert a Cartesian velocity vector in the local frame "
+         " to a global Cartesian frame with components East, North, Up")
+    .def("convert",
+         py::overload_cast<const std::string &>(&Class::Convert),
+         "Convert a string to a SurfaceType.")
+    .def("convert",
+         py::overload_cast<Class::SurfaceType>(&Class::Convert),
+         "Convert a SurfaceType to a string.")
+    .def("distance",
+         &Class::Distance,
+         "Get the distance between two points expressed in geographic "
+         "latitude and longitude. It assumes that both points are at sea level."
+         " Example: _latA = 38.0016667 and _lonA = -123.0016667) represents "
+         "the point with latitude 38d 0'6.00\"N and longitude 123d 0'6.00\"W.")
+    .def("surface",
+         &Class::Surface,
+         "Get SurfaceType currently in use.")
+    .def("latitude_reference",
+         &Class::LatitudeReference,
+         "Get reference geodetic latitude.")
+    .def("longitude_reference",
+         &Class::LongitudeReference,
+         "Get reference longitude.")
+    .def("elevation_reference",
+         &Class::ElevationReference,
+         "Get reference elevation in meters.")
+    .def("heading_offset",
+         &Class::HeadingOffset,
+         "Get heading offset for the reference frame, expressed as "
+         "angle from East to x-axis, or equivalently "
+         "from North to y-axis.")
+    .def("set_surface",
+         &Class::SetSurface,
+         "Set SurfaceType for planetary surface model.")
+    .def("set_latitude_reference",
+         &Class::SetLatitudeReference,
+         "Set reference geodetic latitude.")
+    .def("set_longitude_reference",
+         &Class::SetLongitudeReference,
+         "Set reference longitude.")
+    .def("set_elevation_reference",
+         &Class::SetElevationReference,
+         "Set reference elevation above sea level in meters.")
+    .def("set_heading_offset",
+         &Class::SetHeadingOffset,
+         "Set heading angle offset for the frame.")
+    .def("local_from_spherical_position",
+         &Class::LocalFromSphericalPosition,
+         "Convert a geodetic position vector to Cartesian coordinates.")
+    .def("local_from_global_velocity",
+         &Class::LocalFromGlobalVelocity,
+         "Convert a Cartesian velocity vector with components East, "
+         "North, Up to a local cartesian frame vector XYZ.")
+    .def("update_transformation_matrix",
+         &Class::UpdateTransformationMatrix,
+         "Update coordinate transformation matrix with reference location")
+    .def("position_transform",
+         &Class::PositionTransform,
+         "Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame "
+         "Spherical coordinates use radians, while the other frames use "
+         "meters.")
+    .def("velocity_transform",
+         &Class::VelocityTransform,
+         "Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame "
+         "Spherical coordinates use radians, while the other frames use "
+         "meters.");
+
+   py::enum_<Class::CoordinateType>(sphericalCoordinates, "CoordinateType")
+       .value("SPHERICAL", Class::CoordinateType::SPHERICAL)
+       .value("ECEF", Class::CoordinateType::ECEF)
+       .value("GLOBAL", Class::CoordinateType::GLOBAL)
+       .value("LOCAL", Class::CoordinateType::LOCAL)
+       .value("LOCAL2", Class::CoordinateType::LOCAL2)
+       .export_values();
+   py::enum_<Class::SurfaceType>(sphericalCoordinates, "SurfaceType")
+       .value("EARTH_WGS84", Class::SurfaceType::EARTH_WGS84)
+       .export_values();
+}
+}  // namespace python
+}  // namespace math
+}  // namespace ignition

src/python_pybind11/src/SphericalCoordinates.hh

@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * 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 IGNITION_MATH_PYTHON__SPHERICALCOORDINATES_HH_
+#define IGNITION_MATH_PYTHON__SPHERICALCOORDINATES_HH_
+
+#include <pybind11/pybind11.h>
+#include <string>
+
+namespace py = pybind11;
+
+namespace ignition
+{
+namespace math
+{
+namespace python
+{
+/// Define a pybind11 wrapper for an ignition::math::SphericalCoordinates
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ * \param[in] typestr name of the type used by Python
+ */
+void defineMathSphericalCoordinates(py::module &m, const std::string &typestr);
+}  // namespace python
+}  // namespace math
+}  // namespace ignition
+
+#endif  // IGNITION_MATH_PYTHON__SPHERICALCOORDINATES_HH_

src/python_pybind11/src/_ignition_math_pybind11.cc

@@ -44,6 +44,7 @@
 #include "SemanticVersion.hh"
 #include "SignalStats.hh"
 #include "Sphere.hh"
+#include "SphericalCoordinates.hh"
 #include "Spline.hh"
 #include "StopWatch.hh"
 #include "Temperature.hh"
@@ -108,6 +109,9 @@ PYBIND11_MODULE(math, m)
 
   ignition::math::python::defineMathSemanticVersion(m, "SemanticVersion");
 
+  ignition::math::python::defineMathSphericalCoordinates(
+    m, "SphericalCoordinates");
+
   ignition::math::python::defineMathSpline(m, "Spline");
 
   ignition::math::python::defineMathStopwatch(m, "Stopwatch");

src/python/SphericalCoordinates_TEST.py → src/python_pybind11/test/SphericalCoordinates_TEST.py

@@ -15,8 +15,8 @@
 import unittest
 
 import ignition
-from ignition.math import Angle, IGN_PI, SphericalCoordinates, Vector3d
-
+from ignition.math import Angle, SphericalCoordinates, Vector3d
+import math
 
 class TestSphericalCoordinates(unittest.TestCase):
 
@@ -269,8 +269,8 @@ def test_distance(self):
 
     def test_bad_set_surface(self):
         sc = SphericalCoordinates()
-        sc.set_surface(2)
-        self.assertEqual(sc.surface(), 2)
+        sc.set_surface(SphericalCoordinates.SurfaceType(2))
+        self.assertEqual(sc.surface(), SphericalCoordinates.SurfaceType(2))
 
     def test_transform(self):
         sc = SphericalCoordinates()
@@ -297,11 +297,15 @@ def test_transform(self):
     def test_bad_coordinate_type(self):
         sc = SphericalCoordinates()
         pos = Vector3d(1, 2, -4)
-        result = sc.position_transform(pos, 7, 6)
+        result = sc.position_transform(pos,
+                                       SphericalCoordinates.CoordinateType(7),
+                                       SphericalCoordinates.CoordinateType(6))
 
         self.assertEqual(result, pos)
 
-        result = sc.position_transform(pos, 4, 6)
+        result = sc.position_transform(pos,
+                                       SphericalCoordinates.CoordinateType(4),
+                                       SphericalCoordinates.CoordinateType(6))
 
         self.assertEqual(result, pos)
 
@@ -317,10 +321,14 @@ def test_bad_coordinate_type(self):
             SphericalCoordinates.SPHERICAL)
         self.assertEqual(result, pos)
 
-        result = sc.velocity_transform(pos, 7, SphericalCoordinates.ECEF)
+        result = sc.velocity_transform(pos,
+                                       SphericalCoordinates.CoordinateType(7),
+                                       SphericalCoordinates.ECEF)
         self.assertEqual(result, pos)
 
-        result = sc.velocity_transform(pos, SphericalCoordinates.ECEF, 7)
+        result = sc.velocity_transform(pos,
+                                       SphericalCoordinates.ECEF,
+                                       SphericalCoordinates.CoordinateType(7))
         self.assertEqual(result, pos)
 
     def test_equality_ops(self):
@@ -363,8 +371,8 @@ def test_assigment_op(self):
     def test_no_heading(self):
         # Default heading
         st = SphericalCoordinates.EARTH_WGS84
-        lat = Angle(-22.9 * IGN_PI / 180.0)
-        lon = Angle(-43.2 * IGN_PI / 180.0)
+        lat = Angle(-22.9 * math.pi / 180.0)
+        lon = Angle(-43.2 * math.pi / 180.0)
         heading = Angle(0.0)
         elev = 0
         sc = SphericalCoordinates(st, lat, lon, elev, heading)
@@ -446,9 +454,9 @@ def test_no_heading(self):
     def test_with_heading(self):
         # Heading 90 deg: X == North, Y == West , Z == Up
         st = SphericalCoordinates.EARTH_WGS84
-        lat = Angle(-22.9 * IGN_PI / 180.0)
-        lon = Angle(-43.2 * IGN_PI / 180.0)
-        heading = Angle(90.0 * IGN_PI / 180.0)
+        lat = Angle(-22.9 * math.pi / 180.0)
+        lon = Angle(-43.2 * math.pi / 180.0)
+        heading = Angle(90.0 * math.pi / 180.0)
         elev = 0
         sc = SphericalCoordinates(st, lat, lon, elev, heading)