From bc9b63bf89f94f210dc0d2db33791cc78deaf59d Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 13 Nov 2023 19:53:51 -0800 Subject: [PATCH 1/5] SpeedLimiter: use gz::utils::ImplPtr Signed-off-by: Steve Peters --- include/gz/math/SpeedLimiter.hh | 21 +++------------------ src/SpeedLimiter.cc | 23 ++++++++++++++--------- 2 files changed, 17 insertions(+), 27 deletions(-) diff --git a/include/gz/math/SpeedLimiter.hh b/include/gz/math/SpeedLimiter.hh index b77b81a0b..49ce3ef52 100644 --- a/include/gz/math/SpeedLimiter.hh +++ b/include/gz/math/SpeedLimiter.hh @@ -19,9 +19,9 @@ #define GZ_MATH_SYSTEMS_SPEEDLIMITER_HH_ #include -#include #include #include "gz/math/Helpers.hh" +#include namespace gz { @@ -29,9 +29,7 @@ namespace math { // Inline bracket to help doxygen filtering. inline namespace GZ_MATH_VERSION_NAMESPACE { - // Forward declaration. - class SpeedLimiterPrivate; - + // /// \brief Class to limit velocity, acceleration and jerk. class GZ_MATH_VISIBLE SpeedLimiter { @@ -39,9 +37,6 @@ inline namespace GZ_MATH_VERSION_NAMESPACE { /// There are no limits by default. public: SpeedLimiter(); - /// \brief Destructor. - public: ~SpeedLimiter(); - /// \brief Set minimum velocity limit in m/s, usually <= 0. /// \param[in] _lim Minimum velocity. public: void SetMinVelocity(double _lim); @@ -130,17 +125,7 @@ inline namespace GZ_MATH_VERSION_NAMESPACE { double _prevPrevVel, std::chrono::steady_clock::duration _dt) 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 dataPtr; -#ifdef _WIN32 -#pragma warning(pop) -#endif + GZ_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/src/SpeedLimiter.cc b/src/SpeedLimiter.cc index fe2c1df6f..4ea7fde82 100644 --- a/src/SpeedLimiter.cc +++ b/src/SpeedLimiter.cc @@ -18,11 +18,13 @@ #include "gz/math/Helpers.hh" #include "gz/math/SpeedLimiter.hh" -using namespace gz; -using namespace math; - -/// \brief Private SpeedLimiter data class. -class gz::math::SpeedLimiterPrivate +namespace gz +{ +namespace math +{ +inline namespace GZ_MATH_VERSION_NAMESPACE +{ +class SpeedLimiter::Implementation { /// \brief Minimum velocity limit. public: double minVelocity{-std::numeric_limits::infinity()}; @@ -42,16 +44,19 @@ class gz::math::SpeedLimiterPrivate /// \brief Maximum jerk limit. public: double maxJerk{std::numeric_limits::infinity()}; }; +} +} +} + +using namespace gz; +using namespace math; ////////////////////////////////////////////////// SpeedLimiter::SpeedLimiter() - : dataPtr(std::make_unique()) + : dataPtr(gz::utils::MakeImpl()) { } -////////////////////////////////////////////////// -SpeedLimiter::~SpeedLimiter() = default; - ////////////////////////////////////////////////// void SpeedLimiter::SetMinVelocity(double _lim) { From bda4c99837483b7cc70c152ca2c1db6b003ad0f2 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 13 Nov 2023 20:46:21 -0800 Subject: [PATCH 2/5] MecanumDriveOdometry: minor refactor Move using namespace statements after private class. Signed-off-by: Steve Peters --- src/MecanumDriveOdometry.cc | 27 ++++++++++++++++++--------- 1 file changed, 18 insertions(+), 9 deletions(-) diff --git a/src/MecanumDriveOdometry.cc b/src/MecanumDriveOdometry.cc index 54012a02a..74e9d3782 100644 --- a/src/MecanumDriveOdometry.cc +++ b/src/MecanumDriveOdometry.cc @@ -18,15 +18,18 @@ #include "gz/math/MecanumDriveOdometry.hh" #include "gz/math/RollingMean.hh" -using namespace gz; -using namespace math; - // The implementation was borrowed from: https://github.com/ros-controls/ros_controllers/blob/melodic-devel/diff_drive_controller/src/odometry.cpp // And these calculations are based on the following references: // https://robohub.org/drive-kinematics-skid-steer-and-mecanum-ros-twist-included // https://research.ijcaonline.org/volume113/number3/pxc3901586.pdf -class gz::math::MecanumDriveOdometryPrivate +namespace gz +{ +namespace math +{ +inline namespace GZ_MATH_VERSION_NAMESPACE +{ +class MecanumDriveOdometryPrivate { /// \brief Integrates the pose. /// \param[in] _linear Linear velocity. @@ -51,7 +54,7 @@ class gz::math::MecanumDriveOdometryPrivate public: double y{0.0}; /// \brief Current heading in radians. - public: Angle heading; + public: gz::math::Angle heading; /// \brief Current linear velocity in meter/second. public: double linearVel{0.0}; @@ -60,7 +63,7 @@ class gz::math::MecanumDriveOdometryPrivate public: double lateralVel{0.0}; /// \brief Current angular velocity in radians/second. - public: Angle angularVel; + public: gz::math::Angle angularVel; /// \brief Left wheel radius in meters. public: double leftWheelRadius{0.0}; @@ -87,17 +90,23 @@ class gz::math::MecanumDriveOdometryPrivate public: double backRightWheelOldPos{0.0}; /// \brief Rolling mean accumulators for the linear velocity - public: RollingMean linearMean; + public: gz::math::RollingMean linearMean; /// \brief Rolling mean accumulators for the lateral velocity - public: RollingMean lateralMean; + public: gz::math::RollingMean lateralMean; /// \brief Rolling mean accumulators for the angular velocity - public: RollingMean angularMean; + public: gz::math::RollingMean angularMean; /// \brief Initialized flag. public: bool initialized{false}; }; +} +} +} + +using namespace gz; +using namespace math; ////////////////////////////////////////////////// MecanumDriveOdometry::MecanumDriveOdometry(size_t _windowSize) From 83e174f4753710a83a701246ef9387661031a4df Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 13 Nov 2023 20:47:28 -0800 Subject: [PATCH 3/5] MecanumDriveOdometry: use gz::utils::ImplPtr Signed-off-by: Steve Peters --- include/gz/math/MecanumDriveOdometry.hh | 22 +++------------------- src/MecanumDriveOdometry.cc | 25 +++++++++++++------------ 2 files changed, 16 insertions(+), 31 deletions(-) diff --git a/include/gz/math/MecanumDriveOdometry.hh b/include/gz/math/MecanumDriveOdometry.hh index 2e7a67f9f..76d88e7f3 100644 --- a/include/gz/math/MecanumDriveOdometry.hh +++ b/include/gz/math/MecanumDriveOdometry.hh @@ -18,10 +18,10 @@ #define GZ_MATH_MECANUMDRIVEODOMETRY_HH_ #include -#include #include #include #include +#include namespace gz { @@ -29,9 +29,7 @@ namespace gz { // Inline bracket to help doxygen filtering. inline namespace GZ_MATH_VERSION_NAMESPACE { - // Forward declarations. - class MecanumDriveOdometryPrivate; - + // /// \class MecanumDriveOdometry MecanumDriveOdometry.hh /// gz/math/MecanumDriveOdometry.hh /// @@ -59,9 +57,6 @@ namespace gz /// 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); @@ -134,18 +129,7 @@ namespace gz /// \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 dataPtr; -#ifdef _WIN32 -#pragma warning(pop) -#endif + GZ_UTILS_IMPL_PTR(dataPtr) }; } // namespace GZ_MATH_VERSION_NAMESPACE } // namespace math diff --git a/src/MecanumDriveOdometry.cc b/src/MecanumDriveOdometry.cc index 74e9d3782..879e448a4 100644 --- a/src/MecanumDriveOdometry.cc +++ b/src/MecanumDriveOdometry.cc @@ -29,8 +29,17 @@ namespace math { inline namespace GZ_MATH_VERSION_NAMESPACE { -class MecanumDriveOdometryPrivate +class MecanumDriveOdometry::Implementation { + /// \brief Constructor. + /// \param[in] _windowSize Rolling window size used to compute the + /// velocity mean. + public: explicit Implementation(size_t _windowSize) + : linearMean(_windowSize), lateralMean(_windowSize), + angularMean(_windowSize) + { + } + /// \brief Integrates the pose. /// \param[in] _linear Linear velocity. /// \param[in] _lateral Lateral velocity. @@ -110,15 +119,7 @@ using namespace math; ////////////////////////////////////////////////// MecanumDriveOdometry::MecanumDriveOdometry(size_t _windowSize) - : dataPtr(new MecanumDriveOdometryPrivate) -{ - this->dataPtr->linearMean.SetWindowSize(_windowSize); - this->dataPtr->lateralMean.SetWindowSize(_windowSize); - this->dataPtr->angularMean.SetWindowSize(_windowSize); -} - -////////////////////////////////////////////////// -MecanumDriveOdometry::~MecanumDriveOdometry() + : dataPtr(gz::utils::MakeImpl(_windowSize)) { } @@ -301,7 +302,7 @@ double MecanumDriveOdometry::RightWheelRadius() const } ////////////////////////////////////////////////// -void MecanumDriveOdometryPrivate::IntegrateRungeKutta2( +void MecanumDriveOdometry::Implementation::IntegrateRungeKutta2( double _linear, double _lateral, double _angular) { const double direction = *this->heading + _angular * 0.5; @@ -313,7 +314,7 @@ void MecanumDriveOdometryPrivate::IntegrateRungeKutta2( } ////////////////////////////////////////////////// -void MecanumDriveOdometryPrivate::IntegrateExact(double _linear, +void MecanumDriveOdometry::Implementation::IntegrateExact(double _linear, double _lateral, double _angular) { if (std::fabs(_angular) < 1e-6) From 8edab7dc512261a1df0d585e936c9a526023e1a1 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 13 Nov 2023 20:54:32 -0800 Subject: [PATCH 4/5] Remove unused class *Private declarations Signed-off-by: Steve Peters --- include/gz/math/Capsule.hh | 3 --- include/gz/math/Cylinder.hh | 3 --- include/gz/math/Sphere.hh | 3 --- 3 files changed, 9 deletions(-) diff --git a/include/gz/math/Capsule.hh b/include/gz/math/Capsule.hh index 960f22a68..bf445bac0 100644 --- a/include/gz/math/Capsule.hh +++ b/include/gz/math/Capsule.hh @@ -25,9 +25,6 @@ namespace gz { namespace math { - // Foward declarations - class CapsulePrivate; - // Inline bracket to help doxygen filtering. inline namespace GZ_MATH_VERSION_NAMESPACE { // diff --git a/include/gz/math/Cylinder.hh b/include/gz/math/Cylinder.hh index 1c3a124db..691fc993f 100644 --- a/include/gz/math/Cylinder.hh +++ b/include/gz/math/Cylinder.hh @@ -26,9 +26,6 @@ namespace gz { namespace math { - // Foward declarations - class CylinderPrivate; - // Inline bracket to help doxygen filtering. inline namespace GZ_MATH_VERSION_NAMESPACE { // diff --git a/include/gz/math/Sphere.hh b/include/gz/math/Sphere.hh index 4c0a7b430..2b7dacaca 100644 --- a/include/gz/math/Sphere.hh +++ b/include/gz/math/Sphere.hh @@ -27,9 +27,6 @@ namespace gz { namespace math { - // Foward declarations - class SpherePrivate; - // Inline bracket to help doxygen filtering. inline namespace GZ_MATH_VERSION_NAMESPACE { // From 78a90662c12bcf00e340e3477184caeef656738a Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 17 Jul 2024 00:12:58 -0700 Subject: [PATCH 5/5] Fix comments Signed-off-by: Steve Peters --- include/gz/math/MecanumDriveOdometry.hh | 2 +- include/gz/math/SpeedLimiter.hh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/gz/math/MecanumDriveOdometry.hh b/include/gz/math/MecanumDriveOdometry.hh index 76d88e7f3..21c71e15b 100644 --- a/include/gz/math/MecanumDriveOdometry.hh +++ b/include/gz/math/MecanumDriveOdometry.hh @@ -29,7 +29,6 @@ namespace gz { // Inline bracket to help doxygen filtering. inline namespace GZ_MATH_VERSION_NAMESPACE { - // /// \class MecanumDriveOdometry MecanumDriveOdometry.hh /// gz/math/MecanumDriveOdometry.hh /// @@ -129,6 +128,7 @@ namespace gz /// \return Right wheel radius in meters. public: double RightWheelRadius() const; + /// \brief Private data pointer. GZ_UTILS_IMPL_PTR(dataPtr) }; } // namespace GZ_MATH_VERSION_NAMESPACE diff --git a/include/gz/math/SpeedLimiter.hh b/include/gz/math/SpeedLimiter.hh index 49ce3ef52..db25f4f27 100644 --- a/include/gz/math/SpeedLimiter.hh +++ b/include/gz/math/SpeedLimiter.hh @@ -29,7 +29,6 @@ namespace math { // Inline bracket to help doxygen filtering. inline namespace GZ_MATH_VERSION_NAMESPACE { - // /// \brief Class to limit velocity, acceleration and jerk. class GZ_MATH_VISIBLE SpeedLimiter { @@ -125,6 +124,7 @@ inline namespace GZ_MATH_VERSION_NAMESPACE { double _prevPrevVel, std::chrono::steady_clock::duration _dt) const; + /// \brief Private data pointer. GZ_UTILS_IMPL_PTR(dataPtr) }; }