rclcpp: TF2

The TF2 library provides easy access to transformations. All of the examples below require a dependency on the tf2_ros package.

Broadcasting Transforms

#include <tf2_ros/transform_broadcaster.h>
std::unique_ptr<tf2_ros::TransformBroadcaster> broadcaster;

broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(nodePtr);

geometry_msgs::msg::TransformStamped transform;
transform.header.stamp = node->now();
transform.header.frame_id = "odom";
transform.child_frame_id = "base_link";

// Fill in transform.transform.translation
// Fill in transform.transform.rotation

broadcaster->sendTransform(transform);

Listening for Transforms

#include "tf2_ros/transform_listener.h"

rclcpp::Node node("name_of_node");

auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node.get_clock());
auto tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer);

Applying Transforms

TF2 can be extended by packages that provide implementations of transform. The tf2_geometry_msgs package provides these for _geometry_msgs. The example below uses geometry_msgs::msg::PointStamped - but this should work for any data type in geometry_msgs:

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

geometry_msg::msgs::PointStamped in, out;
in.header.frame_id = "source_frame";

try
{
  tf_buffer->transform(in, out, "target_frame");
}
catch (const tf2::TransformException& ex)
{
  RCLCPP_ERROR(rclcpp::get_logger("logger_name"), "Could not transform point.");
}

The transform function can also take a timeout. It will then wait for the transform to be available up to that amount of time:

tf_buffer->transform(in, out, "target_frame", tf2::durationFromSec(1.0));

Get Latest Transform

A common work flow is to get the "latest" transform. In ROS 2, this can be accomplished using tf2::TimePointZero, but requires using lookupTransform and then calling doTransform (which is basically what transform does internally):

geometry_msgs::msg::PointStamped in, out;

geometry_msgs::msg::TransformStamped transform =
   tf_buffer->lookupTransform("target_frame",
                              in.header.frame_id,
                              tf2::TimePointZero);

tf2::doTransform(in, out, transform);

Constructing Transform from Euler Angles

There are numerous ways to do this (using Eigen, KDL, etc) - but it is also possible with only tf2 APIs:

#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

tf2::Quaternion quat;
// While the documentation for tf2 orders the names of these as yaw, pitch, roll,
// it specifies that yaw = rotation about Y, which is not what most of us expect
quat.setEuler(pitch, roll, yaw);

geometry_msgs::msg::TransformStamped transform;
transform.transform.rotation = tf2::toMsg(quat);
// Probably also fill in transform.header and transform.transform.translation

// Can now use the transform with tf2::doTransform()

Getting Yaw Angle from Quaternion

#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2/utils.hpp>

geometry_msgs::msg::Pose pose;
double yaw = tf2::getYaw(pose.orientation);

[!NOTE] tf2::getYaw requires some pieces from tf2_geometery_msgs/tf2_geometry_msgs.hpp but cannot depend on them because it would create a circular dependency. This means you absolutely need to include tf2_geometry_msgs BEFORE tf2/utils or you will get an undefined reference to tf2::fromMsg

Transform from Eigen::Isometry3d

#include <tf2_eigen/tf2_eigen.hpp>

Eigen::Isometry3d map_to_odom;
geometry_msgs::msg::TransformStamped transform = tf2::eigenToTransform(map_to_odom);
// Fill in header, child_frame_id