# Simple tf transform without broadcaster/listener

I have a vision based control application, where a drone is flying around, finds a visual marker, extracts the coordinates of the visual marker in body frame coordinates and finally flies to the marker with a position setpoint in local NED frame coordinates.

I need to transform the coordinates of the visual marker from body frame to local NED frame based on the position of the drone in local NED coordinates.

I understand that this is possible with a broadcaster + listener, but I don't want to complicate things with extra nodes, all I need is a simple mathematical transformation. Is there a simple function available to do this in C++?

edit retag close merge delete

Sort by ยป oldest newest most voted

BufferCore is the best way to do it while also maintaining a similar api to regular tf transform sets and lookups ( see also #q258116 ). There aren't a lot of examples available however.

# Python

Here is a simple python example (I adapted it from http://docs.ros.org/diamondback/api/t... where the api is different, but did the example survive into kinetic somewhere? I also had to look at /opt/ros/kinetic/lib/python2.7/dist-packages/tf2_ros/buffer.py)

#!/usr/bin/env python
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped

buffer_core = tf2_ros.BufferCore(rospy.Duration(10.0))
ts1 = TransformStamped()
ts1.child_frame_id = 'frame1'
ts1.transform.translation.x = 2.71828183
ts1.transform.rotation.w = 1.0
buffer_core.set_transform(ts1, "default_authority")

# print dir(buffer_core)
a = buffer_core.lookup_transform_core('map', 'frame1', rospy.Time(0))
print a
# ((2.71828183, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0))
b = buffer_core.lookup_transform_core('frame1', 'map', rospy.Time(0))
print b
# ((-2.71828183, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0))

ts2 = TransformStamped()
ts2.child_frame_id = 'frame2'
ts2.transform.translation.y = 0.5
ts2.transform.rotation.w = 1.0
buffer_core.set_transform(ts2, "default_authority")

print buffer_core.lookup_transform_core('map', 'frame2', rospy.Time(0))


I'm not sure why there isn't a lookup_transform, just lookup_transform_core. Maybe I should be using a BufferClient?

Rotations can be handled with transforms3d/transformations.py, I'll update the above with an example of that.

## C++

Haven't tested this yet but it is very similar to python:

#include <tf2/buffer_core.h>
...
tf2::BufferCore buffer_core;
geometry_msgs::TransformStamped ts1;
ts1.child_frame_id = "frame1";
ts1.transform.rotation = tf::createQuaternionMsgFromRollPitchYaw(0, 0.32, 0);
const bool is_static = true;
buffer_core.setTransform(ts1, "default_authority",
is_static);
geometry_msgs::TransformStamped ts_lookup;
ts_lookup = buffer_core.lookupTransform("map", "frame1",
ros::Time(0));

more

Thank you so much for the quick reply. I am working with C++, therefore the example would be very much appreciated.

( 2018-04-22 09:31:56 -0500 )edit

Take a look at the updated answer

( 2018-04-22 11:28:21 -0500 )edit

Here is the tested version of the code in C++

#include <tf2/buffer_core.h>
...

tf2::BufferCore buffer_core;

//Transformation from map to drone coordinates
geometry_msgs::TransformStamped transform1;
transform1.child_frame_id = "drone";
transform1.transform.translation.x = 0;
transform1.transform.translation.y = 1; // 1 meter East
transform1.transform.translation.z = 1; // 1 meter Up
transform1.transform.rotation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0.7854); // 45degrees to the left
buffer_core.setTransform(transform1, "default_authority", true);

//Transformation from drone to visual target coordinates
geometry_msgs::TransformStamped transform2;
transform2.child_frame_id = "target";
transform2.transform.translation.x = 1.4142; // square root of 2 forward
transform2.transform.translation.y = 0;
transform2.transform.translation.z = 0;
transform2.transform.rotation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0);
buffer_core.setTransform(transform2, "default_authority", true);

//Transformation from map to visual target coordinates
geometry_msgs::TransformStamped ts_lookup;
ts_lookup = buffer_core.lookupTransform("map", "target", ros::Time(0));

ROS_INFO("%f, %f, %f", ts_lookup.transform.translation.x, ts_lookup.transform.translation.y, ts_lookup.transform.translation.z);


output: 1, 2, 1 (North, East, Up)

more