Skip to content

Commit

Permalink
SphericalCoordinates: Deprecated LOCAL and LOCAL2 systems.
Browse files Browse the repository at this point in the history 
Use LOCAL_TANGENT instead.

Signed-off-by: Martin Pecka <[email protected]>
  • Loading branch information
@peci1
peci1 committed Aug 13, 2024
1 parent 00deb01 commit 36a07ad
Show file tree
Hide file tree
Showing 7 changed files with 736 additions and 127 deletions.
34 changes: 34 additions & 0 deletions Migration.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,19 @@ release will remove the deprecated code.

## Gazebo Math 8.X to 9.X

### Additions

1. **SphericalCoordinates.hh**
+ `SphericalCoordinates::SphericalPositionFromLocal()`.
+ `SphericalCoordinates::LocalPositionFromSpherical()`.
+ `SphericalCoordinates::GlobalVelocityFromLocal()`.
+ `SphericalCoordinates::LocalVelocityFromGlobal()`.
+ Enum value `SphericalCoordinates::CoordinateType::LOCAL_TANGENT`
which substitutes the wrong `LOCAL` coordinate and improves the
name of `LOCAL2` (it is its functional equivalent).
+ These functions use the newly introduced `LOCAL_TANGENT`
coordinate type instead of the wrong `LOCAL` frame.

### Deprecations

1. **graph/Vertex.hh**
Expand All @@ -19,6 +32,27 @@ release will remove the deprecated code.
`Vertex::NullEdge()` instead.
E.g.: https://github.com/gazebosim/gz-math/pull/606/files#diff-0c0220a7e72be70337975433eeddc3f5e072ade5cd80dfb1ac03da233c39c983L222-R222

1. **SphericalCoordinates.hh**
+ Deprecated `LOCAL` and `LOCAL2` values of enum
`SphericalCoordinates::CoordinateType`. Use `LOCAL_TANGENT`
instead. When replacing `LOCAL`, you have to rotate the output by
180 degrees in heading.
+ Deprecated `SphericalCoordinates::SphericalFromLocalPosition` which
gives incorrect results.
Use `SphericalCoordinates::SphericalPositionFromLocal` instead.
+ Deprecated `SphericalCoordinates::GlobalFromLocalVelocity` which
gives incorrect results (WSU frame instead of ENU).
Use `SphericalCoordinates::GlobalVelocityFromLocal` instead and
negate the `x` component of the result.
+ Deprecated `SphericalCoordinates::LocalFromSphericalPosition` to
unify naming.
Use `SphericalCoordinates::LocalPositionFromSpherical` instead (it
yields identical results).
+ Deprecated `SphericalCoordinates::LocalFromGlobalVelocity` to
unify naming.
Use `SphericalCoordinates::LocalVelocityFromGlobal` instead (it
yields identical results).

## Gazebo Math 7.X to 8.X

### Breaking Changes
Expand Down
78 changes: 60 additions & 18 deletions include/gz/math/SphericalCoordinates.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,11 +65,14 @@ namespace gz::math

/// \brief Heading-adjusted tangent plane (X, Y, Z)
/// This has kept a bug for backwards compatibility, use
/// LOCAL2 for the correct behaviour.
LOCAL = 4,
/// LOCAL_TANGENT for the correct behaviour.
LOCAL GZ_DEPRECATED(8) = 4,

/// \brief Equivalent to LOCAL_TANGENT.
LOCAL2 GZ_DEPRECATED(8) = 5,

/// \brief Heading-adjusted tangent plane (X, Y, Z)
LOCAL2 = 5
LOCAL_TANGENT = 5
};

/// \brief Constructor.
Expand Down Expand Up @@ -104,31 +107,50 @@ namespace gz::math
/// \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.
/// \deprecated This function returns incorrect results.
/// Call `SphericalPositionFromLocal` for correct results.
/// Please note the results of this function are in a wrong
/// frame (West-Sout-Up instead of East-North-Up).
///
/// \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::math::Vector3d SphericalFromLocalPosition(
/// \return Cooordinates: geodetic latitude (deg West),
/// longitude (deg South), altitude above sea level (m).
public: GZ_DEPRECATED(8) gz::math::Vector3d SphericalFromLocalPosition(
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.
/// to a global Cartesian frame with components West, South, 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.
/// \deprecated This function returns incorrect results.
/// Call `GlobalVelocityFromLocal` for correct results.
///
/// \param[in] _xyz Cartesian velocity vector in the heading-adjusted
/// world frame.
/// \return Rotated vector with components (x,y,z): (West, South, Up).
public: GZ_DEPRECATED(8) gz::math::Vector3d GlobalFromLocalVelocity(
const gz::math::Vector3d &_xyz) const;

/// \brief Convert a Cartesian position vector to geodetic coordinates.
/// This performs a `PositionTransform` from LOCAL_TANGENT to SPHERICAL.
///
/// \param[in] _xyz Cartesian position vector in the heading-adjusted
/// world frame.
/// \return Cooordinates: geodetic latitude (deg East),
/// longitude (deg North), altitude above sea level (m).
public: gz::math::Vector3d SphericalPositionFromLocal(
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_TANGENT, GLOBAL)`
///
/// \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::math::Vector3d GlobalFromLocalVelocity(
public: gz::math::Vector3d GlobalVelocityFromLocal(
const gz::math::Vector3d &_xyz) const;

/// \brief Convert a string to a SurfaceType.
Expand Down Expand Up @@ -243,20 +265,40 @@ 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 `LocalPositionFromSpherical` instead (it gives identical
/// results).
/// \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 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 `LocalVelocityFromGlobal` instead (it gives identical
/// results).
/// \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 geodetic position vector to Cartesian coordinates.
/// This performs a `PositionTransform` from SPHERICAL to LOCAL.
/// \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::math::Vector3d LocalFromSphericalPosition(
public: gz::math::Vector3d LocalPositionFromSpherical(
const gz::math::Vector3d &_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)`
/// \param[in] _xyz Vector with components (x,y,z): (East, North, Up).
/// \return Cartesian vector in the world frame.
public: gz::math::Vector3d LocalFromGlobalVelocity(
public: gz::math::Vector3d LocalVelocityFromGlobal(
const gz::math::Vector3d &_xyz) const;

/// \brief Update coordinate transformation matrix with reference location
Expand Down
65 changes: 59 additions & 6 deletions src/SphericalCoordinates.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -377,8 +377,10 @@ void SphericalCoordinates::SetHeadingOffset(const Angle &_angle)
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;
Expand All @@ -391,21 +393,62 @@ Vector3d SphericalCoordinates::LocalFromSphericalPosition(
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
}

//////////////////////////////////////////////////
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
}

//////////////////////////////////////////////////
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
}

//////////////////////////////////////////////////
Vector3d SphericalCoordinates::SphericalPositionFromLocal(
const Vector3d &_xyz) const
{
Vector3d result =
this->PositionTransform(_xyz, LOCAL_TANGENT, SPHERICAL);
result.X(GZ_RTOD(result.X()));
result.Y(GZ_RTOD(result.Y()));
return result;
}

//////////////////////////////////////////////////
Vector3d SphericalCoordinates::LocalPositionFromSpherical(
const Vector3d &_xyz) const
{
Vector3d result = _xyz;
result.X(GZ_DTOR(result.X()));
result.Y(GZ_DTOR(result.Y()));
return this->PositionTransform(result, SPHERICAL, LOCAL_TANGENT);
}

//////////////////////////////////////////////////
Vector3d SphericalCoordinates::GlobalVelocityFromLocal(
const Vector3d &_xyz) const
{
return this->VelocityTransform(_xyz, LOCAL_TANGENT, GLOBAL);
}

//////////////////////////////////////////////////
Vector3d SphericalCoordinates::LocalVelocityFromGlobal(
const Vector3d &_xyz) const
{
return this->VelocityTransform(_xyz, GLOBAL, LOCAL_TANGENT);
}

//////////////////////////////////////////////////
Expand Down Expand Up @@ -535,8 +578,10 @@ 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
// West, South, Up (WSU)
GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
case LOCAL:
GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION
{
tmp.X(-_pos.X() * this->dataPtr->cosHea + _pos.Y() *
this->dataPtr->sinHea);
Expand All @@ -546,7 +591,8 @@ Vector3d SphericalCoordinates::PositionTransform(
break;
}

case LOCAL2:
// East, North, Up (ENU)
case LOCAL_TANGENT:
{
tmp.X(_pos.X() * this->dataPtr->cosHea + _pos.Y() *
this->dataPtr->sinHea);
Expand Down Expand Up @@ -620,8 +666,10 @@ Vector3d SphericalCoordinates::PositionTransform(
break;

// Convert from ECEF TO LOCAL
GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
case LOCAL:
case LOCAL2:
GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION
case LOCAL_TANGENT:
tmp = this->dataPtr->rotECEFToGlobal * (tmp - this->dataPtr->origin);

tmp = Vector3d(
Expand Down Expand Up @@ -659,15 +707,18 @@ Vector3d SphericalCoordinates::VelocityTransform(
// First, convert to an ECEF vector
switch (_in)
{
// ENU
// WSU
GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
case LOCAL:
GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION
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:
// ENU
case LOCAL_TANGENT:
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 @@ -700,8 +751,10 @@ Vector3d SphericalCoordinates::VelocityTransform(
break;

// Convert from ECEF to local
GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
case LOCAL:
case LOCAL2:
GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION
case LOCAL_TANGENT:
tmp = this->dataPtr->rotECEFToGlobal * tmp;
tmp = Vector3d(
tmp.X() * this->dataPtr->cosHea - tmp.Y() * this->dataPtr->sinHea,
Expand Down
Loading

0 comments on commit 36a07ad

Please sign in to comment.