Is there a standard way to transform a pose between coordinate frames?
I have a pose message that I want to transform between two (non-static) coordinate frames. Currently I am doing the following in python:
def rotate_orientation(ori, q):
rot_mat = tf.transformations.quaternion_matrix(q)
pose_rot = rot_mat.dot([ori.x, ori.y, ori.z, ori.w])
ori.x = pose_rot[0]
ori.y = pose_rot[1]
ori.z = pose_rot[2]
ori.w = pose_rot[3]
def translate_position(pos, t):
pos.x += t[0]
pos.y += t[1]
pos.z += t[2]
def odom_to_utm(odom):
global utm_pub
trans,rot = listener.lookupTransform('utm', 'odom', rospy.Time(0))
translate_position(odom.pose.pose.position, trans)
rotate_orientation(odom.pose.pose.orientation, rot)
utm_pub.publish(utm)
But it feels a bit clunky. Is there a more standard way to switch between coordinates frames? Something like a tf.transformPose(pose, frame1, frame2)
that returns pose
in frame 2.