Coordinate Tranforms using TF¶

In this exercise, we will explore the terminal and C++ commands used with TF, the transform library.

Motivation¶

It’s hard to imagine a useful, physical “robot” that doesn’t move itself or watch something else move. A useful application in ROS will inevitably have some component that needs to monitor the position of a part, robot link, or tool. In ROS, the “eco-system” and library that facilitates this is called TF. TF is a fundamental tool that allows for the lookup the transformation between any connected frames, even back through time. It allows you to ask questions like: “What was the transform between A and B 10 seconds ago.” That’s useful stuff.

Reference Example¶

ROS TF2 Listener Tutorial

Further Information and Resources¶

Scan-N-Plan Application: Problem Statement¶

The part pose information returned by our (simulated) camera is given in the optical reference frame of the camera itself. For the robot to do something with this data, we need to transform the data into the robot’s reference frame.

Specifically, edit the service callback inside the vision_node to transform the last known part pose from camera_frame to the service call’s base_frame request field.

Scan-N-Plan Application: Guidance¶

Specify tf2_ros and tf2_geometry_msgs as dependencies of your core package.
- Edit package.xml (+2 lines) and CMakeLists.txt (+4 lines) as in previous exercises

Add tf2_ros::Buffer and tf2_ros::TransformListener objects to the vision node (as class members variables).

#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
...
tf2_ros::Buffer buffer_;
tf2_ros::TransformListener listener_;

Also add include lines to be able to work with geometry_msgs types:

#include <geometry_msgs/PoseStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

The transform listener must be constructed using the buffer. Initialize it in the class constructor:
```
class Localizer
{
public:
  Localizer(ros::NodeHandle& nh) : listener_(buffer_)
```
Add code to the existing localizePart method to convert the reported target pose from its reference frame (“camera_frame”) to the service-request frame:
1. Remove the placeholder line from a previous exercise that sets the resulting pose equal to the pose from the last message
```
- res.pose = p->pose.pose;
```
2. To perform a coordinate transformation, we will use a stamped pose object which bundles a 3D pose with metadata about what coordinate system the pose is in, which is known as a header. These pieces of data both come from the message received by the marker publisher:
```
geometry_msgs::PoseStamped target_pose_from_cam;
target_pose_from_cam.header = p->header;
target_pose_from_cam.pose = p->pose.pose;
```
3. Use the buffer object to transform this PoseStamped object to another coordinate frame, specified by the frame in the service request:
```
geometry_msgs::PoseStamped target_pose_from_req = buffer_.transform(
    target_pose_from_cam, req.base_frame);
```
- Note: The buffer looks up the transformation between the camera frame and the base frame at the specific time when the message was first generated, which is also recorded in the header of the message.
- There are many other Stamped versions messages besides PoseStamped. Most of them can also be transformed to different coordinate system using the same method.
1. Return the transformed pose in the service response.
```
res.pose = target_pose_from_req.pose;
```

Run the nodes to test the transforms:

catkin build
roslaunch myworkcell_support urdf.launch
roslaunch myworkcell_support workcell.launch

Change the “base_frame” parameter in workcell.launch (e.g. to “table”), relaunch the workcell.launch file, and note the different pose result. Change the “base_frame” parameter back to “world” when you’re done.