From 6bcf289f9550371d378b36a051dd9e449e143b96 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 17 Dec 2021 12:45:38 +0100 Subject: [PATCH 1/6] Add DiffDriveOdometry pybind11 interface and examples Signed-off-by: ahcorde --- examples/CMakeLists.txt | 3 + examples/diff_drive_odometry.cc | 101 ++++++++++++++++++ examples/diff_drive_odometry.py | 86 +++++++++++++++ src/python/CMakeLists.txt | 1 - src/python_pybind11/CMakeLists.txt | 6 ++ src/python_pybind11/src/DiffDriveOdometry.cc | 62 +++++++++++ src/python_pybind11/src/DiffDriveOdometry.hh | 44 ++++++++ .../src/_ignition_math_pybind11.cc | 5 +- .../test}/DiffDriveOdometry_TEST.py | 32 +++--- 9 files changed, 321 insertions(+), 19 deletions(-) create mode 100644 examples/diff_drive_odometry.cc create mode 100644 examples/diff_drive_odometry.py create mode 100644 src/python_pybind11/src/DiffDriveOdometry.cc create mode 100644 src/python_pybind11/src/DiffDriveOdometry.hh rename src/{python => python_pybind11/test}/DiffDriveOdometry_TEST.py (86%) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 2b454b6dc..0e6b40e27 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -7,6 +7,9 @@ find_package(ignition-math${IGN_MATH_VER} REQUIRED) add_executable(angle_example angle_example.cc) target_link_libraries(angle_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) +add_executable(diff_drive_odometry diff_drive_odometry.cc) +target_link_libraries(diff_drive_odometry ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) + add_executable(gauss_markov_process gauss_markov_process_example.cc) target_link_libraries(gauss_markov_process ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) diff --git a/examples/diff_drive_odometry.cc b/examples/diff_drive_odometry.cc new file mode 100644 index 000000000..c10db982f --- /dev/null +++ b/examples/diff_drive_odometry.cc @@ -0,0 +1,101 @@ +/* + * 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. + * +*/ +//! [complete] +#include +#include + +#include +#include +#include + +int main(int argc, char **argv) +{ + +//! [Create a DiffDriveOdometry] + ignition::math::DiffDriveOdometry odom; +//! [Create an DiffDriveOdometry] + + double wheelSeparation = 2.0; + double wheelRadius = 0.5; + double wheelCircumference = 2 * IGN_PI * wheelRadius; + + // This is the linear distance traveled per degree of wheel rotation. + double distPerDegree = wheelCircumference / 360.0; + + odom.SetWheelParams(wheelSeparation, wheelRadius, wheelRadius); + auto startTime = std::chrono::steady_clock::now(); + odom.Init(startTime); + + // Sleep for a little while, then update the odometry with the new wheel + // position. + std::cout << "--- Rotate the both wheels by 1 degree. ---" << '\n'; + auto time1 = startTime + std::chrono::milliseconds(100); + odom.Update(IGN_DTOR(1.0), IGN_DTOR(1.0), time1); + + // Linear velocity should be dist_traveled / time_elapsed. + std::cout << "\tLinear velocity: " << distPerDegree / 0.1 + << " Odom linear velocity: " << odom.LinearVelocity() << std::endl; + + // Angular velocity should be zero since the "robot" is traveling in a + // straight line. + std::cout << "Angular velocity should be zero since the 'robot' is traveling" + << " in a straight line:\n" + << "\tOdom angular velocity: " + << *odom.AngularVelocity() << std::endl; + + // Sleep again, this time rotate the right wheel by 1 degree. + std::cout << "--- This time rotate the right wheel by 1 degree. ---" + << std::endl; + auto time2 = time1 + std::chrono::milliseconds(100); + odom.Update(IGN_DTOR(2.0), IGN_DTOR(3.0), 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. + std::cerr << "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.\n" + << "\tHeading: " << atan2(distPerDegree, wheelSeparation) + << " Odom Heading " << *odom.Heading() << '\n'; + + // The X odom reading should have increased by the sine of the heading * + // half the wheel separation. + double xDistTraveled = + sin(atan2(distPerDegree, wheelSeparation)) * wheelSeparation * 0.5; + double prevXPos = distPerDegree * 2.0; + std::cout << "\tX distance traveled " << xDistTraveled + prevXPos + << " Odom X: " << odom.X() << std::endl; + + // The Y odom reading should have increased by the cosine of the header * + // half the wheel separation. + double yDistTraveled = (wheelSeparation * 0.5) - + cos(atan2(distPerDegree, wheelSeparation)) * wheelSeparation * 0.5; + double prevYPos = 0.0; + std::cout << "\tY distance traveled " << yDistTraveled + prevYPos + << " Odom Y: " << odom.Y() << std::endl; + + // Angular velocity should be the difference between the x and y distance + // traveled divided by the wheel separation divided by the seconds + // elapsed. + std::cout << "Angular velocity should be the difference between the x and y" + << " distance traveled divided by the wheel separation divided by" + << " the seconds elapsed.\n" + << "\tAngular velocity " + << ((xDistTraveled - yDistTraveled) / wheelSeparation) / 0.1 + << " Odom angular velocity: " << *odom.AngularVelocity() << std::endl; +} +//! [complete] diff --git a/examples/diff_drive_odometry.py b/examples/diff_drive_odometry.py new file mode 100644 index 000000000..8e59b2daf --- /dev/null +++ b/examples/diff_drive_odometry.py @@ -0,0 +1,86 @@ +# 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 + +from ignition.math import Angle, DiffDriveOdometry + +odom = DiffDriveOdometry() + +wheelSeparation = 2.0 +wheelRadius = 0.5 +wheelCircumference = 2 * 3.1416 * 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(datetime.timedelta()) + +print('--- Rotate the both wheels by 1 degree. ---') +time1 = startTime + datetime.timedelta(milliseconds=100) +odom.update(Angle(1.0 * 3.1416 / 180), + Angle(1.0 * 3.1416 / 180), + time1 - startTime) + +print('Linear velocity: {} Odom linear velocity: {}'. + format(distPerDegree / 0.1, odom.linear_velocity())) + +print('Angular velocity should be zero since the "robot" is traveling' + + ' in a straight line:\n' + + '\tOdom angular velocity: {}' + .format(odom.angular_velocity())) + +# Sleep again, this time rotate the right wheel by 1 degree. +print('--- This time rotate the right wheel by 1 degree. ---'); +time2 = time1 + datetime.timedelta(milliseconds=100) +odom.update(Angle(2.0 * 3.1416 / 180), + Angle(3.0 * 3.1416 / 180), + time2 - startTime) + +print('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.\n' + + '\tHeading: {} Odom Heading: {}'.format( + math.atan2(distPerDegree, wheelSeparation), + odom.heading())) + +# 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 +print('\tX distance traveled: {} Odom X: {}'.format( + xDistTraveled + prevXPos, odom.x())) + +# 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 +print('\tY distance traveled: {} Odom Y: {}'.format( + yDistTraveled + prevYPos, odom.y())) + +# Angular velocity should be the difference between the x and y distance +# traveled divided by the wheel separation divided by the seconds +# elapsed. +print('Angular velocity should be the difference between the x and y' + + ' distance traveled divided by the wheel separation divided by' + + ' the seconds elapsed.\n' + + '\tAngular velocity: {} Odom angular velocity: {}'.format( + ((xDistTraveled - yDistTraveled) / wheelSeparation) / 0.1, + odom.angular_velocity())) diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index 01284951e..fe6bba145 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -92,7 +92,6 @@ if (PYTHONLIBS_FOUND) Box_TEST Color_TEST Cylinder_TEST - DiffDriveOdometry_TEST Filter_TEST Frustum_TEST GaussMarkovProcess_TEST diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index 368492352..e2173176a 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -14,6 +14,11 @@ if (${pybind11_FOUND}) # Split from main extension and converted to pybind11 pybind11_add_module(math SHARED src/_ignition_math_pybind11.cc + src/DiffDriveOdometry.cc + ) + + target_link_libraries(math PRIVATE + ${PROJECT_LIBRARY_TARGET_NAME} ) if(USE_SYSTEM_PATHS_FOR_PYTHON_INSTALLATION) @@ -62,6 +67,7 @@ if (${pybind11_FOUND}) if (BUILD_TESTING) # Add the Python tests set(python_tests + DiffDriveOdometry_TEST Vector2_TEST Vector3_TEST Vector4_TEST diff --git a/src/python_pybind11/src/DiffDriveOdometry.cc b/src/python_pybind11/src/DiffDriveOdometry.cc new file mode 100644 index 000000000..36ec717f3 --- /dev/null +++ b/src/python_pybind11/src/DiffDriveOdometry.cc @@ -0,0 +1,62 @@ +/* + * 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 "DiffDriveOdometry.hh" + +namespace ignition +{ +namespace math +{ +namespace python +{ +void defineMathDiffDriveOdometry( + py::module &m, const std::string &typestr) +{ + using Class = ignition::math::DiffDriveOdometry; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init()) + .def(py::init<>()) + .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("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."); +} +} // namespace python +} // namespace gazebo +} // namespace ignition diff --git a/src/python_pybind11/src/DiffDriveOdometry.hh b/src/python_pybind11/src/DiffDriveOdometry.hh new file mode 100644 index 000000000..4e5bb3c5c --- /dev/null +++ b/src/python_pybind11/src/DiffDriveOdometry.hh @@ -0,0 +1,44 @@ +/* + * 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__DIFFDRIVEODOMETRY_HH_ +#define IGNITION_MATH_PYTHON__DIFFDRIVEODOMETRY_HH_ + +#include +#include +#include + +#include + +namespace py = pybind11; + +namespace ignition +{ +namespace math +{ +namespace python +{ +/// Define a py:: wrapper for an ignition::gazebo::DiffDriveOdometry +/** + * \param[in] module a py:: module to add the definition to + */ +void defineMathDiffDriveOdometry( + py::module &m, const std::string &typestr); +} // namespace python +} // namespace gazebo +} // namespace ignition + +#endif // IGNITION_MATH_PYTHON__DIFFDRIVEODOMETRY_HH_ diff --git a/src/python_pybind11/src/_ignition_math_pybind11.cc b/src/python_pybind11/src/_ignition_math_pybind11.cc index da0029102..b482a4892 100644 --- a/src/python_pybind11/src/_ignition_math_pybind11.cc +++ b/src/python_pybind11/src/_ignition_math_pybind11.cc @@ -14,6 +14,7 @@ #include +#include "DiffDriveOdometry.hh" #include "Vector2.hh" #include "Vector3.hh" #include "Vector4.hh" @@ -24,8 +25,8 @@ PYBIND11_MODULE(math, m) { m.doc() = "Ignition Math Python Library."; - - + ignition::math::python::defineMathDiffDriveOdometry( + m, "DiffDriveOdometry"); ignition::math::python::defineMathVector2(m, "Vector2d"); ignition::math::python::defineMathVector2(m, "Vector2i"); diff --git a/src/python/DiffDriveOdometry_TEST.py b/src/python_pybind11/test/DiffDriveOdometry_TEST.py similarity index 86% rename from src/python/DiffDriveOdometry_TEST.py rename to src/python_pybind11/test/DiffDriveOdometry_TEST.py index 7b68b6639..466a5af65 100644 --- a/src/python/DiffDriveOdometry_TEST.py +++ b/src/python_pybind11/test/DiffDriveOdometry_TEST.py @@ -16,7 +16,7 @@ import math import unittest -from ignition.math import Angle, DiffDriveOdometry, IGN_PI +from ignition.math import Angle, DiffDriveOdometry class TestDiffDriveOdometry(unittest.TestCase): @@ -31,7 +31,7 @@ def test_diff_drive_odometry(self): wheelSeparation = 2.0 wheelRadius = 0.5 - wheelCircumference = 2 * IGN_PI * wheelRadius + wheelCircumference = 2 * IGNPI * wheelRadius # This is the linear distance traveled per degree of wheel rotation. distPerDegree = wheelCircumference / 360.0 @@ -39,14 +39,14 @@ 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(int(startTime.timestamp() * 1000)) + 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 * IGN_PI / 180), - Angle(1.0 * IGN_PI / 180), - int(time1.timestamp() * 1000)) + odom.update(Angle(1.0 * IGNPI / 180), + Angle(1.0 * IGNPI / 180), + time1 - startTime) self.assertEqual(0.0, odom.heading().radian()) self.assertEqual(distPerDegree, odom.x()) self.assertEqual(0.0, odom.y()) @@ -58,9 +58,9 @@ def test_diff_drive_odometry(self): # 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)) + odom.update(Angle(2.0 * IGNPI / 180), + Angle(2.0 * IGNPI / 180), + time2 - startTime) self.assertEqual(0.0, odom.heading().radian()) self.assertAlmostEqual(distPerDegree * 2.0, odom.x(), delta=3e-6) self.assertEqual(0.0, odom.y()) @@ -72,7 +72,7 @@ def test_diff_drive_odometry(self): # Initialize again, and odom values should be reset. startTime = datetime.datetime.now() - odom.init(int(startTime.timestamp() * 1000)) + odom.init(datetime.timedelta()) self.assertEqual(0.0, odom.heading().radian()) self.assertEqual(0.0, odom.x()) self.assertEqual(0.0, odom.y()) @@ -81,9 +81,9 @@ def test_diff_drive_odometry(self): # 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)) + odom.update(Angle(2.0 * IGNPI / 180), + Angle(2.0 * IGNPI / 180), + time1 - startTime) self.assertEqual(0.0, odom.heading().radian()) self.assertAlmostEqual(distPerDegree * 2.0, odom.x(), delta=3e-6) self.assertEqual(0.0, odom.y()) @@ -95,9 +95,9 @@ def test_diff_drive_odometry(self): # 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)) + odom.update(Angle(2.0 * IGNPI / 180), + Angle(3.0 * IGNPI / 180), + time2 - startTime) # 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. From a4060b058fa2d37acb873f145f1a24d5c608b585 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 17 Dec 2021 12:50:47 +0100 Subject: [PATCH 2/6] make linters happy Signed-off-by: ahcorde --- src/python_pybind11/src/DiffDriveOdometry.cc | 2 +- src/python_pybind11/src/DiffDriveOdometry.hh | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/python_pybind11/src/DiffDriveOdometry.cc b/src/python_pybind11/src/DiffDriveOdometry.cc index 36ec717f3..025981181 100644 --- a/src/python_pybind11/src/DiffDriveOdometry.cc +++ b/src/python_pybind11/src/DiffDriveOdometry.cc @@ -14,7 +14,7 @@ * limitations under the License. * */ - +#include #include "DiffDriveOdometry.hh" namespace ignition diff --git a/src/python_pybind11/src/DiffDriveOdometry.hh b/src/python_pybind11/src/DiffDriveOdometry.hh index 4e5bb3c5c..bc261df2d 100644 --- a/src/python_pybind11/src/DiffDriveOdometry.hh +++ b/src/python_pybind11/src/DiffDriveOdometry.hh @@ -20,6 +20,7 @@ #include #include #include +#include #include From 7219b99f2254b6bc6d3dd52d508665434c18d9c2 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 22 Dec 2021 22:53:37 +0100 Subject: [PATCH 3/6] Fix test Signed-off-by: ahcorde --- .../test/DiffDriveOdometry_TEST.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/python_pybind11/test/DiffDriveOdometry_TEST.py b/src/python_pybind11/test/DiffDriveOdometry_TEST.py index 466a5af65..282845403 100644 --- a/src/python_pybind11/test/DiffDriveOdometry_TEST.py +++ b/src/python_pybind11/test/DiffDriveOdometry_TEST.py @@ -31,7 +31,7 @@ def test_diff_drive_odometry(self): wheelSeparation = 2.0 wheelRadius = 0.5 - wheelCircumference = 2 * IGNPI * wheelRadius + wheelCircumference = 2 * math.pi * wheelRadius # This is the linear distance traveled per degree of wheel rotation. distPerDegree = wheelCircumference / 360.0 @@ -44,8 +44,8 @@ def test_diff_drive_odometry(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 * IGNPI / 180), - Angle(1.0 * IGNPI / 180), + odom.update(Angle(1.0 * math.pi / 180), + Angle(1.0 * math.pi / 180), time1 - startTime) self.assertEqual(0.0, odom.heading().radian()) self.assertEqual(distPerDegree, odom.x()) @@ -58,8 +58,8 @@ def test_diff_drive_odometry(self): # Sleep again, then update the odometry with the new wheel position. time2 = time1 + datetime.timedelta(milliseconds=100) - odom.update(Angle(2.0 * IGNPI / 180), - Angle(2.0 * IGNPI / 180), + odom.update(Angle(2.0 * math.pi / 180), + Angle(2.0 * math.pi / 180), time2 - startTime) self.assertEqual(0.0, odom.heading().radian()) self.assertAlmostEqual(distPerDegree * 2.0, odom.x(), delta=3e-6) @@ -81,8 +81,8 @@ def test_diff_drive_odometry(self): # Sleep again, this time move 2 degrees in 100ms. time1 = startTime + datetime.timedelta(milliseconds=100) - odom.update(Angle(2.0 * IGNPI / 180), - Angle(2.0 * IGNPI / 180), + odom.update(Angle(2.0 * math.pi / 180), + Angle(2.0 * math.pi / 180), time1 - startTime) self.assertEqual(0.0, odom.heading().radian()) self.assertAlmostEqual(distPerDegree * 2.0, odom.x(), delta=3e-6) @@ -95,8 +95,8 @@ def test_diff_drive_odometry(self): # Sleep again, this time rotate the right wheel by 1 degree. time2 = time1 + datetime.timedelta(milliseconds=100) - odom.update(Angle(2.0 * IGNPI / 180), - Angle(3.0 * IGNPI / 180), + odom.update(Angle(2.0 * math.pi / 180), + Angle(3.0 * math.pi / 180), time2 - startTime) # The heading should be the arc tangent of the linear distance traveled # by the right wheel (the left wheel was stationary) divided by the From 1ed4c4fc6b722719c5820f07acc2c611c5beed0e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Mon, 27 Dec 2021 20:37:24 +0100 Subject: [PATCH 4/6] Fixed tests MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- src/python/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index 6d505a0ad..94e8a7f3e 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -90,8 +90,6 @@ if (PYTHONLIBS_FOUND) AxisAlignedBox_TEST Box_TEST Cylinder_TEST - DiffDriveOdometry_TEST - Filter_TEST Frustum_TEST GaussMarkovProcess_TEST Inertial_TEST From 7870ccd44df5cea0139bd60053377fe27951dab3 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 28 Dec 2021 09:47:29 -0800 Subject: [PATCH 5/6] consistent formatting, units, alignment Signed-off-by: Louise Poubel --- examples/angle_example.py | 2 +- examples/diff_drive_odometry.cc | 58 +++++++++++++++------------------ examples/diff_drive_odometry.py | 55 +++++++++++++++++-------------- 3 files changed, 58 insertions(+), 57 deletions(-) diff --git a/examples/angle_example.py b/examples/angle_example.py index 0a58f52e6..10f22bb2f 100644 --- a/examples/angle_example.py +++ b/examples/angle_example.py @@ -16,7 +16,7 @@ # installed. # # Modify the PYTHONPATH environment variable to include the ignition math -# library install path. For example, if you install to /user: +# library install path. For example, if you install to /usr: # # $ export PYTHONPATH=/usr/lib/python:$PYTHONPATH # diff --git a/examples/diff_drive_odometry.cc b/examples/diff_drive_odometry.cc index c10db982f..0a0635145 100644 --- a/examples/diff_drive_odometry.cc +++ b/examples/diff_drive_odometry.cc @@ -36,26 +36,25 @@ int main(int argc, char **argv) // This is the linear distance traveled per degree of wheel rotation. double distPerDegree = wheelCircumference / 360.0; + // Setup the wheel parameters, and initialize odom.SetWheelParams(wheelSeparation, wheelRadius, wheelRadius); auto startTime = std::chrono::steady_clock::now(); odom.Init(startTime); // Sleep for a little while, then update the odometry with the new wheel // position. - std::cout << "--- Rotate the both wheels by 1 degree. ---" << '\n'; + std::cout << "--- Rotate both wheels by 1 degree. ---" << '\n'; auto time1 = startTime + std::chrono::milliseconds(100); odom.Update(IGN_DTOR(1.0), IGN_DTOR(1.0), time1); - // Linear velocity should be dist_traveled / time_elapsed. - std::cout << "\tLinear velocity: " << distPerDegree / 0.1 - << " Odom linear velocity: " << odom.LinearVelocity() << std::endl; + std::cout << "\tLinear velocity:\t" << distPerDegree / 0.1 << " m/s" + << "\n\tOdom linear velocity:\t" << odom.LinearVelocity() << " m/s" + << std::endl; - // Angular velocity should be zero since the "robot" is traveling in a - // straight line. - std::cout << "Angular velocity should be zero since the 'robot' is traveling" - << " in a straight line:\n" - << "\tOdom angular velocity: " - << *odom.AngularVelocity() << std::endl; + std::cout << "Angular velocity should be zero since the \"robot\" is traveling\n" + << "in a straight line:\n" + << "\tOdom angular velocity:\t" + << *odom.AngularVelocity() << " rad/s" << std::endl; // Sleep again, this time rotate the right wheel by 1 degree. std::cout << "--- This time rotate the right wheel by 1 degree. ---" @@ -63,39 +62,34 @@ int main(int argc, char **argv) auto time2 = time1 + std::chrono::milliseconds(100); odom.Update(IGN_DTOR(2.0), IGN_DTOR(3.0), 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. - std::cerr << "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.\n" - << "\tHeading: " << atan2(distPerDegree, wheelSeparation) - << " Odom Heading " << *odom.Heading() << '\n'; + std::cout << "The heading should be the arc tangent of the linear distance\n" + << "traveled by the right wheel (the left wheel was stationary)\n" + << "divided by the wheel separation.\n" + << "\tHeading:\t\t" << atan2(distPerDegree, wheelSeparation) << " rad" + << "\n\tOdom Heading:\t\t" << *odom.Heading() << " rad" << '\n'; // The X odom reading should have increased by the sine of the heading * // half the wheel separation. double xDistTraveled = sin(atan2(distPerDegree, wheelSeparation)) * wheelSeparation * 0.5; double prevXPos = distPerDegree * 2.0; - std::cout << "\tX distance traveled " << xDistTraveled + prevXPos - << " Odom X: " << odom.X() << std::endl; + std::cout << "\tX distance traveled:\t" << xDistTraveled + prevXPos << " m" + << "\n\tOdom X:\t\t" << odom.X() << " m" << std::endl; - // The Y odom reading should have increased by the cosine of the header * + // The Y odom reading should have increased by the cosine of the heading * // half the wheel separation. double yDistTraveled = (wheelSeparation * 0.5) - cos(atan2(distPerDegree, wheelSeparation)) * wheelSeparation * 0.5; double prevYPos = 0.0; - std::cout << "\tY distance traveled " << yDistTraveled + prevYPos - << " Odom Y: " << odom.Y() << std::endl; + std::cout << "\tY distance traveled:\t" << yDistTraveled + prevYPos << " m" + << "\n\tOdom Y:\t\t" << odom.Y() << " m" << std::endl; - // Angular velocity should be the difference between the x and y distance - // traveled divided by the wheel separation divided by the seconds - // elapsed. - std::cout << "Angular velocity should be the difference between the x and y" - << " distance traveled divided by the wheel separation divided by" - << " the seconds elapsed.\n" - << "\tAngular velocity " - << ((xDistTraveled - yDistTraveled) / wheelSeparation) / 0.1 - << " Odom angular velocity: " << *odom.AngularVelocity() << std::endl; + std::cout << "Angular velocity should be the difference between the x and y\n" + << "distance traveled divided by the wheel separation divided by\n" + << "the seconds elapsed.\n" + << "\tAngular velocity:\t" + << ((xDistTraveled - yDistTraveled) / wheelSeparation) / 0.1 << " rad/s" + << "\n\tOdom angular velocity:\t" << *odom.AngularVelocity() << " rad/s" + << std::endl; } //! [complete] diff --git a/examples/diff_drive_odometry.py b/examples/diff_drive_odometry.py index 8e59b2daf..218178b90 100644 --- a/examples/diff_drive_odometry.py +++ b/examples/diff_drive_odometry.py @@ -12,6 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. +# This example will only work if the Python interface library was compiled and +# installed. +# +# Modify the PYTHONPATH environment variable to include the ignition math +# library install path. For example, if you install to /usr: +# +# $ export PYTHONPATH=/usr/lib/python:$PYTHONPATH + import datetime import math @@ -21,7 +29,7 @@ wheelSeparation = 2.0 wheelRadius = 0.5 -wheelCircumference = 2 * 3.1416 * wheelRadius +wheelCircumference = 2 * math.pi * wheelRadius # This is the linear distance traveled per degree of wheel rotation. distPerDegree = wheelCircumference / 360.0 @@ -31,31 +39,33 @@ startTime = datetime.datetime.now() odom.init(datetime.timedelta()) -print('--- Rotate the both wheels by 1 degree. ---') +# Sleep for a little while, then update the odometry with the new wheel +# position. +print('--- Rotate both wheels by 1 degree. ---') time1 = startTime + datetime.timedelta(milliseconds=100) -odom.update(Angle(1.0 * 3.1416 / 180), - Angle(1.0 * 3.1416 / 180), +odom.update(Angle(1.0 * math.pi / 180), + Angle(1.0 * math.pi / 180), time1 - startTime) -print('Linear velocity: {} Odom linear velocity: {}'. +print('\tLinear velocity:\t{} m/s\n\tOdom linear velocity:\t{} m/s'. format(distPerDegree / 0.1, odom.linear_velocity())) -print('Angular velocity should be zero since the "robot" is traveling' + - ' in a straight line:\n' + - '\tOdom angular velocity: {}' +print('Angular velocity should be zero since the "robot" is traveling\n' + + 'in a straight line:\n' + + '\tOdom angular velocity:\t{} rad/s' .format(odom.angular_velocity())) # Sleep again, this time rotate the right wheel by 1 degree. print('--- This time rotate the right wheel by 1 degree. ---'); time2 = time1 + datetime.timedelta(milliseconds=100) -odom.update(Angle(2.0 * 3.1416 / 180), - Angle(3.0 * 3.1416 / 180), +odom.update(Angle(2.0 * math.pi / 180), + Angle(3.0 * math.pi / 180), time2 - startTime) -print('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.\n' + - '\tHeading: {} Odom Heading: {}'.format( +print('The heading should be the arc tangent of the linear distance\n' + + 'traveled by the right wheel (the left wheel was stationary)\n' + + 'divided by the wheel separation.\n' + + '\tHeading:\t\t{} rad\n\tOdom Heading:\t\t{} rad'.format( math.atan2(distPerDegree, wheelSeparation), odom.heading())) @@ -64,23 +74,20 @@ xDistTraveled = math.sin( math.atan2(distPerDegree, wheelSeparation)) * wheelSeparation * 0.5 prevXPos = distPerDegree * 2.0 -print('\tX distance traveled: {} Odom X: {}'.format( +print('\tX distance traveled:\t{} m\n\tOdom X:\t\t{} m'.format( xDistTraveled + prevXPos, odom.x())) -# The Y odom reading should have increased by the cosine of the header * +# The Y odom reading should have increased by the cosine of the heading * # half the wheel separation. yDistTraveled = (wheelSeparation * 0.5) - math.cos( math.atan2(distPerDegree, wheelSeparation)) * wheelSeparation * 0.5 prevYPos = 0.0 -print('\tY distance traveled: {} Odom Y: {}'.format( +print('\tY distance traveled:\t{} m\n\tOdom Y:\t\t{} m'.format( yDistTraveled + prevYPos, odom.y())) -# Angular velocity should be the difference between the x and y distance -# traveled divided by the wheel separation divided by the seconds -# elapsed. -print('Angular velocity should be the difference between the x and y' + - ' distance traveled divided by the wheel separation divided by' + - ' the seconds elapsed.\n' + - '\tAngular velocity: {} Odom angular velocity: {}'.format( +print('Angular velocity should be the difference between the x and y\n' + + 'distance traveled divided by the wheel separation divided by\n' + + 'the seconds elapsed.\n' + + '\tAngular velocity:\t{} rad/s\n\tOdom angular velocity:\t{} rad/s'.format( ((xDistTraveled - yDistTraveled) / wheelSeparation) / 0.1, odom.angular_velocity())) From 95abd03dac227af5f55d30bc532df5b040984a88 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Tue, 28 Dec 2021 20:47:29 +0100 Subject: [PATCH 6/6] update examples MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- examples/diff_drive_odometry.cc | 6 ++++-- examples/diff_drive_odometry.py | 6 ++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/examples/diff_drive_odometry.cc b/examples/diff_drive_odometry.cc index 0a0635145..6f2bcdf8e 100644 --- a/examples/diff_drive_odometry.cc +++ b/examples/diff_drive_odometry.cc @@ -56,8 +56,10 @@ int main(int argc, char **argv) << "\tOdom angular velocity:\t" << *odom.AngularVelocity() << " rad/s" << std::endl; - // Sleep again, this time rotate the right wheel by 1 degree. - std::cout << "--- This time rotate the right wheel by 1 degree. ---" + // Sleep again, this time rotate the left wheel by 1 and the right wheel by 2 + // degrees. + std::cout << "--- This time rotate the left wheel by 1 and the right wheel " + << "by 2 degrees ---" << std::endl; auto time2 = time1 + std::chrono::milliseconds(100); odom.Update(IGN_DTOR(2.0), IGN_DTOR(3.0), time2); diff --git a/examples/diff_drive_odometry.py b/examples/diff_drive_odometry.py index 218178b90..33fbbaa13 100644 --- a/examples/diff_drive_odometry.py +++ b/examples/diff_drive_odometry.py @@ -55,8 +55,10 @@ '\tOdom angular velocity:\t{} rad/s' .format(odom.angular_velocity())) -# Sleep again, this time rotate the right wheel by 1 degree. -print('--- This time rotate the right wheel by 1 degree. ---'); +# Sleep again, this time rotate the left wheel by 1 and the right wheel by 2 +# degrees. +print('--- This time rotate the left wheel by 1 and the right wheel ' + + 'by 2 degrees ---'); time2 = time1 + datetime.timedelta(milliseconds=100) odom.update(Angle(2.0 * math.pi / 180), Angle(3.0 * math.pi / 180),