From c7bf964ea5d3475a67308c4a0d3d15c01158824f Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 11 Nov 2022 02:33:37 +0600 Subject: [PATCH] More clean variable names --- aruco_pose/src/utils.h | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/aruco_pose/src/utils.h b/aruco_pose/src/utils.h index b7f2f837..fe70f8ad 100644 --- a/aruco_pose/src/utils.h +++ b/aruco_pose/src/utils.h @@ -106,26 +106,26 @@ inline bool isFlipped(tf::Quaternion& q) return (abs(pitch) > M_PI / 2) || (abs(roll) > M_PI / 2); } -/* Set roll and pitch from "from" to "to", keeping yaw */ -inline void applyVertical(geometry_msgs::Quaternion& to, const geometry_msgs::Quaternion& from, bool auto_flip = false) +/* Apply a vertical to an orientation */ +inline void applyVertical(geometry_msgs::Quaternion& orientation, const geometry_msgs::Quaternion& vertical, bool auto_flip = false) { - tf::Quaternion _from, _to; - tf::quaternionMsgToTF(from, _from); - tf::quaternionMsgToTF(to, _to); + tf::Quaternion _vertical, _orientation; + tf::quaternionMsgToTF(vertical, _vertical); + tf::quaternionMsgToTF(orientation, _orientation); if (auto_flip) { - if (!isFlipped(_from)) { + if (!isFlipped(_vertical)) { static const tf::Quaternion flip = tf::createQuaternionFromRPY(M_PI, 0, 0); - _from *= flip; // flip "from" + _vertical *= flip; // flip vertical } } - auto diff = tf::Matrix3x3(_to).transposeTimes(tf::Matrix3x3(_from)); + auto diff = tf::Matrix3x3(_orientation).transposeTimes(tf::Matrix3x3(_vertical)); double _, yaw; diff.getRPY(_, _, yaw); auto q = tf::createQuaternionFromRPY(0, 0, -yaw); - _from = _from * q; // set yaw from "to" to "from" - tf::quaternionTFToMsg(_from, to); // set "from" to "to" + _vertical = _vertical * q; // set yaw from orientation to vertical + tf::quaternionTFToMsg(_vertical, orientation); // set vertical to orientation } inline void transformToPose(const geometry_msgs::Transform& transform, geometry_msgs::Pose& pose)