Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

To know what your problem is I'd need to see more of your system running. Overall it looks like you're on the right track, however your code is not robust to latencies in the system, with data and coordinate frames arriving in different orders than you are expecting.

I suggest you walk through the tf tutorials: http://wiki.ros.org/tf/Tutorials

Also make sure that you're repeatedly publishing the positions of the frames so that tf can correctly interpolate. If you only publish single datapoints for each frame_id at different times a lookup will never succeed.