ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org
Ask Your Question
0

How to properly use TF2 transform python interface

asked 2018-11-26 14:42:02 -0600

mwrighte38 gravatar image

I'm trying transform a point in my cameras FOV to the coordinate system of my world.

https://github.com/ros/geometry2/blob...

I have calculated a distance in meters from the camera in x,y,z. How can I make the correct data type from the the x/y/z position and xyzw rotation of my camera?

I've tried to input a transformStamped() data type but I receive the error.

File "/opt/ros/kinetic/lib/python2.7/dist-packages/tf2_ros/buffer_interface.py", line 65, in transform do_transform = self.registration.get(type(object_stamped)) File "/opt/ros/kinetic/lib/python2.7/dist-packages/tf2_ros/buffer_interface.py", line 204, in get raise TypeException('Type %s if not loaded or supported'% str(key)) tf2_ros.buffer_interface.TypeException

What type of data type am I supposed to be using?

edit retag flag offensive close merge delete

Comments

I am also having this problem, did you manage to solve it?

mateusguilherme gravatar image mateusguilherme  ( 2021-02-01 10:02:38 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2022-10-11 16:27:45 -0600

updated 2022-10-11 16:28:06 -0600

Okay, I spent hours trying to figure this out, and I solved it! I'll put my code below, but in general my approach is to use the buffer.lookup_transform to get the transform, AND the broadcaster.sendTransform to publish a transform.

What this does is let me read a rosbag, re-publish any /tf or /tf_static messages, and then when it's time to convert frames I can use rospy.Time.now() and get the "current" transform, where "current" is whatever the most recently published transform is.

You might have an issue if your first data appears before your first /tf or /tf_static message appears; if you find this to be the case then you can optionally drop that data. For my personal use case, it doesn't matter. I tried removing the try/catch in the hopes that it'd speed up processing, but it didn't :(

Here's the code, enjoy!

import argparse
import rosbag
import rospy
import os.path
import tf2_ros
from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud

parser = argparse.ArgumentParser(description='Converts all pointclouds to a target frame.')
parser.add_argument('input', type=str, help='Input Rosbag')
parser.add_argument('output', type=str, help='Output Rosbag')

args = parser.parse_args()

# Force input/output files to be .bag files
if ".bag" not in args.input:
    print("Input file needs to have a *.bag ending")
    exit()
if ".bag" not in args.output:
    print("Output file needs to have a *.bag ending")
    exit()

# Does the input file exist?
if not os.path.isfile(args.input):
    print("Input file cannot be found.")
    exit()

# Need to initialize a ROS node to do the conversion
rospy.init_node("converter")

# Setup the tf2 listener:
buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(buffer)

# Setup the tf2 broadcaster
broadcaster = tf2_ros.TransformBroadcaster()

# What are you trying to transform to?
target_frame = "your_target_frame"  # NOTE: You could use this as an input argument instead, for more flexibility!

# Transform the data!
with rosbag.Bag(args.output, 'w') as output:
    for topic, msg, t in rosbag.Bag(args.input).read_messages():

        # Inputs are copied as the basis for outputs
        new_msg = msg

        # Re-publish any /tf or /tf_static messages, so you can look those transforms up later
        if topic == "/tf":
            for msg_tf in msg.transforms:
                broadcaster.sendTransform(msg_tf)
        if topic == "/tf_static":
            for msg_tf in msg.transforms:
                broadcaster.sendTransform(msg_tf)

        # For my use case, I'm trying to transform PointCloud2 messages. You could switch on your own topic here.
        if hasattr(new_msg, 'cloud'):
            try:
                transform = buffer.lookup_transform(target_frame,
                                                    msg.cloud.header.frame_id,
                                                    rospy.Time.now(),
                                                    rospy.Duration(0))
                new_msg.cloud = do_transform_cloud(msg.cloud, transform)
            except:
                print("Failed to find a transform to " + target_frame)
                # Choosing to continue here because you've got a cloud that wasn't transformed!
                continue
        # Output bag file has all the same topics in the input, but you're replacing the transformed messages!
        output.write(topic, new_msg, t)

# Don't forget to close the bag when you're done writing!
output.close()
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: 2018-11-26 14:42:02 -0600

Seen: 461 times

Last updated: Oct 11