Ask Your Question
3

tf2_ros buffer transform PointStamped?

asked 2016-12-05 15:08:21 -0600

lucasw gravatar image

updated 2016-12-05 16:07:21 -0600

I'd like to transform a PointStamped using a python tf2_ros.Buffer, but so far I'm only getting type exceptions. The following example a point with only a y component is supposed to be trivially transformed into the frame it is already in:

#!/usr/bin/env python

import rospy
import sys
import tf2_ros

from geometry_msgs.msg import PointStamped

if __name__ == '__main__':
    rospy.init_node('transform_point_tf2')
    tf_buffer = tf2_ros.Buffer()
    tf_listener = tf2_ros.TransformListener(tf_buffer)

    rospy.sleep(1.0)
    pt = PointStamped()
    pt.header.stamp = rospy.Time.now()
    pt.header.frame_id = "map"
    pt.point.x = 0.0
    pt.point.y = 1.0
    pt.point.z = 0.0

    try:
        pt2 = tf_buffer.transform(pt, "map")
    except:  # tf2_ros.buffer_interface.TypeException as e:
        e = sys.exc_info()[0]
        rospy.logerr(e)
        sys.exit(1)

    rospy.loginfo(pt2)

This results in:

[ERROR] [/transform_point_tf2] [./transform_point_tf2.py]:[27] [<class 'tf2_ros.buffer_interface.TypeException'>]

If PointStamped isn't the right type, then what type can I use?

To do this manually I'm currently doing this:

            trans = self.tf_buffer.lookup_transform("map", target_frame,
                                                        rospy.Time())

            quat = [trans.transform.rotation.x,
                    trans.transform.rotation.y,
                    trans.transform.rotation.z,
                    trans.transform.rotation.w]
            mat = tf.transformations.quaternion_matrix(quat)
            pt_np = [pt.point.x,
                     pt.point.y,
                     pt.point.z,
                     1.0]
            pt_in_map_np = numpy.dot(mat, pt_np)

            pt_in_map.x = pt_in_map_np[0]
            pt_in_map.y = pt_in_map_np[1]
            pt_in_map.z = pt_in_map_np[2]
edit retag flag offensive close merge delete

Comments

Could you post the output of tf_buffer.registration.print_me()?

jsanch2s gravatar imagejsanch2s ( 2016-12-05 15:45:14 -0600 )edit

In this example it is {}

lucasw gravatar imagelucasw ( 2016-12-05 15:58:21 -0600 )edit

2 Answers

Sort by » oldest newest most voted
2

answered 2017-01-30 12:10:39 -0600

J. Bromley gravatar image

The code that registers transforms is in tf2_geometry_msgs. The following code will allow you to call the BufferInterface.transform function.

import tf2_ros
from tf2_geometry_msgs import PointStamped
from geometry_msgs.msg import Point

...

# Let src_pt be the point you want to transform.
tf_buf = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buf)
target_pt = tf_buf.transform(src_pt, "target_frame")

Note that you should not include a leading slash in the name of the target frame.

edit flag offensive delete link more
2

answered 2016-12-05 16:08:24 -0600

jsanch2s gravatar image

Note: This is not really an answer but it was too long for a comment, sorry.

As I understand it, Buffer inherits from BufferInterface, where it uses TransformRegistration (also defined in the same file) to check if a type is supported, in this case PointStamped. Specifically, it uses the method get to check if that type is supported. Since the output you posted is an empty dictionary, I'm assuming there are no supported types.

They also define an add method that requires the type and a callback (no idea where to get this callback, though).

Perhaps the authors of this package can provide an insight?

edit flag offensive delete link more

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: 2016-12-05 15:08:21 -0600

Seen: 2,428 times

Last updated: Dec 05 '16