[WIP] Debug/Fix #485

[rhaschke]

Attempt to fix #485 (comment):

• Sometimes, this assertion is violated in SolutionCollector:
  https://github.com/ros-planning/moveit_task_constructor/blob/564804bd7bb9e320b304b8c4880d96c6ce63b915/core/src/container.cpp#L491
• Solutions are enumerated multiple times (Connector stage has superfluous compute() calls)

[codecov]

Codecov Report

Attention: 12 lines in your changes are missing coverage. Please review.

Comparison is base (a905572) 59.00% compared to head (5674df2) 58.99%.
Report is 1 commits behind head on master.

❗ Current head 5674df2 differs from pull request most recent head e163f57. Consider uploading reports for the commit e163f57 to get more accurate results

Files	Patch %	Lines
core/src/stage.cpp	42.86%	8 Missing ⚠️
core/include/moveit/task_constructor/stage_p.h	0.00%	2 Missing ⚠️
core/src/container.cpp	33.34%	2 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master     #495      +/-   ##
==========================================
- Coverage   59.00%   58.99%   -0.01%     
==========================================
  Files          90       90              
  Lines        8485     8497      +12     
==========================================
+ Hits         5006     5012       +6     
- Misses       3479     3485       +6     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[v4hn]

This was obviously a lot of tedious debugging (which I started to repeat yesterday before noticing this PR again). Thank you for these patches.

Please see my inline comments. I would love to merge this upstream, but am not clear on some details yet.

[v4hn]

I tried to get my head around why this happens in some situations I encountered in the past and failed.
Can you describe a scenario where the assert will be violated with pruning active?
I'm fine with changing it because it is a step necessary if we want to have a flag that disables pruning for a whole task (any unpruned stage with a dead end on the other side of a partial solution will trigger this assert), but I do not yet understand why it can be invalid.

[rhaschke]

I'm not sure about the reasons anymore (it's nearly half a year ago that I handled this).
I think we cannot be sure that only valid solutions enter the start/end interfaces. For example, Connect stages do preserve all InterfaceStates, both successes and failures. We just save failures for later inspection as well.
Thus, we need to explicitly filter them here.
I remember that this was a no-brainer when encountering it. But now I don't remember a concrete example.

[v4hn]

it's nearly half a year ago that I handled this

Fair enough, my fault for letting it lie around for so long.

For example, Connect stages do preserve all InterfaceStates, both successes and failures.

Of course, but they also prune the paths on failure. so unless the solution path forks, the other end should not compute at all (as the interface state is pruned). I came up with an example case below where the assert is violated with forking solution paths:

But I observed a rare stochastic assert in the following situation some days ago where turn yielded a solution which hit the assert when propagating back the priority to the failed attempt in approach grasp:

In this situation there are no obvious forking solution paths (unless ComputeIK factors through a single pose in the wrapped generator, but that should not happen?) and the failure in approach grasp should have pruned the only connected interface start state in turn, so the compute call that generated the turn solution should not have happened.
I tried to write a test case for this, but failed to do so in reasonable time.

So TLDR: I guess dropping the assert is correct, but also obfuscates bugs in the pruning.

[rhaschke]

Strange that you cannot reproduce the error in a simpler unit test.