diff --git a/mavros/include/mavros/frame_tf.h b/mavros/include/mavros/frame_tf.h index 4b26abeff..847550ff1 100644 --- a/mavros/include/mavros/frame_tf.h +++ b/mavros/include/mavros/frame_tf.h @@ -132,6 +132,14 @@ Covariance6d transform_static_frame(const Covariance6d &cov, const StaticTF tran */ Covariance9d transform_static_frame(const Covariance9d &cov, const StaticTF transform); +/** + * @brief Transform data expressed in one frame to another frame + * with additional map origin parameter. + * + * General function. Please use specialized variants. + */ +Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const Eigen::Vector3d &map_origin, const StaticTF transform); + inline double transform_frame_yaw(double yaw) { return -yaw; } @@ -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], + * map_origin - geodetic origin [lla], + * returns local ENU coordinates [m]. */ template -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 &in, const T &map_origin) { + return detail::transform_static_frame(in, map_origin, StaticTF::ECEF_TO_ENU); } /** * @brief Transform data expressed in ENU frame to ECEF frame. * + * in - local ENU coordinates [m]. + * map_origin - geodetic origin [lla], + * returns local ECEF coordinates [m]. */ template -inline T transform_frame_enu_ecef(const T &in) { - return detail::transform_static_frame(in, StaticTF::ENU_TO_ECEF); +inline T transform_frame_enu_ecef(const T &in, const T &map_origin) { + return detail::transform_static_frame(in, map_origin, StaticTF::ENU_TO_ECEF); } /** diff --git a/mavros/src/lib/ftf_frame_conversions.cpp b/mavros/src/lib/ftf_frame_conversions.cpp index 18f0b6317..c70da9653 100644 --- a/mavros/src/lib/ftf_frame_conversions.cpp +++ b/mavros/src/lib/ftf_frame_conversions.cpp @@ -82,13 +82,6 @@ Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond &q, const Stat Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const StaticTF transform) { - Eigen::Matrix3d R; - - const double sin_lat = std::sin(vec.x()); - const double sin_lon = std::sin(vec.y()); - const double cos_lat = std::cos(vec.x()); - const double cos_lon = std::cos(vec.y()); - switch (transform) { case StaticTF::NED_TO_ENU: case StaticTF::ENU_TO_NED: @@ -97,44 +90,6 @@ Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const StaticT case StaticTF::AIRCRAFT_TO_BASELINK: case StaticTF::BASELINK_TO_AIRCRAFT: return AIRCRAFT_BASELINK_AFFINE * vec; - - case StaticTF::ECEF_TO_ENU: - /** - * @brief Apply transform from ECEF to ENU: - * ϕ = latitude - * λ = longitude - * The rotation is composed by a counter-clockwise rotation over the Z-axis - * by an angle of π - ϕ followed by a counter-clockwise over the same axis by - * an angle of π - λ - * R = [-sinλ cosλ 0.0 - * -cosλ*sinϕ -sinλ*sinϕ cosϕ - * cosλ*cosϕ sinλ*cosϕ sinϕ ] - * East, North, Up = R * [∂x, ∂y, ∂z] - */ - R << -sin_lon, cos_lon, 0.0, - -cos_lon * sin_lat, -sin_lon * sin_lat, cos_lat, - cos_lon * cos_lat, sin_lon * cos_lat, sin_lat; - - return R * vec; - - case StaticTF::ENU_TO_ECEF: - /** - * @brief Apply transform from ENU to ECEF: - * ϕ = x - * λ = y - * The rotation is composed by a clockwise rotation over the east-axis - * by an angle of π - ϕ followed by a clockwise over the Z-axis by - * an angle of π + λ - * R = [-sinλ -cosλ*sinϕ cosλ*cosϕ - * cosϕ -sinλ*sinϕ sinλ*cosϕ - * 0.0 cosϕ sinϕ ] - * x, y, z = R * [∂x, ∂y, ∂z] - */ - R << -sin_lon, -cos_lon * sin_lat, cos_lon * cos_lat, - cos_lat, -sin_lon * sin_lat, sin_lon * cos_lat, - 0.0, cos_lat, sin_lat; - - return R * vec; } } @@ -213,6 +168,47 @@ Covariance9d transform_static_frame(const Covariance9d &cov, const StaticTF tran } } +Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const Eigen::Vector3d &map_origin, const StaticTF transform) +{ + //! Degrees to radians + static constexpr double DEG_TO_RAD = (M_PI / 180.0); + + // Don't forget to convert from degrees to radians + const double sin_lat = std::sin(map_origin.x() * DEG_TO_RAD); + const double sin_lon = std::sin(map_origin.y() * DEG_TO_RAD); + const double cos_lat = std::cos(map_origin.x() * DEG_TO_RAD); + const double cos_lon = std::cos(map_origin.y() * DEG_TO_RAD); + + /** + * @brief Compute transform from ECEF to ENU: + * http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates + * ϕ = latitude + * λ = longitude + * The rotation is composed by a counter-clockwise rotation over the Z-axis + * by an angle of 90 + λ followed by a counter-clockwise rotation over the east-axis by + * an angle of 90 - ϕ. + * R = [-sinλ cosλ 0.0 + * -cosλ*sinϕ -sinλ*sinϕ cosϕ + * cosλ*cosϕ sinλ*cosϕ sinϕ ] + * [East, North, Up] = R * [∂x, ∂y, ∂z] + * where both [East, North, Up] and [∂x, ∂y, ∂z] are local coordinates relative to map origin. + */ + Eigen::Matrix3d R; + R << -sin_lon, cos_lon, 0.0, + -cos_lon * sin_lat, -sin_lon * sin_lat, cos_lat, + cos_lon * cos_lat, sin_lon * cos_lat, sin_lat; + + switch (transform) { + case StaticTF::ECEF_TO_ENU: + return R * vec; + + case StaticTF::ENU_TO_ECEF: + // ENU to ECEF rotation is just an inverse rotation from ECEF to ENU, which means transpose. + R.transposeInPlace(); + return R * vec; + } +} + Eigen::Vector3d transform_frame(const Eigen::Vector3d &vec, const Eigen::Quaterniond &q) { Eigen::Affine3d transformation(q); diff --git a/mavros/src/plugins/global_position.cpp b/mavros/src/plugins/global_position.cpp index 0d59a6c96..02cbd2d44 100644 --- a/mavros/src/plugins/global_position.cpp +++ b/mavros/src/plugins/global_position.cpp @@ -126,8 +126,9 @@ class GlobalPositionPlugin : public plugin::PluginBase { double rot_cov; - 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] + Eigen::Vector3d local_ecef {}; //!< local ECEF coordinates on map frame [m] template inline void fill_lla(MsgT &msg, sensor_msgs::NavSatFix::Ptr fix) @@ -289,14 +290,14 @@ class GlobalPositionPlugin : public plugin::PluginBase { vel_cov_out.fill(0.0); vel_cov_out(0) = -1.0; - // ECEF point in "map" frame + // Current fix in ECEF Eigen::Vector3d map_point; try { /** * @brief Conversion from geodetic coordinates (LLA) to ECEF (Earth-Centered, Earth-Fixed) * - * Note: "map_origin" is the origin of "map" frame, in ECEF, and the local coordinates are + * Note: "ecef_origin" is the origin of "map" frame, in ECEF, and the local coordinates are * in spherical coordinates, with the orientation in ENU (just like what is applied * on Gazebo) */ @@ -305,31 +306,33 @@ class GlobalPositionPlugin : public plugin::PluginBase { /** * @brief Checks if the "map" origin is set. - * - If not, and the home position is also not received, it sets the current local ecef as the origin; + * - If not, and the home position is also not received, it sets the current fix as the origin; * - If the home position is received, it sets the "map" origin; * - If the "map" origin is set, then it applies the rotations to the offset between the origin * and the current local geocentric coordinates. */ + // Current fix to ECEF + map.Forward(fix->latitude, fix->longitude, fix->altitude, + map_point.x(), map_point.y(), map_point.z()); + + // Set the current fix as the "map" origin if it's not set if (!is_map_init) { - map.Forward(fix->latitude, fix->longitude, fix->altitude, - map_origin.x(), map_origin.y(), map_origin.z()); + map_origin.x() = fix->latitude; + map_origin.y() = fix->longitude; + map_origin.z() = fix->altitude; - local_ecef = map_origin; + ecef_origin = map_point; // Local position is zero is_map_init = true; } - // If origin is set, compute the local coordinates in ENU - else { - map.Forward(fix->latitude, fix->longitude, fix->altitude, - map_point.x(), map_point.y(), map_point.z()); - - local_ecef = map_origin - map_point; - } } catch (const std::exception& e) { ROS_INFO_STREAM("GP: Caught exception: " << e.what() << std::endl); } - tf::pointEigenToMsg(ftf::transform_frame_ecef_enu(local_ecef), odom->pose.pose.position); + // Compute the local coordinates in ECEF + local_ecef = map_point - ecef_origin; + // Compute the local coordinates in ENU + tf::pointEigenToMsg(ftf::transform_frame_ecef_enu(local_ecef, map_origin), odom->pose.pose.position); /** * @brief By default, we are using the relative altitude instead of the geocentric @@ -450,6 +453,21 @@ class GlobalPositionPlugin : public plugin::PluginBase { map_origin.y() = req->geo.longitude; map_origin.z() = req->geo.altitude; + try { + /** + * @brief Conversion from geodetic coordinates (LLA) to ECEF (Earth-Centered, Earth-Fixed) + */ + GeographicLib::Geocentric map(GeographicLib::Constants::WGS84_a(), + GeographicLib::Constants::WGS84_f()); + + // map_origin to ECEF + map.Forward(map_origin.x(), map_origin.y(), map_origin.z(), + ecef_origin.x(), ecef_origin.y(), ecef_origin.z()); + } + catch (const std::exception& e) { + ROS_INFO_STREAM("GP: Caught exception: " << e.what() << std::endl); + } + is_map_init = true; } diff --git a/mavros/test/test_frame_conversions.cpp b/mavros/test/test_frame_conversions.cpp index 85870ccd9..b5f4732fe 100644 --- a/mavros/test/test_frame_conversions.cpp +++ b/mavros/test/test_frame_conversions.cpp @@ -70,6 +70,58 @@ TEST(FRAME_TF, transform_static_frame__ned_to_enu_123) EXPECT_NEAR(expected.z(), out.z(), epsilon); } +TEST(FRAME_TF, transform_static_frame__ecef_to_enu_123_00) +{ + Eigen::Vector3d input(1, 2, 3); + Eigen::Vector3d map_origin(0, 0, 0); + Eigen::Vector3d expected(2, 3, 1); + + auto out = ftf::detail::transform_static_frame(input, map_origin, ftf::StaticTF::ECEF_TO_ENU); + + EXPECT_NEAR(expected.x(), out.x(), epsilon); + EXPECT_NEAR(expected.y(), out.y(), epsilon); + EXPECT_NEAR(expected.z(), out.z(), epsilon); +} + +TEST(FRAME_TF, transform_static_frame__enu_to_ecef_123_00) +{ + Eigen::Vector3d input(1, 2, 3); + Eigen::Vector3d map_origin(0, 0, 0); + Eigen::Vector3d expected(3, 1, 2); + + auto out = ftf::detail::transform_static_frame(input, map_origin, ftf::StaticTF::ENU_TO_ECEF); + + EXPECT_NEAR(expected.x(), out.x(), epsilon); + EXPECT_NEAR(expected.y(), out.y(), epsilon); + EXPECT_NEAR(expected.z(), out.z(), epsilon); +} + +TEST(FRAME_TF, transform_static_frame__ecef_to_enu_123_4030) +{ + Eigen::Vector3d input(1, 2, 3); + Eigen::Vector3d map_origin(40, 30, 0); + Eigen::Vector3d expected(1.23205080756887, 1.09867532044397, 3.35782122034753); + + auto out = ftf::detail::transform_static_frame(input, map_origin, ftf::StaticTF::ECEF_TO_ENU); + + EXPECT_NEAR(expected.x(), out.x(), epsilon); + EXPECT_NEAR(expected.y(), out.y(), epsilon); + EXPECT_NEAR(expected.z(), out.z(), epsilon); +} + +TEST(FRAME_TF, transform_static_frame__enu_to_ecef_123_4030) +{ + Eigen::Vector3d input(1, 2, 3); + Eigen::Vector3d map_origin(40, 30, 0); + Eigen::Vector3d expected(0.3769010460539777, 1.37230445877637, 3.46045171529757); + + auto out = ftf::detail::transform_static_frame(input, map_origin, ftf::StaticTF::ENU_TO_ECEF); + + EXPECT_NEAR(expected.x(), out.x(), epsilon); + EXPECT_NEAR(expected.y(), out.y(), epsilon); + EXPECT_NEAR(expected.z(), out.z(), epsilon); +} + TEST(FRAME_TF, quaternion_transforms__ned_to_ned_123) { auto input_aircraft_ned_orient = ftf::quaternion_from_rpy(1.0, 2.0, 3.0); diff --git a/mavros_extras/src/plugins/fake_gps.cpp b/mavros_extras/src/plugins/fake_gps.cpp index 115d35044..8e8c8733e 100644 --- a/mavros_extras/src/plugins/fake_gps.cpp +++ b/mavros_extras/src/plugins/fake_gps.cpp @@ -242,7 +242,7 @@ class FakeGPSPlugin : public plugin::PluginBase, Eigen::Affine3d pos_enu; tf::transformMsgToEigen(trans->transform, pos_enu); - send_fake_gps(trans->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()))); + send_fake_gps(trans->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin)); } void mocap_pose_cb(const geometry_msgs::PoseStamped::ConstPtr &req) @@ -250,7 +250,7 @@ class FakeGPSPlugin : public plugin::PluginBase, Eigen::Affine3d pos_enu; tf::poseMsgToEigen(req->pose, pos_enu); - send_fake_gps(req->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()))); + send_fake_gps(req->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin)); } void vision_cb(const geometry_msgs::PoseStamped::ConstPtr &req) @@ -258,7 +258,7 @@ class FakeGPSPlugin : public plugin::PluginBase, Eigen::Affine3d pos_enu; tf::poseMsgToEigen(req->pose, pos_enu); - send_fake_gps(req->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()))); + send_fake_gps(req->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin)); } void transform_cb(const geometry_msgs::TransformStamped &trans) @@ -267,7 +267,7 @@ class FakeGPSPlugin : public plugin::PluginBase, tf::transformMsgToEigen(trans.transform, pos_enu); - send_fake_gps(trans.header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()))); + send_fake_gps(trans.header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin)); } }; } // namespace extra_plugins