Fix TfPublisher subframe publishing

[felixvd]

A silly mistake got through in #1792 . Without this PR, the scene looks like this:

After the change, it looks as expected.

Checklist

• Required by CI: Code is auto formatted using clang-format
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[v4hn]

Aren't you talking about #1757 ?

I actually appreciate the subframe transforms to be published as relative transforms from their object frame. Don't you think so?

[felixvd]

I agree that subframes should be grouped under their parent object, and I would prefer if they were relative transforms as well. But unless I am misremembering, the transforms that are stored in the collision world are absolute transforms from the planning frame :/ (see here) I guess this goes to show that we all want a scene graph.

I noticed after submitting the PR that the frames are not listed under their object name anymore, which is an unintended side effect.

I'll fix this to make the subframe transforms relative unless @JonasTietz has free cycles and picks this up.

PS: I meant #1792.

[v4hn]

unless @JonasTietz has free cycles and picks this up.

He implemented this plugin on WMD19 because I asked him to. Please go ahead and improve it as you see fit.

[felixvd]

I'll send candy for his next contribution ;)

I pushed a change that requires no retransforms and seems to work. I will test a bit more this week and remove the WIP after, but it looks good.

[rhaschke]

LGTM: subframes of world objects are relative to the planning frame, subframes of attached objects are relative to the attached object.

[felixvd]

I'm still seeing some issues with the subframes when the objects are attached, so I'll keep this as WIP.

[tylerjw]

This needs a test once it is fixed to make sure published transforms are as expected. Do you have an minimal example project that could be used as a basis for a test?

• Create tests, which fail without this PR

[tylerjw]

This is probably just my own lack of knowledge of how to use rviz, but how do I get the transforms of stubframes to show up? If it is some special configuration that should be added to the subframe tutorial as this seems like it'd be a really useful feature if I could figure out how to use it.

I'd be happy to help make those changes to the tutorials and write a test for this if I can get some help figuring out how to use this?

[v4hn]

This is probably just my own lack of knowledge of how to use rviz, but how do I get the transforms of stubframes to show up?

• you need to explicitly load this MoveGroupCapability by adding it to the capabilities parameter.
  In contrast to the name of the folder it is in.... it is not actually loaded by default. @felixvd you might want to change that while you are at it 😎
• add a TF display to RViz. All TF frames should show up. you can enable/disable individual ones in the display properties.

[tylerjw]

@v4hn I tried adding this to move_group.launch but couldn't get it work:

<param name="capabilities" value="
                  default_capabilities/MoveGroupCapability
                  " />

Is there documentation on how to use this parameter you could point me towards?

[tylerjw]

@v4hn I added this to move_group.launch and got it to work.

<param name="capabilities" value="
                  move_group/TfPublisher
                  " />

Is there documentation on how to use this parameter? This took me a while to figure out.

[tylerjw]

I tested this locally and it now works by default. Thank you for this. I would still like a test but I'll approve this and put it on the list of things to figure out how to test.

[v4hn]

@tylerjw

Is there documentation on how to use this parameter? This took me a while to figure out.

I'm not sure? There's a comment on the capability in the generated launch file.
I don't know whether there is a tutorial to write/add a custom capability.
And there is https://moveit.ros.org/documentation/plugins/#movegroupcapability .

@felixvd Sorry, you misunderstood my comment! I believe @rhaschke did not want to have this capability loaded by default. I proposed to move it out of the folder.

[tylerjw]

This seems like such a useful feature to be so difficult (for me atleast) to figure out how to use it. I found the comment in the generated launch file only after asking here, and then asking someone else what you might mean. After I found that I still had to search through the code to try to figure out what the actual string is I should put in there as the comment wasn't that helpful.

If we don't want this enabled by default I think this should be explicitly called out in the tutorial for subframes on how you would turn this on.

And, lastly, if there are other non-default capabilities that are useful, there should be some documentation around using move_group (ideally a tutorial) that briefly explains their use and how you would enable them.

[v4hn]

The only other one I ever used/wrote (or know about) is https://github.com/TAMS-Group/move_group_online_collision_predictor .
But the plugin system is a convenient way to adjust things in the move_group node, so I wouldn't be surprised to find users wrote a lot of those for trivial things.

This seems like such a useful feature to be so difficult (for me atleast) to figure out how to use it.

Personally, I think the MoveGroupCapability plugin system is one of the simplest once we have!
You are right though, a tutorial for it would be a good idea. Be our guest 🌞

[felixvd]

I agree with @tylerjw on this, it's more difficult than it seems to find all the spots you need to change things. I was lost on where to (mistakenly) add this to the default capabilities, too. This sort of C++ interface is not easy for the average robot user, I think.

@v4hn I see. What about enabling the capability by default, but activating it through a service or via a rosparam that is checked every second? It would be good if one could activate this from RViz without changing the moveit_config files.

[v4hn]

What about enabling the capability by default, but activating it through a service or via a `rosparam` that is checked every second? It would be good if one could activate this from RViz without changing the `moveit_config` files.

The argument for not using the capability in RViz was that the PlanningSceneDisplay already has all information and should not rely on TF to visualize it. Using TF, we also see the additional prefix (for no obvious reason) in the visualization. So, if you just want to see the frames in RViz, you should extend the Display's functionality instead of polluting tf. :-) I'd be fine with having the capability by default if startup is controlled through a parameter and you can start/stop it via a service. (Did you know that, in theory, nodes can and do subscribe to ROS parameters and get callbacks for updates? :) There's no well-exposed API for it though...).

[felixvd]

I've been using this for 2 weeks and it seems to work fine, so I'll remove the WIP label. Since this is a bugfix and the rest of what we discussed would be an improvement, I'd merge this first and open an issue to track the rest.

[felixvd]

The Travis job says "Pending" on this page, but when I look at the Details it says "Passed". Am I missing something?

[rhaschke]

Yeah, unfortunately, github <-> travis sometimes have issues communicating with each other.
We didn't yet figure out the reasons...

[tylerjw]

I vote you merge this and create an issue for testing the functionality of the TfPublisher. I can see your point that asking for a test on a bug fix is scope creep... but only barely.

[tylerjw]

I changed my opinion. Please write a test for this change. This is a useful fix but not useful enough to merge without a test imo. Please help us keep this working in the future.

[v4hn]

Sorry for stabbing you in the back @tylerjw, but no, I don't think it's mandatory to have a test for this TF publisher in the same pull request. Please feel free to create issues for all the concrete modules without test coverage you are aware of for WMD20! It would be awesome to see coverage increased a lot by the end of next month.

However, I don't agree with the solution in this patch! (sorry for only looking at the patch after posting multiple comments here...)

LGTM: subframes of world objects are relative to the planning frame, subframes of attached objects are relative to the attached object.

Gosh, did we really agree to that? That is awful semantics.

Even if that is the way we have to model it internally (I believe due to the reference in the API?),
I would propose to publish the subframes always relative to its object.
If we do that, the "parent arrows" in RViz's TF plugin make a lot more sense.

@felixvd Could I ask you to adjust that, or do you disagree?

[tylerjw]

You could write a test that just tests what you are changing in this PR and not all of TF Publisher. I think bug fixes should come with tests to show that they are fixed and stay fixed.

If not now, when, and by who?

The person fixing the bug is the person most familiar with the code being changed and therefore it should be easiest for them to implement a test for it. Wouldn't it also be much easier for reviewers to see that the bug is fixed if it comes with a test that they can run and validate?

[v4hn]

In general I agree and I'm happy to see MoveIt move in this direction. In this case I don't. You are free to disagree. Purists w.r.t. pull-request requirements always either end up writing most things themselves (see Robert 😉 ) or with lingering pull-requests instead of merged improvements (welcome back to the older days of MoveIt). I tend to be more liberal in that regard, trying to bridge the gap between Dave and Robert. 😎

The overhead involved, in terms of development time, lines of code (and actually runtime) to setup the move_group, spawn objects, wait for tf transforms to be published, isn't worth it to me to make it a requirement for fixing the existing issue.

I could imagine Felix will actually provide a rostest now, due to your bickering. 😄

[tylerjw]

I feel a bit more liberty to poke @felixvd over this as I was the one wrote the subframes test.

Disclaimer: I am not a maintainer, my views are not that of a maintainer, I'm just someone here trying to contribute and care about the robustness and maintainability of MoveIt. :)

[tylerjw]

The overhead involved, in terms of development time, lines of code (and actually runtime) to setup the move_group, spawn objects, wait for tf transforms to be published, isn't worth it to me to make it a requirement for fixing the existing issue.

I would hope the subframes test could just be extended to test this or at-least much of it could just be copy-pasta from the subframes test for this to skip the boring parts of writing this test.

[felixvd]

I agree with both of you actually. I don't think tests should be a requirement to merging bugfixes, but figured I'd write one yesterday for good measure, then thought "a separate test file spawning a scene and MGI is a lot of overhead to test just one feature, but adding it to another random test would be sort of disorganized and hard to maintain" and ran out of cycles.

Oh, also I didn't take it negatively since I know my karma bank on writing tests is negative :) I'd be glad to take this on as a World MoveIt Day project.

Gosh, did we really agree to that? That is awful semantics.

Even if that is the way we have to model it internally (I believe due to the reference in the API?), I would propose to publish the subframes always relative to its object. If we do that, the "parent arrows" in RViz's TF plugin make a lot more sense.

@felixvd Could I ask you to adjust that, or do you disagree?

I do agree - it was originally done that way because of the internal model (world represents everything in the global coordinate system), but in hindsight it is so confusing that I myself forgot the way it was working when I explained it to others, and it's my own code! While working on #2037 I tried to disentangle this by storing both relative and global poses of subframes and shapes for all objects and attached_bodys, but I kept it out of the PR for now, because I know that would balloon into more work. The WIP branch is here, though.

Rather than adding semantics into this PR to transform from/to global coordinates, I would prefer add them to the internal representation, since that makes everything easier to work with. Also, even though currently the TF tree created by the TfPublisher capability looks sort of awkward, the user can still transform between all the poses, so there's no functional problem with the PR as is.

PS: Naturally, this is all screaming for a scene graph, but that's a separate discussion.

[tylerjw]

Another alternative to the test approach would be if there was some way to unit test the TfPublisher instead of making the test depend on all the other code and things like the move_group executable.

I know the tests I've been writing have been the easier to write integration test style. This is for two reasons, first I'm a beginner with MoveIt, and secondly because testing functionality externally with our most beginner friendly interfaces seemed like an important thing to do.

I hope to work towards more focused unit tests though because they have two advantages over the ones that depend on huge chunks of code. When they fail they are much easier to debug what failed and therefore less frustrating for someone who breaks them with a new change. Also, they take less time to execute. CI already takes forever to run and adding a bunch of integration tests that spin up MGI over and over and over again is going to make that much worse.

Please excuse my ramblings. I'm happy to help work with you on the test for this and try to learn from anything you discover while doing it so we can make future test writing easier. I know tests suck to write, but if we figure out what the pain points are and find ways to document how to do the boring parts of writing them hopefully they can be easier to write in the future. Let me know how I can help. Writing tests doesn't have to be painful, they just are when there is very few examples to look at and almost no documentation. Each test we write makes the next one easier.

[v4hn]

figured I'd write one yesterday for good measure, then thought "a separate test file spawning a scene and MGI is a lot of overhead to test just one feature, but adding it to another random test would be sort of disorganized and hard to maintain" and ran out of cycles.

That's exactly what I anticipated. 😄

Also, they take less time to execute. CI already takes forever to run and adding a bunch of integration tests that spin up MGI over and over and over again is going to make that much worse.

Yes, rostest units should either test higher integration or at least a whole battery of interface behaviors.

it is so confusing that I myself forgot the way it was working when I explained it to others, and it's my own code!

I actually used this capabilities a few days ago to make a screenshot (for presentation) and did not notice any problem:

That is because I just passed in relative poses for the subframes and apparently planning would not even work like that...

While working on #2037 I tried to disentangle this by storing both relative and global poses of subframes and shapes for all objects and attached_bodys, but I kept it out of the PR for now, because I know that would balloon into more work. The WIP branch is here, though.
Rather than adding semantics into this PR to transform from/to global coordinates, I would prefer add them to the internal representation, since that makes everything easier to work with

I seem to recall @rhaschke was unhappy with storing global poses of subframes and other shapes but I'm open to discuss this as long as we get subframe poses always relative to its object.
The current interpretation rather follows the rule of most surprise which is a very big anti-pattern in API development...
Please go ahead and continue your WIP branch so we can get this semantic change in as soon as possible.

[v4hn]

so there's no functional problem with the PR as is

Does this mean you want to merge this request for now and revert it once the internal representation was changed?

[felixvd]

Yes, I would merge this request for now since it works, and then include updates to it in the WIP branch.

[v4hn]

CI passed, but did not report back.

I'll merge with two approvals.

Please change the semantics in your WIP patches though.