search

safijari / tiny_tf

Python reimplementation of part of the tf library used in ROS. Based on this paper http://wiki.ros.org/Papers/TePRA2013_Foote?action=AttachFile&do=view&target=TePRA2013_Foote.pdf
MIT License
6 stars 5 forks source link

Scope and roadmap #2

Open felixvd opened 5 years ago

felixvd commented 5 years ago

Hi,

I found this project via this thread and would be interested in using something like it for tools that might become a ROS package later.

Is there a reason you did not use the same class names as the original TF package, and are you planning to add Pose transformations?

Cheers

safijari commented 5 years ago

I had code set up that was already using tf2 but it was in an environment where I didn't really need ROS (if anything it was detrimental). If I had found or able to build a "python version" of tf2 I would have just done a drop in replacement and be done with it.

As I started writing my own code I ended up making some changes along the way that feel more ergonomic to my usecase. Possibly the biggest departure is using the included Transform class which has some (what I consider) nice quality of life features (multiple constructors, properties for representing the underlying transform as various things like quaternion or matrix, etc).

If you can give me a summary of what changes/additions to the API would suit your usecase, I can consider implementing them. That said I would probably add support for them without modifying the current API rather than change the current API (since I really like it, that may be my internal bias speaking through :sweat_smile: ).

felixvd commented 5 years ago

1) It would be great if the same API as in ROS was available, so that no code rewriting is needed when moving a package away from ROS. From what I can see, it could just be an additional layer (similar to how Transformer and TransformerROS are built on top of one another in TF), or alternative member functions (like getOrigin, setOrigin...). I think even ROSy Pose and Point objects would be useful, to replace the syntax inherited from geometry_msgs and to be clear about the quaternion conventions, but that's not a pressing issue.

2) I am sorely missing the transformPose command in the current version. That's mostly what kept me from using this package for an intern of mine.

3) Coming from ROS, it wasn't intuitive to understand what "TFNode", "TFTree" correspond to. Why not keep the same names as in TF? Most people who would use this will already be familiar with them.

safijari commented 5 years ago

Ok. This makes sense. All of this could certainly be built on top of what's already present.

  1. Sounds good.
  2. I've been missing that functionality myself. Easy peasy to add.
  3. Errr ... I've honestly forgotten how this is laid out in ROS. The tree is ... the tree :sweat_smile:. And each node represents a parent, child, transform tuple that helps build the tree. If I build a ros like API on top of this I don't think these would need to change.
safijari commented 5 years ago

How about this: give me a small example that works using tf2, and I'll plan out how to support it with tiny_tf.

felixvd commented 5 years ago

Alright! Here you go.

Yes, conceptually the tree is the TF tree. I don't remember if in the original the tree is held in TransformLists or one of the Buffer objects, but it's not that important. Users generally interact with the Transformer, TransformerROS or TransformListener objects.

I guess an equivalent setup would be to make a Transformer and TransformerROS class that own a TFTree and offer the usual APIs. The third layer (Listener) makes no sense to reimplement in my opinion, but the other two would be sort of convenient.

One more thing: Why does TFTree offer "get_parent"? Does that return the "TFNode" that has no parent? I would personally use something like "frame" or "transform" rather than "node", so there's no confusion with ROS nodes.

felixvd commented 5 years ago

I don't mean to be pushy, but if you know you won't have time soon, would you prefer a PR? Just asking so we don't duplicate work unnecesarily.

safijari commented 5 years ago

Sure, feel free. Sorry I'm half way through a pretty involved project at work. I'll be available for reviews/discussions if needed though.

felixvd commented 5 years ago

This went way better and easier than I thought! Have a look here.

I got this result with the converted test script, which is off by a little because of numerical precision, but on point compared to the original as far as I could see:

Pose nr. 0: 0.0, 0.0, 1.0; 0.6532814824381884, 0.6532814824381883, -0.27059805007309845, 0.27059805007309856
Pose nr. 1: 0.0, 0.0, 1.0; -0.27059805007309845, 0.6532814824381884, -0.6532814824381883, 0.27059805007309856
Pose nr. 2: 0.0, 0.0, 1.0; -0.6532814824381884, 0.27059805007309856, 0.6532814824381884, 0.27059805007309856
Pose nr. 3: 0.0, 1.0, 2.3686212597530973e-17; -0.7010573846499779, 0.701057384649978, -0.09229595564125764, -0.09229595564125764
Pose nr. 4: 0.0, 1.0, -1.1891139150155847e-32; -0.09229595564125764, 0.701057384649978, 0.701057384649978, -0.09229595564125768
Pose nr. 5: 0.0, 1.0, 6.522187408836978e-34; 0.701057384649978, -0.09229595564125768, -0.701057384649978, -0.09229595564125764
Pose nr. 6: 1.0, -1.673117952244272e-17, -1.673117952244274e-17; -0.09229595564125732, 0.4304593345768795, 0.5609855267969311, 0.7010573846499778
Pose nr. 7: 1.0, 2.1651968869753108e-17, 8.871815602075444e-18; 0.5609855267969309, 0.43045933457687935, 0.09229595564125735, 0.701057384649978
Pose nr. 8: 1.0, 1.5488736100669274e-17, -1.3795939805233356e-32; 0.09229595564125737, 0.7010573846499781, -0.09229595564125728, 0.7010573846499778
Pose nr. 9: 1.0, 0.0, 0.9999999999999998; 0.0, 0.1305261922200516, 0.0, 0.9914448613738105
Pose nr. 10: 1.0, -1.0007599823009208e-17, 1.0; 0.0, 0.0, 0.1305261922200516, 0.9914448613738105
Pose nr. 11: 1.0, 0.0, 1.0; 0.1305261922200516, 0.0, 0.0, 0.9914448613738105
Pose nr. 12: -9.676441861234117e-49, 0.9999999999999999, 2.0; 5.551115123125783e-17, 0.7071067811865475, 5.551115123125783e-17, 0.7071067811865475
Pose nr. 13: 0.0, 1.0, 1.9999999999999998; -6.123233995736767e-17, 6.123233995736767e-17, -6.123233995736767e-17, 1.0
Pose nr. 14: 4.930380657631324e-32, 1.0, 2.0; -1.2246467991473537e-16, -1.2246467991473532e-16, -1.2246467991473537e-16, 1.0
Pose nr. 15: -0.9659258262890684, 1.258819045102522, -3.191891195797325e-16; -0.6123724356957945, 0.3535533905932741, -0.3535533905932742, 0.6123724356957945
Pose nr. 16: -0.9659258262890681, 1.2588190451025216, -3.201795739107433e-16; 0.7329629131445345, 0.5624222244434794, 0.30360317934095915, -0.23296291314453396
Pose nr. 17: -0.965925826289068, 1.2588190451025218, -1.7549249239034206e-16; 0.3794095225512605, -0.049950211252315135, -0.12059047744874017, 0.9159756150367534
Pose nr. 18: -0.25881904510252185, 0.03407417371093158, -2.7755575615628914e-17; 0.5000000000000001, 0.5000000000000001, 0.5000000000000001, 0.5000000000000001
Pose nr. 19: -0.25881904510252185, 0.0340741737109316, -2.594264827603041e-17; 0.3705904774487392, -0.48296291314453427, 0.48296291314453416, 0.6294095225512611
Pose nr. 20: -0.25881904510252185, 0.03407417371093157, -3.1447194445336383e-17; 0.12940952255126095, -0.017037086855466, -0.12940952255126092, 0.9829629131445341
Pose nr. 21: 0.0, 1.0, -1.0000000000000004; 0.7499999999999998, -0.43301270189221974, -0.2500000000000001, 0.4330127018922193
Pose nr. 22: -7.751183968975937e-17, 0.9999999999999997, -1.0000000000000002; 0.732962913144534, -0.5624222244434802, -0.14644660940672613, -0.3535533905932739
Pose nr. 23: -1.1291144527723928e-16, 1.0000000000000002, -1.0000000000000002; 0.9829629131445341, -0.12940952255126098, 0.01703708685546609, -0.12940952255126048
Pose nr. 24: -0.9659258262890682, 1.258819045102522, -1.0000000000000004; 0.7865660924854931, -0.10355339059327422, -0.6035533905932737, -0.07945931129894589
Pose nr. 25: -0.9659258262890685, 1.2588190451025218, -1.0000000000000004; -0.6830127018922194, 0.1830127018922197, 0.7071067811865476, 3.532708032038494e-16
Pose nr. 26: -0.9659258262890681, 1.258819045102522, -1.0000000000000002; 0.6830127018922193, -0.18301270189221971, -0.6830127018922193, -0.18301270189221974
Pose nr. 27: -2.1906706976806576, 0.5517122639159752, -6.66133814775094e-16; 0.9914448613738104, -0.1305261922200522, 5.249077009405429e-17, -1.398635076929392e-32
Pose nr. 28: -2.1906706976806576, 0.5517122639159753, -6.66133814775094e-16; 0.701057384649978, -0.09229595564125757, -0.7010573846499777, -0.09229595564125766
Pose nr. 29: -2.1906706976806585, 0.5517122639159754, -6.661338147750937e-16; -0.701057384649978, 0.09229595564125749, 0.701057384649978, 0.09229595564125764
Received a pose to transform to EE link:
0.0, 0.0, 8.0
0.0, 0.0, 0.0, 1.0
New pose:
0.0, 0.0, 4.0
0.6532814824381884, 0.6532814824381883, -0.27059805007309845, 0.27059805007309856
Pose nr. 30: 0.0, 0.0, 4.0; 0.6532814824381884, 0.6532814824381883, -0.27059805007309845, 0.27059805007309856
============ Done!

I just plopped the geometry_msgs folder from my ROS installation in there as a makeshift solution. Maybe you have thoughts on that? It might make sense to package them inside for users coming from or going towards ROS.

I'm also not a real developer, so whatever you can say about clean formatting of packages is welcome.

safijari commented 5 years ago

That looks great.

On the geometry_msgs bit I think it comes down to if you think serialization/deserialization from the ROS binary format will be needed. If (I'm hoping) not then I can just generate the necessary classes from the ROS messages and keep those in this package. Thoughts?

felixvd commented 5 years ago

Whoops, forgot to respond. I don't think I need the serialization, no, but is it worth putting in the work to generate the classes yourself?

The only thought I had was that it would be nice to namespace the geometry_msgs under tf and package them along with this.

safijari commented 5 years ago

Okay. It's not much work to generate classes (I've already had to to it to migrate a codebase away from ROS). I can merge your PR as is and then work on doing that after.