From 62768395a0386e596f5a761fb5f426e0a3f1d050 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 1 Sep 2023 10:53:54 -0500 Subject: [PATCH 01/14] Prepare for 6.15.0 release (#554) Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- Changelog.md | 11 +++++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c3e2bcd4..86b4a979 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-math6 VERSION 6.14.0) +project(ignition-math6 VERSION 6.15.0) #============================================================================ # Find ignition-cmake diff --git a/Changelog.md b/Changelog.md index d578e6c0..3930f045 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,16 @@ ## Gazebo Math 6.x +## 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 From ea9ae349cf87755074b2c97d4241e9270f13dbee Mon Sep 17 00:00:00 2001 From: Bi0T1N <28802083+Bi0T1N@users.noreply.github.com> Date: Fri, 6 Oct 2023 20:57:08 +0200 Subject: [PATCH 02/14] Replace CMake Python variables with new ones from FindPython3 module (#402) Replaces the old variables with the new ones used by the FindPython3 module. Right now IgnPython cannot be used as it only searches for the interpreter but pybind11 also needs the development components, thus it provides a workaround. The Win32 PYTHON_LIBRARIES (PYTHON_DEBUG_LIBRARIES) variable was also removed as there is no equivalent in the new CMake module for getting the debug libraries. --------- Signed-off-by: Bi0T1N Signed-off-by: Addisu Z. Taddese Co-authored-by: Bi0T1N Co-authored-by: Jose Luis Rivero Co-authored-by: Steve Peters Co-authored-by: Addisu Z. Taddese --- CMakeLists.txt | 30 +++++++++++++++++++++++------- src/python_pybind11/CMakeLists.txt | 21 +++++++++++---------- 2 files changed, 34 insertions(+), 17 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 86b4a979..29aef1da 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -95,16 +95,32 @@ endif() if (SKIP_PYBIND11) message(STATUS "SKIP_PYBIND11 set - disabling python bindings") else() - include(IgnPython) - find_package(PythonLibs QUIET) - if (NOT PYTHONLIBS_FOUND) - IGN_BUILD_WARNING("Python is missing: Python interfaces are disabled.") - message (STATUS "Searching for Python - not found.") + #include(IgnPython) 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 + if(${CMAKE_VERSION} VERSION_LESS "3.12.0") + set(IGN_PYTHON_VERSION "3") + find_package(PythonInterp ${IGN_PYTHON_VERSION} QUIET) + if(PYTHONINTERP_FOUND) + set(Python3_FOUND ${PYTHONINTERP_FOUND}) + set(Python3_Interpreter_FOUND ${PYTHONINTERP_FOUND}) + set(Python3_EXECUTABLE ${PYTHON_EXECUTABLE}) + find_package(PythonLibs QUIET) + # we found the interpreter but did we also find the libs? both are required + set(Python3_FOUND ${PYTHONLIBS_FOUND}) + set(Python3_VERSION ${PYTHONLIBS_VERSION_STRING}) + endif() + else() + find_package(Python3 QUIET COMPONENTS Interpreter Development) + endif() + + if (NOT Python3_FOUND) + IGN_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/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index e179e138..56442a58 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -1,8 +1,11 @@ -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}") +if(${CMAKE_VERSION} VERSION_LESS "3.12.0") + # TODO: remove once the minimum CMake version is increased + 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() endif() message(STATUS "Building pybind11 interfaces") @@ -56,17 +59,15 @@ target_link_libraries(math PRIVATE ) if(USE_SYSTEM_PATHS_FOR_PYTHON_INSTALLATION) + # Get install variable from Python3 module if(${CMAKE_VERSION} VERSION_LESS "3.12.0") + # Python3_SITEARCH is available via FindPython3 from 3.12 on, workaround if needed execute_process( - COMMAND "${PYTHON_EXECUTABLE}" -c "if True: + COMMAND "${Python3_EXECUTABLE}" -c "if True: from distutils import sysconfig as sc print(sc.get_python_lib(plat_specific=True))" OUTPUT_VARIABLE Python3_SITEARCH OUTPUT_STRIP_TRAILING_WHITESPACE) - else() - # Get install variable from Python3 module - # Python3_SITEARCH is available from 3.12 on, workaround if needed: - find_package(Python3 COMPONENTS Interpreter) endif() if(USE_DIST_PACKAGES_FOR_PYTHON) From 9a5ad69e92c7b01a981482d49af5baf361d03e0e Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Fri, 13 Oct 2023 17:07:16 -0500 Subject: [PATCH 03/14] Suppress warnings on MSVC (#564) Fixes #257 Signed-off-by: Michael Carroll --- include/gz/math/Helpers.hh | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/include/gz/math/Helpers.hh b/include/gz/math/Helpers.hh index 20222197..62f79b31 100644 --- a/include/gz/math/Helpers.hh +++ b/include/gz/math/Helpers.hh @@ -951,7 +951,10 @@ namespace ignition dayString.erase(dayString.length() - 1); try { - std::stoi(dayString); + // We are only checking if it sucessfully parses, + // and are not concerned with the actual value. + auto day = std::stoi(dayString); + (void) day; } catch (const std::out_of_range &) { From da26a3914516b4e3eb55b79c3a1b40730ddf97d7 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 13 Nov 2023 10:16:32 -0600 Subject: [PATCH 04/14] Update github action workflows (#569) * Use on `push` only on stable branches to avoid duplicate runs * Update project automation Signed-off-by: Addisu Z. Taddese --- .github/workflows/ci.yml | 12 +++++++++--- .github/workflows/triage.yml | 9 +++------ 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index d1d9b2ae..944fb8ff 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: focal-ci: @@ -8,7 +14,7 @@ jobs: name: Ubuntu Focal CI steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Compile and test id: ci uses: ignition-tooling/action-ignition-ci@focal @@ -20,7 +26,7 @@ jobs: name: Ubuntu Jammy CI steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Compile and test id: ci uses: ignition-tooling/action-ignition-ci@jammy diff --git a/.github/workflows/triage.yml b/.github/workflows/triage.yml index 736670e0..2332244b 100644 --- a/.github/workflows/triage.yml +++ b/.github/workflows/triage.yml @@ -10,10 +10,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Add ticket to inbox - uses: technote-space/create-project-card-action@v1 + uses: actions/add-to-project@v0.5.0 with: - PROJECT: Core development - COLUMN: Inbox - GITHUB_TOKEN: ${{ secrets.TRIAGE_TOKEN }} - CHECK_ORG_PROJECT: true - + project-url: https://github.com/orgs/gazebosim/projects/7 + github-token: ${{ secrets.TRIAGE_TOKEN }} From 82ef851a769df89e1b4969e632e4c0311678401f Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 21 Dec 2023 09:47:50 -0800 Subject: [PATCH 05/14] Update CI badges in README (#571) Signed-off-by: Ian Chen --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index ca4bb6f2..e1d6e54d 100644 --- a/README.md +++ b/README.md @@ -10,9 +10,9 @@ Build | Status -- | -- Test coverage | [![codecov](https://codecov.io/gh/gazebosim/gz-math/branch/gz-math7/graph/badge.svg)](https://codecov.io/gh/gazebosim/gz-math/branch/gz-math7) -Ubuntu Focal | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_math-ci-gz-math7-focal-amd64)](https://build.osrfoundation.org/job/ignition_math-ci-gz-math7-focal-amd64) -Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_math-ci-gz-math7-homebrew-amd64)](https://build.osrfoundation.org/job/ignition_math-ci-gz-math7-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) +Ubuntu Jammy | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=gz_math-ci-gz-math7-jammy-amd64)](https://build.osrfoundation.org/job/gz_math-ci-gz-math7-jammy-amd64) +Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=gz_math-ci-gz-math7-homebrew-amd64)](https://build.osrfoundation.org/job/gz_math-ci-gz-math7-homebrew-amd64) +Windows | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=gz_math-7-win)](https://build.osrfoundation.org/job/gz_math-7-win) Gazebo Math, a component of [Gazebo](https://gazebosim.org), provides general purpose math classes and functions designed for robotic applications. From c5c3fc4f5f74a4b014834b8bfe4b995929dc2533 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 5 Jan 2024 14:36:33 -0600 Subject: [PATCH 06/14] Prepare for 6.15.1 (#572) Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- Changelog.md | 11 +++++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 29aef1da..42dc198e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-math6 VERSION 6.15.0) +project(ignition-math6 VERSION 6.15.1) #============================================================================ # Find ignition-cmake diff --git a/Changelog.md b/Changelog.md index 3930f045..c20ffba2 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,16 @@ ## 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 From 5b2522aa68be38d52bcd8f84736b4a20649f845a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 13 Feb 2024 23:16:05 +0100 Subject: [PATCH 07/14] Added MecanumDriveOdometry Python wrapper (#549) 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 | 74 +++++++++++ .../src/MecanumDriveOdometry.hh | 44 +++++++ .../src/_ignition_math_pybind11.cc | 3 + .../test/DiffDriveOdometry_TEST.py | 17 +-- .../test/MecanumDriveOdometry_TEST.py | 123 ++++++++++++++++++ 6 files changed, 255 insertions(+), 8 deletions(-) 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 56442a58..6d009a92 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -31,6 +31,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 @@ -124,6 +125,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 00000000..b299c182 --- /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 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()) + .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 00000000..c6c4502e --- /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 c88ff50c..559f541e 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/DiffDriveOdometry_TEST.py b/src/python_pybind11/test/DiffDriveOdometry_TEST.py index 28284540..a5d11cee 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,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 00000000..8cef51c1 --- /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 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.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() From 1649f76ddee22971c10c9c4bd51d16653145f8b9 Mon Sep 17 00:00:00 2001 From: Jagadeeshan S Date: Thu, 22 Feb 2024 12:15:18 +0530 Subject: [PATCH 08/14] Expose non-const reference to edges Signed-off-by: Jagadeeshan S --- include/gz/math/graph/Graph.hh | 21 +++++++++++++++++---- src/graph/GraphUndirected_TEST.cc | 2 ++ 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/include/gz/math/graph/Graph.hh b/include/gz/math/graph/Graph.hh index e5c4395f..8475b541 100644 --- a/include/gz/math/graph/Graph.hh +++ b/include/gz/math/graph/Graph.hh @@ -423,7 +423,7 @@ namespace graph /// \return A map of edges, where keys are Ids and values are /// references to the edges. An empty map is returned when the provided /// vertex does not exist, or when there are no outgoing edges. - public: const EdgeRef_M IncidentsFrom(const VertexId &_vertex) + public: EdgeRef_M IncidentsFrom(const VertexId &_vertex) const { EdgeRef_M res; @@ -448,7 +448,7 @@ namespace graph /// \return A map of edges, where keys are Ids and values are /// references to the edges. An empty map is returned when the provided /// vertex does not exist, or when there are no outgoing edges. - public: const EdgeRef_M IncidentsFrom( + public: EdgeRef_M IncidentsFrom( const Vertex &_vertex) const { return this->IncidentsFrom(_vertex.Id()); @@ -459,7 +459,7 @@ namespace graph /// \return A map of edges, where keys are Ids and values are /// references to the edges. An empty map is returned when the provided /// vertex does not exist, or when there are no incoming edges. - public: const EdgeRef_M IncidentsTo( + public: EdgeRef_M IncidentsTo( const VertexId &_vertex) const { EdgeRef_M res; @@ -484,7 +484,7 @@ namespace graph /// \return A map of edges, where keys are Ids and values are /// references to the edges. An empty map is returned when the provided /// vertex does not exist, or when there are no incoming edges. - public: const EdgeRef_M IncidentsTo(const Vertex &_vertex) + public: EdgeRef_M IncidentsTo(const Vertex &_vertex) const { return this->IncidentsTo(_vertex.Id()); @@ -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/src/graph/GraphUndirected_TEST.cc b/src/graph/GraphUndirected_TEST.cc index a49ee433..20f5e920 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); From 4db514b722d2c7a59cd1feb7e077883579bb5d79 Mon Sep 17 00:00:00 2001 From: Jagadeeshan S Date: Fri, 1 Mar 2024 11:02:02 +0530 Subject: [PATCH 09/14] Revert changes break ABI Signed-off-by: Jagadeeshan S --- include/gz/math/graph/Graph.hh | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/gz/math/graph/Graph.hh b/include/gz/math/graph/Graph.hh index 8475b541..0b48ea2d 100644 --- a/include/gz/math/graph/Graph.hh +++ b/include/gz/math/graph/Graph.hh @@ -423,7 +423,7 @@ namespace graph /// \return A map of edges, where keys are Ids and values are /// references to the edges. An empty map is returned when the provided /// vertex does not exist, or when there are no outgoing edges. - public: EdgeRef_M IncidentsFrom(const VertexId &_vertex) + public: const EdgeRef_M IncidentsFrom(const VertexId &_vertex) const { EdgeRef_M res; @@ -448,7 +448,7 @@ namespace graph /// \return A map of edges, where keys are Ids and values are /// references to the edges. An empty map is returned when the provided /// vertex does not exist, or when there are no outgoing edges. - public: EdgeRef_M IncidentsFrom( + public: const EdgeRef_M IncidentsFrom( const Vertex &_vertex) const { return this->IncidentsFrom(_vertex.Id()); @@ -459,7 +459,7 @@ namespace graph /// \return A map of edges, where keys are Ids and values are /// references to the edges. An empty map is returned when the provided /// vertex does not exist, or when there are no incoming edges. - public: EdgeRef_M IncidentsTo( + public: const EdgeRef_M IncidentsTo( const VertexId &_vertex) const { EdgeRef_M res; @@ -484,7 +484,7 @@ namespace graph /// \return A map of edges, where keys are Ids and values are /// references to the edges. An empty map is returned when the provided /// vertex does not exist, or when there are no incoming edges. - public: EdgeRef_M IncidentsTo(const Vertex &_vertex) + public: const EdgeRef_M IncidentsTo(const Vertex &_vertex) const { return this->IncidentsTo(_vertex.Id()); From 9f5559212059871c9d1e458dcfe623a0c3667110 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 15 Mar 2024 10:16:50 -0500 Subject: [PATCH 10/14] Prepare for 7.4.0 (#583) Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- Changelog.md | 41 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 42 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 64c6b7c5..8e1e0be5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(gz-math7 VERSION 7.3.0) +project(gz-math7 VERSION 7.4.0) #============================================================================ # Find gz-cmake diff --git a/Changelog.md b/Changelog.md index f4edd5f4..488ae993 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,46 @@ ## 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` From 5bf896fd19c09dd5bd9f2c4381758d15af6486cd Mon Sep 17 00:00:00 2001 From: shameekganguly Date: Mon, 1 Apr 2024 15:37:47 -0700 Subject: [PATCH 11/14] Add missing eigen3.hh header for bazel build (#585) Signed-off-by: Shameek Ganguly --- eigen3/BUILD.bazel | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/eigen3/BUILD.bazel b/eigen3/BUILD.bazel index 2b3ba953..4107f59b 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 = [ From f0d4e59e66b54949937ceed725e50ba26177d667 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Fri, 5 Apr 2024 14:27:26 -0500 Subject: [PATCH 12/14] bazel: correctly export license (#586) Signed-off-by: Michael Carroll --- BUILD.bazel | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/BUILD.bazel b/BUILD.bazel index b89cd10a..968198e9 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"]) From a76819bfa99da5dd79b7de224d80c1058e03ae3e Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 19 Apr 2024 14:11:47 -0500 Subject: [PATCH 13/14] Add package.xml (#581) * Add workflow Signed-off-by: Addisu Z. Taddese --- .github/workflows/package_xml.yml | 11 +++++++++++ README.md | 2 +- package.xml | 25 +++++++++++++++++++++++++ 3 files changed, 37 insertions(+), 1 deletion(-) create mode 100644 .github/workflows/package_xml.yml create mode 100644 package.xml diff --git a/.github/workflows/package_xml.yml b/.github/workflows/package_xml.yml new file mode 100644 index 00000000..4bd4a9aa --- /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/README.md b/README.md index e1d6e54d..22163aeb 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) diff --git a/package.xml b/package.xml new file mode 100644 index 00000000..5e6ff7ee --- /dev/null +++ b/package.xml @@ -0,0 +1,25 @@ + + + + gz-math7 + 7.4.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-cmake3 + pybind11-dev + + eigen + gz-utils2 + + python3-pytest + + + cmake + + From 02e37a63e9e24959424e1b2463a6dbe9195a79bb Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 2 May 2024 10:27:28 -0700 Subject: [PATCH 14/14] Enable 24.04 CI on harmonic (#590) * Don't install python3-distutils on 22.04, 24.04 * Remove old ruby cmake code Signed-off-by: Steve Peters --- .github/ci/packages-focal.apt | 1 + .github/ci/packages.apt | 1 - .github/workflows/ci.yml | 9 +++++++++ src/ruby/CMakeLists.txt | 6 +----- 4 files changed, 11 insertions(+), 6 deletions(-) create mode 100644 .github/ci/packages-focal.apt diff --git a/.github/ci/packages-focal.apt b/.github/ci/packages-focal.apt new file mode 100644 index 00000000..87405495 --- /dev/null +++ b/.github/ci/packages-focal.apt @@ -0,0 +1 @@ +python3-distutils diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt index 09f6e9c5..87b3bbd5 100644 --- a/.github/ci/packages.apt +++ b/.github/ci/packages.apt @@ -2,7 +2,6 @@ libeigen3-dev libgz-cmake3-dev libgz-utils2-dev libpython3-dev -python3-distutils python3-pybind11 python3-pytest ruby-dev diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 932726d5..93ceece0 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -32,3 +32,12 @@ jobs: - name: Compile and test id: ci uses: gazebo-tooling/action-gz-ci@jammy + noble-ci: + runs-on: ubuntu-latest + name: Ubuntu Noble CI + steps: + - name: Checkout + uses: actions/checkout@v4 + - name: Compile and test + id: ci + uses: gazebo-tooling/action-gz-ci@noble diff --git a/src/ruby/CMakeLists.txt b/src/ruby/CMakeLists.txt index 8ad32202..7c541051 100644 --- a/src/ruby/CMakeLists.txt +++ b/src/ruby/CMakeLists.txt @@ -44,11 +44,7 @@ if (RUBY_FOUND) # Create the ruby library set(CMAKE_SWIG_OUTDIR "${CMAKE_BINARY_DIR}/lib/ruby") - if(CMAKE_VERSION VERSION_GREATER 3.8.0) - SWIG_ADD_LIBRARY(${SWIG_RB_LIB} LANGUAGE ruby SOURCES ruby.i ${swig_i_files}) - else() - SWIG_ADD_MODULE(${SWIG_RB_LIB} ruby ruby.i ${swig_i_files}) - endif() + SWIG_ADD_LIBRARY(${SWIG_RB_LIB} LANGUAGE ruby SOURCES ruby.i ${swig_i_files}) # Suppress warnings on SWIG-generated files target_compile_options(${SWIG_RB_LIB} PRIVATE