diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index fa94b79fd..e8b8ef863 100644 --- a/src/python/CMakeLists.txt +++ b/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 diff --git a/src/python/DiffDriveOdometry.i b/src/python/DiffDriveOdometry.i new file mode 100644 index 000000000..216c511c1 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +%} + +%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::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); + }; + } +} diff --git a/src/python/DiffDriveOdometry_TEST.py b/src/python/DiffDriveOdometry_TEST.py new file mode 100644 index 000000000..7b68b6639 --- /dev/null +++ b/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() diff --git a/src/python/python.i b/src/python/python.i index 1dec3721e..38aa5843a 100644 --- a/src/python/python.i +++ b/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