MecanumDriveOdometry to handle odometry estimation of Mecanum wheeled…

… models (#486) * Class ignition::math::MecanumDriveOdometryPrivate to handle Mecanum wheels odometry Signed-off-by: danilo_gsch <danilo_gsch@hotmail.com> * Update include/ignition/math/MecanumDriveOdometry.hh Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com> Signed-off-by: danilo_gsch <danilo_gsch@hotmail.com> * Update include/ignition/math/MecanumDriveOdometry.hh Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com> Signed-off-by: danilo_gsch <danilo_gsch@hotmail.com> * Update include/ignition/math/MecanumDriveOdometry.hh Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com> Signed-off-by: danilo_gsch <danilo_gsch@hotmail.com> * Update src/MecanumDriveOdometry.cc Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com> Signed-off-by: danilo_gsch <danilo_gsch@hotmail.com> * MecanumDrive doc Signed-off-by: danilo_gsch <danilo_gsch@hotmail.com> * MecanumDrive doc Signed-off-by: danilo_gsch <danilo_gsch@hotmail.com> * Update MecanumDriveOdometry.cc line length corrections * Update include/ignition/math/MecanumDriveOdometry.hh * CI workflow: use checkout v3 (#519) Version v2 of the actions/checkout workflow is deprecated, so switch to v3. Part of gazebo-tooling/release-tools#862. Signed-off-by: Steve Peters <scpeters@openrobotics.org> * Rename COPYING to LICENSE (#521) The LICENSE file contained a copy of the stanze used at the top of source code files, while the actual license was in the COPYING file. So remove the stanza and put the actual Apache 2.0 license text in LICENSE. Signed-off-by: Steve Peters <scpeters@openrobotics.org> * Custom PID error rate (#525) * Custom PID error rate Signed-off-by: Nate Koenig <natekoenig@gmail.com> * added test Signed-off-by: Nate Koenig <natekoenig@gmail.com> --------- Signed-off-by: Nate Koenig <natekoenig@gmail.com> * Add option to skip pybind11 and SWIG (#480) Signed-off-by: Michael Carroll <michael@openrobotics.org> * Disable pybind11 on windows by default (#529) Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org> * 🎈 6.14.0 (#531) Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org> * Lint Signed-off-by: Michael Carroll <mjcarroll@intrinsic.ai> * Port changes and tests from muttistefano Co-authored-by: muttistefano <mutti.stefano.jp@gmail.com> Signed-off-by: Michael Carroll <mjcarroll@intrinsic.ai> * Add accessor and TODO note Signed-off-by: Michael Carroll <mjcarroll@intrinsic.ai> --------- Signed-off-by: danilo_gsch <danilo_gsch@hotmail.com> Signed-off-by: Steve Peters <scpeters@openrobotics.org> Signed-off-by: Nate Koenig <natekoenig@gmail.com> Signed-off-by: Michael Carroll <michael@openrobotics.org> Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org> Signed-off-by: Michael Carroll <mjcarroll@intrinsic.ai> Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com> Co-authored-by: Steve Peters <scpeters@openrobotics.org> Co-authored-by: Nate Koenig <natekoenig@gmail.com> Co-authored-by: Michael Carroll <michael@openrobotics.org> Co-authored-by: Addisu Z. Taddese <addisu@openrobotics.org> Co-authored-by: Michael Carroll <mjcarroll@intrinsic.ai> Co-authored-by: muttistefano <mutti.stefano.jp@gmail.com>
gazebosim · Aug 22, 2023 · 469540c · 469540c
1 parent 3b4cfb2
commit 469540c
Show file tree

Hide file tree

Showing 4 changed files with 606 additions and 0 deletions.
diff --git a/include/gz/math/MecanumDriveOdometry.hh b/include/gz/math/MecanumDriveOdometry.hh
@@ -0,0 +1,153 @@
+/*
+ * Copyright (C) 2022 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_MECANUMDRIVEODOMETRY_HH_
+#define GZ_MATH_MECANUMDRIVEODOMETRY_HH_
+
+#include <chrono>
+#include <memory>
+#include <gz/math/Angle.hh>
+#include <gz/math/Export.hh>
+#include <gz/math/config.hh>
+
+namespace ignition
+{
+  namespace math
+  {
+    // Inline bracket to help doxygen filtering.
+    inline namespace IGNITION_MATH_VERSION_NAMESPACE {
+    // Forward declarations.
+    class MecanumDriveOdometryPrivate;
+
+    /// \class MecanumDriveOdometry MecanumDriveOdometry.hh
+    /// gz/math/MecanumDriveOdometry.hh
+    ///
+    /// \brief Computes odometry values based on a set of kinematic
+    /// properties and wheel speeds for a Mecanum-drive vehicle.
+    ///
+    /// Note: when computing velocity the math currently assumes that
+    /// all wheels have a radius of 1.0.
+    ///
+    /// A vehicle with a heading of zero degrees has a local
+    /// reference frame according to the diagram below.
+    ///
+    ///       Y
+    ///       ^
+    ///       |
+    ///       |
+    ///       O--->X(forward)
+    class IGNITION_MATH_VISIBLE MecanumDriveOdometry
+    {
+      // Use a steady clock
+      public: using clock = std::chrono::steady_clock;
+
+      /// \brief Constructor.
+      /// \param[in] _windowSize Rolling window size used to compute the
+      /// velocity mean
+      public: explicit MecanumDriveOdometry(size_t _windowSize = 10);
+
+      /// \brief Destructor.
+      public: ~MecanumDriveOdometry();
+
+      /// \brief Initialize the odometry
+      /// \param[in] _time Current time.
+      public: void Init(const clock::time_point &_time);
+
+      /// \brief Get whether Init has been called.
+      /// \return True if Init has been called, false otherwise.
+      public: bool Initialized() const;
+
+      /// \brief Updates the odometry class with latest wheels and
+      /// steerings position
+      /// \param[in] _frontLeftPos Left wheel position in radians.
+      /// \param[in] _frontRightPos Right wheel postion in radians.
+      /// \param[in] _backLeftPos Left wheel position in radians.
+      /// \param[in] _backRightPos Right wheel postion in radians.
+      /// \param[in] _time Current time point.
+      /// \return True if the odometry is actually updated.
+      public: bool Update(
+        const Angle &_frontLeftPos, const Angle &_frontRightPos,
+        const Angle &_backLeftPos, const Angle &_backRightPos,
+        const clock::time_point &_time);
+
+      /// \brief Get the heading.
+      /// \return The heading in radians.
+      public: const Angle &Heading() const;
+
+      /// \brief Get the X position.
+      ///  \return The X position in meters
+      public: double X() const;
+
+      /// \brief Get the Y position.
+      /// \return The Y position in meters.
+      public: double Y() const;
+
+      /// \brief Get the linear velocity.
+      /// \return The linear velocity in meter/second.
+      public: double LinearVelocity() const;
+
+      /// \brief Get the lateral velocity.
+      /// \return The lateral velocity in meter/second.
+      public: double LateralVelocity() const;
+
+      /// \brief Get the angular velocity.
+      /// \return The angular velocity in radian/second.
+      public: const Angle &AngularVelocity() const;
+
+      /// \brief Set the wheel parameters including the radius and separation.
+      /// \param[in] _wheelSeparation Distance between left and right wheels.
+      /// \param[in] _wheelBase Distance between front and back wheels.
+      /// \param[in] _leftWheelRadius Radius of the left wheel.
+      /// \param[in] _rightWheelRadius Radius of the right wheel.
+      public: void SetWheelParams(double _wheelSeparation, double _wheelBase,
+                      double _leftWheelRadius, double _rightWheelRadius);
+      /// \brief Set the velocity rolling window size.
+      /// \param[in] _size The Velocity rolling window size.
+      public: void SetVelocityRollingWindowSize(size_t _size);
+
+      /// \brief Get the wheel separation
+      /// \return Distance between left and right wheels in meters.
+      public: double WheelSeparation() const;
+
+      /// \brief Get the wheel base
+      /// \return Distance between front and back wheels in meters.
+      public: double WheelBase() const;
+
+      /// \brief Get the left wheel radius
+      /// \return Left wheel radius in meters.
+      public: double LeftWheelRadius() const;
+
+      /// \brief Get the rightwheel radius
+      /// \return Right wheel radius in meters.
+      public: double RightWheelRadius() const;
+
+
+#ifdef _WIN32
+// Disable warning C4251 which is triggered by
+// std::unique_ptr
+#pragma warning(push)
+#pragma warning(disable: 4251)
+#endif
+      /// \brief Private data pointer.
+      private: std::unique_ptr<MecanumDriveOdometryPrivate> dataPtr;
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
+    };
+    }  // namespace IGNITION_MATH_VERSION_NAMESPACE
+  }  // namespace math
+}  // namespace ignition
+#endif  // GZ_MATH_MECANUMDRIVEODOMETRY_HH_
diff --git a/include/ignition/math/MecanumDriveOdometry.hh b/include/ignition/math/MecanumDriveOdometry.hh
@@ -0,0 +1,19 @@
+/*
+ * Copyright (C) 2022 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 <gz/math/MecanumDriveOdometry.hh>
+#include <ignition/math/config.hh>