Fix issue #136

[Levi-Armstrong]

Fix for issue #136:

When dynamically attaching a object to a given robot link without geometry, it would be ignored during collision checking because links without geometry are not included in the active_comonents_only_ set.

This was solved by exposing an existing JointModelGroup class member providing a complete set of updated links (with and without geometry) and using this method in collision_detection::CollisionData::enableGroup.

[v4hn]

This is so awsome ✨ ! Thank you a lot for looking into this!

With this patch links without collision geometry are also added to active_components_only_, right?
Does that mean that collision checking is also called for links without geometry? That means that we have to look out for code that expects valid collision geometry.
Are they filtered out before? If yes, where?

[Levi-Armstrong]

With this patch links without collision geometry are also added to active_components_only_, right?

That is correct, all links that get updated for a specified move group will be included even if it does not have collision geometry.

Does that mean that collision checking is also called for links without geometry? That means that we have to look out for code that expects valid collision geometry.
Are they filtered out before? If yes, where?

The active_components_only_ is only used for filtering found here. It first checks if either of the objects are of type ROBOT_LINK and if true it checks if either of the links are in the active_components_only_. If one is found to be true it proceeds with the collision check. Now if the type is ROBOT_ATTACHED it checks whether the parent link is in the active_components_only_ set. This is why it did not work before because links without geometry were not being included in the set.

Note: This change does not affect the list of robot collision objects because they are populated using this method which only get links with collision geometry. The attached objects then get added when a collision request is made here. This change should not break any of the existing functionality because it is only used for filtering whether two objects should be checked for collision.

[Levi-Armstrong]

I have one concern. Is it possible to attached an object to an existing attached object? Based on the AttachedBody constructor it looks like you can only attach objects to a robot link. If this is true, then this PR fix is good.

[v4hn]

The active_components_only_ is only used for filtering found here. It first checks if either of the objects are of type ROBOT_LINK and if true it checks if either of the links are in the active_components_only_. If one is found to be true it proceeds with the collision check. Now if the type is ROBOT_ATTACHED it checks whether the parent link is in the active_components_only_ set. This is why it did not work before because links without geometry were not being included in the set.

Thank you, that is an easy to understand explanation of the problem! I honestly didn't expect it to be that simple...

The patch looks good to me. +1
This has to be picked to jade/kinetic.

Addition: to address your concern: No, it is not possible to attach an object to an attached object.
So this is no problem.

[jrgnicho]

Looks good to me +1.

[gavanderhoorn]

Wishful thinking perhaps (due to it being involved), but should this PR perhaps include a unit test so we can guard ourselves against regressions in the future?

[v4hn]

ok, this has a clear explanation, it works over here, has 2 +1s and several _horray_s, so I'll merge it and cherry-pick to j/k.
@gavanderhoorn Feel free to create a ticket for the test - it would definitely make sense.

Thanks a lot @Levi-Armstrong !