From 0eb2d282f471fa51f24233d5014bc66a6e721bc3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 23 Aug 2023 12:54:58 +0200 Subject: [PATCH 1/6] Added MecanumDriveOdometry MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- src/python_pybind11/CMakeLists.txt | 2 + .../src/MecanumDriveOdometry.cc | 75 +++++++++++ .../src/MecanumDriveOdometry.hh | 44 +++++++ .../src/_ignition_math_pybind11.cc | 3 + .../test/MecanumDriveOdometry_TEST.py | 121 ++++++++++++++++++ 5 files changed, 245 insertions(+) create mode 100644 src/python_pybind11/src/MecanumDriveOdometry.cc create mode 100644 src/python_pybind11/src/MecanumDriveOdometry.hh create mode 100644 src/python_pybind11/test/MecanumDriveOdometry_TEST.py diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index e179e138a..c9a52652b 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -28,6 +28,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 @@ -123,6 +124,7 @@ if (BUILD_TESTING) Matrix3_TEST Matrix4_TEST Matrix6_TEST + MecanumDriveOdometry_TEST MovingWindowFilter_TEST OrientedBox_TEST PID_TEST diff --git a/src/python_pybind11/src/MecanumDriveOdometry.cc b/src/python_pybind11/src/MecanumDriveOdometry.cc new file mode 100644 index 000000000..16d6f5141 --- /dev/null +++ b/src/python_pybind11/src/MecanumDriveOdometry.cc @@ -0,0 +1,75 @@ +/* + * 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 + +#include + +#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_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init(), 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 diff --git a/src/python_pybind11/src/MecanumDriveOdometry.hh b/src/python_pybind11/src/MecanumDriveOdometry.hh new file mode 100644 index 000000000..c6c4502ec --- /dev/null +++ b/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 +#include +#include + +#include +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_ diff --git a/src/python_pybind11/src/_ignition_math_pybind11.cc b/src/python_pybind11/src/_ignition_math_pybind11.cc index c88ff50ca..559f541ef 100644 --- a/src/python_pybind11/src/_ignition_math_pybind11.cc +++ b/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"); diff --git a/src/python_pybind11/test/MecanumDriveOdometry_TEST.py b/src/python_pybind11/test/MecanumDriveOdometry_TEST.py new file mode 100644 index 000000000..f91d85c58 --- /dev/null +++ b/src/python_pybind11/test/MecanumDriveOdometry_TEST.py @@ -0,0 +1,121 @@ +# 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 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.datetime.now() + odom.init(datetime.timedelta()) + + # 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), + Angle(1.0 * math.pi / 180), + Angle(1.0 * math.pi / 180), + time1 - startTime) + 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(2.0 * math.pi / 180), + Angle(2.0 * math.pi / 180), + Angle(2.0 * math.pi / 180), + Angle(2.0 * math.pi / 180), + time2 - startTime) + 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.datetime.now() + odom.init(datetime.timedelta()) + 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(2.0 * math.pi / 180), + Angle(2.0 * math.pi / 180), + Angle(2.0 * math.pi / 180), + Angle(2.0 * math.pi / 180), + time1 - startTime) + 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(datetime.timedelta()) + time1 = startTime + datetime.timedelta(milliseconds=100) + odom.update(Angle(-2.0 * math.pi / 180), + Angle(2.0 * math.pi / 180), + Angle(2.0 * math.pi / 180), + Angle(-2.0 * math.pi / 180), + time1 - startTime) + 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() From d1c70f2ce6660b13bc66b9a5aafddf8c04112d88 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 16 Oct 2023 15:01:36 +0200 Subject: [PATCH 2/6] Feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../src/MecanumDriveOdometry.cc | 3 +- .../test/MecanumDriveOdometry_TEST.py | 32 +++++++++---------- 2 files changed, 17 insertions(+), 18 deletions(-) diff --git a/src/python_pybind11/src/MecanumDriveOdometry.cc b/src/python_pybind11/src/MecanumDriveOdometry.cc index 16d6f5141..b299c1822 100644 --- a/src/python_pybind11/src/MecanumDriveOdometry.cc +++ b/src/python_pybind11/src/MecanumDriveOdometry.cc @@ -33,8 +33,7 @@ void defineMathMecanumDriveOdometry(py::module &m, const std::string &typestr) std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), - py::buffer_protocol(), - py::dynamic_attr()) + py::buffer_protocol()) .def(py::init(), py::arg("_windowSize") = 10) .def("init", &Class::Init, "Initialize the odometry") .def("initialized", &Class::Initialized, "Get whether Init has been called.") diff --git a/src/python_pybind11/test/MecanumDriveOdometry_TEST.py b/src/python_pybind11/test/MecanumDriveOdometry_TEST.py index f91d85c58..d94e61a52 100644 --- a/src/python_pybind11/test/MecanumDriveOdometry_TEST.py +++ b/src/python_pybind11/test/MecanumDriveOdometry_TEST.py @@ -44,10 +44,10 @@ def test_constructor(self): # 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), - Angle(1.0 * math.pi / 180), - Angle(1.0 * math.pi / 180), + odom.update(Angle(math.radians(1.0)), + Angle(math.radians(1.0)), + Angle(math.radians(1.0)), + Angle(math.radians(1.0)), time1 - startTime) self.assertAlmostEqual(0.0, odom.heading().radian()) self.assertAlmostEqual(distPerDegree, odom.x()) @@ -60,10 +60,10 @@ def test_constructor(self): # Sleep again, then update the odometry with the new wheel position. time2 = time1 + datetime.timedelta(milliseconds=100) - odom.update(Angle(2.0 * math.pi / 180), - Angle(2.0 * math.pi / 180), - Angle(2.0 * math.pi / 180), - Angle(2.0 * math.pi / 180), + odom.update(Angle(math.radians(2.0)), + Angle(math.radians(2.0)), + Angle(math.radians(2.0)), + Angle(math.radians(2.0)), time2 - startTime) self.assertAlmostEqual(0.0, odom.heading().radian()) self.assertAlmostEqual(distPerDegree * 2.0, odom.x(), delta=3e-6) @@ -85,10 +85,10 @@ def test_constructor(self): # Sleep again, this time move 2 degrees in 100ms. time1 = startTime + datetime.timedelta(milliseconds=100) - odom.update(Angle(2.0 * math.pi / 180), - Angle(2.0 * math.pi / 180), - Angle(2.0 * math.pi / 180), - Angle(2.0 * math.pi / 180), + odom.update(Angle(math.radians(2.0)), + Angle(math.radians(2.0)), + Angle(math.radians(2.0)), + Angle(math.radians(2.0)), time1 - startTime) self.assertAlmostEqual(0.0, odom.heading().radian()) self.assertAlmostEqual(distPerDegree * 2.0, odom.x(), delta=3e-6) @@ -102,10 +102,10 @@ def test_constructor(self): # Sleep again, this time move 2 degrees in 100ms. odom.init(datetime.timedelta()) time1 = startTime + datetime.timedelta(milliseconds=100) - odom.update(Angle(-2.0 * math.pi / 180), - Angle(2.0 * math.pi / 180), - Angle(2.0 * math.pi / 180), - Angle(-2.0 * math.pi / 180), + odom.update(Angle(math.radians(-2.0)), + Angle(math.radians(2.0)), + Angle(math.radians(2.0)), + Angle(math.radians(-2.0)), time1 - startTime) self.assertAlmostEqual(0.0, odom.heading().radian()) self.assertAlmostEqual(distPerDegree * 2.0, odom.y(), delta=3e-6) From fc1b7d22182bb25fc55e29f06947570d9cf6173b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 14 Dec 2023 13:43:03 +0100 Subject: [PATCH 3/6] Added feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- src/python_pybind11/test/DiffDriveOdometry_TEST.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/python_pybind11/test/DiffDriveOdometry_TEST.py b/src/python_pybind11/test/DiffDriveOdometry_TEST.py index 282845403..74a6614bb 100644 --- a/src/python_pybind11/test/DiffDriveOdometry_TEST.py +++ b/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,7 +39,7 @@ def test_diff_drive_odometry(self): # Setup the wheel parameters, and initialize odom.set_wheel_params(wheelSeparation, wheelRadius, wheelRadius) - startTime = datetime.datetime.now() + startTime = datetime.timedelta(time.monotonic()) odom.init(datetime.timedelta()) # Sleep for a little while, then update the odometry with the new wheel @@ -71,7 +72,7 @@ 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() + startTime = datetime.timedelta(time.monotonic()) odom.init(datetime.timedelta()) self.assertEqual(0.0, odom.heading().radian()) self.assertEqual(0.0, odom.x()) From 88ab851d3c68cc21b25e9f327ff3c7026a6e2aaa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 14 Dec 2023 13:44:13 +0100 Subject: [PATCH 4/6] Added feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- src/python_pybind11/test/MecanumDriveOdometry_TEST.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/python_pybind11/test/MecanumDriveOdometry_TEST.py b/src/python_pybind11/test/MecanumDriveOdometry_TEST.py index d94e61a52..16bde3771 100644 --- a/src/python_pybind11/test/MecanumDriveOdometry_TEST.py +++ b/src/python_pybind11/test/MecanumDriveOdometry_TEST.py @@ -14,7 +14,9 @@ import datetime import math +import time import unittest + from ignition.math import MecanumDriveOdometry, Angle @@ -38,7 +40,7 @@ def test_constructor(self): # Setup the wheel parameters, and initialize odom.set_wheel_params(wheelSeparation, wheelRadius, wheelRadius,wheelRadius) - startTime = datetime.datetime.now() + startTime = datetime.timedelta(time.monotonic()) odom.init(datetime.timedelta()) # Sleep for a little while, then update the odometry with the new wheel @@ -75,7 +77,7 @@ def test_constructor(self): self.assertAlmostEqual(0.0, odom.angular_velocity().radian(), delta=1e-3) # Initialize again, and odom values should be reset. - startTime = datetime.datetime.now() + startTime = datetime.timedelta(time.monotonic()) odom.init(datetime.timedelta()) self.assertAlmostEqual(0.0, odom.heading().radian()) self.assertAlmostEqual(0.0, odom.x()) From e0aaab15bb6c936eb495b91fa08eee280cb1aa39 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 13 Feb 2024 11:19:06 +0100 Subject: [PATCH 5/6] Added feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../test/MecanumDriveOdometry_TEST.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/python_pybind11/test/MecanumDriveOdometry_TEST.py b/src/python_pybind11/test/MecanumDriveOdometry_TEST.py index 16bde3771..8cef51c19 100644 --- a/src/python_pybind11/test/MecanumDriveOdometry_TEST.py +++ b/src/python_pybind11/test/MecanumDriveOdometry_TEST.py @@ -41,7 +41,7 @@ def test_constructor(self): # Setup the wheel parameters, and initialize odom.set_wheel_params(wheelSeparation, wheelRadius, wheelRadius,wheelRadius) startTime = datetime.timedelta(time.monotonic()) - odom.init(datetime.timedelta()) + odom.init(startTime) # Sleep for a little while, then update the odometry with the new wheel # position. @@ -50,7 +50,7 @@ def test_constructor(self): Angle(math.radians(1.0)), Angle(math.radians(1.0)), Angle(math.radians(1.0)), - time1 - startTime) + time1) self.assertAlmostEqual(0.0, odom.heading().radian()) self.assertAlmostEqual(distPerDegree, odom.x()) self.assertAlmostEqual(0.0, odom.y()) @@ -66,7 +66,7 @@ def test_constructor(self): Angle(math.radians(2.0)), Angle(math.radians(2.0)), Angle(math.radians(2.0)), - time2 - startTime) + time2) self.assertAlmostEqual(0.0, odom.heading().radian()) self.assertAlmostEqual(distPerDegree * 2.0, odom.x(), delta=3e-6) self.assertAlmostEqual(0.0, odom.y()) @@ -78,7 +78,7 @@ def test_constructor(self): # Initialize again, and odom values should be reset. startTime = datetime.timedelta(time.monotonic()) - odom.init(datetime.timedelta()) + odom.init(startTime) self.assertAlmostEqual(0.0, odom.heading().radian()) self.assertAlmostEqual(0.0, odom.x()) self.assertAlmostEqual(0.0, odom.y()) @@ -91,7 +91,7 @@ def test_constructor(self): Angle(math.radians(2.0)), Angle(math.radians(2.0)), Angle(math.radians(2.0)), - time1 - startTime) + time1) self.assertAlmostEqual(0.0, odom.heading().radian()) self.assertAlmostEqual(distPerDegree * 2.0, odom.x(), delta=3e-6) self.assertAlmostEqual(0.0, odom.y()) @@ -102,13 +102,13 @@ def test_constructor(self): self.assertAlmostEqual(0.0, odom.angular_velocity().radian(), delta=1e-3) # Sleep again, this time move 2 degrees in 100ms. - odom.init(datetime.timedelta()) + 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 - startTime) + time1) self.assertAlmostEqual(0.0, odom.heading().radian()) self.assertAlmostEqual(distPerDegree * 2.0, odom.y(), delta=3e-6) # self.assertAlmostEqual(0.0, odom.y()) From 811947f7feb764d117e5bc7049b38568c638f8df Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 13 Feb 2024 22:46:10 +0100 Subject: [PATCH 6/6] Diffdrive test MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- src/python_pybind11/test/DiffDriveOdometry_TEST.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/python_pybind11/test/DiffDriveOdometry_TEST.py b/src/python_pybind11/test/DiffDriveOdometry_TEST.py index 74a6614bb..a5d11cee1 100644 --- a/src/python_pybind11/test/DiffDriveOdometry_TEST.py +++ b/src/python_pybind11/test/DiffDriveOdometry_TEST.py @@ -40,14 +40,14 @@ def test_diff_drive_odometry(self): # Setup the wheel parameters, and initialize odom.set_wheel_params(wheelSeparation, wheelRadius, wheelRadius) startTime = datetime.timedelta(time.monotonic()) - odom.init(datetime.timedelta()) + 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()) @@ -61,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()) @@ -73,7 +73,7 @@ def test_diff_drive_odometry(self): # Initialize again, and odom values should be reset. startTime = datetime.timedelta(time.monotonic()) - odom.init(datetime.timedelta()) + odom.init(startTime) self.assertEqual(0.0, odom.heading().radian()) self.assertEqual(0.0, odom.x()) self.assertEqual(0.0, odom.y()) @@ -84,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()) @@ -98,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.