ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

On Python you can do the transformation using the function tf2_geometry_msgs.do_transform_point( ).

See the following snippet:

import rospy
import tf2_ros
from tf2_geometry_msgs import do_transform_point
from geometry_msgs.msg import Point

tf_buffer = tf2_ros.Buffer(rospy.Duration(tf_cache_duration))
tf2_ros.TransformListener(tf_buffer)

# get the transformation from source_frame to target_frame.
try:
    transformation = tf_buffer.lookup_transform(target_frame,
            source_frame, rospy.Time(0), rospy.Duration(0.1))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
        tf2_ros.ExtrapolationException):
    rospy.logerr('Unable to find the transformation from %s to %s'
                 % source_frame, target_frame)

# Transform point coordinates to the target frame.
    source_frame = 'my_camera_link'
    target_frame = 'base'

    point_source = Point(x=0.1, y=1.2, z=2.3)

    point_target = do_transform_point(transformation, point_source)