Transforms in ROS
Transforms
- In robotics, the positions and orientations of objects (e.g., robots, sensors, and obstacles) are often expressed as transformations of one coordinate frame into another
- As time advances, these transformations change (e.g., if the robot moves).
- Although the choice of frames (and therefore the coordinates used to represent the state of the system) is arbitrary, the right choice of frames can greatly simplify the math required
- ROS provides an API for tracking these frames as they evolve in time
- tf2 Tutorials
- Information about Transforms in ROS 2
Transform Tree
- In ROS, the transforms form a tree, with every node corresponding to a frame
- Every frame has one parent and an unlimited number of children
- The location of a frame is specified relative to its parent
- Every frame has a unique name, called a frame_id
- Every transform on the tree is timestamped. All queries specify not just the
frame_id
but also the time.
ROS Transform API
- The transform (tf) tree is available on the
/tf
topic - Any node can publish transforms on
/tf
or retrieve information from/tf
- ROS has a special API for manipulating the Transform Tree.
- The
tf2_ros
package implements this API
- The
Listeners
- a listener subscribes to
/tf
and maintains a time-history of the received frames - The buffer can then be queried for information about the transforms
- For example, what is the transformation between frame X at time T and frame Y at time U?
Broadcasters
- A broadcaster publishes frames to
/tf
- Multiple nodes can publish different frames to
/tf
and they will be added into the tree based on their names - Each message you broadcast on
/tf
creates an edge in the tree- The transform converts frame_id into child_frame_id
- Equivalently the transform converts coordinates in child_frame_id into coordinates in frame_id
- Be careful not to broadcast conflicting frame information or information that violates the tree structure.
Static frames
- Some frames do not change: these are known as static frames
- Static frames can be broadcast to the
/tf_static
to consume less bandwidth - These frames are latched, which means that the last message sent on
/tf_static
will be sent to new subscribers.
Datatypes and Representation
- A transformation consists of a Translation and a Rotation
- The main message type of a transformation is a geometry_msgs/msg/TransformStamped
- It contains
- A timestamp.
- A parent frame
- A child frame
- A Transform, which consists of
- A translation
- A rotation (represented as a quaternion)
- The
tf2
python tutorials provide some sample code for converting between quaternions and Euler angles - A rote conversion from axis-angle representation to a quaternion is as follows:
- Let $\hat{w} = \begin{bmatrix}w_x \\ w_y \\ w_z \end{bmatrix} be a unit rotation axis and let \(\theta\) be the angle of rotation
- The equivalent quaternion is \(cos\frac{\theta}{2} + (w_x \hat{i} + w_y \hat{j} + w_z \hat{k}) \sin\frac{\theta}{2}\)
- The transforms3d package can be used to handle 3D transformations (including quaternions).
- Install with
sudo apt install python3-transforms3d
, the<exec_depend>
is onpython3-transforms3d
- Install with
Frame Conventions
ROS follows a set of conventions for frames.
- From REP 0103 Standard Units of Measure and Coordinate Conventions
- All distances are in meters
- All angles are in radians
- All coordinate systems are right-handed
- Transforms frame_id into child_frame_id: thus the transform converts points in child_frame_id to into the frame frame_id
- There are four descriptions for orientation, in preferred order
- Quaternions
- Rotation matrices in \(SO(3)\)
- Fixed-axis rotations using roll-pitch-yaw (a.k.a. fixed XYZ Euler angles)
- Relative-axis ZYX Euler axis
- From REP 105 Coordinate Frames for Mobile Platforms
- On a mobile robot, \(x\) is forward, \(y\) is left, and \(z\) is up
- For cameras \(z\) is forward, \(x\) is right, \(y\) is down, and
frame_id
ends with_optical
Tools
There are several tools that can be used to debug the transform tree.
ros2 run tf2_tools view_frames
: generatesframes.pdf
file image of the current frames and how they are connected.ros2 run tf2_ros tf2_echo [from_frame] [to_frame]
- print transformations between framesros2 run tf2_ros static_transform_publisher
- Creates a static tf2 frameros2 doctor
May scan for failures in the tfros2 run rqt_tf_tree rqt_tf_tree
lets you see examine the tf tree interactively- You may need to separately install the
rqt_tf_tree
package
- You may need to separately install the
rviz
can be used to visualize frames.- You must choose the correct
fixed frame
in rviz to see any of the frames. - All frames are displayed relative to the
fixed frame
- Having an invalid
fixed frame
will prevent your data from showing up in rviz. - You can add the
Tf
view to see all the frames on tf - You can also add individual
axes
objects
- You must choose the correct
Tips and Tricks
- The timing of the transforms you look up is crucial.
- If two frames came in too far apart
tf
will throw an exception rather than interpolating - The ideal way to solve this problem is to
- Understand the frequency at which the frames are coming in
- Only query the listener when recent-enough data is available
- The practical way to solve this problem is to
- Use
try-except
around transform calls - Ignore the exceptions and try again on your next iteration.
- Use
- If two frames came in too far apart
- To ask for the most recent transforms, use
rclpy.Time()
as the time:rclpy.Time.now()
gets the current time, but a transform may not be available at the current time.
- Make sure your transforms form a connected tree
- There should be a path from the root of the tree to every node (i.e., no disconnected components)
- Every frame should have only one parent.
- Each transform should be provided from a single location
- There are no default frames, only frames that you add.
Other Resources
- The activity that goes with this work can be found TF Activity
- tf2_ros API Docs
- Jazzy documentation is very incomplete as of 10/2024
- Rolling documentation is somewhat incomplete as of 10/24
Self generate the complete documentation:
git clone https://github.com/ros2/geometry2 rosdoc2 build --package-path geometry2/tf2_ros # Documentation is under docs_output/tf2_ros
- 3Blue1Brown Quaternion Video
- Interactive quaternions