Added MecanumDriveOdometry Python wrapper (#549)

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

src/python_pybind11/CMakeLists.txt

@@ -31,6 +31,7 @@ pybind11_add_module(math SHARED
   src/Matrix3.cc
   src/Matrix4.cc
   src/Matrix6.cc
+  src/MecanumDriveOdometry.cc
   src/MovingWindowFilter.cc
   src/PID.cc
   src/Polynomial3.cc
@@ -124,6 +125,7 @@ if (BUILD_TESTING)
     Matrix3_TEST
     Matrix4_TEST
     Matrix6_TEST
+    MecanumDriveOdometry_TEST
     MovingWindowFilter_TEST
     OrientedBox_TEST
     PID_TEST

src/python_pybind11/src/MecanumDriveOdometry.cc

@@ -0,0 +1,74 @@
+/*
+ * Copyright (C) 2023 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 <string>
+
+#include <gz/math/MecanumDriveOdometry.hh>
+
+#include "MecanumDriveOdometry.hh"
+
+namespace ignition
+{
+namespace math
+{
+namespace python
+{
+void defineMathMecanumDriveOdometry(py::module &m, const std::string &typestr)
+{
+  using Class = gz::math::MecanumDriveOdometry;
+  std::string pyclass_name = typestr;
+  py::class_<Class>(m,
+                    pyclass_name.c_str(),
+                    py::buffer_protocol())
+  .def(py::init<size_t>(), py::arg("_windowSize") = 10)
+  .def("init", &Class::Init, "Initialize the odometry")
+  .def("initialized", &Class::Initialized, "Get whether Init has been called.")
+  .def("update",
+       &Class::Update,
+       "Updates the odometry class with latest wheels and "
+       "steerings position")
+  .def("heading", &Class::Heading, "Get the heading.")
+  .def("x", &Class::X, "Get the X position.")
+  .def("y", &Class::Y, "Get the Y position.")
+  .def("linear_velocity",
+       &Class::LinearVelocity,
+       "Get the linear velocity.")
+  .def("angular_velocity",
+       &Class::AngularVelocity,
+       "Get the angular velocity.")
+  .def("lateral_velocity",
+       &Class::LateralVelocity,
+       "Get the lateral velocity.")
+  .def("set_wheel_params",
+       &Class::SetWheelParams,
+       "Set the wheel parameters including the radius and separation.")
+  .def("set_velocity_rolling_window_size",
+       &Class::SetVelocityRollingWindowSize,
+       "Set the velocity rolling window size.")
+  .def("wheel_separation", &Class::WheelSeparation, "Get the wheel separation")
+  .def("wheel_base", &Class::WheelBase, "Get the wheel base")
+  .def("left_wheel_radius",
+       &Class::LeftWheelRadius,
+       "Get the left wheel radius")
+  .def("right_wheel_radius",
+       &Class::RightWheelRadius,
+       "Get the rightwheel radius");
+
+}
+}  // namespace python
+}  // namespace math
+}  // namespace ignition

src/python_pybind11/src/MecanumDriveOdometry.hh

@@ -0,0 +1,44 @@
+/*
+ * Copyright (C) 2023 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 GZ_MATH_PYTHON__MECANUMDRIVEODOMETRY_HH_
+#define GZ_MATH_PYTHON__MECANUMDRIVEODOMETRY_HH_
+
+#include <pybind11/chrono.h>
+#include <pybind11/operators.h>
+#include <pybind11/pybind11.h>
+
+#include <string>
+namespace py = pybind11;
+
+namespace ignition
+{
+namespace math
+{
+namespace python
+{
+/// Define a pybind11 wrapper for an gz::math::MecanumDriveOdometry
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ * \param[in] typestr name of the type used by Python
+ */
+void defineMathMecanumDriveOdometry(py::module &m, const std::string &typestr);
+}  // namespace python
+}  // namespace math
+}  // namespace ignition
+
+#endif  // GZ_MATH_PYTHON__MECANUMDRIVEODOMETRY_HH_

src/python_pybind11/src/_ignition_math_pybind11.cc

@@ -36,6 +36,7 @@
 #include "Matrix3.hh"
 #include "Matrix4.hh"
 #include "Matrix6.hh"
+#include "MecanumDriveOdometry.hh"
 #include "MovingWindowFilter.hh"
 #include "OrientedBox.hh"
 #include "PID.hh"
@@ -151,6 +152,8 @@ PYBIND11_MODULE(math, m)
 
   gz::math::python::defineMathMatrix6(m, "Matrix6");
 
+  gz::math::python::defineMathMecanumDriveOdometry(m, "MecanumDriveOdometry");
+
   gz::math::python::defineMathTriangle(m, "Triangle");
 
   gz::math::python::defineMathTriangle3(m, "Triangle3");

src/python_pybind11/test/DiffDriveOdometry_TEST.py

@@ -14,6 +14,7 @@
 
 import datetime
 import math
+import time
 import unittest
 
 from ignition.math import Angle, DiffDriveOdometry
@@ -38,15 +39,15 @@ def test_diff_drive_odometry(self):
 
         # Setup the wheel parameters, and initialize
         odom.set_wheel_params(wheelSeparation, wheelRadius, wheelRadius)
-        startTime = datetime.datetime.now()
-        odom.init(datetime.timedelta())
+        startTime = datetime.timedelta(time.monotonic())
+        odom.init(startTime)
 
         # Sleep for a little while, then update the odometry with the new wheel
         # position.
         time1 = startTime + datetime.timedelta(milliseconds=100)
         odom.update(Angle(1.0 * math.pi / 180),
                     Angle(1.0 * math.pi / 180),
-                    time1 - startTime)
+                    time1)
         self.assertEqual(0.0, odom.heading().radian())
         self.assertEqual(distPerDegree, odom.x())
         self.assertEqual(0.0, odom.y())
@@ -60,7 +61,7 @@ def test_diff_drive_odometry(self):
         time2 = time1 + datetime.timedelta(milliseconds=100)
         odom.update(Angle(2.0 * math.pi / 180),
                     Angle(2.0 * math.pi / 180),
-                    time2 - startTime)
+                    time2)
         self.assertEqual(0.0, odom.heading().radian())
         self.assertAlmostEqual(distPerDegree * 2.0, odom.x(), delta=3e-6)
         self.assertEqual(0.0, odom.y())
@@ -71,8 +72,8 @@ def test_diff_drive_odometry(self):
         self.assertAlmostEqual(0.0, odom.angular_velocity().radian(), delta=1e-3)
 
         # Initialize again, and odom values should be reset.
-        startTime = datetime.datetime.now()
-        odom.init(datetime.timedelta())
+        startTime = datetime.timedelta(time.monotonic())
+        odom.init(startTime)
         self.assertEqual(0.0, odom.heading().radian())
         self.assertEqual(0.0, odom.x())
         self.assertEqual(0.0, odom.y())
@@ -83,7 +84,7 @@ def test_diff_drive_odometry(self):
         time1 = startTime + datetime.timedelta(milliseconds=100)
         odom.update(Angle(2.0 * math.pi / 180),
                     Angle(2.0 * math.pi / 180),
-                    time1 - startTime)
+                    time1)
         self.assertEqual(0.0, odom.heading().radian())
         self.assertAlmostEqual(distPerDegree * 2.0, odom.x(), delta=3e-6)
         self.assertEqual(0.0, odom.y())
@@ -97,7 +98,7 @@ def test_diff_drive_odometry(self):
         time2 = time1 + datetime.timedelta(milliseconds=100)
         odom.update(Angle(2.0 * math.pi / 180),
                     Angle(3.0 * math.pi / 180),
-                    time2 - startTime)
+                    time2)
         # The heading should be the arc tangent of the linear distance traveled
         # by the right wheel (the left wheel was stationary) divided by the
         # wheel separation.

src/python_pybind11/test/MecanumDriveOdometry_TEST.py

@@ -0,0 +1,123 @@
+# Copyright (C) 2023 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.
+
+import datetime
+import math
+import time
+import unittest
+
+from ignition.math import MecanumDriveOdometry, Angle
+
+
+class TestMecanumDriveOdometry(unittest.TestCase):
+
+    def test_constructor(self):
+        odom = MecanumDriveOdometry()
+        self.assertAlmostEqual(0.0, odom.heading().radian())
+        self.assertAlmostEqual(0.0, odom.x())
+        self.assertAlmostEqual(0.0, odom.y())
+        self.assertAlmostEqual(0.0, odom.linear_velocity())
+        self.assertAlmostEqual(0.0, odom.lateral_velocity())
+        self.assertAlmostEqual(0.0, odom.angular_velocity().radian())
+
+        wheelSeparation = 2.0
+        wheelRadius = 0.5
+        wheelCircumference = 2 * math.pi * wheelRadius
+
+        # This is the linear distance traveled per degree of wheel rotation.
+        distPerDegree = wheelCircumference / 360.0
+
+        # Setup the wheel parameters, and initialize
+        odom.set_wheel_params(wheelSeparation, wheelRadius, wheelRadius,wheelRadius)
+        startTime = datetime.timedelta(time.monotonic())
+        odom.init(startTime)
+
+        # Sleep for a little while, then update the odometry with the new wheel
+        # position.
+        time1 = startTime + datetime.timedelta(milliseconds=100)
+        odom.update(Angle(math.radians(1.0)),
+                    Angle(math.radians(1.0)),
+                    Angle(math.radians(1.0)),
+                    Angle(math.radians(1.0)),
+                    time1)
+        self.assertAlmostEqual(0.0, odom.heading().radian())
+        self.assertAlmostEqual(distPerDegree, odom.x())
+        self.assertAlmostEqual(0.0, odom.y())
+        # Linear velocity should be dist_traveled / time_elapsed.
+        self.assertAlmostEqual(distPerDegree / 0.1, odom.linear_velocity(), delta=1e-3)
+        # Angular velocity should be zero since the "robot" is traveling in a
+        # straight line.
+        self.assertAlmostEqual(0.0, odom.angular_velocity().radian(), delta=1e-3)
+
+        # Sleep again, then update the odometry with the new wheel position.
+        time2 = time1 + datetime.timedelta(milliseconds=100)
+        odom.update(Angle(math.radians(2.0)),
+                    Angle(math.radians(2.0)),
+                    Angle(math.radians(2.0)),
+                    Angle(math.radians(2.0)),
+                    time2)
+        self.assertAlmostEqual(0.0, odom.heading().radian())
+        self.assertAlmostEqual(distPerDegree * 2.0, odom.x(), delta=3e-6)
+        self.assertAlmostEqual(0.0, odom.y())
+        # Linear velocity should be dist_traveled / time_elapsed.
+        self.assertAlmostEqual(distPerDegree / 0.1, odom.linear_velocity(), delta=1e-3)
+        # Angular velocity should be zero since the "robot" is traveling in a
+        # straight line.
+        self.assertAlmostEqual(0.0, odom.angular_velocity().radian(), delta=1e-3)
+
+        # Initialize again, and odom values should be reset.
+        startTime = datetime.timedelta(time.monotonic())
+        odom.init(startTime)
+        self.assertAlmostEqual(0.0, odom.heading().radian())
+        self.assertAlmostEqual(0.0, odom.x())
+        self.assertAlmostEqual(0.0, odom.y())
+        self.assertAlmostEqual(0.0, odom.linear_velocity())
+        self.assertAlmostEqual(0.0, odom.angular_velocity().radian())
+
+        # Sleep again, this time move 2 degrees in 100ms.
+        time1 = startTime + datetime.timedelta(milliseconds=100)
+        odom.update(Angle(math.radians(2.0)),
+                    Angle(math.radians(2.0)),
+                    Angle(math.radians(2.0)),
+                    Angle(math.radians(2.0)),
+                    time1)
+        self.assertAlmostEqual(0.0, odom.heading().radian())
+        self.assertAlmostEqual(distPerDegree * 2.0, odom.x(), delta=3e-6)
+        self.assertAlmostEqual(0.0, odom.y())
+        # Linear velocity should be dist_traveled / time_elapsed.
+        self.assertAlmostEqual(distPerDegree * 2 / 0.1, odom.linear_velocity(), delta=1e-3)
+        # Angular velocity should be zero since the "robot" is traveling in a
+        # straight line.
+        self.assertAlmostEqual(0.0, odom.angular_velocity().radian(), delta=1e-3)
+
+        # Sleep again, this time move 2 degrees in 100ms.
+        odom.init(startTime)
+        time1 = startTime + datetime.timedelta(milliseconds=100)
+        odom.update(Angle(math.radians(-2.0)),
+                    Angle(math.radians(2.0)),
+                    Angle(math.radians(2.0)),
+                    Angle(math.radians(-2.0)),
+                    time1)
+        self.assertAlmostEqual(0.0, odom.heading().radian())
+        self.assertAlmostEqual(distPerDegree * 2.0, odom.y(), delta=3e-6)
+        # self.assertAlmostEqual(0.0, odom.y())
+        # Linear velocity should be dist_traveled / time_elapsed.
+        self.assertAlmostEqual(distPerDegree * 2 / 0.1, odom.lateral_velocity(), delta=1e-3)
+        # Angular velocity should be zero since the "robot" is traveling in a
+        # straight line.
+        self.assertAlmostEqual(0.0, odom.angular_velocity().radian(), delta=1e-3)
+
+
+if __name__ == '__main__':
+    unittest.main()