From 5a4bd76b098022aa2bbcfd06159c3f041fc8f317 Mon Sep 17 00:00:00 2001 From: zhouzhiwen2000 Date: Sun, 14 Feb 2021 03:07:01 +0800 Subject: [PATCH 1/8] fix inconsistency in direction of yaw when using set_position in BODY frames and fix problems with yaw in setponit_raw --- mavros/src/plugins/setpoint_position.cpp | 2 +- mavros/src/plugins/setpoint_raw.cpp | 11 ++++++++--- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/mavros/src/plugins/setpoint_position.cpp b/mavros/src/plugins/setpoint_position.cpp index a2165a6b1..3b3a74dab 100644 --- a/mavros/src/plugins/setpoint_position.cpp +++ b/mavros/src/plugins/setpoint_position.cpp @@ -146,7 +146,7 @@ class SetpointPositionPlugin : public plugin::PluginBase, auto q = [&] () { if (mav_frame == MAV_FRAME::BODY_NED || mav_frame == MAV_FRAME::BODY_OFFSET_NED) { - return ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation())); + return ftf::quaternion_from_rpy(M_PI, 0.0, 0.0)*Eigen::Quaterniond(tr.rotation()); } else { return ftf::transform_orientation_enu_ned( ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))); diff --git a/mavros/src/plugins/setpoint_raw.cpp b/mavros/src/plugins/setpoint_raw.cpp index d75cc99f1..2976f2066 100644 --- a/mavros/src/plugins/setpoint_raw.cpp +++ b/mavros/src/plugins/setpoint_raw.cpp @@ -166,16 +166,21 @@ class SetpointRawPlugin : public plugin::PluginBase, position = ftf::transform_frame_baselink_aircraft(position); velocity = ftf::transform_frame_baselink_aircraft(velocity); af = ftf::transform_frame_baselink_aircraft(af); + yaw = ftf::quaternion_get_yaw( + ftf::quaternion_from_rpy(M_PI, 0.0, 0.0)*( + ftf::quaternion_from_rpy(0.0, 0.0, req->yaw))); } else { position = ftf::transform_frame_enu_ned(position); velocity = ftf::transform_frame_enu_ned(velocity); af = ftf::transform_frame_enu_ned(af); - } - - yaw = ftf::quaternion_get_yaw( + yaw = ftf::quaternion_get_yaw( ftf::transform_orientation_aircraft_baselink( ftf::transform_orientation_ned_enu( ftf::quaternion_from_rpy(0.0, 0.0, req->yaw)))); + } + + ROS_INFO("FROM MAVROS,YAW=%f,YAW_ORG=%f",ftf::quaternion_get_yaw( + ftf::quaternion_from_rpy(0.0, 0.0, req->yaw)),req->yaw); Eigen::Vector3d ang_vel_enu(0.0, 0.0, req->yaw_rate); auto ang_vel_ned = ftf::transform_frame_ned_enu(ang_vel_enu); yaw_rate = ang_vel_ned.z(); From ea4c91f85bba0a0e697422c1ea87b45e445ad08c Mon Sep 17 00:00:00 2001 From: zhouzhiwen2000 Date: Sun, 14 Feb 2021 03:26:00 +0800 Subject: [PATCH 2/8] delete debug messages --- mavros/src/plugins/setpoint_raw.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/mavros/src/plugins/setpoint_raw.cpp b/mavros/src/plugins/setpoint_raw.cpp index 2976f2066..581f0f407 100644 --- a/mavros/src/plugins/setpoint_raw.cpp +++ b/mavros/src/plugins/setpoint_raw.cpp @@ -179,8 +179,6 @@ class SetpointRawPlugin : public plugin::PluginBase, ftf::quaternion_from_rpy(0.0, 0.0, req->yaw)))); } - ROS_INFO("FROM MAVROS,YAW=%f,YAW_ORG=%f",ftf::quaternion_get_yaw( - ftf::quaternion_from_rpy(0.0, 0.0, req->yaw)),req->yaw); Eigen::Vector3d ang_vel_enu(0.0, 0.0, req->yaw_rate); auto ang_vel_ned = ftf::transform_frame_ned_enu(ang_vel_enu); yaw_rate = ang_vel_ned.z(); From 97287c0f370f2b6b816eb5db693a816fd852fdc7 Mon Sep 17 00:00:00 2001 From: zhouzhiwen2000 Date: Sun, 14 Feb 2021 04:20:40 +0800 Subject: [PATCH 3/8] move transforms to ftf namespace --- mavros/include/mavros/frame_tf.h | 22 ++++++++++++++++++++++ mavros/src/lib/ftf_frame_conversions.cpp | 4 ++++ 2 files changed, 26 insertions(+) diff --git a/mavros/include/mavros/frame_tf.h b/mavros/include/mavros/frame_tf.h index 5bac04da4..2cc99f652 100644 --- a/mavros/include/mavros/frame_tf.h +++ b/mavros/include/mavros/frame_tf.h @@ -62,6 +62,8 @@ enum class StaticTF { ENU_TO_NED, //!< change from expressed WRT ENU frame to WRT NED frame AIRCRAFT_TO_BASELINK, //!< change from expressed WRT aircraft frame to WRT to baselink frame BASELINK_TO_AIRCRAFT, //!< change from expressed WRT baselnk to WRT aircraft + FIXED_FRAME_AIRCRAFT_TO_BASELINK, + FIXED_FRAME_BASELINK_TO_AIRCRAFT, }; /** @@ -186,6 +188,26 @@ inline T transform_orientation_baselink_aircraft(const T &in) { return detail::transform_orientation(in, StaticTF::BASELINK_TO_AIRCRAFT); } +/** + * @brief Transform from attitude represented WRT aircraft frame to + * attitude represented WRT base_link frame, treating aircraft frame + * as if it were fixed frame as opposed to the function above. + */ +template +inline T transform_orientation_fixed_frame_aircraft_baselink(const T &in) { + return detail::transform_orientation(in, StaticTF::FIXED_FRAME_AIRCRAFT_TO_BASELINK); +} + +/** + * @brief Transform from attitude represented WRT baselink frame to + * attitude represented WRT body frame, treating baselink frame + * as if it were fixed frame. + */ +template +inline T transform_orientation_fixed_frame_baselink_aircraft(const T &in) { + return detail::transform_orientation(in, StaticTF::FIXED_FRAME_BASELINK_TO_AIRCRAFT); +} + /** * @brief Transform data expressed in NED to ENU frame. */ diff --git a/mavros/src/lib/ftf_frame_conversions.cpp b/mavros/src/lib/ftf_frame_conversions.cpp index 9266da31f..82aef1bb9 100644 --- a/mavros/src/lib/ftf_frame_conversions.cpp +++ b/mavros/src/lib/ftf_frame_conversions.cpp @@ -85,6 +85,10 @@ Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond &q, const Stat case StaticTF::AIRCRAFT_TO_BASELINK: case StaticTF::BASELINK_TO_AIRCRAFT: return q * AIRCRAFT_BASELINK_Q; + + case StaticTF::FIXED_FRAME_AIRCRAFT_TO_BASELINK: + case StaticTF::FIXED_FRAME_BASELINK_TO_AIRCRAFT: + return AIRCRAFT_BASELINK_Q * q; } } From d85c70d72b685238e94d2c64864d12a526c6f827 Mon Sep 17 00:00:00 2001 From: zhouzhiwen2000 Date: Sun, 14 Feb 2021 04:30:03 +0800 Subject: [PATCH 4/8] use transforms from tft instead of raw calculation --- mavros/src/plugins/setpoint_position.cpp | 2 +- mavros/src/plugins/setpoint_raw.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/mavros/src/plugins/setpoint_position.cpp b/mavros/src/plugins/setpoint_position.cpp index 3b3a74dab..0a16f061a 100644 --- a/mavros/src/plugins/setpoint_position.cpp +++ b/mavros/src/plugins/setpoint_position.cpp @@ -146,7 +146,7 @@ class SetpointPositionPlugin : public plugin::PluginBase, auto q = [&] () { if (mav_frame == MAV_FRAME::BODY_NED || mav_frame == MAV_FRAME::BODY_OFFSET_NED) { - return ftf::quaternion_from_rpy(M_PI, 0.0, 0.0)*Eigen::Quaterniond(tr.rotation()); + return ftf::transform_orientation_fixed_frame_baselink_aircraft(Eigen::Quaterniond(tr.rotation())); } else { return ftf::transform_orientation_enu_ned( ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))); diff --git a/mavros/src/plugins/setpoint_raw.cpp b/mavros/src/plugins/setpoint_raw.cpp index 581f0f407..c06b31679 100644 --- a/mavros/src/plugins/setpoint_raw.cpp +++ b/mavros/src/plugins/setpoint_raw.cpp @@ -167,7 +167,7 @@ class SetpointRawPlugin : public plugin::PluginBase, velocity = ftf::transform_frame_baselink_aircraft(velocity); af = ftf::transform_frame_baselink_aircraft(af); yaw = ftf::quaternion_get_yaw( - ftf::quaternion_from_rpy(M_PI, 0.0, 0.0)*( + ftf::transform_orientation_fixed_frame_baselink_aircraft( ftf::quaternion_from_rpy(0.0, 0.0, req->yaw))); } else { position = ftf::transform_frame_enu_ned(position); From 49b93aff6ffa12c3fe0c7922ff45d792a01d7971 Mon Sep 17 00:00:00 2001 From: zhouzhiwen2000 Date: Sun, 14 Feb 2021 04:45:30 +0800 Subject: [PATCH 5/8] change names --- mavros/include/mavros/frame_tf.h | 16 ++++++++-------- mavros/src/lib/ftf_frame_conversions.cpp | 4 ++-- mavros/src/plugins/setpoint_position.cpp | 2 +- mavros/src/plugins/setpoint_raw.cpp | 2 +- 4 files changed, 12 insertions(+), 12 deletions(-) diff --git a/mavros/include/mavros/frame_tf.h b/mavros/include/mavros/frame_tf.h index 2cc99f652..5287483c2 100644 --- a/mavros/include/mavros/frame_tf.h +++ b/mavros/include/mavros/frame_tf.h @@ -62,8 +62,8 @@ enum class StaticTF { ENU_TO_NED, //!< change from expressed WRT ENU frame to WRT NED frame AIRCRAFT_TO_BASELINK, //!< change from expressed WRT aircraft frame to WRT to baselink frame BASELINK_TO_AIRCRAFT, //!< change from expressed WRT baselnk to WRT aircraft - FIXED_FRAME_AIRCRAFT_TO_BASELINK, - FIXED_FRAME_BASELINK_TO_AIRCRAFT, + ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK, + ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT, }; /** @@ -191,21 +191,21 @@ inline T transform_orientation_baselink_aircraft(const T &in) { /** * @brief Transform from attitude represented WRT aircraft frame to * attitude represented WRT base_link frame, treating aircraft frame - * as if it were fixed frame as opposed to the function above. + * as if it were absolute frame as opposed to the function above. */ template -inline T transform_orientation_fixed_frame_aircraft_baselink(const T &in) { - return detail::transform_orientation(in, StaticTF::FIXED_FRAME_AIRCRAFT_TO_BASELINK); +inline T transform_orientation_absolute_frame_aircraft_baselink(const T &in) { + return detail::transform_orientation(in, StaticTF::ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK); } /** * @brief Transform from attitude represented WRT baselink frame to * attitude represented WRT body frame, treating baselink frame - * as if it were fixed frame. + * as if it were absolute frame. */ template -inline T transform_orientation_fixed_frame_baselink_aircraft(const T &in) { - return detail::transform_orientation(in, StaticTF::FIXED_FRAME_BASELINK_TO_AIRCRAFT); +inline T transform_orientation_absolute_frame_baselink_aircraft(const T &in) { + return detail::transform_orientation(in, StaticTF::ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT); } /** diff --git a/mavros/src/lib/ftf_frame_conversions.cpp b/mavros/src/lib/ftf_frame_conversions.cpp index 82aef1bb9..b414931bc 100644 --- a/mavros/src/lib/ftf_frame_conversions.cpp +++ b/mavros/src/lib/ftf_frame_conversions.cpp @@ -86,8 +86,8 @@ Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond &q, const Stat case StaticTF::BASELINK_TO_AIRCRAFT: return q * AIRCRAFT_BASELINK_Q; - case StaticTF::FIXED_FRAME_AIRCRAFT_TO_BASELINK: - case StaticTF::FIXED_FRAME_BASELINK_TO_AIRCRAFT: + case StaticTF::ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK: + case StaticTF::ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT: return AIRCRAFT_BASELINK_Q * q; } } diff --git a/mavros/src/plugins/setpoint_position.cpp b/mavros/src/plugins/setpoint_position.cpp index 0a16f061a..ebce9ec7b 100644 --- a/mavros/src/plugins/setpoint_position.cpp +++ b/mavros/src/plugins/setpoint_position.cpp @@ -146,7 +146,7 @@ class SetpointPositionPlugin : public plugin::PluginBase, auto q = [&] () { if (mav_frame == MAV_FRAME::BODY_NED || mav_frame == MAV_FRAME::BODY_OFFSET_NED) { - return ftf::transform_orientation_fixed_frame_baselink_aircraft(Eigen::Quaterniond(tr.rotation())); + return ftf::transform_orientation_absolute_frame_aircraft_baselink(Eigen::Quaterniond(tr.rotation())); } else { return ftf::transform_orientation_enu_ned( ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))); diff --git a/mavros/src/plugins/setpoint_raw.cpp b/mavros/src/plugins/setpoint_raw.cpp index c06b31679..dbefe5edb 100644 --- a/mavros/src/plugins/setpoint_raw.cpp +++ b/mavros/src/plugins/setpoint_raw.cpp @@ -167,7 +167,7 @@ class SetpointRawPlugin : public plugin::PluginBase, velocity = ftf::transform_frame_baselink_aircraft(velocity); af = ftf::transform_frame_baselink_aircraft(af); yaw = ftf::quaternion_get_yaw( - ftf::transform_orientation_fixed_frame_baselink_aircraft( + ftf::transform_orientation_absolute_frame_aircraft_baselink( ftf::quaternion_from_rpy(0.0, 0.0, req->yaw))); } else { position = ftf::transform_frame_enu_ned(position); From 2c32c21f1a11a9554f048e0b4c874f68cdc8d0a7 Mon Sep 17 00:00:00 2001 From: zhouzhiwen2000 Date: Sun, 14 Feb 2021 04:53:21 +0800 Subject: [PATCH 6/8] Update mavros/include/mavros/frame_tf.h Co-authored-by: Nuno Marques --- mavros/include/mavros/frame_tf.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavros/include/mavros/frame_tf.h b/mavros/include/mavros/frame_tf.h index 5287483c2..15a76cf02 100644 --- a/mavros/include/mavros/frame_tf.h +++ b/mavros/include/mavros/frame_tf.h @@ -191,7 +191,7 @@ inline T transform_orientation_baselink_aircraft(const T &in) { /** * @brief Transform from attitude represented WRT aircraft frame to * attitude represented WRT base_link frame, treating aircraft frame - * as if it were absolute frame as opposed to the function above. + * as in an absolute frame of reference (local NED). */ template inline T transform_orientation_absolute_frame_aircraft_baselink(const T &in) { From 9aac9ed80e7c49e03779fea44d7cf2f6f9c3330f Mon Sep 17 00:00:00 2001 From: zhouzhiwen2000 Date: Sun, 14 Feb 2021 04:53:26 +0800 Subject: [PATCH 7/8] Update mavros/include/mavros/frame_tf.h Co-authored-by: Nuno Marques --- mavros/include/mavros/frame_tf.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavros/include/mavros/frame_tf.h b/mavros/include/mavros/frame_tf.h index 15a76cf02..ff07b2651 100644 --- a/mavros/include/mavros/frame_tf.h +++ b/mavros/include/mavros/frame_tf.h @@ -201,7 +201,7 @@ inline T transform_orientation_absolute_frame_aircraft_baselink(const T &in) { /** * @brief Transform from attitude represented WRT baselink frame to * attitude represented WRT body frame, treating baselink frame - * as if it were absolute frame. + * as in an absolute frame of reference (local NED). */ template inline T transform_orientation_absolute_frame_baselink_aircraft(const T &in) { From 7f4b52a7d6b42a736ad2d1e7672919d6820f5ea3 Mon Sep 17 00:00:00 2001 From: zhouzhiwen2000 Date: Sun, 14 Feb 2021 04:56:14 +0800 Subject: [PATCH 8/8] Update frame_tf.h --- mavros/include/mavros/frame_tf.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mavros/include/mavros/frame_tf.h b/mavros/include/mavros/frame_tf.h index ff07b2651..34fbf9fea 100644 --- a/mavros/include/mavros/frame_tf.h +++ b/mavros/include/mavros/frame_tf.h @@ -62,8 +62,8 @@ enum class StaticTF { ENU_TO_NED, //!< change from expressed WRT ENU frame to WRT NED frame AIRCRAFT_TO_BASELINK, //!< change from expressed WRT aircraft frame to WRT to baselink frame BASELINK_TO_AIRCRAFT, //!< change from expressed WRT baselnk to WRT aircraft - ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK, - ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT, + ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK,//!< change orientation from being expressed in aircraft frame to baselink frame in an absolute frame of reference. + ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT,//!< change orientation from being expressed in baselink frame to aircraft frame in an absolute frame of reference }; /**