Reduce preempt stop time in stages

[captain-yoshi]

This is a follow up to #597. To reduce time when preempting a task for each stages.

Stages now checks if there has been a preempt request before doing the compute() method. Let me know if there would be a cleaner way to achieve this.

This does not preempt a stage that has already began the compute() method.

[codecov-commenter]

⚠️ Please install the to ensure uploads and comments are reliably processed by Codecov.

Codecov Report

Attention: Patch coverage is 78.94737% with 4 lines in your changes missing coverage. Please review.

Project coverage is 55.70%. Comparing base (6b0f2c8) to head (f3a1abe).
Report is 36 commits behind head on master.

Files	Patch %	Lines
core/src/task.cpp	66.67%	2 Missing ⚠️
core/include/moveit/task_constructor/stage_p.h	85.72%	1 Missing ⚠️
core/src/stage.cpp	50.00%	1 Missing ⚠️

❗ Your organization needs to install the Codecov GitHub app to enable full functionality.

Additional details and impacted files

@@            Coverage Diff             @@
##           master     #598      +/-   ##
==========================================
- Coverage   58.82%   55.70%   -3.12%     
==========================================
  Files          91      132      +41     
  Lines        8623    10796    +2173     
  Branches        0      901     +901     
==========================================
+ Hits         5072     6013     +941     
- Misses       3551     4736    +1185     
- Partials        0       47      +47     

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

[rhaschke]

A std::function object is somewhat costly. What about using a direct pointer to the tasks's preempt_requested_ flag? Initially, that pointer will be NULL and the task's init function sets it.

[rhaschke]

I merged #597. Please rebase.

[rhaschke]

Thanks for incorporating my suggestions. I have a few more.

[captain-yoshi]

Second wave of suggestions done.

I think we should add a meaningful error message when throwing the PreemptStageException (if it throws somewhere e.g. in test units). Because I got something like this and did not know what it was at first glance:

[ RUN      ] CostTerm.SolutionConnected
unknown file: Failure
C++ exception with description "" thrown in the test body.
[  FAILED  ] CostTerm.SolutionConnected (0 ms)
[ RUN      ] CostTerm.SetLambdaCostTerm
unknown file: Failure
C++ exception with description "" thrown in the test body.

[rhaschke]

I think we should add a meaningful error message when throwing the PreemptStageException

Isn't the exception always caught in Task::compute()?

[captain-yoshi]

Yes when using the Task API. But when running some tests with only containers/mockups it may throw and no message is displayed.

[ RUN      ] GeneratorMockup.delayed
unknown file: Failure
C++ exception with description "std::exception" thrown in the test body.
[  FAILED  ] GeneratorMockup.delayed (0 ms)

[  FAILED  ] 1 test, listed below:
[  FAILED  ] GeneratorMockup.delayed

...

[  FAILED  ] 9 tests, listed below:
[  FAILED  ] CostTerm.SolutionConnected
[  FAILED  ] CostTerm.SetLambdaCostTerm
[  FAILED  ] CostTerm.CostOverwrite
[  FAILED  ] CostTerm.StageTypes
[  FAILED  ] CostTerm.PassThroughUsesCost
[  FAILED  ] CostTerm.PassThroughOverwritesCost
[  FAILED  ] CostTerm.PassThroughCanModifyCost
[  FAILED  ] CostTerm.CompositeSolutions
[  FAILED  ] CostTerm.CompositeSolutionsContainerCost

[rhaschke]

But when running some tests with only containers/mockups it may throw and no message is displayed.

I cannot reproduce these results. As far as I can see, those tests don't call preempt().

[captain-yoshi]

Yup. It happened to me because the preempted() function had bad logic initially.

bool preempted() const { return !preempt_requested_ || *preempt_requested_); }