From 6a146ad0210a967ca10d253340647f4cc618262f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Thu, 5 Dec 2024 16:45:12 +0100 Subject: [PATCH] Remove deprecations: tock (#653) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Remove deprecations: tock Signed-off-by: Carlos Agüero --- Migration.md | 37 ++ include/gz/math/SphericalCoordinates.hh | 102 --- include/gz/math/Stopwatch.hh | 7 - include/gz/math/graph/Edge.hh | 20 - include/gz/math/graph/Vertex.hh | 9 - src/SphericalCoordinates.cc | 182 +----- src/SphericalCoordinates_TEST.cc | 580 ------------------ .../src/SphericalCoordinates.cc | 103 +--- .../test/SphericalCoordinates_TEST.py | 510 --------------- src/ruby/SphericalCoordinates.i | 28 +- 10 files changed, 44 insertions(+), 1534 deletions(-) diff --git a/Migration.md b/Migration.md index 39977009..ce5c083a 100644 --- a/Migration.md +++ b/Migration.md @@ -5,6 +5,43 @@ Deprecated code produces compile-time warnings. These warning serve as notification to users that their code should be upgraded. The next major release will remove the deprecated code. +## Gazebo Math 8.X to 9.X + +1. **SphericalCoordinates.hh** + Remove `math::SphericalCoordinates::LOCAL2` and related functions. + See https://github.com/gazebosim/gz-math/pull/235 . The current behavior + converting `LOCAL` to `GLOBAL` should be correct now, unrolling the previous + incoherent result, only present to preserve behavior. + + + Removed: `gz::math::Vector3d SphericalFromLocalPosition( + const gz::math::Vector3d &_xyz) const` + + Removed: `gz::math::Vector3d GlobalFromLocalVelocity( + const gz::math::Vector3d &_xyz) const` + + Removed: `gz::math::Vector3d LocalFromSphericalPosition( + const gz::math::Vector3d &_latLonEle) const` + + Removed: `gz::math::Vector3d LocalFromGlobalVelocity( + const gz::math::Vector3d &_xyz) const` + + Removed: `gz::math::Vector3d + PositionTransform(const gz::math::Vector3d &_pos, + const CoordinateType &_in, const CoordinateType &_out) const` + + Removed: `gz::math::Vector3d VelocityTransform( + const gz::math::Vector3d &_vel, + const CoordinateType &_in, const CoordinateType &_out) const` + +1. **Stopwatch.hh** + + The `math::clock` type alias has been removed. Please use the underlying + type (`std::chrono::steady_clock`) directly. + +1. **graph/Vertex.hh** + + The `Vertex::NullVertex` static member has been removed. Please use + `Vertex::NullVertex()` instead. + E.g.: https://github.com/gazebosim/gz-math/pull/606/files#diff-0c0220a7e72be70337975433eeddc3f5e072ade5cd80dfb1ac03da233c39c983L153-R153 + +1. **graph/Edge.hh** + + The `Edge::NullEdge` static member has been removed. Please use + `Vertex::NullEdge()` instead. + E.g.: https://github.com/gazebosim/gz-math/pull/606/files#diff-0c0220a7e72be70337975433eeddc3f5e072ade5cd80dfb1ac03da233c39c983L222-R222 + ## Gazebo Math 7.X to 8.X ### Breaking Changes diff --git a/include/gz/math/SphericalCoordinates.hh b/include/gz/math/SphericalCoordinates.hh index ada3bd26..8d0e5853 100644 --- a/include/gz/math/SphericalCoordinates.hh +++ b/include/gz/math/SphericalCoordinates.hh @@ -27,13 +27,6 @@ #include #include -// MSVC doesn't like deprecating enum values with function macros -#if _MSC_VER -#define GZ_LOCAL2_DEPRECATED [[deprecated]] -#else -#define GZ_LOCAL2_DEPRECATED GZ_DEPRECATED(8) -#endif - namespace gz::math { // Inline bracket to help doxygen filtering. @@ -74,12 +67,6 @@ namespace gz::math /// \brief Heading-adjusted tangent plane (X, Y, Z) LOCAL = 4, - - /// \brief Heading-adjusted tangent plane (X, Y, Z) - /// \deprecated The computation bugs for LOCAL have been fixed - /// in Gazebo Ionic (8) so the temporary LOCAL2 type - /// is no more needed. - LOCAL2 GZ_LOCAL2_DEPRECATED = 5 }; /// \brief Constructor. @@ -111,21 +98,6 @@ namespace gz::math const double _elevation, const gz::math::Angle &_heading); - /// \brief Convert a Cartesian position vector to geodetic coordinates. - /// This performs a `PositionTransform` from LOCAL to SPHERICAL. - /// - /// \deprecated There's a known bug with this computation that can't be - /// fixed until Gazebo 8 to avoid behaviour changes. - /// Call `SphericalFromLocalPosition(CoordinateVector3)` for correct - /// results. - /// - /// \param[in] _xyz Cartesian position vector in the heading-adjusted - /// world frame. - /// \return Cooordinates: geodetic latitude (deg), longitude (deg), - /// altitude above sea level (m). - public: GZ_DEPRECATED(8) gz::math::Vector3d SphericalFromLocalPosition( - const gz::math::Vector3d &_xyz) const; - /// \brief Convert a Cartesian position vector to geodetic coordinates. /// This performs a `PositionTransform` from LOCAL to SPHERICAL. /// @@ -136,21 +108,6 @@ namespace gz::math public: std::optional SphericalFromLocalPosition( const gz::math::CoordinateVector3 &_xyz) const; - /// \brief Convert a Cartesian velocity vector in the local frame - /// to a global Cartesian frame with components East, North, Up. - /// This is a wrapper around `VelocityTransform(_xyz, LOCAL, GLOBAL)` - /// - /// \deprecated There's a known bug with this computation that can't be - /// fixed until Gazebo 8 to avoid behaviour changes. - /// Call `GlobalFromLocalVelocity(CoordinateVector3)` for correct - /// results. - /// - /// \param[in] _xyz Cartesian velocity vector in the heading-adjusted - /// world frame. - /// \return Rotated vector with components (x,y,z): (East, North, Up). - public: GZ_DEPRECATED(8) gz::math::Vector3d GlobalFromLocalVelocity( - const gz::math::Vector3d &_xyz) const; - /// \brief Convert a Cartesian velocity vector in the local frame /// to a global Cartesian frame with components East, North, Up. /// This is a wrapper around `VelocityTransform(_xyz, LOCAL, GLOBAL)` @@ -192,8 +149,6 @@ namespace gz::math /// latitude and longitude. It assumes that both points are at sea level. /// Example: _latA = 38.0016667 and _lonA = -123.0016667) represents /// the point with latitude 38d 0'6.00"N and longitude 123d 0'6.00"W. - /// This is different from the deprecated static Distance() method as it - /// takes into account the set surface's radius. /// \param[in] _latA Latitude of point A. /// \param[in] _lonA Longitude of point A. /// \param[in] _latB Latitude of point B. @@ -273,15 +228,6 @@ namespace gz::math /// \param[in] _angle Heading offset for the frame. public: void SetHeadingOffset(const gz::math::Angle &_angle); - /// \brief Convert a geodetic position vector to Cartesian coordinates. - /// This performs a `PositionTransform` from SPHERICAL to LOCAL. - /// \deprecated Use `LocalFromSphericalPosition(CoordinateVector3)` instead. - /// \param[in] _latLonEle Geodetic position in the planetary frame of - /// reference. X: latitude (deg), Y: longitude (deg), X: altitude. - /// \return Cartesian position vector in the heading-adjusted world frame. - public: GZ_DEPRECATED(8) gz::math::Vector3d LocalFromSphericalPosition( - const gz::math::Vector3d &_latLonEle) const; - /// \brief Convert a geodetic position vector to Cartesian coordinates. /// This performs a `PositionTransform` from SPHERICAL to LOCAL. /// \param[in] _latLonEle Geodetic position in the planetary frame of @@ -290,15 +236,6 @@ namespace gz::math public: std::optional LocalFromSphericalPosition( const gz::math::CoordinateVector3 &_latLonEle) const; - /// \brief Convert a Cartesian velocity vector with components East, - /// North, Up to a local cartesian frame vector XYZ. - /// This is a wrapper around `VelocityTransform(_xyz, GLOBAL, LOCAL)` - /// \deprecated Use `LocalFromSphericalPosition(CoordinateVector3)` instead. - /// \param[in] _xyz Vector with components (x,y,z): (East, North, Up). - /// \return Cartesian vector in the world frame. - public: GZ_DEPRECATED(8) gz::math::Vector3d LocalFromGlobalVelocity( - const gz::math::Vector3d &_xyz) const; - /// \brief Convert a Cartesian velocity vector with components East, /// North, Up to a local cartesian frame vector XYZ. /// This is a wrapper around `VelocityTransform(_xyz, GLOBAL, LOCAL)` @@ -310,26 +247,6 @@ namespace gz::math /// \brief Update coordinate transformation matrix with reference location public: void UpdateTransformationMatrix(); - /// \brief Convert between positions in SPHERICAL/ECEF/LOCAL/GLOBAL frame - /// using the reference point. - /// \deprecated To keep compatibility with Gazebo 7 and earlier, this - /// function returns incorrect result for LOCAL->SPHERICAL - /// and LOCAL->GLOBAL cases. Preferably, switch to using - /// `PositionTransform(CoordinateVector3, ..., LOCAL)` which - /// yields correct results. If you need to use this deprecated - /// overload, use coordinate type LOCAL2 to get the correct - /// result. - /// \param[in] _pos Position vector in frame defined by parameter _in. - /// If it is spherical, it has to be in radians. - /// \param[in] _in CoordinateType for input - /// \param[in] _out CoordinateType for output - /// \return Transformed coordinate using cached origin. Spherical - /// coordinates will be in radians. In case of error, `_pos` is - /// returned unchanged. - public: GZ_DEPRECATED(8) gz::math::Vector3d - PositionTransform(const gz::math::Vector3d &_pos, - const CoordinateType &_in, const CoordinateType &_out) const; - /// \brief Convert between positions in SPHERICAL/ECEF/LOCAL/GLOBAL frame /// using the cached reference point. /// \param[in] _pos Position vector in frame defined by parameter _in @@ -340,25 +257,6 @@ namespace gz::math PositionTransform(const gz::math::CoordinateVector3 &_pos, const CoordinateType &_in, const CoordinateType &_out) const; - /// \brief Convert between velocity in ECEF/LOCAL/GLOBAL frame using the - /// cached reference point. - /// \note Spherical coordinates are not supported. - /// \deprecated To keep compatibility with Gazebo 7 and earlier, this - /// function returns incorrect result for LOCAL->SPHERICAL - /// and LOCAL->GLOBAL cases. Preferably, switch to using - /// `VelocityTransform(CoordinateVector3, ..., LOCAL)` which - /// yields correct results. If you need to use this deprecated - /// overload, use coordinate type LOCAL2 to get the correct - /// result. - /// \param[in] _vel Velocity vector in frame defined by parameter _in - /// \param[in] _in CoordinateType for input - /// \param[in] _out CoordinateType for output - /// \return Transformed velocity vector. In case of error, `_vel` is - /// returned unchanged. - public: GZ_DEPRECATED(8) gz::math::Vector3d VelocityTransform( - const gz::math::Vector3d &_vel, - const CoordinateType &_in, const CoordinateType &_out) const; - /// \brief Convert between velocity in ECEF/LOCAL/GLOBAL frame. /// \note Spherical coordinates are not supported. /// \param[in] _vel Velocity vector in frame defined by parameter _in diff --git a/include/gz/math/Stopwatch.hh b/include/gz/math/Stopwatch.hh index e9d4bc08..74a9396c 100644 --- a/include/gz/math/Stopwatch.hh +++ b/include/gz/math/Stopwatch.hh @@ -25,13 +25,6 @@ namespace gz::math { - // Use a steady clock - // This alias is now deprecated; please use std::chrono::steady_clock - // directly instead. - using clock - [[deprecated("As of 8.0, use std::chrono::steady_clock directly.")]] - = std::chrono::steady_clock; - // Inline bracket to help doxygen filtering. inline namespace GZ_MATH_VERSION_NAMESPACE { /// \class Stopwatch Stopwatch.hh gz/math/Stopwatch.hh diff --git a/include/gz/math/graph/Edge.hh b/include/gz/math/graph/Edge.hh index f02f4ba4..83bebc70 100644 --- a/include/gz/math/graph/Edge.hh +++ b/include/gz/math/graph/Edge.hh @@ -203,10 +203,6 @@ namespace graph template class UndirectedEdge : public Edge { - /// \brief An invalid undirected edge. - // Deprecated in favor of NullEdge(). - public: static UndirectedEdge GZ_DEPRECATED(8) NullEdge; - /// \brief Constructor. /// \param[in] _vertices The vertices of the edge. /// \param[in] _data The data stored in the edge. @@ -256,22 +252,12 @@ namespace graph } }; - /// \brief An invalid undirected edge. - // Deprecated in favor of NullEdge(). - template - UndirectedEdge UndirectedEdge::NullEdge( - VertexId_P(kNullId, kNullId), E(), 1.0, kNullId); - /// \brief A directed edge represents a connection between two vertices. /// The connection is unidirectional, it's only possible to traverse the edge /// in one direction (from the tail to the head). template class DirectedEdge : public Edge { - /// \brief An invalid directed edge. - // Deprecated in favor of NullEdge(). - public: static DirectedEdge GZ_DEPRECATED(8) NullEdge; - /// \brief Constructor. /// \param[in] _vertices The vertices of the edge. /// \param[in] _data The data stored in the edge. @@ -333,12 +319,6 @@ namespace graph } }; - /// \brief An invalid directed edge. - // Deprecated in favor of NullEdge(). - template - DirectedEdge DirectedEdge::NullEdge( - VertexId_P(kNullId, kNullId), E(), 1.0, kNullId); - /// \brief An invalid edge. template EdgeType &NullEdge() diff --git a/include/gz/math/graph/Vertex.hh b/include/gz/math/graph/Vertex.hh index f6d2a71e..fd6333d8 100644 --- a/include/gz/math/graph/Vertex.hh +++ b/include/gz/math/graph/Vertex.hh @@ -52,10 +52,6 @@ namespace graph template class Vertex { - /// \brief An invalid vertex. - // Deprecated in favor of NullVertex(). - public: static Vertex GZ_DEPRECATED(8) NullVertex; - /// \brief Constructor. /// \param[in] _name Non-unique vertex name. /// \param[in] _data User information. @@ -135,11 +131,6 @@ namespace graph private: VertexId id = kNullId; }; - /// \brief An invalid vertex. - // Deprecated in favor of NullVertex(). - template - Vertex Vertex::NullVertex("__null__", V(), kNullId); - /// \brief An invalid vertex. template Vertex &NullVertex() diff --git a/src/SphericalCoordinates.cc b/src/SphericalCoordinates.cc index f84a8a6b..4e427e8e 100644 --- a/src/SphericalCoordinates.cc +++ b/src/SphericalCoordinates.cc @@ -373,18 +373,6 @@ void SphericalCoordinates::SetHeadingOffset(const Angle &_angle) this->UpdateTransformationMatrix(); } -////////////////////////////////////////////////// -Vector3d SphericalCoordinates::SphericalFromLocalPosition( - const Vector3d &_xyz) const -{ - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - Vector3d result = this->PositionTransform(_xyz, LOCAL, SPHERICAL); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - result.X(GZ_RTOD(result.X())); - result.Y(GZ_RTOD(result.Y())); - return result; -} - ////////////////////////////////////////////////// std::optional SphericalCoordinates::SphericalFromLocalPosition( @@ -393,18 +381,6 @@ SphericalCoordinates::SphericalFromLocalPosition( return this->PositionTransform(_xyz, LOCAL, SPHERICAL); } -////////////////////////////////////////////////// -Vector3d SphericalCoordinates::LocalFromSphericalPosition( - const Vector3d &_xyz) const -{ - Vector3d result = _xyz; - result.X(GZ_DTOR(result.X())); - result.Y(GZ_DTOR(result.Y())); - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - return this->PositionTransform(result, SPHERICAL, LOCAL); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION -} - ////////////////////////////////////////////////// std::optional SphericalCoordinates::LocalFromSphericalPosition( @@ -413,15 +389,6 @@ SphericalCoordinates::LocalFromSphericalPosition( return this->PositionTransform(_xyz, SPHERICAL, LOCAL); } -////////////////////////////////////////////////// -Vector3d SphericalCoordinates::GlobalFromLocalVelocity( - const Vector3d &_xyz) const -{ - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - return this->VelocityTransform(_xyz, LOCAL, GLOBAL); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION -} - ////////////////////////////////////////////////// std::optional SphericalCoordinates::GlobalFromLocalVelocity( @@ -430,15 +397,6 @@ SphericalCoordinates::GlobalFromLocalVelocity( return this->VelocityTransform(_xyz, LOCAL, GLOBAL); } -////////////////////////////////////////////////// -Vector3d SphericalCoordinates::LocalFromGlobalVelocity( - const Vector3d &_xyz) const -{ - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - return this->VelocityTransform(_xyz, GLOBAL, LOCAL); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION -} - ////////////////////////////////////////////////// std::optional SphericalCoordinates::LocalFromGlobalVelocity( @@ -553,20 +511,11 @@ void SphericalCoordinates::UpdateTransformationMatrix() *this->PositionTransform(this->dataPtr->origin, SPHERICAL, ECEF); } -namespace -{ - ///////////////////////////////////////////////// -std::optional PositionTransformTmp( - const gz::utils::ImplPtr& dataPtr, - const CoordinateVector3 &_pos, - const SphericalCoordinates::CoordinateType &_in, - const SphericalCoordinates::CoordinateType &_out) +std::optional SphericalCoordinates::PositionTransform( + const CoordinateVector3 &_pos, + const CoordinateType &_in, const CoordinateType &_out) const { - // This unusual concept of passing dataPtr as a free-function argument is just - // a temporary measure for GZ 8. Starting with GZ 9, the code of this function - // should be moved inside PositionTransform(CoordinateVector3). - if ((_in == SphericalCoordinates::SPHERICAL) != _pos.IsSpherical()) { std::cerr << "Invalid input to PositionTransform. " @@ -579,24 +528,8 @@ std::optional PositionTransformTmp( switch (_in) { // East, North, Up (ENU) - // When branching code for GZ 9, replace this code block with the block - // from LOCAL2 and remove the LOCAL2 block. case SphericalCoordinates::LOCAL: { - // This is incorrect computation - tmp.X(-*_pos.X() * dataPtr->cosHea + *_pos.Y() * dataPtr->sinHea); - tmp.Y(-*_pos.X() * dataPtr->sinHea - *_pos.Y() * dataPtr->cosHea); - tmp.Z(*_pos.Z()); - tmp = *dataPtr->origin.AsMetricVector() + - dataPtr->rotGlobalToECEF * tmp; - break; - } - - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - case SphericalCoordinates::LOCAL2: - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - { - // This is correct computation tmp.X(*_pos.X() * dataPtr->cosHea + *_pos.Y() * dataPtr->sinHea); tmp.Y(-*_pos.X() * dataPtr->sinHea + *_pos.Y() * dataPtr->cosHea); tmp.Z(*_pos.Z()); @@ -685,9 +618,6 @@ std::optional PositionTransformTmp( // Convert from ECEF TO LOCAL case SphericalCoordinates::LOCAL: - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - case SphericalCoordinates::LOCAL2: - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION tmp = dataPtr->rotECEFToGlobal * (tmp - *dataPtr->origin.AsMetricVector()); @@ -710,63 +640,11 @@ std::optional PositionTransformTmp( return res; } -} - -///////////////////////////////////////////////// -Vector3d SphericalCoordinates::PositionTransform( - const Vector3d &_pos, - const CoordinateType &_in, const CoordinateType &_out) const -{ - // This deprecated implementation accepts and returns radians for spherical - // coordinates and has a computation bug when working with LOCAL frames. - - CoordinateVector3 vec = _in == SPHERICAL ? - CoordinateVector3::Spherical(_pos.X(), _pos.Y(), _pos.Z()) : - CoordinateVector3::Metric(_pos.X(), _pos.Y(), _pos.Z()); - - const auto result = PositionTransformTmp(this->dataPtr, vec, _in, _out); - if (!result) - return _pos; - - return result->IsMetric() ? - *result->AsMetricVector() : - Vector3d{result->Lat()->Radian(), result->Lon()->Radian(), *result->Z()}; -} - -///////////////////////////////////////////////// -std::optional SphericalCoordinates::PositionTransform( - const CoordinateVector3 &_pos, - const CoordinateType &_in, const CoordinateType &_out) const -{ - // Temporarily, for Gazebo 8, this function turns all LOCAL frames into - // LOCAL2 to get correct results. Basically, LOCAL and LOCAL2 are equal - // when PositionTransform() is called with a CoordinateVector3 argument, while - // it returns the compatible (but wrong) result when called with Vector3d and - // LOCAL. From Gazebo 9 onwards, LOCAL2 frame will be removed and these - // differences will disappear. - // TODO(peci1): Move PositionTransformTmp code into this function in GZ 9. - - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - const auto in = _in == LOCAL ? LOCAL2 : _in; - const auto out = _out == LOCAL ? LOCAL2 : _out; - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - return PositionTransformTmp(this->dataPtr, _pos, in, out); -} - -namespace -{ - ////////////////////////////////////////////////// -std::optional VelocityTransformTmp( - const gz::utils::ImplPtr& dataPtr, +std::optional SphericalCoordinates::VelocityTransform( const CoordinateVector3 &_vel, - const SphericalCoordinates::CoordinateType &_in, - const SphericalCoordinates::CoordinateType &_out) + const CoordinateType &_in, const CoordinateType &_out) const { - // This unusual concept of passing dataPtr as a free-function argument is just - // a temporary measure for GZ 8. Starting with GZ 9, the code of this function - // should be moved inside VelocityTransform(CoordinateVector3). - // Sanity check -- velocity should not be expressed in spherical coordinates if (_in == SphericalCoordinates::SPHERICAL || _out == SphericalCoordinates::SPHERICAL || @@ -783,18 +661,7 @@ std::optional VelocityTransformTmp( switch (_in) { // ENU - // When branching code for GZ 9, replace this code block with the block - // from LOCAL2 and remove the LOCAL2 block. case SphericalCoordinates::LOCAL: - // This is incorrect computation - tmp.X(-*_vel.X() * dataPtr->cosHea + *_vel.Y() * dataPtr->sinHea); - tmp.Y(-*_vel.X() * dataPtr->sinHea - *_vel.Y() * dataPtr->cosHea); - tmp = dataPtr->rotGlobalToECEF * tmp; - break; - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - case SphericalCoordinates::LOCAL2: - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - // This is correct computation tmp.X(*_vel.X() * dataPtr->cosHea + *_vel.Y() * dataPtr->sinHea); tmp.Y(-*_vel.X() * dataPtr->sinHea + *_vel.Y() * dataPtr->cosHea); tmp = dataPtr->rotGlobalToECEF * tmp; @@ -829,9 +696,6 @@ std::optional VelocityTransformTmp( // Convert from ECEF to local case SphericalCoordinates::LOCAL: - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - case SphericalCoordinates::LOCAL2: - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION tmp = dataPtr->rotECEFToGlobal * tmp; res.SetMetric( tmp.X() * dataPtr->cosHea - tmp.Y() * dataPtr->sinHea, @@ -847,42 +711,6 @@ std::optional VelocityTransformTmp( return res; } -} - -////////////////////////////////////////////////// -Vector3d SphericalCoordinates::VelocityTransform( - const Vector3d &_vel, - const CoordinateType &_in, const CoordinateType &_out) const -{ - auto vec = CoordinateVector3::Metric(_vel); - - const auto result = VelocityTransformTmp(this->dataPtr, vec, _in, _out); - if (!result || !result->IsMetric()) - return _vel; - - return *result->AsMetricVector(); -} - -////////////////////////////////////////////////// -std::optional SphericalCoordinates::VelocityTransform( - const CoordinateVector3 &_vel, - const CoordinateType &_in, const CoordinateType &_out) const -{ - // Temporarily, for Gazebo 8, this function turns all LOCAL frames into - // LOCAL2 to get correct results. Basically, LOCAL and LOCAL2 are equal - // when VelocityTransform() is called with a CoordinateVector3 argument, while - // it returns the compatible (but wrong) result when called with Vector3d and - // LOCAL. From Gazebo 9 onwards, LOCAL2 frame will be removed and these - // differences will disappear. - // TODO(peci1): Move VelocityTransformTmp code into this function in GZ 9. - - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - const auto in = _in == LOCAL ? LOCAL2 : _in; - const auto out = _out == LOCAL ? LOCAL2 : _out; - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - return VelocityTransformTmp(this->dataPtr, _vel, in, out); -} - ////////////////////////////////////////////////// bool SphericalCoordinates::operator==(const SphericalCoordinates &_sc) const { diff --git a/src/SphericalCoordinates_TEST.cc b/src/SphericalCoordinates_TEST.cc index 4e6ed798..c1f444e1 100644 --- a/src/SphericalCoordinates_TEST.cc +++ b/src/SphericalCoordinates_TEST.cc @@ -430,235 +430,6 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) } } -////////////////////////////////////////////////// -// Test coordinate transformations -// TODO(peci1): Remove this test in Gazebo 9 -TEST(SphericalCoordinatesTest, CoordinateTransformsDeprecated) -{ - // Default surface type - math::SphericalCoordinates::SurfaceType st = - math::SphericalCoordinates::EARTH_WGS84; - - { - // Parameters - math::Angle lat(0.3), lon(-1.2), - heading(math::Angle::HalfPi); - double elev = 354.1; - math::SphericalCoordinates sc(st, lat, lon, elev, heading); - - // Check GlobalFromLocal with heading offset of 90 degrees - // Heading 0: X == East, Y == North, Z == Up - // Heading 90: X == North, Y == West , Z == Up - { - // local frame - math::Vector3d xyz; - // east, north, up - math::Vector3d enu; - - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - xyz.Set(1, 0, 0); - enu = sc.GlobalFromLocalVelocity(xyz); - EXPECT_NEAR(enu.Y()/*North*/, xyz.X(), 1e-6); - EXPECT_NEAR(enu.X()/*East*/, -xyz.Y(), 1e-6); - EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); - - xyz.Set(0, 1, 0); - enu = sc.GlobalFromLocalVelocity(xyz); - EXPECT_NEAR(enu.Y(), xyz.X(), 1e-6); - EXPECT_NEAR(enu.X(), -xyz.Y(), 1e-6); - EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); - - xyz.Set(1, -1, 0); - enu = sc.GlobalFromLocalVelocity(xyz); - EXPECT_NEAR(enu.Y(), xyz.X(), 1e-6); - EXPECT_NEAR(enu.X(), -xyz.Y(), 1e-6); - EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); - - xyz.Set(2243.52334, 556.35, 435.6553); - enu = sc.GlobalFromLocalVelocity(xyz); - EXPECT_NEAR(enu.Y(), xyz.X(), 1e-6); - EXPECT_NEAR(enu.X(), -xyz.Y(), 1e-6); - EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - } - - // Check SphericalFromLocal - { - // local frame - math::Vector3d xyz; - // spherical coordinates - math::Vector3d sph; - - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - // No offset - xyz.Set(0, 0, 0); - sph = sc.SphericalFromLocalPosition(xyz); - // latitude - EXPECT_NEAR(sph.X(), lat.Degree(), 1e-6); - // longitude - EXPECT_NEAR(sph.Y(), lon.Degree(), 1e-6); - // elevation - EXPECT_NEAR(sph.Z(), elev, 1e-6); - - // 200 km offset in x (pi/2 heading offset means North). We use - // SphericalFromLocal, which means that xyz is a linear movement on - // a plane (not along the curvature of Earth). This will result in - // a large height offset. - xyz.Set(2e5, 0, 0); - sph = sc.SphericalFromLocalPosition(xyz); - // increase in latitude about 1.8 degrees - EXPECT_NEAR(sph.X(), lat.Degree() + 1.8, 0.008); - // no change in longitude - EXPECT_NEAR(sph.Z(), 3507.024791, 1e-6); - - math::Vector3d xyz2 = sc.LocalFromSphericalPosition(sph); - EXPECT_EQ(xyz, xyz2); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - } - - // Check position projection - { - // WGS84 coordinate obtained from online mapping software - // > gdaltransform -s_srs WGS84 -t_srs EPSG:4978 - // > latitude longitude altitude - // > X Y Z - math::Vector3d tmp; - math::Vector3d osrf_s(37.3877349, -122.0651166, 32.0); - math::Vector3d osrf_e( - -2693701.91434394, -4299942.14687992, 3851691.0393571); - math::Vector3d goog_s(37.4216719, -122.0821853, 30.0); - - // Local tangent plane coordinates (ENU = GLOBAL) coordinates of - // Google when OSRF is taken as the origin: - // > proj +ellps=WGS84 +proj=tmerc - // +lat_0=37.3877349 +lon_0=-122.0651166 +k=1 +x_0=0 +y_0=0 - // > -122.0821853 37.4216719 (LON,LAT) - // > -1510.88 3766.64 (EAST,NORTH) - math::Vector3d vec(-1510.88, 3766.64, -3.29); - - // Convert degrees to radians - osrf_s.X() *= 0.0174532925; - osrf_s.Y() *= 0.0174532925; - - // Set the ORIGIN to be the Open Source Robotics Foundation - math::SphericalCoordinates sc2(st, math::Angle(osrf_s.X()), - math::Angle(osrf_s.Y()), osrf_s.Z(), - math::Angle::Zero); - - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - // Check that SPHERICAL -> ECEF works - tmp = sc2.PositionTransform(osrf_s, - math::SphericalCoordinates::SPHERICAL, - math::SphericalCoordinates::ECEF); - - EXPECT_NEAR(tmp.X(), osrf_e.X(), 8e-2); - EXPECT_NEAR(tmp.Y(), osrf_e.Y(), 8e-2); - EXPECT_NEAR(tmp.Z(), osrf_e.Z(), 1e-2); - - // Check that ECEF -> SPHERICAL works - tmp = sc2.PositionTransform(tmp, - math::SphericalCoordinates::ECEF, - math::SphericalCoordinates::SPHERICAL); - - EXPECT_NEAR(tmp.X(), osrf_s.X(), 1e-2); - EXPECT_NEAR(tmp.Y(), osrf_s.Y(), 1e-2); - EXPECT_NEAR(tmp.Z(), osrf_s.Z(), 1e-2); - - // Check that SPHERICAL -> LOCAL works - tmp = sc2.LocalFromSphericalPosition(goog_s); - EXPECT_NEAR(tmp.X(), vec.X(), 8e-2); - EXPECT_NEAR(tmp.Y(), vec.Y(), 8e-2); - EXPECT_NEAR(tmp.Z(), vec.Z(), 1e-2); - - // Check that SPHERICAL -> LOCAL -> SPHERICAL works - tmp = sc2.SphericalFromLocalPosition(tmp); - EXPECT_NEAR(tmp.X(), goog_s.X(), 8e-2); - EXPECT_NEAR(tmp.Y(), goog_s.Y(), 8e-2); - EXPECT_NEAR(tmp.Z(), goog_s.Z(), 1e-2); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - } - } - - // Give no heading offset to confirm ENU frame - { - math::Angle lat(0.3), lon(-1.2), heading(0.0); - double elev = 354.1; - math::SphericalCoordinates sc(st, lat, lon, elev, heading); - - // Check GlobalFromLocal with no heading offset - { - // local frame - math::Vector3d xyz; - // east, north, up - math::Vector3d enu; - math::Vector3d wsu; - - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - xyz.Set(1, 0, 0); - enu = sc.VelocityTransform(xyz, - math::SphericalCoordinates::LOCAL2, - math::SphericalCoordinates::GLOBAL); - EXPECT_EQ(xyz, enu); - EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); - - xyz.Set(0, 1, 0); - enu = sc.VelocityTransform(xyz, - math::SphericalCoordinates::LOCAL2, - math::SphericalCoordinates::GLOBAL); - EXPECT_EQ(xyz, enu); - EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); - - xyz.Set(1, -1, 0); - enu = sc.VelocityTransform(xyz, - math::SphericalCoordinates::LOCAL2, - math::SphericalCoordinates::GLOBAL); - EXPECT_EQ(xyz, enu); - EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); - - xyz.Set(2243.52334, 556.35, 435.6553); - enu = sc.VelocityTransform(xyz, - math::SphericalCoordinates::LOCAL2, - math::SphericalCoordinates::GLOBAL); - EXPECT_EQ(xyz, enu); - EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); - - // This is the incorrect and deprecated behavior of LOCAL frame - xyz.Set(1, 0, 0); - wsu = math::Vector3d(-xyz.X(), -xyz.Y(), xyz.Z()); - enu = sc.VelocityTransform(xyz, - math::SphericalCoordinates::LOCAL, - math::SphericalCoordinates::GLOBAL); - EXPECT_EQ(wsu, enu); - EXPECT_EQ(wsu, sc.LocalFromGlobalVelocity(enu)); - - xyz.Set(0, 1, 0); - wsu = math::Vector3d(-xyz.X(), -xyz.Y(), xyz.Z()); - enu = sc.VelocityTransform(xyz, - math::SphericalCoordinates::LOCAL, - math::SphericalCoordinates::GLOBAL); - EXPECT_EQ(wsu, enu); - EXPECT_EQ(wsu, sc.LocalFromGlobalVelocity(enu)); - - xyz.Set(1, -1, 0); - wsu = math::Vector3d(-xyz.X(), -xyz.Y(), xyz.Z()); - enu = sc.VelocityTransform(xyz, - math::SphericalCoordinates::LOCAL, - math::SphericalCoordinates::GLOBAL); - EXPECT_EQ(wsu, enu); - EXPECT_EQ(wsu, sc.LocalFromGlobalVelocity(enu)); - - xyz.Set(2243.52334, 556.35, 435.6553); - wsu = math::Vector3d(-xyz.X(), -xyz.Y(), xyz.Z()); - enu = sc.VelocityTransform(xyz, - math::SphericalCoordinates::LOCAL, - math::SphericalCoordinates::GLOBAL); - EXPECT_EQ(wsu, enu); - EXPECT_EQ(wsu, sc.LocalFromGlobalVelocity(enu)); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - } - } -} - ////////////////////////////////////////////////// // Test distance TEST(SphericalCoordinatesTest, Distance) @@ -742,32 +513,6 @@ TEST(SphericalCoordinatesTest, Transform) std::cout << "NEW POS[" << *result << "]\n"; } -////////////////////////////////////////////////// -// TODO(peci1): Remove this test in Gazebo 9 -TEST(SphericalCoordinatesTest, TransformDeprecated) -{ - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - const math::SphericalCoordinates sc; - constexpr math::Vector3d vel(1, 2, -4); - math::Vector3d result = sc.VelocityTransform(vel, - math::SphericalCoordinates::ECEF, - math::SphericalCoordinates::ECEF); - - EXPECT_EQ(result, vel); - - constexpr math::Vector3d pos(-1510.88, 2, -4); - result = sc.PositionTransform(pos, - math::SphericalCoordinates::ECEF, - math::SphericalCoordinates::GLOBAL); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - - EXPECT_NEAR(result.X(), 2, 1e-6); - EXPECT_NEAR(result.Y(), -4, 1e-6); - EXPECT_NEAR(result.Z(), -6379647.8799999999, 1e-6); - - std::cout << "NEW POS[" << result << "]\n"; -} - ////////////////////////////////////////////////// TEST(SphericalCoordinatesTest, BadCoordinateType) { @@ -806,47 +551,6 @@ TEST(SphericalCoordinatesTest, BadCoordinateType) EXPECT_FALSE(result.has_value()); } -////////////////////////////////////////////////// -// TODO(peci1): Remove this test in Gazebo 9 -TEST(SphericalCoordinatesTest, BadCoordinateTypeDeprecated) -{ - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - math::SphericalCoordinates sc; - math::Vector3d pos(1, 2, -4); - math::Vector3d result = sc.PositionTransform(pos, - static_cast(7), - static_cast(6)); - - EXPECT_EQ(result, pos); - - result = sc.PositionTransform(pos, - static_cast(4), - static_cast(6)); - - EXPECT_EQ(result, pos); - - result = sc.VelocityTransform(pos, - math::SphericalCoordinates::SPHERICAL, - math::SphericalCoordinates::ECEF); - EXPECT_EQ(result, pos); - - result = sc.VelocityTransform(pos, - math::SphericalCoordinates::ECEF, - math::SphericalCoordinates::SPHERICAL); - EXPECT_EQ(result, pos); - - result = sc.VelocityTransform(pos, - static_cast(7), - math::SphericalCoordinates::ECEF); - EXPECT_EQ(result, pos); - - result = sc.VelocityTransform(pos, - math::SphericalCoordinates::ECEF, - static_cast(7)); - EXPECT_EQ(result, pos); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION -} - ////////////////////////////////////////////////// // Test [in]equality operators. TEST(SphericalCoordinatesTest, EqualityOps) @@ -1017,115 +721,6 @@ TEST(SphericalCoordinatesTest, NoHeading) } } -////////////////////////////////////////////////// -// TODO(peci1): Remove this test in Gazebo 9 -TEST(SphericalCoordinatesTest, NoHeadingDeprecated) -{ - // Default heading - auto st = math::SphericalCoordinates::EARTH_WGS84; - math::Angle lat(GZ_DTOR(-22.9)); - math::Angle lon(GZ_DTOR(-43.2)); - math::Angle heading(0.0); - double elev = 0; - math::SphericalCoordinates sc(st, lat, lon, elev, heading); - - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - - // Origin matches input - auto latLonAlt = sc.SphericalFromLocalPosition({0, 0, 0}); - EXPECT_DOUBLE_EQ(lat.Degree(), latLonAlt.X()); - EXPECT_DOUBLE_EQ(lon.Degree(), latLonAlt.Y()); - EXPECT_NEAR(elev, latLonAlt.Z(), 1e-6); - - auto xyzOrigin = sc.LocalFromSphericalPosition(latLonAlt); - EXPECT_EQ(math::Vector3d::Zero, xyzOrigin); - - // Check how different lat/lon affect the local position - - // Increase latitude == go North == go +Y - { - auto xyz = sc.LocalFromSphericalPosition( - {lat.Degree() + 1.0, lon.Degree(), elev}); - EXPECT_NEAR(xyzOrigin.X(), xyz.X(), 1e-6); - EXPECT_LT(xyzOrigin.Y(), xyz.Y()); - } - - // Decrease latitude == go South == go -Y - { - auto xyz = sc.LocalFromSphericalPosition( - {lat.Degree() - 1.0, lon.Degree(), elev}); - EXPECT_NEAR(xyzOrigin.X(), xyz.X(), 1e-6); - EXPECT_GT(xyzOrigin.Y(), xyz.Y()); - } - - // Increase longitude == go East == go +X - // Also move a bit -Y because this is the Southern Hemisphere - { - auto xyz = sc.LocalFromSphericalPosition( - {lat.Degree(), lon.Degree() + 1.0, elev}); - EXPECT_LT(xyzOrigin.X(), xyz.X()); - EXPECT_GT(xyzOrigin.Y(), xyz.Y()); - } - - // Decrease longitude == go West == go -X - // Also move a bit -Y because this is the Southern Hemisphere - { - auto xyz = sc.LocalFromSphericalPosition( - {lat.Degree(), lon.Degree() - 1.0, elev}); - EXPECT_GT(xyzOrigin.X(), xyz.X()); - EXPECT_GT(xyzOrigin.Y(), xyz.Y()); - } - - // Increase altitude - { - auto xyz = sc.LocalFromSphericalPosition( - {lat.Degree(), lon.Degree(), elev + 10.0}); - EXPECT_NEAR(xyzOrigin.X(), xyz.X(), 1e-6); - EXPECT_NEAR(xyzOrigin.Y(), xyz.Y(), 1e-6); - EXPECT_NEAR(xyzOrigin.Z() + 10.0, xyz.Z(), 1e-6); - } - - // Decrease altitude - { - auto xyz = sc.LocalFromSphericalPosition( - {lat.Degree(), lon.Degree(), elev - 10.0}); - EXPECT_NEAR(xyzOrigin.X(), xyz.X(), 1e-6); - EXPECT_NEAR(xyzOrigin.Y(), xyz.Y(), 1e-6); - EXPECT_NEAR(xyzOrigin.Z() - 10.0, xyz.Z(), 1e-6); - } - - // Check how global and local velocities are connected - - math::Vector3d wsu; - - // Velocity in - // +X (East), +Y (North), -X (West), -Y (South), +Z (up), -Z (down) - for (auto global : { - math::Vector3d::UnitX, - math::Vector3d::UnitY, - math::Vector3d::UnitZ, - -math::Vector3d::UnitX, - -math::Vector3d::UnitY, - -math::Vector3d::UnitZ}) - { - auto local = sc.LocalFromGlobalVelocity(global); - EXPECT_EQ(global, local); - - global = sc.GlobalFromLocalVelocity(local); - wsu = {-global.X(), -global.Y(), global.Z()}; - EXPECT_EQ(wsu, local); - - // Directly call VelocityTransform - global = sc.VelocityTransform(local, - math::SphericalCoordinates::LOCAL, - math::SphericalCoordinates::GLOBAL); - wsu = {-global.X(), -global.Y(), global.Z()}; - EXPECT_EQ(wsu, local); - } - - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION -} - ////////////////////////////////////////////////// TEST(SphericalCoordinatesTest, WithHeading) { @@ -1221,91 +816,6 @@ TEST(SphericalCoordinatesTest, WithHeading) } } -////////////////////////////////////////////////// -// TODO(peci1): Remove this test in Gazebo 9 -TEST(SphericalCoordinatesTest, WithHeadingDeprecated) -{ - // Heading 90 deg: X == North, Y == West , Z == Up - auto st = math::SphericalCoordinates::EARTH_WGS84; - math::Angle lat(GZ_DTOR(-22.9)); - math::Angle lon(GZ_DTOR(-43.2)); - math::Angle heading(GZ_DTOR(90.0)); - double elev = 0; - math::SphericalCoordinates sc(st, lat, lon, elev, heading); - - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - - // Origin matches input - auto latLonAlt = sc.SphericalFromLocalPosition({0, 0, 0}); - EXPECT_DOUBLE_EQ(lat.Degree(), latLonAlt.X()); - EXPECT_DOUBLE_EQ(lon.Degree(), latLonAlt.Y()); - EXPECT_NEAR(elev, latLonAlt.Z(), 1e-6); - - auto xyzOrigin = sc.LocalFromSphericalPosition(latLonAlt); - EXPECT_EQ(math::Vector3d::Zero, xyzOrigin); - - // Check how different lat/lon affect the local position - - // Increase latitude == go North == go +X - { - auto xyz = sc.LocalFromSphericalPosition( - {lat.Degree() + 1.0, lon.Degree(), elev}); - EXPECT_NEAR(xyzOrigin.Y(), xyz.Y(), 1e-6); - EXPECT_LT(xyzOrigin.X(), xyz.X()); - } - - // Decrease latitude == go South == go -X - { - auto xyz = sc.LocalFromSphericalPosition( - {lat.Degree() - 1.0, lon.Degree(), elev}); - EXPECT_NEAR(xyzOrigin.Y(), xyz.Y(), 1e-6); - EXPECT_GT(xyzOrigin.X(), xyz.X()); - } - - // Increase longitude == go East == go -Y (and a bit -X) - { - auto xyz = sc.LocalFromSphericalPosition( - {lat.Degree(), lon.Degree() + 1.0, elev}); - EXPECT_GT(xyzOrigin.Y(), xyz.Y()); - EXPECT_GT(xyzOrigin.X(), xyz.X()); - } - - // Decrease longitude == go West == go +Y (and a bit -X) - { - auto xyz = sc.LocalFromSphericalPosition( - {lat.Degree(), lon.Degree() - 1.0, elev}); - EXPECT_LT(xyzOrigin.Y(), xyz.Y()); - EXPECT_GT(xyzOrigin.X(), xyz.X()); - } - - // Check how global and local velocities are connected - - // Global | Local - // ---------- | ------ - // +X (East) | -Y - // -X (West) | +Y - // +Y (North) | +X - // -Y (South) | -X - std::vector> globalLocal = - {{math::Vector3d::UnitX, -math::Vector3d::UnitY}, - {-math::Vector3d::UnitX, math::Vector3d::UnitY}, - {math::Vector3d::UnitY, math::Vector3d::UnitX}, - {-math::Vector3d::UnitY, -math::Vector3d::UnitX}}; - for (auto [global, local] : globalLocal) - { - auto localRes = sc.LocalFromGlobalVelocity(global); - EXPECT_EQ(local, localRes); - - // Directly call VelocityTransform - auto globalRes = sc.VelocityTransform(local, - math::SphericalCoordinates::LOCAL, - math::SphericalCoordinates::GLOBAL); - EXPECT_EQ(global, globalRes); - } - - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION -} - ////////////////////////////////////////////////// TEST(SphericalCoordinatesTest, Inverse) { @@ -1367,93 +877,3 @@ TEST(SphericalCoordinatesTest, Inverse) EXPECT_EQ(in, *reverse); } } - -////////////////////////////////////////////////// -// TODO(peci1): Remove this test in Gazebo 9 -TEST(SphericalCoordinatesTest, InverseDeprecated) -{ - auto st = math::SphericalCoordinates::EARTH_WGS84; - math::Angle lat(0.3), lon(-1.2), heading(0.5); - double elev = 354.1; - math::SphericalCoordinates sc(st, lat, lon, elev, heading); - - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - - // GLOBAL <-> LOCAL2 - { - math::Vector3d in(1, 2, -4); - auto out = sc.VelocityTransform(in, - math::SphericalCoordinates::LOCAL2, - math::SphericalCoordinates::GLOBAL); - EXPECT_NE(in, out); - auto reverse = sc.VelocityTransform(out, - math::SphericalCoordinates::GLOBAL, - math::SphericalCoordinates::LOCAL2); - EXPECT_EQ(in, reverse); - } - - { - math::Vector3d in(1, 2, -4); - auto out = sc.PositionTransform(in, - math::SphericalCoordinates::LOCAL2, - math::SphericalCoordinates::GLOBAL); - EXPECT_NE(in, out); - auto reverse = sc.PositionTransform(out, - math::SphericalCoordinates::GLOBAL, - math::SphericalCoordinates::LOCAL2); - EXPECT_EQ(in, reverse); - } - - // SPHERICAL <-> LOCAL2 - { - math::Vector3d in(1, 2, -4); - auto out = sc.PositionTransform(in, - math::SphericalCoordinates::LOCAL2, - math::SphericalCoordinates::SPHERICAL); - EXPECT_NE(in, out); - auto reverse = sc.PositionTransform(out, - math::SphericalCoordinates::SPHERICAL, - math::SphericalCoordinates::LOCAL2); - EXPECT_EQ(in, reverse); - } - - // GLOBAL <-> LOCAL - { - math::Vector3d in(1, 2, -4); - auto out = sc.VelocityTransform(in, - math::SphericalCoordinates::LOCAL, - math::SphericalCoordinates::GLOBAL); - EXPECT_NE(in, out); - auto reverse = sc.VelocityTransform(out, - math::SphericalCoordinates::GLOBAL, - math::SphericalCoordinates::LOCAL); - EXPECT_NE(in, reverse); - } - - { - math::Vector3d in(1, 2, -4); - auto out = sc.PositionTransform(in, - math::SphericalCoordinates::LOCAL, - math::SphericalCoordinates::GLOBAL); - EXPECT_NE(in, out); - auto reverse = sc.PositionTransform(out, - math::SphericalCoordinates::GLOBAL, - math::SphericalCoordinates::LOCAL); - EXPECT_NE(in, reverse); - } - - // SPHERICAL <-> LOCAL - { - math::Vector3d in(1, 2, -4); - auto out = sc.PositionTransform(in, - math::SphericalCoordinates::LOCAL, - math::SphericalCoordinates::SPHERICAL); - EXPECT_NE(in, out); - auto reverse = sc.PositionTransform(out, - math::SphericalCoordinates::SPHERICAL, - math::SphericalCoordinates::LOCAL); - EXPECT_NE(in, reverse); - } - - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION -} diff --git a/src/python_pybind11/src/SphericalCoordinates.cc b/src/python_pybind11/src/SphericalCoordinates.cc index 697eea84..2561d79e 100644 --- a/src/python_pybind11/src/SphericalCoordinates.cc +++ b/src/python_pybind11/src/SphericalCoordinates.cc @@ -53,43 +53,11 @@ void defineMathSphericalCoordinates(py::module &m, const std::string &typestr) py::overload_cast( &Class::SphericalFromLocalPosition, py::const_), "Convert a Cartesian position vector to geodetic coordinates.") - .def("spherical_from_local_position", - [](const Class &self, const gz::math::Vector3d &_xyz) - { - PyErr_WarnEx( - PyExc_DeprecationWarning, - "Passing Vector3d to spherical_from_local_position() is deprecated" - " and will be removed. Pass CoordinateVector3 instead and arrange " - "for the different behavior. See Migration.md ", - 1); - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - return self.SphericalFromLocalPosition(_xyz); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - }, - "DEPRECATED: Passing Vector3d is deprecated and will be removed. " - "Pass CoordinateVector3 instead and arrange for the different " - "behavior. See Migration.md .") .def("global_from_local_velocity", py::overload_cast( &Class::GlobalFromLocalVelocity, py::const_), "Convert a Cartesian velocity vector in the local frame " " to a global Cartesian frame with components East, North, Up") - .def("global_from_local_velocity", - [](const Class &self, const gz::math::Vector3d &_xyz) - { - PyErr_WarnEx( - PyExc_DeprecationWarning, - "Passing Vector3d to global_from_local_velocity() is deprecated" - " and will be removed. Pass CoordinateVector3 instead and arrange " - "for the different behavior. See Migration.md ", - 1); - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - return self.GlobalFromLocalVelocity(_xyz); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - }, - "DEPRECATED: Passing Vector3d is deprecated and will be removed. " - "Pass CoordinateVector3 instead and arrange for the different " - "behavior. See Migration.md .") .def("convert", py::overload_cast(&Class::Convert), "Convert a string to a SurfaceType.") @@ -162,39 +130,11 @@ void defineMathSphericalCoordinates(py::module &m, const std::string &typestr) py::overload_cast( &Class::LocalFromSphericalPosition, py::const_), "Convert a geodetic position vector to Cartesian coordinates.") - .def("local_from_spherical_position", - [](const Class &self, const gz::math::Vector3d &_xyz) - { - PyErr_WarnEx( - PyExc_DeprecationWarning, - "Passing Vector3d to local_from_spherical_position() is deprecated" - " and will be removed. Pass CoordinateVector3 instead.", - 1); - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - return self.LocalFromSphericalPosition(_xyz); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - }, - "DEPRECATED: Passing Vector3d is deprecated and will be removed. " - "Pass CoordinateVector3 instead.") .def("local_from_global_velocity", py::overload_cast( &Class::LocalFromGlobalVelocity, py::const_), "Convert a Cartesian velocity vector with components East, " "North, Up to a local cartesian frame vector XYZ.") - .def("local_from_global_velocity", - [](const Class &self, const gz::math::Vector3d &_xyz) - { - PyErr_WarnEx( - PyExc_DeprecationWarning, - "Passing Vector3d to local_from_global_velocity() is deprecated" - " and will be removed. Pass CoordinateVector3 instead.", - 1); - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - return self.LocalFromGlobalVelocity(_xyz); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - }, - "DEPRECATED: Passing Vector3d is deprecated and will be removed. " - "Pass CoordinateVector3 instead.") .def("update_transformation_matrix", &Class::UpdateTransformationMatrix, "Update coordinate transformation matrix with reference location") @@ -207,25 +147,6 @@ void defineMathSphericalCoordinates(py::module &m, const std::string &typestr) "Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame " "Spherical coordinates use radians, while the other frames use " "meters.") - .def("position_transform", - [](const Class &self, - const gz::math::Vector3d &_pos, - const CoordinateType& _in, - const CoordinateType& _out) -> gz::math::Vector3d - { - PyErr_WarnEx( - PyExc_DeprecationWarning, - "Passing Vector3d to position_transform() is deprecated and will " - "be removed. Pass CoordinateVector3 instead and arrange for the " - "different behavior. See Migration.md .", - 1); - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - return self.PositionTransform(_pos, _in, _out); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - }, - "DEPRECATED: Passing Vector3d is deprecated and will be removed. " - "Pass CoordinateVector3 instead and arrange for the different " - "behavior. See Migration.md .") .def("velocity_transform", py::overload_cast< const CoordinateVector3&, @@ -234,36 +155,14 @@ void defineMathSphericalCoordinates(py::module &m, const std::string &typestr) >(&Class::VelocityTransform, py::const_), "Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame " "Spherical coordinates use radians, while the other frames use " - "meters.") - .def("velocity_transform", - [](const Class &self, - const gz::math::Vector3d &_vel, - const CoordinateType& _in, - const CoordinateType& _out) -> gz::math::Vector3d - { - PyErr_WarnEx( - PyExc_DeprecationWarning, - "Passing Vector3d to velocity_transform() is deprecated and will " - "be removed. Pass CoordinateVector3 instead and arrange for the " - "different behavior. See Migration.md .", - 1); - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - return self.VelocityTransform(_vel, _in, _out); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - }, - "DEPRECATED: Passing Vector3d is deprecated and will be removed. " - "Pass CoordinateVector3 instead and arrange for the different " - "behavior. See Migration.md ."); + "meters."); - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION py::enum_(sphericalCoordinates, "CoordinateType") .value("SPHERICAL", Class::CoordinateType::SPHERICAL) .value("ECEF", Class::CoordinateType::ECEF) .value("GLOBAL", Class::CoordinateType::GLOBAL) .value("LOCAL", Class::CoordinateType::LOCAL) - .value("LOCAL2", Class::CoordinateType::LOCAL2) .export_values(); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION py::enum_(sphericalCoordinates, "SurfaceType") .value("EARTH_WGS84", Class::SurfaceType::EARTH_WGS84) diff --git a/src/python_pybind11/test/SphericalCoordinates_TEST.py b/src/python_pybind11/test/SphericalCoordinates_TEST.py index feb659e7..3862d373 100644 --- a/src/python_pybind11/test/SphericalCoordinates_TEST.py +++ b/src/python_pybind11/test/SphericalCoordinates_TEST.py @@ -19,14 +19,6 @@ from gz.math9 import Angle, CoordinateVector3, SphericalCoordinates, Vector3d -def ignore_deprecation_warnings(test_func): - def do_test(self, *args, **kwargs): - with warnings.catch_warnings(): - warnings.simplefilter("ignore", category=DeprecationWarning) - test_func(self, *args, **kwargs) - return do_test - - class TestSphericalCoordinates(unittest.TestCase): def test_constructor(self): # Default surface type @@ -370,200 +362,6 @@ def test_coordinate_transforms(self): self.assertEqual(xyz, enu) self.assertEqual(xyz, sc.local_from_global_velocity(enu)) - xyz.set_metric(2243.52334, 556.35, 435.6553) - enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL2, SphericalCoordinates.GLOBAL) - self.assertIsNotNone(enu) - self.assertTrue(enu.is_metric()) - self.assertEqual(xyz, enu) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) - - # TODO(peci1): Remove this test in Gazebo 9 - @ignore_deprecation_warnings - def test_coordinate_transforms_deprecated(self): - # Default surface type - st = SphericalCoordinates.EARTH_WGS84 - - # Parameters - lat = Angle(0.3) - lon = Angle(-1.2) - heading = Angle(Angle.HALF_PI) - elev = 354.1 - sc = SphericalCoordinates(st, lat, lon, elev, heading) - - # Check GlobalFromLocal with heading offset of 90 degrees - # Heading 0: X == East, Y == North, Z == Up - # Heading 90: X == North, Y == West , Z == Up - # local frame - xyz = Vector3d() - # east, north, up - enu = Vector3d() - - xyz.set(1, 0, 0) - enu = sc.global_from_local_velocity(xyz) - self.assertAlmostEqual(enu.y(), xyz.x(), delta=1e-6) - self.assertAlmostEqual(enu.x(), -xyz.y(), delta=1e-6) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) - - xyz.set(0, 1, 0) - enu = sc.global_from_local_velocity(xyz) - self.assertAlmostEqual(enu.y(), xyz.x(), delta=1e-6) - self.assertAlmostEqual(enu.x(), -xyz.y(), delta=1e-6) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) - - xyz.set(1, -1, 0) - enu = sc.global_from_local_velocity(xyz) - self.assertAlmostEqual(enu.y(), xyz.x(), delta=1e-6) - self.assertAlmostEqual(enu.x(), -xyz.y(), delta=1e-6) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) - - xyz.set(2243.52334, 556.35, 435.6553) - enu = sc.global_from_local_velocity(xyz) - self.assertAlmostEqual(enu.y(), xyz.x(), delta=1e-6) - self.assertAlmostEqual(enu.x(), -xyz.y(), delta=1e-6) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) - - # Check SphericalFromLocal - # local frame - xyz = Vector3d() - # spherical coordinates - sph = Vector3d() - - # No offset - xyz.set(0, 0, 0) - sph = sc.spherical_from_local_position(xyz) - # latitude - self.assertAlmostEqual(sph.x(), lat.degree(), delta=1e-6) - # longitude - self.assertAlmostEqual(sph.y(), lon.degree(), delta=1e-6) - # elevation - self.assertAlmostEqual(sph.z(), elev, delta=1e-6) - - # 200 km offset in x (pi/2 heading offset means North). We use - # SphericalFromLocal, which means that xyz is a linear movement on - # a plane (not along the curvature of Earth). This will result in - # a large height offset. - xyz.set(2e5, 0, 0) - sph = sc.spherical_from_local_position(xyz) - # increase in latitude about 1.8 degrees - self.assertAlmostEqual(sph.x(), lat.degree() + 1.8, delta=0.008) - # no change in longitude - self.assertAlmostEqual(sph.z(), 3507.024791, delta=1e-6) - - xyz2 = sc.local_from_spherical_position(sph) - self.assertEqual(xyz, xyz2) - - # Check position projection - # WGS84 coordinate obtained from online mapping software - # > gdaltransform -s_srs WGS84 -t_srs EPSG:4978 - # > latitude longitude altitude - # > X Y Z - tmp = Vector3d() - osrf_s = Vector3d(37.3877349, -122.0651166, 32.0) - osrf_e = Vector3d(-2693701.91434394, -4299942.14687992, 3851691.0393571) - goog_s = Vector3d(37.4216719, -122.0821853, 30.0) - - # Local tangent plane coordinates (ENU = GLOBAL) coordinates of - # Google when OSRF is taken as the origin: - # > proj +ellps=WGS84 +proj=tmerc - # +lat_0=37.3877349 +lon_0=-122.0651166 +k=1 +x_0=0 +y_0=0 - # > -122.0821853 37.4216719 (LON,LAT) - # > -1510.88 3766.64 (EAST,NORTH) - vec = Vector3d(-1510.88, 3766.64, -3.29) - - # Convert degrees to radians - osrf_s.x(osrf_s.x() * 0.0174532925) - osrf_s.y(osrf_s.y() * 0.0174532925) - - # Set the ORIGIN to be the Open Source Robotics Foundation - sc2 = SphericalCoordinates(st, Angle(osrf_s.x()), - Angle(osrf_s.y()), osrf_s.z(), Angle.ZERO) - - # Check that SPHERICAL -> ECEF works - tmp = sc2.position_transform(osrf_s, SphericalCoordinates.SPHERICAL, - SphericalCoordinates.ECEF) - - self.assertAlmostEqual(tmp.x(), osrf_e.x(), delta=8e-2) - self.assertAlmostEqual(tmp.y(), osrf_e.y(), delta=8e-2) - self.assertAlmostEqual(tmp.z(), osrf_e.z(), delta=1e-2) - - # Check that ECEF -> SPHERICAL works - tmp = sc2.position_transform(tmp, SphericalCoordinates.ECEF, SphericalCoordinates.SPHERICAL) - - self.assertAlmostEqual(tmp.x(), osrf_s.x(), delta=1e-2) - self.assertAlmostEqual(tmp.y(), osrf_s.y(), delta=1e-2) - self.assertAlmostEqual(tmp.z(), osrf_s.z(), delta=1e-2) - - # Check that SPHERICAL -> LOCAL works - tmp = sc2.local_from_spherical_position(goog_s) - self.assertAlmostEqual(tmp.x(), vec.x(), delta=8e-2) - self.assertAlmostEqual(tmp.y(), vec.y(), delta=8e-2) - self.assertAlmostEqual(tmp.z(), vec.z(), delta=1e-2) - - # Check that SPHERICAL -> LOCAL -> SPHERICAL works - tmp = sc2.spherical_from_local_position(tmp) - self.assertAlmostEqual(tmp.x(), goog_s.x(), delta=8e-2) - self.assertAlmostEqual(tmp.y(), goog_s.y(), delta=8e-2) - self.assertAlmostEqual(tmp.z(), goog_s.z(), delta=1e-2) - - # Give no heading offset to confirm ENU frame - lat = Angle(0.3) - lon = Angle(-1.2) - heading = Angle(0.0) - elev = 354.1 - sc = SphericalCoordinates(st, lat, lon, elev, heading) - - # Check GlobalFromLocal with no heading offset - # local frame - xyz = Vector3d() - # east, north, up - enu = Vector3d() - - xyz.set(1, 0, 0) - enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL2, SphericalCoordinates.GLOBAL) - self.assertEqual(xyz, enu) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) - - xyz.set(0, 1, 0) - enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL2, SphericalCoordinates.GLOBAL) - self.assertEqual(xyz, enu) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) - - xyz.set(1, -1, 0) - enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL2, SphericalCoordinates.GLOBAL) - self.assertEqual(xyz, enu) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) - - xyz.set(2243.52334, 556.35, 435.6553) - enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL2, SphericalCoordinates.GLOBAL) - self.assertEqual(xyz, enu) - self.assertEqual(xyz, sc.local_from_global_velocity(enu)) - - # This is the incorrect and deprecated behavior of LOCAL frame - - xyz.set(1, 0, 0) - wsu = Vector3d(-xyz.x(), -xyz.y(), xyz.z()) - enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL, SphericalCoordinates.GLOBAL) - self.assertEqual(wsu, enu) - self.assertEqual(wsu, sc.local_from_global_velocity(enu)) - - xyz.set(0, 1, 0) - wsu = Vector3d(-xyz.x(), -xyz.y(), xyz.z()) - enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL, SphericalCoordinates.GLOBAL) - self.assertEqual(wsu, enu) - self.assertEqual(wsu, sc.local_from_global_velocity(enu)) - - xyz.set(1, -1, 0) - wsu = Vector3d(-xyz.x(), -xyz.y(), xyz.z()) - enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL, SphericalCoordinates.GLOBAL) - self.assertEqual(wsu, enu) - self.assertEqual(wsu, sc.local_from_global_velocity(enu)) - - xyz.set(2243.52334, 556.35, 435.6553) - wsu = Vector3d(-xyz.x(), -xyz.y(), xyz.z()) - enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL, SphericalCoordinates.GLOBAL) - self.assertEqual(wsu, enu) - self.assertEqual(wsu, sc.local_from_global_velocity(enu)) - def test_distance(self): latA = Angle() longA = Angle() @@ -636,30 +434,6 @@ def test_transform(self): print('NEW POS[', result.x(), ' ', result.y(), ' ', result.z(), ']\n') - # TODO(peci1): Remove this test in Gazebo 9 - @ignore_deprecation_warnings - def test_transform_deprecated(self): - sc = SphericalCoordinates() - vel = Vector3d(1, 2, -4) - result = sc.velocity_transform( - vel, - SphericalCoordinates.ECEF, - SphericalCoordinates.ECEF) - - self.assertEqual(result, vel) - - pos = Vector3d(-1510.88, 2, -4) - result = sc.position_transform( - pos, - SphericalCoordinates.ECEF, - SphericalCoordinates.GLOBAL) - - self.assertAlmostEqual(result.x(), 2, delta=1e-6) - self.assertAlmostEqual(result.y(), -4, delta=1e-6) - self.assertAlmostEqual(result.z(), -6379647.8799999999, delta=1e-6) - - print('NEW POS[', result.x(), ' ', result.y(), ' ', result.z(), ']\n') - def test_bad_coordinate_type(self): sc = SphericalCoordinates() pos = CoordinateVector3.metric(1, 2, -4) @@ -697,45 +471,6 @@ def test_bad_coordinate_type(self): SphericalCoordinates.CoordinateType(7)) self.assertIsNone(result) - # TODO(peci1): Remove this test in Gazebo 9 - @ignore_deprecation_warnings - def test_bad_coordinate_type_deprecated(self): - sc = SphericalCoordinates() - pos = Vector3d(1, 2, -4) - result = sc.position_transform(pos, - SphericalCoordinates.CoordinateType(7), - SphericalCoordinates.CoordinateType(6)) - - self.assertEqual(result, pos) - - result = sc.position_transform(pos, - SphericalCoordinates.CoordinateType(4), - SphericalCoordinates.CoordinateType(6)) - - self.assertEqual(result, pos) - - result = sc.velocity_transform( - pos, - SphericalCoordinates.SPHERICAL, - SphericalCoordinates.ECEF) - self.assertEqual(result, pos) - - result = sc.velocity_transform( - pos, - SphericalCoordinates.ECEF, - SphericalCoordinates.SPHERICAL) - self.assertEqual(result, pos) - - result = sc.velocity_transform(pos, - SphericalCoordinates.CoordinateType(7), - SphericalCoordinates.ECEF) - self.assertEqual(result, pos) - - result = sc.velocity_transform(pos, - SphericalCoordinates.ECEF, - SphericalCoordinates.CoordinateType(7)) - self.assertEqual(result, pos) - def test_equality_ops(self): # Default surface type st = SphericalCoordinates.EARTH_WGS84 @@ -876,95 +611,6 @@ def test_no_heading(self): self.assertTrue(global_var.is_metric()) self.assertEqual(global_var, local) - @ignore_deprecation_warnings - def test_no_heading_deprecated(self): - # Default heading - st = SphericalCoordinates.EARTH_WGS84 - lat = Angle(-22.9 * math.pi / 180.0) - lon = Angle(-43.2 * math.pi / 180.0) - heading = Angle(0.0) - elev = 0 - sc = SphericalCoordinates(st, lat, lon, elev, heading) - - # Origin matches input - latLonAlt = sc.spherical_from_local_position(Vector3d(0, 0, 0)) - self.assertEqual(lat.degree(), latLonAlt.x()) - self.assertEqual(lon.degree(), latLonAlt.y()) - self.assertEqual(elev, latLonAlt.z()) - - xyzOrigin = sc.local_from_spherical_position(latLonAlt) - self.assertEqual(Vector3d.ZERO, xyzOrigin) - - # Check how different lat/lon affect the local position - - # Increase latitude == go North == go +Y - xyz = sc.local_from_spherical_position( - Vector3d(lat.degree() + 1.0, lon.degree(), elev)) - self.assertAlmostEqual(xyzOrigin.x(), xyz.x(), delta=1e-6) - self.assertLess(xyzOrigin.y(), xyz.y()) - - # Decrease latitude == go South == go -Y - xyz = sc.local_from_spherical_position( - Vector3d(lat.degree() - 1.0, lon.degree(), elev)) - self.assertAlmostEqual(xyzOrigin.x(), xyz.x(), delta=1e-6) - self.assertGreater(xyzOrigin.y(), xyz.y()) - - # Increase longitude == go East == go +X - # Also move a bit -Y because this is the Southern Hemisphere - xyz = sc.local_from_spherical_position( - Vector3d(lat.degree(), lon.degree() + 1.0, elev)) - self.assertLess(xyzOrigin.x(), xyz.x()) - self.assertGreater(xyzOrigin.y(), xyz.y()) - - # Decrease longitude == go West == go -X - # Also move a bit -Y because this is the Southern Hemisphere - xyz = sc.local_from_spherical_position( - Vector3d(lat.degree(), lon.degree() - 1.0, elev)) - self.assertGreater(xyzOrigin.x(), xyz.x()) - self.assertGreater(xyzOrigin.y(), xyz.y()) - - # Increase altitude - xyz = sc.local_from_spherical_position( - Vector3d(lat.degree(), lon.degree(), elev + 10.0)) - self.assertAlmostEqual(xyzOrigin.x(), xyz.x(), delta=1e-6) - self.assertAlmostEqual(xyzOrigin.y(), xyz.y(), delta=1e-6) - self.assertAlmostEqual(xyzOrigin.z() + 10.0, xyz.z(), delta=1e-6) - - # Decrease altitude - xyz = sc.local_from_spherical_position( - Vector3d(lat.degree(), lon.degree(), elev - 10.0)) - self.assertAlmostEqual(xyzOrigin.x(), xyz.x(), delta=1e-6) - self.assertAlmostEqual(xyzOrigin.y(), xyz.y(), delta=1e-6) - self.assertAlmostEqual(xyzOrigin.z() - 10.0, xyz.z(), delta=1e-6) - - # Check how global and local velocities are connected - - # Velocity in - # +X (East), +Y (North), -X (West), -Y (South), +Z (up), -Z (down) - for global_var in [Vector3d.UNIT_X, Vector3d.UNIT_Y, Vector3d.UNIT_Z, - -Vector3d.UNIT_X, -Vector3d.UNIT_Y, -Vector3d.UNIT_Z]: - local = sc.local_from_global_velocity(global_var) - self.assertEqual(global_var, local) - - # This function is broken for horizontal velocities - global_var = sc.global_from_local_velocity(local) - wsu = CoordinateVector3.metric( - -global_var.x(), -global_var.y(), global_var.z()) - self.assertAlmostEqual(wsu.x(), local.x(), delta=1e-6) - self.assertAlmostEqual(wsu.y(), local.y(), delta=1e-6) - self.assertAlmostEqual(wsu.z(), local.z(), delta=1e-6) - - # Directly call velocity_transform - global_var = sc.velocity_transform( - local, - SphericalCoordinates.LOCAL, - SphericalCoordinates.GLOBAL) - wsu = CoordinateVector3.metric( - -global_var.x(), -global_var.y(), global_var.z()) - self.assertAlmostEqual(wsu.x(), local.x(), delta=1e-6) - self.assertAlmostEqual(wsu.y(), local.y(), delta=1e-6) - self.assertAlmostEqual(wsu.z(), local.z(), delta=1e-6) - def test_with_heading(self): # Heading 90 deg: X == North, Y == West , Z == Up st = SphericalCoordinates.EARTH_WGS84 @@ -1051,76 +697,6 @@ def test_with_heading(self): self.assertTrue(globalRes.is_metric()) self.assertEqual(global_var, globalRes.as_metric_vector()) - # TODO(peci1): Remove this test in Gazebo 9 - @ignore_deprecation_warnings - def test_with_heading_deprecated(self): - # Heading 90 deg: X == North, Y == West , Z == Up - st = SphericalCoordinates.EARTH_WGS84 - lat = Angle(-22.9 * math.pi / 180.0) - lon = Angle(-43.2 * math.pi / 180.0) - heading = Angle(90.0 * math.pi / 180.0) - elev = 0 - sc = SphericalCoordinates(st, lat, lon, elev, heading) - - # Origin matches input - latLonAlt = sc.spherical_from_local_position(Vector3d(0, 0, 0)) - self.assertEqual(lat.degree(), latLonAlt.x()) - self.assertEqual(lon.degree(), latLonAlt.y()) - self.assertEqual(elev, latLonAlt.z()) - - xyzOrigin = sc.local_from_spherical_position(latLonAlt) - self.assertEqual(Vector3d.ZERO, xyzOrigin) - - # Check how different lat/lon affect the local position - - # Increase latitude == go North == go +X - xyz = sc.local_from_spherical_position( - Vector3d(lat.degree() + 1.0, lon.degree(), elev)) - self.assertAlmostEqual(xyzOrigin.y(), xyz.y(), delta=1e-6) - self.assertLess(xyzOrigin.x(), xyz.x()) - - # Decrease latitude == go South == go -X - xyz = sc.local_from_spherical_position( - Vector3d(lat.degree() - 1.0, lon.degree(), elev)) - self.assertAlmostEqual(xyzOrigin.y(), xyz.y(), delta=1e-6) - self.assertGreater(xyzOrigin.x(), xyz.x()) - - # Increase longitude == go East == go -Y (and a bit -X) - xyz = sc.local_from_spherical_position( - Vector3d(lat.degree(), lon.degree() + 1.0, elev)) - self.assertGreater(xyzOrigin.y(), xyz.y()) - self.assertGreater(xyzOrigin.x(), xyz.x()) - - # Decrease longitude == go West == go +Y (and a bit -X) - xyz = sc.local_from_spherical_position( - Vector3d(lat.degree(), lon.degree() - 1.0, elev)) - self.assertLess(xyzOrigin.y(), xyz.y()) - self.assertGreater(xyzOrigin.x(), xyz.x()) - - # Check how global and local velocities are connected - - # Global | Local - # ---------- | ------ - # +X (East) | -Y - # -X (West) | +Y - # +Y (North) | +X - # -Y (South) | -X - globalLocal = [ - [Vector3d.UNIT_X, -Vector3d.UNIT_Y], - [-Vector3d.UNIT_X, Vector3d.UNIT_Y], - [Vector3d.UNIT_Y, Vector3d.UNIT_X], - [-Vector3d.UNIT_Y, -Vector3d.UNIT_X]] - for [global_var, local] in globalLocal: - localRes = sc.local_from_global_velocity(global_var) - self.assertEqual(local, localRes) - - # Directly call fixed version - globalRes = sc.velocity_transform( - local, - SphericalCoordinates.LOCAL, - SphericalCoordinates.GLOBAL) - self.assertEqual(global_var, globalRes) - def test_inverse(self): st = SphericalCoordinates.EARTH_WGS84 lat = Angle(0.3) @@ -1179,92 +755,6 @@ def test_inverse(self): self.assertTrue(reverse.is_metric()) self.assertEqual(in_vector, reverse) - # TODO(peci1): Remove this test in Gazebo 9 - @ignore_deprecation_warnings - def test_inverse_deprecated(self): - st = SphericalCoordinates.EARTH_WGS84 - lat = Angle(0.3) - lon = Angle(-1.2) - heading = Angle(0.5) - elev = 354.1 - sc = SphericalCoordinates(st, lat, lon, elev, heading) - - # GLOBAL <-> LOCAL2 - in_vector = Vector3d(1, 2, -4) - out = sc.velocity_transform( - in_vector, - SphericalCoordinates.LOCAL2, - SphericalCoordinates.GLOBAL) - self.assertNotEqual(in_vector, out) - reverse = sc.velocity_transform( - out, - SphericalCoordinates.GLOBAL, - SphericalCoordinates.LOCAL2) - self.assertEqual(in_vector, reverse) - - in_vector = Vector3d(1, 2, -4) - out = sc.position_transform( - in_vector, - SphericalCoordinates.LOCAL2, - SphericalCoordinates.GLOBAL) - self.assertNotEqual(in_vector, out) - reverse = sc.position_transform( - out, - SphericalCoordinates.GLOBAL, - SphericalCoordinates.LOCAL2) - self.assertEqual(in_vector, reverse) - - # SPHERICAL <-> LOCAL2 - in_vector = Vector3d(1, 2, -4) - out = sc.position_transform( - in_vector, - SphericalCoordinates.LOCAL2, - SphericalCoordinates.SPHERICAL) - self.assertNotEqual(in_vector, out) - reverse = sc.position_transform( - out, - SphericalCoordinates.SPHERICAL, - SphericalCoordinates.LOCAL2) - self.assertEqual(in_vector, reverse) - - # GLOBAL <-> LOCAL - in_vector = Vector3d(1, 2, -4) - out = sc.velocity_transform( - in_vector, - SphericalCoordinates.LOCAL, - SphericalCoordinates.GLOBAL) - self.assertNotEqual(in_vector, out) - reverse = sc.velocity_transform( - out, - SphericalCoordinates.GLOBAL, - SphericalCoordinates.LOCAL) - self.assertNotEqual(in_vector, reverse) - - in_vector = Vector3d(1, 2, -4) - out = sc.position_transform( - in_vector, - SphericalCoordinates.LOCAL, - SphericalCoordinates.GLOBAL) - self.assertNotEqual(in_vector, out) - reverse = sc.position_transform( - out, - SphericalCoordinates.GLOBAL, - SphericalCoordinates.LOCAL) - self.assertNotEqual(in_vector, reverse) - - # SPHERICAL <-> LOCAL - in_vector = Vector3d(1, 2, -4) - out = sc.position_transform( - in_vector, - SphericalCoordinates.LOCAL, - SphericalCoordinates.SPHERICAL) - self.assertNotEqual(in_vector, out) - reverse = sc.position_transform( - out, - SphericalCoordinates.SPHERICAL, - SphericalCoordinates.LOCAL) - self.assertNotEqual(in_vector, reverse) - if __name__ == '__main__': unittest.main() diff --git a/src/ruby/SphericalCoordinates.i b/src/ruby/SphericalCoordinates.i index 4a60417c..07b42fa5 100644 --- a/src/ruby/SphericalCoordinates.i +++ b/src/ruby/SphericalCoordinates.i @@ -33,7 +33,7 @@ #include #include -GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION +// GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION // GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION is not present; // that's not a problem in SWIG generated files @@ -72,12 +72,7 @@ namespace gz GLOBAL = 3, /// \brief Heading-adjusted tangent plane (X, Y, Z) - /// This has kept a bug for backwards compatibility, use - /// LOCAL2 for the correct behaviour. LOCAL = 4, - - /// \brief Heading-adjusted tangent plane (X, Y, Z) - LOCAL2 = 5 }; public: SphericalCoordinates(); @@ -95,16 +90,10 @@ namespace gz /// \brief Destructor. public: ~SphericalCoordinates(); - public: gz::math::Vector3 SphericalFromLocalPosition( - const gz::math::Vector3 &_xyz) const; - public: std::optional SphericalFromLocalPosition( const gz::math::CoordinateVector3 &_xyz) const; - public: gz::math::Vector3 GlobalFromLocalVelocity( - const gz::math::Vector3 &_xyz) const; - public: std::optional GlobalFromLocalVelocity( const gz::math::CoordinateVector3 &_xyz) const; @@ -138,35 +127,20 @@ namespace gz public: void SetHeadingOffset(const gz::math::Angle &_angle); - public: gz::math::Vector3 LocalFromSphericalPosition( - const gz::math::Vector3 &_latLonEle) const; - public: std::optional LocalFromSphericalPosition( const gz::math::CoordinateVector3 &_latLonEle) const; - public: gz::math::Vector3 LocalFromGlobalVelocity( - const gz::math::Vector3 &_xyz) const; - public: std::optional LocalFromGlobalVelocity( const gz::math::CoordinateVector3 &_xyz) const; public: void UpdateTransformationMatrix(); - public: gz::math::Vector3 - PositionTransform(const gz::math::Vector3 &_pos, - const CoordinateType &_in, const CoordinateType &_out) const; - public: std::optional PositionTransform( const gz::math::CoordinateVector3 &_pos, const CoordinateType &_in, const CoordinateType &_out) const; - /// \return Transformed velocity vector - public: gz::math::Vector3 VelocityTransform( - const gz::math::Vector3 &_vel, - const CoordinateType &_in, const CoordinateType &_out) const; - public: std::optional VelocityTransform( const gz::math::CoordinateVector3 &_vel, const CoordinateType &_in, const CoordinateType &_out) const;