Design decisions for cl-tf2

[fairlight1337]

Hi,

In the light of tf2-refactoring:

I'm currently refactoring all major CRAM packages to use the new cl-tf2 infrastructure (cl-tf2, cl-transforms-plugin). The most parts are just replacing calls to tf:... or cl-tf:... by cl-transforms:... or cl-transforms-plugin:... as applicable, but there are more grave ones. Specifically:

• with-transforms-changed-callback is now missing from cl-tf2 while it was present in cl-tf. While I personally don't use that macro, major CRAM parts use it:
  • cram_plan_library: at_location (https://github.com/cram-code/cram_highlevel/blob/master/cram_plan_library/src/at-location.lisp#L83)
  • bullet_reasoning uses its subsequent infrastructure such as set-transform and execute-changed-callbacks (https://github.com/cram-code/cram_physics/blob/master/bullet_reasoning/src/robot-model-utils.lisp)

I would not say we need these functions, but what we need is a solution that either

• reflects the behavior of the cl-tf functions or
• replaces it by something suitable.

So, here's my question:

• (1) Was it a design decision (i.e., is there a strong reason) to leave the transformer*.lisp functionalities out of cl-tf2 or
• (2) was it simply not done yet?

I volunteer for pulling the code over from cl-tf and adapt it accordingly in case of (2), as we really need this functionality.

@airballking, can you comment on this?

[fairlight1337]

Okay, having a look at https://github.com/airballking/roslisp_common/blob/tf2-refactoring/cl_tf2/src/cl-tf2/broadcast-publisher.lisp makes me believe that you don't favor the callbacks paradigm.

I'd like to re-introduce it anyway, as everything else would bloat the high level code with tf-specifics. How about it?

[airballking]

I did not include with-transforms-changed-callback because I did not get what it is supposed to do. Once we have a clear specification of its functionality and still think it is useful, we can port it.

[fairlight1337]

It basically listens to /tf and memorizes all transforms it sees. When one changes, it executes a list of registered callbacks that are interested in changed transforms. Such ones are for example at-location, which only calls for action when the robot pose in the world changes, and bullet_reasoning that updates its visualization and physics world when the robot kinematics move.

So, in a nutshell, it maintains a list of currently active transforms and calls functions when those change. We do have use-cases for that.

[airballking]

Also: The whole design of TF has changed with the introduction of the client-server option.

Before, the client would get callbacks through the tf-topic. Now, the client would have to constantly pull on the server to see whether a transform has changed...

[fairlight1337]

But /tf is still there, right? Or is this going away anytime soon?

[airballking]

yes, /tf is still there, and I see we are heading for quite the hack ;)

[fairlight1337]

Hm, I won't consider this a hack. Many components in ROS rely on /tf constantly pushing out changes, even with TF2 present (and that's been around for a while already).

I see that pulling from the server using only new TF2 functionality would be pretty costly, but that won't be needed. /tf and the TF2 buffer server serve two different mechanisms here: While the former pushes out the current state fast, the second one makes transformations easy in a reliable way. So intermixing the two is fine as they are part of the same infrastructure.

The only hack would be if we intermix cl-tf and cl-tf2 again, but making use of /tf is along the lines of TF2 imo.

[airballking]

I think we have a misunderstanding here.

I see only one possible solution to create with-transforms-changed-callback with the TF2 buffer-server:

• the TF2 buffer-client subscribes to the `/tf``
• in its callback, it queries the buffer-server for all the transforms it shall monitor
• it compares the new transforms with its own buffer (which it does not have yet), and triggers the internal callback where suitable

3 things I do not like about this:

• the buffer-client now has to have a buffer, keep it updated, and compare transforms; I wanted to avoid writing and maintaining this sort of buffering code with the switch to TF2
• /tf is updated with more than 50Hz; we can only lookup transforms one at a time; this will result in constant polling of the server
• (this is my own fetish) it further exposes the notion of a tf-transform to the rest of the system; I actually tried to offer an API which would invite users to mainly transform stamped datatypes

[fairlight1337]

So how do we do this? Any suggestions on tackling the underlying problem? I'm afraid this is a blocker for adopting the new cl-tf2-paradigms.

[airballking]

Well, if this is a shot-stopper, we have to include it with the above implementation.

There would be 2 better alternatives which are more long-term:

• include this functionality into the C++ buffer-server and get the main TF-guys to merge this (ideal)
• refactor the relevant code in CRAM and get rid of this macro (not ideal because MAYBE 1 or 2 use cases are justified and not hacky)

[fairlight1337]

Okay, I guess I can think of something for at-location such that it can do its job without the callbacks. But for bullet, I can think of another way right now.

For the upcoming demo, the latter is not crucial, but would be good to have. As long as cl-tf is still around, it will keep working anyway. So I'll push the second one on the long bench for now and concentrate on getting demo centric components up to speed again.

On 15.04.2015, at 09:29, Georg Bartels notifications@github.com wrote:

Well, if this is a shot-stopper, we have to include it with the above implementation.

There would be 2 better alternatives which are more long-term:

include this functionality into the C++ buffer-server and get the main TF-guys to merge this (ideal) refactor the relevant code in CRAM and get rid of this macro (not ideal because MAYBE 1 or 2 use cases are justified and not hacky) — Reply to this email directly or view it on GitHub.

[moesenle]

Hi guys,

I think Jan is right in one of his previous mails: using the /tf topic is not hacky in any way. It is still published for processes that need to transform a lot and/or quickly or that need to subscribe to transforms. The ros package tf2_ros actually still provides a TransformListener besides the BufferClient class. The TransformListener is very similar to my old cl-tf and the old tf c++ library and subscribes to /tf and /tf_static.

I think instead of trying to get something like transform subscription into the tf2 server which would probably never be accepted because that's what the TransformListener class is for, it would be better to find a solution that is similar to tf2_ros's TransformListener.

Lorenz

On Wed, Apr 15, 2015 at 9:42 AM, Jan Winkler notifications@github.com wrote:

Okay, I guess I can think of something for at-location such that it can do its job without the callbacks. But for bullet, I can think of another way right now.

For the upcoming demo, the latter is not crucial, but would be good to have. As long as cl-tf is still around, it will keep working anyway. So I'll push the second one on the long bench for now and concentrate on getting demo centric components up to speed again.

On 15.04.2015, at 09:29, Georg Bartels notifications@github.com wrote:

Well, if this is a shot-stopper, we have to include it with the above implementation.

There would be 2 better alternatives which are more long-term:

include this functionality into the C++ buffer-server and get the main TF-guys to merge this (ideal) refactor the relevant code in CRAM and get rid of this macro (not ideal because MAYBE 1 or 2 use cases are justified and not hacky) — Reply to this email directly or view it on GitHub.

— Reply to this email directly or view it on GitHub https://github.com/ros/roslisp_common/issues/26#issuecomment-93244394.

[gaya-]

Ok, I'm going to wrap this thing with cffi and get done with it. @moesenle, as a person who's got experience with cffi, anything I should beware of?

[airballking]

At the time of this discussion, Jan and me had an offline discussion about this and a look into the parts of CRAM which use with-transforms-changed-callback. Result: There was a misunderstanding.

I originally thought that one could register a callback with the semantics like, for instance, "Wake me up when the transform from base-link to r-gripper-tool-frame has changed!". Turns out, I was wrong.

with-transforms-changed-callback allows the registration of callbacks which get called whenever there is a new message on the /tf-topic. So, much more basic functionality. (Side-note, I think with-tf-update-callback might be a clearer name but that is not relevant.)

As a result, I think we can implement such a callback mechanism in the buffer-client to ease porting CRAM to cl-tf2.

@gaya- Regarding your last post, if you need a local tf-buffer in your application then wrapping the c++ reference implementation with CFFI sounds like an interesting endeavour. It would be cool to see if the wrapper implemented the buffer-interface of cl-tf2.

[moesenle]

Hi Gaya,

sorry for my late response, I was on vacation.

As a general note on cffi: only use it if there is really no other way, in particular when you need to wrap C++ library not just a C library.

I have to admit I do not really know which problems were solved by switching to TF2. As I see it, the ROS TF2 implementation extends TF with a client/server based approach and the possibility to use different transform data types. The use of a client/server architecture has some benefits (simlicity, less band width, less memory) when only a few transforms are required but it also has its drawbacks, in particular when a lot of transformations have to be made (in particular latency). Anyway, I think the most important aspect of the TF2 ROS library is that it extends the old TF library with a client/server based approach but does not replace the old buffer functionality. I actually even believe that large parts of the old TF code base is still in TF2.

What I basically want to say is that I do not see a reason why the old cl-tf library cannot coexist with the new cl tf2 library that is only a client for a TF2 server as I understand it. Can't you just take the old cl-tf implementation and change it to your needs, maybe update the data types and fix the problems you might have with it. The transform functionality is all there and I think that approach is much less painful (and less error prone) than writing a C library that wraps the C++ TF2 library and then wrap the C library using CFFI.

Lorenz

On Thu, May 7, 2015 at 4:17 PM, Gayane Kazhoyan notifications@github.com wrote:

Ok, I'm going to wrap this thing with cffi and get done with it. @moesenle https://github.com/moesenle, as a person who's got experience with cffi, anything I should beware of?

— Reply to this email directly or view it on GitHub https://github.com/ros/roslisp_common/issues/26#issuecomment-99882765.

[gaya-]

sorry for my late response, I was on vacation.

Thanks a lot for your answer!

As a general note on cffi: only use it if there is really no other way, in particular when you need to wrap C++ library not just a C library.

Is this your experience-based gut feeling talking or is there a specific big problem with CFFI and C wrappers? Haven't thought about a C wrapper for TF2 yet but Lisp CFFI seems to be quite mature and easy to use (not sure how the callbacks will work out though yet). I'd rather rely on TF2 interfaces not to change much in future and leave the bug-fixing and maintenance of the C++ library itself to OSRF instead of maintaining the Lisp ported version. So, is the biggest concern with this FFI approach that it actually takes unexpectedly much time to get the thing working properly?

[moesenle]

Hi,

As a general note on cffi: only use it if there is really no other way, in

particular when you need to wrap C++ library not just a C library.

Is this your experience-based gut feeling talking or is there a specific big problem with CFFI and C wrappers? Haven't thought about a C wrapper for TF2 yet but Lisp CFFI seems to be quite mature and easy to use (not sure how the callbacks will work out though yet). I'd rather rely on TF2 interfaces not to change much in future and leave the bug-fixing and maintenance of thett C++ library itself to OSRF instead of maintaining the Lisp ported version. So, is the biggest concern with this FFI approach that it actually takes unexpectedly much time to get the thing working properly?

I would call it more than a gut-feeling. A CFFI based wrapper for TF2 can be quite some work. What you'll have to do is:

• Write a C-library that wraps all methods of all C++ classes you want to export in Lisp, including exception handling, allocation and deallocation functions and maybe the correct use of shared pointers.
• Write a CFFI wrapper for the C library. Don't forget to get garbage collection right, i.e. you'll have to use finalizers to free allocated classes.

While such an implementation is not hard (see cl-bullet), it is still prone to subtile bugs coming from the wrong use of pointers, memory layout etc. and even copy/paste errors. You'll have to know a little about how the C/C++ memory model looks like.

For complex functionality (e.g. bullet or assimp) I considered the effort of wrapping a C++ library worth it. But for something as simple as TF (basically just transform buffering and multiplication of transforms) or a URDF parser, writing the wrapper turns out to be more more work than implementing the library natively. And in the case of TF you already have a working implementation in Lisp.

In my experience, the argument that a CFFI wrapper avoids maintenance work because bugs need to be fixed only once never turned out to be true. You'll probably still have bugs in the wrapper. Also, when you are facing bugs, finding out if it is in the native library or in the wrapper can be harder because if something is wrong in your marshalling code or pointer magic you'll require in the wrapper can be quite cumbersome.

If you still want to write the wrapper (will be quite a good learning experience :)) and run into any specific problems, in particular regarding memory management and pointers, don't hesitate to ask.

Lorenz

[airballking]

Side-note: It might be worth giving the old cl-tf implementation another try (no matter through which interface).

Originally, we got errors when looking up transforms which definitely should have been in the buffer. At the time, we never found the bug. A couple of months ago, we discovered that (roslisp::ros-time) had a sub-optimal implementation which caused timestamps to have quite some uncertainty (past and future). This is now fixed. I wonder whether this was the reason for our original issues with cl-tf...

[gaya-]

@airballking The old ros-time used to have an error of 0.5s and the cl-tf timestamps on TurtleBots had errors of multiple seconds (up to 10s?), so I doubt that it fixes all the problems. But, yes, I'll see if I can write some unit tests for cl-tf the next couple of days and If we're lucky it might not even be a thread synchronization problem (:S).

[gaya-]

In the meantime, I haven't been able to reproduce the problems I used to have with the transforms on Turtlebots. I've made two bag files, one with the Turtlebot and one with PR2 and both give good results for the latest available timestamp using cl-tf. I haven't done that much testing, mostly comparing the transforms between different frame combinations at time 0 with cl-tf and tf_echo and they were exactly the same. I've built in a timeout argument for transform-pose, similar to the corresponding function from cl-tf2 and unified the two APIs, so from functionality point of view the two listeners are the same for me now. I must say that, as expected, TF1 is much faster than TF2, so I'll go ahead and continue using it and fix any bugs that come along.