diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 578ad5c62..76e0be122 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/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 new file mode 100644 index 000000000..6f2bcdf8e --- /dev/null +++ b/examples/diff_drive_odometry.cc @@ -0,0 +1,97 @@ +/* + * 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; + + // 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 both wheels by 1 degree. ---" << '\n'; + auto time1 = startTime + std::chrono::milliseconds(100); + odom.Update(IGN_DTOR(1.0), IGN_DTOR(1.0), time1); + + std::cout << "\tLinear velocity:\t" << distPerDegree / 0.1 << " m/s" + << "\n\tOdom linear velocity:\t" << odom.LinearVelocity() << " m/s" + << 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 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); + + 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:\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 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:\t" << yDistTraveled + prevYPos << " m" + << "\n\tOdom Y:\t\t" << odom.Y() << " m" << 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 new file mode 100644 index 000000000..33fbbaa13 --- /dev/null +++ b/examples/diff_drive_odometry.py @@ -0,0 +1,95 @@ +# 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. + +# 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 + +from ignition.math import Angle, DiffDriveOdometry + +odom = DiffDriveOdometry() + +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) +startTime = datetime.datetime.now() +odom.init(datetime.timedelta()) + +# 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 * math.pi / 180), + Angle(1.0 * math.pi / 180), + time1 - startTime) + +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\n' + + 'in a straight line:\n' + + '\tOdom angular velocity:\t{} rad/s' + .format(odom.angular_velocity())) + +# 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), + time2 - startTime) + +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())) + +# 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:\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 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:\t{} m\n\tOdom Y:\t\t{} m'.format( + yDistTraveled + prevYPos, odom.y())) + +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())) diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index bb7874824..ac3436bb5 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -89,7 +89,6 @@ if (PYTHONLIBS_FOUND) set(python_tests Box_TEST Cylinder_TEST - DiffDriveOdometry_TEST Frustum_TEST Inertial_TEST Matrix4_TEST diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index 1926715ec..67df2f119 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -17,6 +17,7 @@ if (${pybind11_FOUND}) src/Angle.cc src/AxisAlignedBox.cc src/Color.cc + src/DiffDriveOdometry.cc src/GaussMarkovProcess.cc src/Helpers.cc src/Kmeans.cc @@ -82,6 +83,7 @@ if (${pybind11_FOUND}) Angle_TEST AxisAlignedBox_TEST Color_TEST + DiffDriveOdometry_TEST Filter_TEST GaussMarkovProcess_TEST Helpers_TEST diff --git a/src/python_pybind11/src/DiffDriveOdometry.cc b/src/python_pybind11/src/DiffDriveOdometry.cc new file mode 100644 index 000000000..025981181 --- /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 +#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..bc261df2d --- /dev/null +++ b/src/python_pybind11/src/DiffDriveOdometry.hh @@ -0,0 +1,45 @@ +/* + * 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 + +#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 c4918147c..1cbe0e5ac 100644 --- a/src/python_pybind11/src/_ignition_math_pybind11.cc +++ b/src/python_pybind11/src/_ignition_math_pybind11.cc @@ -17,6 +17,7 @@ #include "Angle.hh" #include "AxisAlignedBox.hh" #include "Color.hh" +#include "DiffDriveOdometry.hh" #include "Filter.hh" #include "GaussMarkovProcess.hh" #include "Helpers.hh" @@ -53,6 +54,9 @@ PYBIND11_MODULE(math, m) ignition::math::python::defineMathColor(m, "Color"); + ignition::math::python::defineMathDiffDriveOdometry( + m, "DiffDriveOdometry"); + ignition::math::python::defineMathGaussMarkovProcess( m, "GaussMarkovProcess"); 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..282845403 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 * math.pi * 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 * math.pi / 180), + Angle(1.0 * math.pi / 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 * 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) 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 * 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) 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 * 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 # wheel separation.