Is there a function to convert coords between two frames?

galactic
tf2

asked 2023-02-14 08:52:24 -0500

130 ●11 ●23 ●22

Hi!

With TF2 we can obtain the transformation that relates two frames. Said transformation provides us with the translation and rotation (in the form of a quaternion) between both reference systems. With the quaternion can be obtained the rotation matrix, and using it the coordinates of a point can be transformed from one frame to the other. The question is, is there a library or function that already provides the transformation of coordinates between frames?

Regards.

edit retag flag offensive close merge delete

add a comment

0

answered 2023-02-15 16:29:43 -0500

joseecm

130 ●11 ●23 ●22

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)

edit flag offensive delete link

add a comment

0

answered 2023-02-14 11:36:12 -0500

robustify

956 ●10 ●25 ●18

tf2 has a Transform class that overrides the * operator to actually convert points between frames using the geometry_msgs/TransformStamped results from the TF lookup. Below is a snippet:

geometry_msgs::TransformStamped tf_result;
try {
  tf_result = buffer.lookupTransform("parent_frame", "child_frame", ros::Time(0));
} catch (tf2::TransformException& ex) {
  // TODO: handle lookup failure appropriately
}

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
tf2::Quaternion q(
    tf_result.transform.rotation.x,
    tf_result.transform.rotation.y,
    tf_result.transform.rotation.z,
    tf_result.transform.rotation.w
);
tf2::Vector3 p(
    tf_result.transform.translation.x,
    tf_result.transform.translation.y,
    tf_result.transform.translation.z
);
tf2::Transform transform(q, p);
tf2::Vector3 point_in_child_coordinates(1, 2, 3);
tf2::Vector3 point_in_parent_coordinates = transform * point_in_child_coordinates;

If you want to convert the other way, from parent coordinate to child coordinates:

tf2::Vector3 child_coordinates = transform.inverse() * point_in_parent_coordinates;

edit flag offensive delete link

Comments

Thanks for your answer. I am using ROS2 Galactic and when I try to import tf2 in python I get the following error: ModuleNotFoundError: No module named 'tf2'.

But I have checked that tf2 package is installed. Any idea what could be the problem?

Regards.

joseecm ( 2023-02-15 10:15:10 -0500 )edit

The above code is in C++, and I don't think there is any equivalent to the tf2::Transform in Python. You'll have to do the coordinate frame conversions yourself in Python.

robustify ( 2023-02-15 15:57:33 -0500 )edit

add a comment

Is there a function to convert coords between two frames?

2 Answers

Comments

Question Tools

Stats

Related questions

Is there a function to convert coords between two frames? edit

2 Answers

Comments

Question Tools

Stats

Related questions

Is there a function to convert coords between two frames?