Using ROS 1 Transforms to Calculate Object Positions

In previous posts, we've covered how visualizing ROS 1 transforms in Foxglove Studio can help us describe the relative positions of a robot’s components.
In this tutorial, we'll learn how ROS 1 transforms can help us calculate the position of an object not explicitly linked to a robot – in relation to both the sensor that detected it and the actuator that will interact with it.
Calculating positions in different frames
Let’s consider the robot model from our previous ROS transforms tutorials:

If our robot’s sensor were to detect an object, we'd have that object’s position in the sensor_link
frame. To use our robot’s arm to grab it, however, we’d then need to calculate the object’s position in the arm_end_link
frame.
With the ROS 1 tf2
library, we can easily transform our object’s position across frames – all without worrying about any of the complicated mathematical operations happening under the hood – and deduce how our arm needs to move to complete this task.
Create a sensor node to simulate object detection
First, let’s create a node that simulates our robot’s sensor.
In a sensor.cpp
file, include the following libraries:
#include <ros/ros.h>
#include "geometry_msgs/PoseStamped.h"
Define a Sensor
class with the following private variables:
class Sensor: ros::NodeHandle{
private:
// Timer for the simulated detected object
ros::Timer object_timer_;
// Publisher for the simulated detected object
ros::Publisher pose_pub_;
// Pose to publish
geometry_msgs::PoseStamped pose_;
// Aux variable that determines the detected object’s position
int count_ = 0;
It should also include a timer callback, to periodically publish the detected object’s position:
void objectCallback(){
// All transforms must be correctly time-stamped
pose_.header.stamp = ros::Time::now();
pose_.header.frame_id = "sensor_link";
if (count_ % 2) {
pose_.pose.position.x = 1.0;
pose_.pose.position.y = 1.0;
} else {
pose_.pose.position.x = 2.0;
pose_.pose.position.y = 3.0;
}
// Change the detected object’s position, depending on whether count_ is even or odd
count_++;
pose_pub_.publish(pose_);
}
Next, write a class constructor that publishes the detected object’s pose on a timer (every second). Since poses are not transform messages, they will be published on a /detected_object
topic (instead of /tf
or /tf_static
):
public:
Sensor():
NodeHandle("~"){
// Initialize timer for object detection
object_timer_ = createTimer(ros::Duration(1),
std::bind(&Sensor::objectCallback, this));
// Initialize pub for object detection
pose_pub_ = advertise<geometry_msgs::PoseStamped>(
"/detected_object", 10);
}
~Sensor(){}
};
Finally, add the main
function that declares and spins the node:
int main(int argc, char **argv){
ros::init(argc, argv, "sensor");
auto node = Sensor();
ros::spin();
return 0;
}
Create a listener node to calculate the object’s position
Next, let’s create a listener node to transform the object’s position from the sensor_link
frame to the arm_end_link
frame.
In a tf_listener.cpp
file, include the following libraries:
#include <ros/ros.h>
#include "geometry_msgs/PoseStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2/exceptions.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
Define a TfListener
class with the following private variables:
class TfListener : ros::NodeHandle{
private:
// Subscription to pose published by sensor node
ros::Subscriber pose_sub_;
// Buffer that stores several seconds of transforms for easy lookup by the listener
tf2_ros::Buffer tf_buffer_;
// Listener for the broadcast transform message
tf2_ros::TransformListener* tf_listener_;
// Pose in source frame (`sensor_link`)
geometry_msgs::PoseStamped pose_in_;
// Pose in target frame (`arm_end_link`)
geometry_msgs::PoseStamped pose_out_;
It should also include a poseCallback
function, which looks up the transform between the source and target frames every time the sensor sends a new pose:
void poseCallback(const geometry_msgs::PoseStamped msg) {
try {
// Store the incoming pose in pose_in_
pose_in_ = msg;
// Transforms the pose between the source frame and target frame
tf_buffer_.transform<geometry_msgs::PoseStamped>(pose_in_, pose_out_,
"arm_end_link", ros::Duration(1));
// Log coordinates of pose in target frame
ROS_INFO("Object pose in 'arm_end_link' is:\n x,y,z = %.1f,%.1f,%.1f",
pose_out_.pose.position.x,
pose_out_.pose.position.y,
pose_out_.pose.position.z);
} catch (const tf2::TransformException & ex) {
ROS_WARN("Could not find object position in 'arm_end_link' frame.");
return;
}
}
Next, write a class constructor that loads and listens to your buffer of transforms, and then subscribe’s to the sensor’s /detected_object
pose topic:
public:
TfListener():
NodeHandle("~") {
// Listen to the buffer of transforms
tf_listener_ = new tf2_ros::TransformListener(tf_buffer_);
// Subscribe to pose published by sensor node (check every second)
pose_sub_ = subscribe<geometry_msgs::PoseStamped>
("/detected_object", 10, &TfListener::poseCallback, this);
}
~TfListener(){}
};
Lastly, write a main
function that declares and spins the node:
int main(int argc, char **argv) {
ros::init(argc, argv, "tf_listener");
TfListener node;
ros::spin();
return 0;
}
Add the following lines to your CMakeLists.txt
to add the executables to the compilation list:
add_executable(sensor src/sensor.cpp)
add_executable(tf_listener src/tf_listener.cpp)
target_link_libraries(sensor
${catkin_LIBRARIES}
)
target_link_libraries(tf_listener
${catkin_LIBRARIES}
)
Compile your package with $ catkin_make
.
Launch nodes and visualize transforms
Let’s first publish the transforms between our base_link
and both arm_base_link
and sensor_link
. Use the static_transform_publisher
we’ve learned in the previous post:
$ rosrun tf2_ros static_transform_publisher 1 1 0 0 0 0 base_link arm_base_link
$ rosrun tf2_ros static_transform_publisher 1 -1 0 0 0 0 base_link sensor_link
You will also need the tf_broadcaster
that publishes the arm_end_link
frame:
$ rosrun transforms tf_broadcaster
Finally, run your new nodes:
$ rosrun transforms sensor_node
$ rosrun transforms tf_listener
Open a connection to your data in Foxglove Studio using the Foxglove bridge. Open a 3D panel to visualize the frames and detected object positions, and a Log panel to see the printout of our object’s position.
The object is represented by a purple arrow in the 3D panel. Its position in the
arm_end_link
frame is printed by the tf_listener
node in the Log panel.
Notice that the object arrow is different from the axes arrows that represent each frame. This is because the object is not a transform, but a Pose
. If you use the Teleop panel to move the arm_end_link
frame around, you can see firsthand how the position of your detected object changes.
Create a launch file
In a launch
folder, let’s create a tf_launch.launch
file to launch all nodes together:
<?xml version="1.0"?>
<launch>
<!-- Two static transforms publishers -->
<node pkg="tf2_ros" type="static_transform_publisher" name="sensor_link" args="1 -1 0 0 0 0 base_link sensor_link" />
<node pkg="tf2_ros" type="static_transform_publisher" name="arm_base_link" args="1 1 0 0 0 0 base_link arm_base_link" />
<!-- Three nodes from transform package-->
<node pkg="transforms" type="tf_broadcaster" name="tf_broadcaster" output="screen"/>
<node pkg="transforms" type="sensor" name="sensor" output="screen"/>
<node pkg="transforms" type="tf_listener" name="tf_listener" output="screen"/>
<!-- Foxglove Bridge node -->
<node pkg="foxglove_bridge" type="foxglove_bridge" name="foxglove_bridge" output="screen"/>
</launch>
Now you can start all nodes, the Foxglove bridge connection, and Foxglove Studio with a single command.
Stay in touch
By leveraging ROS 1 transforms and the tf2
library, you can easily integrate your robots' sensors and actuators to work better together.
For a reference to all the code covered in this post, check out our foxglove/tutorials
GitHub repo. And as always, feel free to reach out to us directly in our Slack community to request a topic for the next tutorial!
Read more:
Configure your storage region for Foxglove-hosted data.

Foxglove community member contributes internationalization support.

Get blog posts sent directly to your inbox.