Ask Your Question
1

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

asked 2019-05-13 11:11:23 -0500

singular.river gravatar image

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 flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-05-14 13:06:09 -0500

singular.river gravatar image

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
    pose_stamped.header.frame_id = from_frame
    pose_stamped.header.stamp = rospy.Time.now()

    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)
edit flag offensive delete link more

Comments

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.

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

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2019-05-13 11:09:41 -0500

Seen: 163 times

Last updated: May 14