DiffDriveOdometry Python interface (#265)

Signed-off-by: ahcorde <[email protected]>
Co-authored-by: Steve Peters <[email protected]>

src/python/CMakeLists.txt

@@ -91,6 +91,7 @@ if (PYTHONLIBS_FOUND)
       AxisAlignedBox_TEST
       Color_TEST
       Cylinder_TEST
+      DiffDriveOdometry_TEST
       Filter_TEST
       GaussMarkovProcess_TEST
       Helpers_TEST

src/python/DiffDriveOdometry.i

@@ -0,0 +1,74 @@
+/*
+ * 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.
+ *
+*/
+
+%module diffdriveodometry
+%{
+#include <chrono>
+#include <memory>
+#include <ignition/math/DiffDriveOdometry.hh>
+#include <ignition/math/Angle.hh>
+#include <ignition/math/Export.hh>
+#include <ignition/math/config.hh>
+%}
+
+%include "typemaps.i"
+%typemap(in, numinputs=1) const std::chrono::steady_clock::time_point &_time %{
+if (!PyInt_Check($input)) {
+    PyErr_SetString(PyExc_ValueError, "Expecting an integer");
+    return NULL;
+}
+using std::chrono::duration_cast;
+$1 = new std::chrono::steady_clock::time_point();
+*$1 += duration_cast<std::chrono::steady_clock::duration>(std::chrono::milliseconds(PyInt_AsLong($input)));
+%}
+
+namespace ignition
+{
+  namespace math
+  {
+    class DiffDriveOdometry
+    {
+      %rename("%(undercase)s", %$isfunction, notregexmatch$name="^[A-Z]*$") "";
+
+      public: explicit DiffDriveOdometry(size_t _windowSize = 10);
+
+      public: ~DiffDriveOdometry();
+
+      public: void Init(const std::chrono::steady_clock::time_point &_time);
+
+      public: bool Initialized() const;
+
+      public: bool Update(const Angle &_leftPos, const Angle &_rightPos,
+                          const std::chrono::steady_clock::time_point &_time);
+
+      public: const Angle &Heading() const;
+
+      public: double X() const;
+
+      public: double Y() const;
+
+      public: double LinearVelocity() const;
+
+      public: const Angle &AngularVelocity() const;
+
+      public: void SetWheelParams(double _wheelSeparation,
+                      double _leftWheelRadius, double _rightWheelRadius);
+
+      public: void SetVelocityRollingWindowSize(size_t _size);
+    };
+  }
+}

src/python/DiffDriveOdometry_TEST.py

@@ -0,0 +1,129 @@
+# 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.
+
+import datetime
+import math
+import unittest
+
+from ignition.math import Angle, DiffDriveOdometry, IGN_PI
+
+
+class TestDiffDriveOdometry(unittest.TestCase):
+
+    def test_diff_drive_odometry(self):
+        odom = DiffDriveOdometry()
+        self.assertEqual(0.0, odom.heading().radian())
+        self.assertEqual(0.0, odom.x())
+        self.assertEqual(0.0, odom.y())
+        self.assertEqual(0.0, odom.linear_velocity())
+        self.assertEqual(0.0, odom.angular_velocity().radian())
+
+        wheelSeparation = 2.0
+        wheelRadius = 0.5
+        wheelCircumference = 2 * IGN_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)
+        startTime = datetime.datetime.now()
+        odom.init(int(startTime.timestamp() * 1000))
+
+        # 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 * IGN_PI / 180),
+                    Angle(1.0 * IGN_PI / 180),
+                    int(time1.timestamp() * 1000))
+        self.assertEqual(0.0, odom.heading().radian())
+        self.assertEqual(distPerDegree, odom.x())
+        self.assertEqual(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(2.0 * IGN_PI / 180),
+                    Angle(2.0 * IGN_PI / 180),
+                    int(time2.timestamp() * 1000))
+        self.assertEqual(0.0, odom.heading().radian())
+        self.assertAlmostEqual(distPerDegree * 2.0, odom.x(), delta=3e-6)
+        self.assertEqual(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.datetime.now()
+        odom.init(int(startTime.timestamp() * 1000))
+        self.assertEqual(0.0, odom.heading().radian())
+        self.assertEqual(0.0, odom.x())
+        self.assertEqual(0.0, odom.y())
+        self.assertEqual(0.0, odom.linear_velocity())
+        self.assertEqual(0.0, odom.angular_velocity().radian())
+
+        # Sleep again, this time move 2 degrees in 100ms.
+        time1 = startTime + datetime.timedelta(milliseconds=100)
+        odom.update(Angle(2.0 * IGN_PI / 180),
+                    Angle(2.0 * IGN_PI / 180),
+                    int(time1.timestamp() * 1000))
+        self.assertEqual(0.0, odom.heading().radian())
+        self.assertAlmostEqual(distPerDegree * 2.0, odom.x(), delta=3e-6)
+        self.assertEqual(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 rotate the right wheel by 1 degree.
+        time2 = time1 + datetime.timedelta(milliseconds=100)
+        odom.update(Angle(2.0 * IGN_PI / 180),
+                    Angle(3.0 * IGN_PI / 180),
+                    int(time2.timestamp() * 1000))
+        # 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.
+        self.assertAlmostEqual(math.atan2(distPerDegree, wheelSeparation),
+                               odom.heading().radian(),
+                               delta=1e-6)
+
+        # The X odom reading should have increased by the sine of the heading *
+        # half the wheel separation.
+        xDistTraveled = math.sin(math.atan2(distPerDegree, wheelSeparation)) * wheelSeparation * 0.5
+        prevXPos = distPerDegree * 2.0
+        self.assertAlmostEqual(xDistTraveled + prevXPos, odom.x(), delta=3e-6)
+
+        # The Y odom reading should have increased by the cosine of the header *
+        # half the wheel separation.
+        yDistTraveled = (wheelSeparation * 0.5) - math.cos(math.atan2(distPerDegree, wheelSeparation)) * wheelSeparation * 0.5
+        prevYPos = 0.0
+        self.assertAlmostEqual(yDistTraveled + prevYPos, odom.y(), delta=3e-6)
+
+        # Angular velocity should be the difference between the x and y distance
+        # traveled divided by the wheel separation divided by the seconds
+        # elapsed.
+        self.assertAlmostEqual(
+          ((xDistTraveled - yDistTraveled) / wheelSeparation) / 0.1,
+          odom.angular_velocity().radian(), delta=1e-3)
+
+
+if __name__ == '__main__':
+    unittest.main()

src/python/python.i

@@ -1,5 +1,6 @@
 %module "math"
 %include Angle.i
+%include DiffDriveOdometry.i
 %include GaussMarkovProcess.i
 %include Helpers.i
 %include Rand.i