ros / geometry2

A set of ROS packages for keeping track of coordinate transforms.
190 stars 276 forks source link

Add possibility to delete static TF frame. #370

Open peci1 opened 5 years ago

peci1 commented 5 years ago

With tf2, there's no possibility to remove already published static TF frames. I think it'd make sense to have such possibility.

There's a relevant ARO question: https://answers.ros.org/question/311864/is-it-possible-to-delete-a-static-tfs/ .

My use-case: a sensor is removed from the robot, I regenerate the URDF, restart robot_status_publisher, but RViz and all other nodes are still showing the removed frames until they're restarted.

I'm not sure about the ideal implementation of such a thing. My idea: static_transform_broadcaster could get a removeFrame() or clear() method, remove the frame from its net_message_ cache, and publish to /tf_static. The listeners could notice that they received fewer frames from this specific publisher, and seek for the ones that should be deleted from their caches. However, that'd need storing the publisher id with each transform in the cache, which is probably not a good idea. So maybe there could be a special value of TfMessage that'd instruct listeners to remove the frame from their caches. NaNs can't be this value, because TransformListener ignores all messages with NaNs and changing this behavior would break a lot of code... Infinity also doesn't seem very good... Any ideas? Maybe misusing timestamps by storing the maximum uint value?

tfoote commented 5 years ago

Yeah, that's kind of violating the assumption of a static frame being static. The mechanism for tf being distributed doesn't allow for deleting messages based on some node stopping publishing.

For regular transforms the timestamps won't match and this is generally how I'd recommend connecting an intermittent link with a standard frame not a static frame as the frame is not static.

If you do need to invalidate a static transform I think that disconnecting it from the tree is the appropriate method. You'll have to be careful about making sure to use different unused frame_ids and not collide.

As a potential workaround, if we added support for transforms changing from static to dynamic or vice versa then you could just publish a regular transform, that would then age out as any other transform would.

But in general if you're doing something that has any expectation of going away it doesn't really make sense to be classified as static.

peci1 commented 5 years ago

I understand your remarks. Apparently, "static transform" can mean several things for people.

A sensor connected to a fixed location never changes its position, it can just appear/disappear. In such situation, it doesn't seem right to publish the TF at 1-100 Hz. And I think that was one of the reasons why /tf_static was created.

Also, the tf/tf2 system allows changing static transforms - that's why I'd expect deleting is also an option.

The workaround with disconnecting the TFs from the main tree sounds doable and I'll probably use it in our project.

I'm thinking about the rationale/use-case for switches between static/dynamic tfs. Breakable joints? Locking joints? But anyways, I think it'd mean relaxing the definition of static transforms you advocated...

tfoote commented 5 years ago

The goal of static transforms was to remove the need for recommunicating things that don't change. The ability to update the values was implemented in case they are subject to uncertainty and might be reestimated later with improved values. But importantly those updated values are expected to be true at all times.

The adding and deleting of transforms is problematic specifically because of the assumptions about true for all time. If you have a detachable arm represented by a static transform, and you go through the process of 1) Install, 2) Remove, 3) Install 4) Remove with the arm.

If you add a static transform at t=1 and 3 and delete the transform at t=2 and 4. Any queries for transforms from the gripper to the body should work with a timestamp of 1.5. However if that query is made between t=1 and t=2 it will pass, but suddenly the previously working query with timestamp of t=1.5 will break at t=2 due to the static transform being deleted. And the other issue holds. At t=2.5 the transform should never lookup, However if queried at time t=3.5 a lookup of at timestamp 2.5 will pass if a static transform is used.

These incorrect results will also be produced if you break the tree by changing the connectivity. If you have a frame that's coming and going you should really use a standard frame and publish it at an appropriately low rate that's appropriate to capture the expected frequency of changing the topology of your robot.

The biggest way that static transforms differ from regular transforms is that they don't keep a time history This primarily saves storage and lookup time, though the publishing overhead is also better.

In your case if you're actually stopping and regenerating the robot there might be other state that's stored/cached as well. I don't have a good suggestion for that as it's likely coming from robot_state_publisher's assumption that joints/urdf's are static.

peci1 commented 5 years ago

That's a nice explanation of the static TF design goal. I couldn't find it anywhere under wiki.ros.org/tf2 , not even in the design document or REPs.

It almost seems that there's need for a third type of transforms, something like /tf_slow, which would work as normal /tf, but would be latched. But I understand that would be a big change that's maybe not possible to be done.

tfoote commented 5 years ago

Unfortunately latching doesn't help communicate new data any quicker. It just communicates old data more reliably. The important thing is to know when things change and to get that you have to publish. I guess you could add a new type of transform that's purely event driven and would keep the history but be assumed to be valid into the future until otherwise specified. Though this would make the system much more stateful and consequently harder to be robust to noise and communication disruptions.

peci1 commented 5 years ago

@tfoote Feel free to close this issue. In the end, it seems the idea isn't well aligned with design goals of TF2. And there's the workaround with disconnecting static TFs from their parents if you need to get rid of them.

machinekoder commented 3 years ago

@tfoote Is there a way to unpublish/delete non-static transforms? E.g. MoveIt's planning scene is generating lot's warning messages when a transform is not published anymore, but the data is still in the buffer. But, well they are also using a private function, so who could blame you: https://github.com/ros-planning/moveit/blob/master/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp#L1317

v4hn commented 3 years ago

If you do need to invalidate a static transform I think that disconnecting it from the tree is the appropriate method. You'll have to be careful about making sure to use different unused frame_ids and not collide.

Just to understand the discussion here, this method refers to publishing a new static transform overwriting the parent frame of the transform with a non-existent one. Is that correct? If that is so, what do you say about adding a special keyword "DELETE" or something similar that one can pass as the parent id to avoid name clashes when disconnecting multiple transforms using this method?

peci1 commented 3 years ago

That's basically what I'm doing now. All deleted transforms are reparented under frame nonexistent. I think the name is pretty self-explanatory and a chance for a name clash is pretty low...

v4hn commented 3 years ago

Did you also add special handling for the frame too? Otherwise you actually end up with being able to "transform" between multiple frames you disconnected that way frame1->nonexistent<-frame2.

peci1 commented 3 years ago

Yes, that's true. In our use-cases, it doesn't matter, as we're only interested in being able to transform links relative to the robot's base link. I don't see an easy way of generating random but unique parent frame names. Maybe UUID would be of help here?

v4hn commented 3 years ago

That's why I would propose to define a keyword, nonexistent is nice, and handle it differently in tf2. The easiest implementation could just refuse to transform if nonexistent is the common root of the transform tree between looked-up frames.

peci1 commented 3 years ago

That sounds interesting. I'm curious whether the community would be willing to accept this small (and improbably affecting) API break.

tfoote commented 3 years ago

If the frame or transform is transient, I somewhat wonder if it should be called static.

I'd suggest that this be added targeting a new distro and it wouldn't be a problem. I would also suggest using a longer const defined in the message to avoid collisions better.