diff --git a/Migration.md b/Migration.md index 37fffd4b..d78a50a2 100644 --- a/Migration.md +++ b/Migration.md @@ -5,6 +5,12 @@ 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 7.X to 8.X + +### Breaking Changes + + 1. Removed `SphericalCoordinates::CoordinateType::LOCAL2` enum value and fixed the behavior of `LOCAL` coordinates. This might result in some computations using `SphericalCoordinates` to output a value that is rotated 180 degrees in heading. + ## Gazebo Math 6.X to 7.X ### Breaking Changes diff --git a/include/gz/math/SphericalCoordinates.hh b/include/gz/math/SphericalCoordinates.hh index 2808c738..b4331935 100644 --- a/include/gz/math/SphericalCoordinates.hh +++ b/include/gz/math/SphericalCoordinates.hh @@ -66,12 +66,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 + LOCAL = 4 }; /// \brief Constructor. @@ -106,12 +101,6 @@ namespace gz /// \brief Convert a Cartesian position vector to geodetic coordinates. /// This performs a `PositionTransform` from LOCAL to SPHERICAL. /// - /// There's a known bug with this computation that can't be fixed on - /// version 6 to avoid behaviour changes. Directly call - /// `PositionTransform(_xyz, LOCAL2, SPHERICAL)` for correct results. - /// Note that `PositionTransform` returns spherical coordinates in - /// radians. - /// /// \param[in] _xyz Cartesian position vector in the heading-adjusted /// world frame. /// \return Cooordinates: geodetic latitude (deg), longitude (deg), @@ -123,10 +112,6 @@ namespace gz /// to a global Cartesian frame with components East, North, Up. /// This is a wrapper around `VelocityTransform(_xyz, LOCAL, GLOBAL)` /// - /// There's a known bug with this computation that can't be fixed on - /// version 6 to avoid behaviour changes. Directly call - /// `VelocityTransform(_xyz, LOCAL2, GLOBAL)` 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). diff --git a/src/SphericalCoordinates.cc b/src/SphericalCoordinates.cc index ed7f77ee..c786296f 100644 --- a/src/SphericalCoordinates.cc +++ b/src/SphericalCoordinates.cc @@ -548,18 +548,8 @@ Vector3d SphericalCoordinates::PositionTransform( // Convert whatever arrives to a more flexible ECEF coordinate switch (_in) { - // East, North, Up (ENU), note no break at end of case + // East, North, Up (ENU) case LOCAL: - { - tmp.X(-_pos.X() * this->dataPtr->cosHea + _pos.Y() * - this->dataPtr->sinHea); - tmp.Y(-_pos.X() * this->dataPtr->sinHea - _pos.Y() * - this->dataPtr->cosHea); - tmp = this->dataPtr->origin + this->dataPtr->rotGlobalToECEF * tmp; - break; - } - - case LOCAL2: { tmp.X(_pos.X() * this->dataPtr->cosHea + _pos.Y() * this->dataPtr->sinHea); @@ -634,7 +624,6 @@ Vector3d SphericalCoordinates::PositionTransform( // Convert from ECEF TO LOCAL case LOCAL: - case LOCAL2: tmp = this->dataPtr->rotECEFToGlobal * (tmp - this->dataPtr->origin); tmp = Vector3d( @@ -674,13 +663,6 @@ Vector3d SphericalCoordinates::VelocityTransform( { // ENU case LOCAL: - tmp.X(-_vel.X() * this->dataPtr->cosHea + _vel.Y() * - this->dataPtr->sinHea); - tmp.Y(-_vel.X() * this->dataPtr->sinHea - _vel.Y() * - this->dataPtr->cosHea); - tmp = this->dataPtr->rotGlobalToECEF * tmp; - break; - case LOCAL2: tmp.X(_vel.X() * this->dataPtr->cosHea + _vel.Y() * this->dataPtr->sinHea); tmp.Y(-_vel.X() * this->dataPtr->sinHea + _vel.Y() * @@ -714,7 +696,6 @@ Vector3d SphericalCoordinates::VelocityTransform( // Convert from ECEF to local case LOCAL: - case LOCAL2: tmp = this->dataPtr->rotECEFToGlobal * tmp; tmp = Vector3d( tmp.X() * this->dataPtr->cosHea - tmp.Y() * this->dataPtr->sinHea, diff --git a/src/SphericalCoordinates_TEST.cc b/src/SphericalCoordinates_TEST.cc index 655d1f29..75d4118a 100644 --- a/src/SphericalCoordinates_TEST.cc +++ b/src/SphericalCoordinates_TEST.cc @@ -360,28 +360,28 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) xyz.Set(1, 0, 0); enu = sc.VelocityTransform(xyz, - math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::LOCAL, 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::LOCAL, 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::LOCAL, 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::LOCAL, math::SphericalCoordinates::GLOBAL); EXPECT_EQ(xyz, enu); EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); @@ -641,20 +641,12 @@ TEST(SphericalCoordinatesTest, NoHeading) auto local = sc.LocalFromGlobalVelocity(global); EXPECT_EQ(global, local); - // This function is broken for horizontal velocities global = sc.GlobalFromLocalVelocity(local); - if (abs(global.Z()) < 0.1) - { - EXPECT_NE(global, local); - } - else - { - EXPECT_EQ(global, local); - } + EXPECT_EQ(global, local); - // Directly call fixed version + // Directly call VelocityTransform global = sc.VelocityTransform(local, - math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::LOCAL, math::SphericalCoordinates::GLOBAL); EXPECT_EQ(global, local); } @@ -734,7 +726,7 @@ TEST(SphericalCoordinatesTest, WithHeading) // Directly call fixed version auto globalRes = sc.VelocityTransform(local, - math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::LOCAL, math::SphericalCoordinates::GLOBAL); EXPECT_EQ(global, globalRes); } @@ -748,41 +740,41 @@ TEST(SphericalCoordinatesTest, Inverse) double elev = 354.1; math::SphericalCoordinates sc(st, lat, lon, elev, heading); - // GLOBAL <-> LOCAL2 + // GLOBAL <-> LOCAL { math::Vector3d in(1, 2, -4); auto out = sc.VelocityTransform(in, - math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::LOCAL, math::SphericalCoordinates::GLOBAL); EXPECT_NE(in, out); auto reverse = sc.VelocityTransform(out, math::SphericalCoordinates::GLOBAL, - math::SphericalCoordinates::LOCAL2); + math::SphericalCoordinates::LOCAL); EXPECT_EQ(in, reverse); } { math::Vector3d in(1, 2, -4); auto out = sc.PositionTransform(in, - math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::LOCAL, math::SphericalCoordinates::GLOBAL); EXPECT_NE(in, out); auto reverse = sc.PositionTransform(out, math::SphericalCoordinates::GLOBAL, - math::SphericalCoordinates::LOCAL2); + math::SphericalCoordinates::LOCAL); EXPECT_EQ(in, reverse); } - // SPHERICAL <-> LOCAL2 + // SPHERICAL <-> LOCAL { math::Vector3d in(1, 2, -4); auto out = sc.PositionTransform(in, - math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::LOCAL, math::SphericalCoordinates::SPHERICAL); EXPECT_NE(in, out); auto reverse = sc.PositionTransform(out, math::SphericalCoordinates::SPHERICAL, - math::SphericalCoordinates::LOCAL2); + math::SphericalCoordinates::LOCAL); EXPECT_EQ(in, reverse); } } diff --git a/src/python_pybind11/src/SphericalCoordinates.cc b/src/python_pybind11/src/SphericalCoordinates.cc index f2d27ed2..d6c8c950 100644 --- a/src/python_pybind11/src/SphericalCoordinates.cc +++ b/src/python_pybind11/src/SphericalCoordinates.cc @@ -147,7 +147,6 @@ void defineMathSphericalCoordinates(py::module &m, const std::string &typestr) .value("ECEF", Class::CoordinateType::ECEF) .value("GLOBAL", Class::CoordinateType::GLOBAL) .value("LOCAL", Class::CoordinateType::LOCAL) - .value("LOCAL2", Class::CoordinateType::LOCAL2) .export_values(); 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 ef333f30..d1f3dca8 100644 --- a/src/python_pybind11/test/SphericalCoordinates_TEST.py +++ b/src/python_pybind11/test/SphericalCoordinates_TEST.py @@ -311,22 +311,22 @@ def test_coordinate_transforms(self): enu = Vector3d() xyz.set(1, 0, 0) - enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL2, SphericalCoordinates.GLOBAL) + enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL, 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) + enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL, 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) + enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL, 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) + enu = sc.velocity_transform(xyz, SphericalCoordinates.LOCAL, SphericalCoordinates.GLOBAL) self.assertEqual(xyz, enu) self.assertEqual(xyz, sc.local_from_global_velocity(enu)) @@ -541,17 +541,13 @@ def test_no_heading(self): 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) - if abs(global_var.z()) < 0.1: - self.assertNotEqual(global_var, local) - else: - self.assertEqual(global_var, local) + self.assertEqual(global_var, local) - # Directly call fixed version + # Directly call velocity_transform global_var = sc.velocity_transform( local, - SphericalCoordinates.LOCAL2, + SphericalCoordinates.LOCAL, SphericalCoordinates.GLOBAL) self.assertEqual(global_var, local) @@ -619,7 +615,7 @@ def test_with_heading(self): # Directly call fixed version globalRes = sc.velocity_transform( local, - SphericalCoordinates.LOCAL2, + SphericalCoordinates.LOCAL, SphericalCoordinates.GLOBAL) self.assertEqual(global_var, globalRes) @@ -631,42 +627,42 @@ def test_inverse(self): elev = 354.1 sc = SphericalCoordinates(st, lat, lon, elev, heading) - # GLOBAL <-> LOCAL2 + # GLOBAL <-> LOCAL in_vector = Vector3d(1, 2, -4) out = sc.velocity_transform( in_vector, - SphericalCoordinates.LOCAL2, + SphericalCoordinates.LOCAL, SphericalCoordinates.GLOBAL) self.assertNotEqual(in_vector, out) reverse = sc.velocity_transform( out, SphericalCoordinates.GLOBAL, - SphericalCoordinates.LOCAL2) + SphericalCoordinates.LOCAL) self.assertEqual(in_vector, reverse) in_vector = Vector3d(1, 2, -4) out = sc.position_transform( in_vector, - SphericalCoordinates.LOCAL2, + SphericalCoordinates.LOCAL, SphericalCoordinates.GLOBAL) self.assertNotEqual(in_vector, out) reverse = sc.position_transform( out, SphericalCoordinates.GLOBAL, - SphericalCoordinates.LOCAL2) + SphericalCoordinates.LOCAL) self.assertEqual(in_vector, reverse) - # SPHERICAL <-> LOCAL2 + # SPHERICAL <-> LOCAL in_vector = Vector3d(1, 2, -4) out = sc.position_transform( in_vector, - SphericalCoordinates.LOCAL2, + SphericalCoordinates.LOCAL, SphericalCoordinates.SPHERICAL) self.assertNotEqual(in_vector, out) reverse = sc.position_transform( out, SphericalCoordinates.SPHERICAL, - SphericalCoordinates.LOCAL2) + SphericalCoordinates.LOCAL) self.assertEqual(in_vector, reverse) diff --git a/src/ruby/SphericalCoordinates.i b/src/ruby/SphericalCoordinates.i index 801536e2..70eb9c1f 100644 --- a/src/ruby/SphericalCoordinates.i +++ b/src/ruby/SphericalCoordinates.i @@ -51,12 +51,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 + LOCAL = 4 }; public: SphericalCoordinates();