# Transform the coordinate frame of a pose from one fixed frame to another

I have a Pose message defined in a user preferred fixed reference frame. For planning, I would like to chance the reference coordinate system to "world" (which again is a fixed reference frame). Since these reference frames are part of robot definition in URDF - is there a simple way to transform pose from one reference frame to another without having to go through broadcasting and listening tf transformations? Even if I have to use the tf, does it require one to calculate the transforms and broadcast it? I tried using tf2_ros.Buffer.transform() method - but this does not seem to work unless, there is an explicit transform being broadcasted between the two frames? Thank you.

edit retag close merge delete

Sort by » oldest newest most voted

Transforming a pose from one fixed coordinate frame to another seemed like a trivial problem. But I ended up spending lot of time researching the solution and came up with the following, I hope it helps others like me. There are couple of things that tripped me up. 1) Ensure that the PoseStamped that you are passing to transform() is from tf2_geometry_msgs and not geometry_msgs. 2) Use rospy.Duration() to wait for the listener to start listening.

# Transform a given input pose from one fixed frame to another
import rospy
from geometry_msgs.msg import Pose

import tf2_ros
import tf2_geometry_msgs  # **Do not use geometry_msgs. Use this instead for PoseStamped

def transform_pose(input_pose, from_frame, to_frame):

# **Assuming /tf2 topic is being broadcasted
tf_buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tf_buffer)

pose_stamped = tf2_geometry_msgs.PoseStamped()
pose_stamped.pose = input_pose

try:
# ** It is important to wait for the listener to start listening. Hence the rospy.Duration(1)
output_pose_stamped = tf_buffer.transform(pose_stamped, to_frame, rospy.Duration(1))
return output_pose_stamped.pose

except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
raise

# Test Case
rospy.init_node("transform_test")

my_pose = Pose()
my_pose.position.x = -0.25
my_pose.position.y = -0.50
my_pose.position.z = +1.50
my_pose.orientation.x = 0.634277921154
my_pose.orientation.y = 0.597354098852
my_pose.orientation.z = 0.333048372508
my_pose.orientation.w = 0.360469667089

transformed_pose = transform_pose(my_pose, "fixture", "world")

print(transformed_pose)

more

Ensure that the PoseStamped that you are passing to transform() is from tf2_geometry_msgs and not geometry_msgs

this should not actually be necessary. As long as you import tf2_geometry_msgs the conversion should be done automatically. tf2_geometry_msgs only provides functions to convert the messages to types and back.

( 2019-05-14 13:52:37 -0500 )edit