Allow incremental modifications to the AllowedCollisionMatrix

[v4hn]

Yet another long-standing issue I see in MoveIt is that users can't modify the AllowedCollisionMatrix incrementally.

The current code to incorporate received PlanningScene diffs overwrites the whole maintained ACM whenever a non-empty matrix field is received.
This does not allow for atomic modifications of the ACM.
In practice user-code has to get/monitor the current state of the PlanningScene, modify the current ACM in their own node and send a new full ACM to the move_group node.

I would like to propose to change the semantics in this context (the code referenced above) and incorporate updates of the ACM in diff-PlanningScenes as incremental modifications.

This would break the (imho rather unintuitive) use-case where users store the old ACM before adding new entries (referring to new objects) and expect these entries to vanish when they send the old ACM again.
On the other hand, this provides a more intuitive and useful interface that would simplify and robustify standard use-cases.

Before I spend time on this, I would like to know whether we can agree to do this.
@bmagyar @rhaschke @davetcoleman .

@2scholz for reference.

[davetcoleman]

I agree to this in Melodic as long as it lands in the CHANGELOG

[rhaschke]

Looks like a useful extension.

[felixvd]

Agreed. I'm not the benchmark, but as expected I misunderstood this behavior a few weeks ago and was deleting existing collision objects when adding new ones.

[v4hn]

as expected I misunderstood this behavior a few weeks ago

Feel free to provide pull-requests ;)

[felixvd]

Since it came up in the other issue, I had a quick look at the PlanningScene and remembered that a) I don't understand how the PlanningScene really works (decoupleParent?), and b) how you would envision this implemented in detail.

If a new ACM message arrives with a different number of entries than the current ACM, how do you deal with the difference? If a 3x3 ACM arrives, with only "collision disabled" entries, should we assume that the entries with the items that are not mentioned in the 3x3 ACM are also disabled (which might be dangerous)? I am not sure this is intuitive, or at least intuitive to code and read. It also makes me worry about dangling entries if only new entries are added, and none removed.

For atomic updates, it might be easier to expose setCollisions(entry_names1, entry_names2, value) in the PlanningScene and PlanningSceneInterface? I could imagine a CollisionMatrixUpdate message for that, and could reuse the logic in #1440 for it, as @v4hn suggested. Thoughts?

I am not sure if a separate message will be clutter, but I remember that when I wanted to disable collisions for a few objects the first time, I struggled with having to manipulate the ACM and then push it. It would probably be an easier interface like this, but since there will be the convenience methods in the PlanningSceneInterface, this might not be a concern.

[v4hn]

I am not a big fan of having yet another ROS interface to manipulate PlanningScene components, so if possible I would restrict it to updates through the existing interfaces.

I would assume that any entry in the ACM of a diff message you send takes priority over any existing entry in the collision matrix (and thus updates it entry-per-entry).

But you are right in that this does not provide a convenient way to remove elements from the ACM again (instead of just changing their entries).
You could still send a non-diff planning scene with a complete ACM that removes elements.
Alternatively, I would expect that "unnecessary" entries in the ACM are actually removed when the corresponding element is removed from the scene.
Is this actually the case now and would this be a sufficient way to remove elements from the ACM?

[felixvd]

I was looking at this a few days ago and couldn't think of any way to merge two ACMs efficiently. Would be very useful to know via #2717 how much performance would be affected.