ENU<->ECEF transforms fix. #847
Conversation
There was a problem hiding this comment.
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.
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) { |
| @@ -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.
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) | |||
| 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] |
| /** | ||
| * @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.
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? |
|
Updated according to review. Added unit tests. APM Planner path before changes Rviz global_position\local path before changes APM Planner path after changes Rviz global_position\local path after changes |
|
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.
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.
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! |
|
@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.