Skip to content

Commit

Permalink
Removed the workaround LOCAL2 coordinate system from SphericalCoordin…
Browse files Browse the repository at this point in the history 
…ates.

All operations with the LOCAL coordinate system will now correctly use ENU.

Note that this breaks contract to code programmed for gz-math7 and earlier, where LOCAL system was actually WSU instead of ENU (rotated by 180 degrees heading).

Signed-off-by: Martin Pecka <[email protected]>
  • Loading branch information
@peci1
peci1 committed May 26, 2024
1 parent cbe7c0d commit 678b447
Show file tree
Hide file tree
Showing 7 changed files with 41 additions and 87 deletions.
6 changes: 6 additions & 0 deletions Migration.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
17 changes: 1 addition & 16 deletions include/gz/math/SphericalCoordinates.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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),
Expand All @@ -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).
Expand Down
21 changes: 1 addition & 20 deletions src/SphericalCoordinates.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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() *
Expand Down Expand Up @@ -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,
Expand Down
40 changes: 16 additions & 24 deletions src/SphericalCoordinates_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down Expand Up @@ -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);
}
Expand Down Expand Up @@ -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);
}
Expand All @@ -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);
}
}
1 change: 0 additions & 1 deletion src/python_pybind11/src/SphericalCoordinates.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_<Class::SurfaceType>(sphericalCoordinates, "SurfaceType")
.value("EARTH_WGS84", Class::SurfaceType::EARTH_WGS84)
Expand Down
36 changes: 16 additions & 20 deletions src/python_pybind11/test/SphericalCoordinates_TEST.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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))

Expand Down Expand Up @@ -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)

Expand Down Expand Up @@ -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)

Expand All @@ -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)


Expand Down
7 changes: 1 addition & 6 deletions src/ruby/SphericalCoordinates.i
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down

0 comments on commit 678b447

Please sign in to comment.