Add DiffDriveOdometry pybind11 interface and examples

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})
 

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
 #

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 <iostream>
+#include <chrono>
+
+#include <ignition/math/Angle.hh>
+#include <ignition/math/Helpers.hh>
+#include <ignition/math/DiffDriveOdometry.hh>
+
+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]

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()))

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

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

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 <string>
+#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_<Class>(m,
+                    pyclass_name.c_str(),
+                    py::buffer_protocol(),
+                    py::dynamic_attr())
+    .def(py::init<size_t>())
+    .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

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 <pybind11/pybind11.h>
+#include <pybind11/operators.h>
+#include <pybind11/chrono.h>
+#include <string>
+
+#include <ignition/math/DiffDriveOdometry.hh>
+
+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_

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");