Ask Your Question
1

Simple tf transform without broadcaster/listener

asked 2018-04-22 04:28:29 -0500

chillax gravatar image

updated 2018-04-23 03:28:46 -0500

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 flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
2

answered 2018-04-22 09:26:07 -0500

lucasw gravatar image

updated 2018-04-22 17:18:48 -0500

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.header.stamp = rospy.Time(0)
ts1.header.frame_id = 'map'
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.header.stamp = rospy.Time(0)
ts2.header.frame_id = 'frame1'
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.header.frame_id = "map";
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));
edit flag offensive delete link more

Comments

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

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

Take a look at the updated answer

lucasw gravatar imagelucasw ( 2018-04-22 11:28:21 -0500 )edit
0

answered 2018-04-23 03:33:44 -0500

chillax gravatar image

updated 2018-04-23 03:36:43 -0500

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.header.frame_id = "map";
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.header.frame_id = "drone";
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)

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2018-04-22 04:28:29 -0500

Seen: 278 times

Last updated: Apr 23 '18