diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 1a3c3663a..626ce2ae4 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,6 +1,12 @@ name: Ubuntu CI -on: [push, pull_request] +on: + pull_request: + push: + branches: + - 'ign-math[0-9]' + - 'gz-math[0-9]' + - 'main' jobs: jammy-ci: @@ -8,7 +14,7 @@ jobs: name: Ubuntu Jammy CI steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Compile and test id: ci uses: gazebo-tooling/action-gz-ci@jammy @@ -22,7 +28,7 @@ jobs: name: Ubuntu Noble CI steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Compile and test id: ci uses: gazebo-tooling/action-gz-ci@noble diff --git a/.github/workflows/package_xml.yml b/.github/workflows/package_xml.yml new file mode 100644 index 000000000..4bd4a9aa0 --- /dev/null +++ b/.github/workflows/package_xml.yml @@ -0,0 +1,11 @@ +name: Validate package.xml + +on: + pull_request: + +jobs: + package-xml: + runs-on: ubuntu-latest + name: Validate package.xml + steps: + - uses: gazebo-tooling/action-gz-ci/validate_package_xml@jammy diff --git a/.github/workflows/triage.yml b/.github/workflows/triage.yml index 2c94852da..2332244bf 100644 --- a/.github/workflows/triage.yml +++ b/.github/workflows/triage.yml @@ -14,4 +14,3 @@ jobs: with: project-url: https://github.com/orgs/gazebosim/projects/7 github-token: ${{ secrets.TRIAGE_TOKEN }} - diff --git a/BUILD.bazel b/BUILD.bazel index b89cd10a1..968198e92 100644 --- a/BUILD.bazel +++ b/BUILD.bazel @@ -11,13 +11,23 @@ load( "@gz//bazel/lint:lint.bzl", "add_lint_tests", ) +load( + "@rules_license//rules:license.bzl", + "license", +) package( + default_applicable_licenses = [GZ_ROOT + "math:license"], default_visibility = GZ_VISIBILITY, features = GZ_FEATURES, ) -licenses(["notice"]) # Apache-2.0 +license( + name = "license", + package_name = "gz-math", +) + +licenses(["notice"]) exports_files(["LICENSE"]) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2526df888..7654f04ec 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -99,16 +99,18 @@ endif() if (SKIP_PYBIND11) message(STATUS "SKIP_PYBIND11 set - disabling python bindings") else() - include(GzPython) - find_package(PythonLibs QUIET) - if (NOT PYTHONLIBS_FOUND) - GZ_BUILD_WARNING("Python is missing: Python interfaces are disabled.") - message (STATUS "Searching for Python - not found.") + #include(GzPython) TODO: allow to specify for what it should search and then + # the code below can be removed; e.g. pybind needs Interpreter and Development components + # see https://pybind11.readthedocs.io/en/stable/cmake/index.html#new-findpython-mode + find_package(Python3 QUIET COMPONENTS Interpreter Development) + + if (NOT Python3_FOUND) + GZ_BUILD_WARNING("Python3 is missing: Python interfaces are disabled.") + message (STATUS "Searching for Python3 - not found.") else() - message (STATUS "Searching for Python - found version ${PYTHONLIBS_VERSION_STRING}.") + message (STATUS "Searching for Python3 - found version ${Python3_VERSION}.") set(PYBIND11_PYTHON_VERSION 3) - find_package(Python3 QUIET COMPONENTS Interpreter Development) find_package(pybind11 2.2 QUIET) if (${pybind11_FOUND}) diff --git a/Changelog.md b/Changelog.md index 93b6d74ea..6dbd39459 100644 --- a/Changelog.md +++ b/Changelog.md @@ -4,6 +4,47 @@ ## Gazebo Math 7.x +### Gazebo Math 7.4.0 (2024-03-14) + +1. Added MecanumDriveOdometry Python wrapper + * [Pull request #549](https://github.com/gazebosim/gz-math/pull/549) + +1. Update CI badges in README + * [Pull request #571](https://github.com/gazebosim/gz-math/pull/571) + +1. Infrastructure + * [Pull request #569](https://github.com/gazebosim/gz-math/pull/569) + +1. Suppress warnings on MSVC + * [Pull request #564](https://github.com/gazebosim/gz-math/pull/564) + +1. Remove the use of numeric_limits in appendToStream test + * [Pull request #553](https://github.com/gazebosim/gz-math/pull/553) + +1. Replace CMake Python variables with new ones from FindPython3 module + * [Pull request #402](https://github.com/gazebosim/gz-math/pull/402) + +1. Fix `Matrix3_TEST.py` on Windows with conda-forge dependencies + * [Pull request #561](https://github.com/gazebosim/gz-math/pull/561) + +1. Fix small typo cppgetstarted.md + * [Pull request #560](https://github.com/gazebosim/gz-math/pull/560) + +1. Update Ubuntu Binary installation since apt-key is deprecated + * [Pull request #559](https://github.com/gazebosim/gz-math/pull/559) + +1. Update file tree in README to point out pybind11 + * [Pull request #558](https://github.com/gazebosim/gz-math/pull/558) + +1. Update tutorial/color.md + * [Pull request #557](https://github.com/gazebosim/gz-math/pull/557) + +1. ign->gz in README.md + * [Pull request #556](https://github.com/gazebosim/gz-math/pull/556) + +1. Update example_triangle.md + * [Pull request #555](https://github.com/gazebosim/gz-math/pull/555) + ### Gazebo Math 7.3.0 (2023-08-29) 1. Adds a validity check for Sessions created using the `TimeVaryingVolumetricGrid` @@ -274,6 +315,28 @@ ## Gazebo Math 6.x +## Gazebo Math 6.15.1 (2024-01-05) + +1. Replace CMake Python variables with new ones from FindPython3 module + * [Pull request #402](https://github.com/gazebosim/gz-math/pull/402) + +1. Suppress warnings on MSVC + * [Pull request #564](https://github.com/gazebosim/gz-math/pull/564) + +1. Infrastructure + * [Pull request #569](https://github.com/gazebosim/gz-math/pull/569) + +## Gazebo Math 6.15.0 (2023-09-01) + +1. Fixes for testing in non standard architectures + * [Pull request #546](https://github.com/gazebosim/gz-math/pull/546) + +1. MecanumDriveOdometry to handle odometry estimation of Mecanum wheeled models + * [Pull request #486](https://github.com/gazebosim/gz-math/pull/486) + +1. Infrastructure + * [Pull request #547](https://github.com/gazebosim/gz-math/pull/547) + ## Gazebo Math 6.14.0 (2023-04-14) 1. Disable pybind11 on windows by default diff --git a/README.md b/README.md index a85e80b1e..ce21de7f4 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # Gazebo Math : Math classes and functions for robot applications -**Maintainer:** nate AT openrobotics DOT org +**Maintainer:** scpeters AT openrobotics DOT org [![GitHub open issues](https://img.shields.io/github/issues-raw/gazebosim/gz-math.svg)](https://github.com/gazebosim/gz-math/issues) [![GitHub open pull requests](https://img.shields.io/github/issues-pr-raw/gazebosim/gz-math.svg)](https://github.com/gazebosim/gz-math/pulls) @@ -9,10 +9,10 @@ Build | Status -- | -- -Test coverage | [![codecov](https://codecov.io/gh/gazebosim/gz-math/branch/gz-math8/graph/badge.svg)](https://codecov.io/gh/gazebosim/gz-math/branch/gz-math8) -Ubuntu Focal | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_math-ci-gz-math8-focal-amd64)](https://build.osrfoundation.org/job/ignition_math-ci-gz-math8-focal-amd64) -Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_math-ci-gz-math8-homebrew-amd64)](https://build.osrfoundation.org/job/ignition_math-ci-gz-math8-homebrew-amd64) -Windows | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ign_math-ci-win)](https://build.osrfoundation.org/job/ign_math-ci-win) +Test coverage | [![codecov](https://codecov.io/gh/gazebosim/gz-math/branch/main/graph/badge.svg)](https://codecov.io/gh/gazebosim/gz-math/branch/main) +Ubuntu Jammy | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=gz_math-ci-main-jammy-amd64)](https://build.osrfoundation.org/job/gz_math-ci-main-jammy-amd64) +Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=gz_math-ci-main-homebrew-amd64)](https://build.osrfoundation.org/job/gz_math-ci-main-homebrew-amd64) +Windows | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=gz_math-main-win)](https://build.osrfoundation.org/job/gz_math-main-win) Gazebo Math, a component of [Gazebo](https://gazebosim.org), provides general purpose math classes and functions designed for robotic applications. diff --git a/eigen3/BUILD.bazel b/eigen3/BUILD.bazel index 2b3ba9537..4107f59b8 100644 --- a/eigen3/BUILD.bazel +++ b/eigen3/BUILD.bazel @@ -1,21 +1,28 @@ load( "@gz//bazel/skylark:build_defs.bzl", - "GZ_FEATURES", "GZ_ROOT", "GZ_VISIBILITY", - "gz_configure_header", - "gz_export_header", "gz_include_header", ) +package(default_applicable_licenses = [GZ_ROOT + "math:license"]) + public_headers = glob([ "include/gz/math/eigen3/*.hh", ]) +gz_include_header( + name = "eigen3_hh_genrule", + out = "include/gz/math/eigen3.hh", + hdrs = public_headers, +) + cc_library( name = "eigen3", srcs = public_headers, - hdrs = public_headers, + hdrs = public_headers + [ + "include/gz/math/eigen3.hh", + ], includes = ["include"], visibility = GZ_VISIBILITY, deps = [ diff --git a/include/gz/math/graph/Graph.hh b/include/gz/math/graph/Graph.hh index e5c4395f4..0b48ea2dc 100644 --- a/include/gz/math/graph/Graph.hh +++ b/include/gz/math/graph/Graph.hh @@ -681,6 +681,19 @@ namespace graph return iter->second; } + /// \brief Get a mutable reference to an edge using its Id. + /// \param[in] _id The Id of the edge. + /// \return A mutable reference to the edge with Id = _id or NullEdge if + /// not found. + public: EdgeType &EdgeFromId(const EdgeId &_id) + { + auto iter = this->edges.find(_id); + if (iter == this->edges.end()) + return EdgeType::NullEdge; + + return iter->second; + } + /// \brief Stream insertion operator. The output uses DOT graph /// description language. /// \param[out] _out The output stream. diff --git a/package.xml b/package.xml new file mode 100644 index 000000000..34fb9b8fd --- /dev/null +++ b/package.xml @@ -0,0 +1,25 @@ + + + + gz-math8 + 8.0.0 + Gazebo Math : Math classes and functions for robot applications + Steve Peters + Aditya Pande + Apache License 2.0 + https://github.com/gazebosim/gz-math + + cmake + + gz-cmake4 + pybind11-dev + + eigen + gz-utils3 + + python3-pytest + + + cmake + + diff --git a/src/graph/GraphUndirected_TEST.cc b/src/graph/GraphUndirected_TEST.cc index a49ee4337..20f5e9209 100644 --- a/src/graph/GraphUndirected_TEST.cc +++ b/src/graph/GraphUndirected_TEST.cc @@ -368,6 +368,8 @@ TEST(UndirectedGraphTest, AddEdge) auto edge = graph.EdgeFromId(e2.Id()); EXPECT_DOUBLE_EQ(5.0, edge.Data()); EXPECT_DOUBLE_EQ(6.0, edge.Weight()); + edge.Data() = 7.0; + EXPECT_DOUBLE_EQ(7.0, edge.Data()); // Check that the edges point to the right vertices. EXPECT_EQ(0u, e0.Vertices().first); diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index 6f9fb755a..51f7a6abc 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -1,10 +1,3 @@ -if(WIN32 AND CMAKE_BUILD_TYPE STREQUAL "Debug") - # pybind11 logic for setting up a debug build when both a debug and release - # python interpreter are present in the system seems to be pretty much broken. - # This works around the issue. - set(PYTHON_LIBRARIES "${PYTHON_DEBUG_LIBRARIES}") -endif() - message(STATUS "Building pybind11 interfaces") set(BINDINGS_MODULE_NAME "math${PROJECT_VERSION_MAJOR}") # Split from main extension and converted to pybind11 @@ -29,6 +22,7 @@ pybind11_add_module(${BINDINGS_MODULE_NAME} MODULE src/Matrix3.cc src/Matrix4.cc src/Matrix6.cc + src/MecanumDriveOdometry.cc src/MovingWindowFilter.cc src/PID.cc src/Polynomial3.cc @@ -136,6 +130,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..abcebbf9e --- /dev/null +++ b/src/python_pybind11/src/MecanumDriveOdometry.cc @@ -0,0 +1,74 @@ +/* + * 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 gz +{ +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()) + .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 gz diff --git a/src/python_pybind11/src/MecanumDriveOdometry.hh b/src/python_pybind11/src/MecanumDriveOdometry.hh new file mode 100644 index 000000000..e23435079 --- /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 gz +{ +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 gz + +#endif // GZ_MATH_PYTHON__MECANUMDRIVEODOMETRY_HH_ diff --git a/src/python_pybind11/src/_gz_math_pybind11.cc b/src/python_pybind11/src/_gz_math_pybind11.cc index 8a82494ca..8af6c1476 100644 --- a/src/python_pybind11/src/_gz_math_pybind11.cc +++ b/src/python_pybind11/src/_gz_math_pybind11.cc @@ -37,6 +37,7 @@ #include "Matrix3.hh" #include "Matrix4.hh" #include "Matrix6.hh" +#include "MecanumDriveOdometry.hh" #include "MovingWindowFilter.hh" #include "OrientedBox.hh" #include "PID.hh" @@ -152,6 +153,8 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, 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/DiffDriveOdometry_TEST.py b/src/python_pybind11/test/DiffDriveOdometry_TEST.py index 645839227..707a1363c 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 gz.math8 import Angle, DiffDriveOdometry @@ -38,15 +39,15 @@ 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(datetime.timedelta()) + startTime = datetime.timedelta(time.monotonic()) + 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()) @@ -60,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()) @@ -71,8 +72,8 @@ 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() - odom.init(datetime.timedelta()) + startTime = datetime.timedelta(time.monotonic()) + odom.init(startTime) self.assertEqual(0.0, odom.heading().radian()) self.assertEqual(0.0, odom.x()) self.assertEqual(0.0, odom.y()) @@ -83,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()) @@ -97,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. diff --git a/src/python_pybind11/test/MecanumDriveOdometry_TEST.py b/src/python_pybind11/test/MecanumDriveOdometry_TEST.py new file mode 100644 index 000000000..952118d41 --- /dev/null +++ b/src/python_pybind11/test/MecanumDriveOdometry_TEST.py @@ -0,0 +1,123 @@ +# 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 time +import unittest + +from gz.math8 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.timedelta(time.monotonic()) + 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(math.radians(1.0)), + Angle(math.radians(1.0)), + Angle(math.radians(1.0)), + Angle(math.radians(1.0)), + time1) + 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(math.radians(2.0)), + Angle(math.radians(2.0)), + Angle(math.radians(2.0)), + Angle(math.radians(2.0)), + time2) + 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.timedelta(time.monotonic()) + odom.init(startTime) + 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(math.radians(2.0)), + Angle(math.radians(2.0)), + Angle(math.radians(2.0)), + Angle(math.radians(2.0)), + time1) + 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(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) + 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()