-
Notifications
You must be signed in to change notification settings - Fork 988
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
ENU<->ECEF transforms fix. #847
Conversation
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Thanks for checking this out @pavloblindnology. I did not particularly checked this on a real thing, so I applied the rotations with a fixed frame.
Can you give us a comparison from before/after this changes? After you make the requested changes, I will merge it.
mavros/include/mavros/frame_tf.h
Outdated
*/ | ||
template<class T> | ||
inline T transform_frame_ecef_enu(const T &in) { | ||
return detail::transform_static_frame(in, StaticTF::ECEF_TO_ENU); | ||
inline T transform_frame_ecef_enu(const T &map_origin, const T &in) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Keep it as is - pass the origin like:
inline T transform_frame_ecef_enu(const T &in,const T &map_origin) {
return detail::transform_static_frame(in, map_origin, StaticTF::ECEF_TO_ENU);
mavros/include/mavros/frame_tf.h
Outdated
*/ | ||
template<class T> | ||
inline T transform_frame_enu_ecef(const T &in) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
same as above
@@ -64,6 +64,48 @@ using Matrix6d = Eigen::Matrix<double, 6, 6>; | |||
using Matrix9d = Eigen::Matrix<double, 9, 9>; | |||
|
|||
|
|||
Eigen::Matrix3d ecef_enu_rotation(const Eigen::Vector3d &map_origin) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
You don't have to move this out: Just create the same method transform_static_frame
but with a new parameter, keeping the previous:
Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const Eigen::Vector3d &map_origin, const StaticTF transform)
@@ -213,6 +217,11 @@ Covariance9d transform_static_frame(const Covariance9d &cov, const StaticTF tran | |||
} | |||
} | |||
|
|||
Eigen::Vector3d transform_frame(const Eigen::Vector3d &vec, const Eigen::Matrix3d &R) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
no need
Eigen::Vector3d map_origin {}; //!< origin of map frame | ||
Eigen::Vector3d local_ecef {}; //!< local ECEF coordinates on map frame | ||
Eigen::Vector3d map_origin {}; //!< geodetic origin of map frame [lla] | ||
Eigen::Vector3d ecef_origin; //!< geocentric origin of map frame [m] |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Eigen::Vector3d ecef_origin {};
/** | ||
* @brief Conversion from geodetic coordinates (LLA) to ECEF (Earth-Centered, Earth-Fixed) | ||
*/ | ||
GeographicLib::Geocentric map(GeographicLib::Constants::WGS84_a(), |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Please do it the same way it is done in fake_gps
: https://github.com/mavlink/mavros/blob/master/mavros_extras/src/plugins/fake_gps.cpp#L85-L98
@vooon why is Travis not launching at all? |
Do not know, Travis do not list that PR at all. But we also have ROS CI, lets rely in it. |
@pavloblindnology common for ftf - rotation arg after input vec. Can you please add two tests to test_frame_conversions.cpp? |
Additionally fixed this copy paste bug 0c4b0f7, which is already fixed. |
@@ -214,19 +222,25 @@ inline T transform_frame_baselink_aircraft(const T &in) { | |||
/** | |||
* @brief Transform data expressed in ECEF frame to ENU frame. | |||
* | |||
* in - local ECEF coordinates [m], |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
For Doxygen doumentation:
@param in - local ECEF coordinates [m]
@param map_origin - geodetic origin [lla]
@return local ENU coordinates [m]
} | ||
|
||
/** | ||
* @brief Transform data expressed in ENU frame to ECEF frame. | ||
* | ||
* in - local ENU coordinates [m]. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
For Doxygen doumentation:
@param in - local ECEF coordinates [m]
@param map_origin - geodetic origin [lla]
@return local ENU coordinates [m]
@pavloblindnology almost there. Please rebase (solve conflicts) and make the changes for docs. Then, all will be good. Thanks for the fix! |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
LGTM.
@pavloblindnology thank you! Small fixes i done myself. And after CI done i will release, as it is significant bugfix. |
@vooon You're welcome) What is the schedule of CI for ROS? |
@pavloblindnology it is already solved. Thanks! |
ENU<->ECEF transformations are totally screwed up resulting in crazy positions in global_position\local topic. fake_gps plugin is also affected.
P.S. This is my 1st contributing to mavros - tried to follow the conventions as much as possible.