Added TF publisher to display joint origins

[akgoins]

Rebased and combined all commits from #59 per request and added in change from #67 as discussed in today's meeting. This is the only "clean" way that I found to do a rebase without affecting the previous commits. If this is acceptable, then #59 and #67 can be removed after merging this pull request.

[gavanderhoorn]

I'll look into #69 and #56 tomorrow and then see if this PR works as intended.

[gavanderhoorn]

An update: I've been testing this over the weekend, and there are two issues with the current approach:

1. it creates a broadcaster thread for every joint in the model: each of these threads broadcasts a single transform, which in principle is fine, but quickly leads to very large nrs of threads for models such as the PR2. Ideally a single broadcaster would be created that does all the work (and at a low frequency. For a static model, even 10Hz is a lot).
2. TF (as in: the technology) doesn't really have the concept of 'removing a frame'. This leads to the inability to reset the visualisation / TF display. So loading a new model after closing another causes the union of their frames to be displayed. The TF display has a reset method, but it's unclear whether that can be utilised to work around this.

[gavanderhoorn]

As to the issues with the PR2 model (as reported in #69 and also in #65) wrt the TF tree shown: afaict, all parent-child relationships are in-tact in the in-memory URDF model. The application creates some proxies, but those seem to point to the right data structures.

[gavanderhoorn]

I'll add some additional comments in-line.

[gavanderhoorn]

Afaict, urdf_transforms.h doesn't use anything from Qt, right? The editor_HDRS variable is only used to list headers that need to be put through the moc-ing process when building.

It can probably be removed.

[gavanderhoorn]

Screenshot illustrating the issue reported by @pbeeson (this is after loading the PR2 and then clicking New):

[phvass]

@akgoins can you look at this issue?

[gavanderhoorn]

@akgoins: due to #73 and #75, this won't merge cleanly anymore, but I'll fix that when merging, if you want.

If you want to test on indigo-devel, then a rebase (or a merge) would be needed.

[gavanderhoorn]

these lines could potentially help with the Fixed Frame.

They're from the MoveIt Setup Assistant which has a similar requirement.

[akgoins]

@gavanderhoorn, I think I have fixed the two remaining issues. I am now getting the root link of the model to set as the manager fixed link and I am publishing timestamps with the transforms which makes them disappear when they are no longer published. For some reason there is still a base_link transform which remains, but I don't know why that is the case since there are no frames being published. If it is still a concern, then I vote that we open an issue for it and address it later as I think that it is a very minor issue.

[akgoins]

@gavanderhoorn, conflicts resolved. Please review and merge.

[gavanderhoorn]

Please remove commented lines if they are no longer needed.

[akgoins]

@gavanderhoorn, I noticed that some of the links being loaded do not have the origin property added to them. Without this, the TF link information does not get loaded properly. I have added a warning statement to show this. Here are the results.

I don't know where the file is being parsed, but the problem is that the joints are passed to the addJointProperty function before they have had an origin property added to them.

[gavanderhoorn]

@akgoins wrote:

@gavanderhoorn, I noticed that some of the links being loaded do not have the origin property added to them. Without this, the TF link information does not get loaded properly. I have added a warning statement to show this. Here are the results.

ok, but apart from the warning, does this change anything (ie: correctness of tf tree fi)?

[gavanderhoorn]

@akgoins wrote:

I don't know where the file is being parsed, but the problem is that the joints are passed to the addJointProperty function before they have had an origin property added to them.

The raw urdf is parsed here, but it is actually delegated to the urdfdom library. Creation of JointPropertys is done here, where the JointProperty ctor doesn't add a origin property (here) if its an 'all-zeros' origin in the urdf. @Levi-Armstrong did this to avoid having the tree full of zero-origins.

JointPropertys not having an Origin property is something I think should just be handled by URDFTransformer, and using a zero-origin is the right thing to do. I don't believe it should result in warnings, as it's essentially ok (given how the application treats all-zeros origins).

As a test I've disabled the norm checking in the JointProperty ctor, just to see whether that changed anything. It did, but I'm not sure I understand why at this point: and the TF frames make a bit more sense (a lot actually, when comparing to the previous screenshot), but logically an all-zeros origin should be identical to what you do in URDFTransformer::updateLink(JointProperty *property)?

Note the difference with the previous screenshot.

[gavanderhoorn]

@akgoins what do you think of pr70_rebased and the unguarded updateBaseLink(..) in URDFProperty::on_propertyWidget_jointValueChanged()?

[gavanderhoorn]

As to the issues with the PR2 model: I'm leaning towards merging your PR and ticketing the issues with the PR2 TF tree to be fixed later. All other models in test_models seem to work ok, it's just the PR2. It might be something particular about its urdf that causes the problems we see.

[akgoins]

@gavanderhoorn wrote:

Could you check gavanderhoorn/CAD-to-ROS/pr70_rebased? I added some minor commits to clean up some thing, but other than that it should be identical to your tf_publisher branch.

I am unable to see your fork or any branches on it. When I click on the link provided, It does not take me to your repo. Probably a permissions issue. I have not had time to look at addressing your other comments but should be able to work on it tomorrow.

[gavanderhoorn]

I am unable to see your fork or any branches on it. When I click on the link provided, It does not take me to your repo. Probably a permissions issue.

Well if you mean the link in my previous comment (this link), that was not meant to take you to a repository, but to the comment before that. Apart from that though, my fork is probably not accessible for you, even with the correct link. I'll push it to this repository.

[gavanderhoorn]

Just pushed pr70_rebased here.

[gavanderhoorn]

@gavanderhoorn wrote:

All other models in test_models seem to work ok, it's just the PR2.

Just an additional data point: a quick test with akgoins/tf_publisher and the Baxter model in test_models shows that it also suffers from the same issues as the PR2 (lots of frames missing). Disabling the norm checks (here) (so it always adds OriginProperty instances) makes things appear much better, but the transforms look incorrect (ie: floating outside baxter's head).

[akgoins]

@gavanderhoorn, I have made some changes to my own branch here. This fixes the issue with some of the TF frames not showing up. I tried it with the Baxter and everything looks fine. The problem with the frames not showing up correctly for the PR2 has to do with the joint limits. If the joint limits are (-N,N), then the robot is visualized with the joint at 0. If the joint limit is (0,N) or (-N, 0), then the robot is visualized with the joint set to midscale. This can be seen by running roslaunch urdf_tutorial urdf_display model:=<dir>/pr2.urdf gui:=true. The gui shows some of the joint values at a non-zero start state. The TF publisher currently assumes that all joint values are set to zero. We will need to add some sort of mechanism that can set each joint to its appropriate value for correct visualization. This may entail using the robot_state and joint_state publishers to ensure that all of the TF frames are visualized in the appropriate location. @Levi-Armstrong is looking into how the Rviz RobotModelDisplay works as well as robot_link and robot_joint and has some ideas on a way that we could create an internal representation of the model in order to display everything ourselves rather than rely on publishers and Rviz subscribers.

So, the question is, do we use joint_state and robot_state to publish TF frames or do we use our own representation? Can robot_state publisher handle what we want to do with it or is it best for us to switch to something else now?

[gavanderhoorn]

@gavanderhoorn, I have made some changes to my own branch here. This fixes the issue with some of the TF frames not showing up. I tried it with the Baxter and everything looks fine.

Nice, thanks.

The problem with the frames not showing up correctly for the PR2 has to do with the joint limits. If the joint limits are (-N,N), then the robot is visualized with the joint at 0. If the joint limit is (0,N) or (-N, 0), then the robot is visualized with the joint set to midscale. This can be seen by running roslaunch urdf_tutorial urdf_display model:=<dir>/pr2.urdf gui:=true. The gui shows some of the joint values at a non-zero start state. The TF publisher currently assumes that all joint values are set to zero.

Ah, that makes sense, yes.

We will need to add some sort of mechanism that can set each joint to its appropriate value for correct visualization. This may entail using the robot_state and joint_state publishers to ensure that all of the TF frames are visualized in the appropriate location. @Levi-Armstrong is looking into how the Rviz RobotModelDisplay works as well as robot_link and robot_joint and has some ideas on a way that we could create an internal representation of the model in order to display everything ourselves rather than rely on publishers and Rviz subscribers.

So, the question is, do we use joint_state and robot_state to publish TF frames or do we use our own representation? Can robot_state publisher handle what we want to do with it or is it best for us to switch to something else now?

The problem with joint_state_publisher and robot_state_publisher is that they weren't really written with the kind of dynamic changes we are doing to the urdf in mind. See ros/robot_model#124 for a PR to add exactly that (tbh I don't think the PR is very elegant, but urdf/robot_model just wasn't written for these kind of things).

Without things like ros/robot_model#124, I don't think we can use any of the existing infrastructure, which is why I wrote #25. Your PR is essentially a mini version of ros/robot_model#124.

A custom version of RobotModelDisplay (and associated classes) could be an option and would avoid the need for the publishing infrastructure (and the parameter(s) kludge), and I've been thinking about that myself as well. I just didn't want to suggest it in this milestone.

[gavanderhoorn]

I think we can merge this PR as it is right now: the additional issues can be dealt with at a later time. The only thing I think must be added (but later) is a toolbar button / menu item that allows to (dis|en)able the display of the TF frames. That is a UI / usability issue, and I think we can ticket that for later work.

[phvass]

+1

[gavanderhoorn]

I've pushed an additional commit to pr70_rebased (diff here), which takes care of some SEGFAULTs I got when trying to create a model from scratch. It will probably work better when #97 is merged.

I also still have issues when not disabling norm checks (here), not all frames get displayed, but I'm not sure whether that is an issue with this code or with the norm checks in JointProperty.

[akgoins]

@gavanderhoorn, see commit 912ffb3 for the fixes to the TF frames.

[gavanderhoorn]

@akgoins wrote:

@gavanderhoorn, see commit 912ffb3 for the fixes to the TF frames.

Yeah, as far as I can tell, ad8f301 should be the rebased version of that in pr70_rebased. Correct?

[akgoins]

Yes, you are correct.

[gavanderhoorn]

Just did a fresh build of pr70_rebased, started editor, loaded the denso model from test_models and I see no frames.

[akgoins]

@gavanderhoorn, I found the problem with the denso model loading. For some reason, if the urdf does not include an origin property, the quaternion (x,y,z,w) gets set to all zeros, which is an invalid rotation. I have added some checks for NaN's and zero rotations and pushed the changes to pr70_rebased on my repo.

[gavanderhoorn]

I've just pushed a Milestone 1 merge candidate that includes this PR, see m1_merge_candidate (diff here).

It contains all changes proposed in this PR (plus some additional ones), so once m1_merge_candidate is merged, we can close this PR as merged as well.

[gavanderhoorn]

Closing, as these changes have been merged into m1_merge_candidate_v2.

[gavanderhoorn]

Thanks for the contribution @akgoins 👍.