Skip to content

Commit

Permalink
Merge ign-math6 into gz-math7
Browse files Browse the repository at this point in the history
Signed-off-by: Addisu Z. Taddese <[email protected]>
  • Loading branch information
azeey committed Aug 23, 2023
2 parents e806d4c + a0c9148 commit 551ae2c
Show file tree
Hide file tree
Showing 7 changed files with 625 additions and 6 deletions.
15 changes: 15 additions & 0 deletions Changelog.md
Original file line number Diff line number Diff line change
Expand Up @@ -245,6 +245,21 @@

## Gazebo Math 6.x

## Gazebo Math 6.14.0 (2023-04-14)

1. Disable pybind11 on windows by default
* [Pull request #529](https://github.com/gazebosim/gz-math/pull/529)

1. Add option to skip pybind11 and SWIG
* [Pull request #480](https://github.com/gazebosim/gz-math/pull/480)

1. Custom PID error rate
* [Pull request #525](https://github.com/gazebosim/gz-math/pull/525)

1. Infrastructure
* [Pull request #521](https://github.com/gazebosim/gz-math/pull/521)
* [Pull request #519](https://github.com/gazebosim/gz-math/pull/519)

## Gazebo Math 6.13.0 (2022-08-31)

1. Support migration and migrate headers
Expand Down
153 changes: 153 additions & 0 deletions include/gz/math/MecanumDriveOdometry.hh
Original file line number Diff line number Diff line change
@@ -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 gz
{
namespace math
{
// Inline bracket to help doxygen filtering.
inline namespace GZ_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 GZ_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 GZ_MATH_VERSION_NAMESPACE
} // namespace math
} // namespace gz
#endif // GZ_MATH_MECANUMDRIVEODOMETRY_HH_
19 changes: 19 additions & 0 deletions include/ignition/math/MecanumDriveOdometry.hh
Original file line number Diff line number Diff line change
@@ -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>
4 changes: 2 additions & 2 deletions src/Helpers_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -1017,7 +1017,7 @@ TEST(HelpersTest, AppendToStream)
<< std::setprecision(std::numeric_limits<long double>::digits10 + 1);

math::appendToStream(out, pi);
#ifdef _WIN32
#if defined(_WIN32) && !defined(__amd64__) && !defined(__i386__)
EXPECT_EQ(out.str(), "0 456 0 3.14159 3.141592654 3.141592653589793");
#else
EXPECT_EQ(out.str(), "0 456 0 3.14159 3.141592654 3.141592653589793239");
Expand All @@ -1027,7 +1027,7 @@ TEST(HelpersTest, AppendToStream)
<< std::setprecision(3);

math::appendToStream(out, pi);
#ifdef _WIN32
#if defined(_WIN32) && !defined(__amd64__) && !defined(__i386__)
EXPECT_EQ(out.str(), "0 456 0 3.14159 3.141592654 3.141592653589793 3.14");
#else
EXPECT_EQ(out.str(), "0 456 0 3.14159 3.141592654 3.141592653589793239 3.14");
Expand Down
Loading

0 comments on commit 551ae2c

Please sign in to comment.