Fix early preempt when planning #597

captain-yoshi · 2024-07-16T13:34:54Z

We have a usecase in which we need to plan the task in another thread and sometimes preempt it. When preempting early, this does not work because the preempt flag is reseted inside the plan() method.

This fix makes the preempt flag changeable from the user. So for the first planning nothing changes. But users must reset the preempt flag before each subsequent planning (if the preempt method is used).

Even with this fix, preempting a task can take some time. The preempt flag is only checked at the start of the plan compute loop. #312 seems far away, but I think we could reduce the time it takes to preempt a task without too much changes ?

codecov-commenter · 2024-07-16T13:45:05Z

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

Codecov Report

Attention: Patch coverage is 41.17647% with 10 lines in your changes missing coverage. Please review.

Project coverage is 56.44%. Comparing base (6b0f2c8) to head (167a57b).
Report is 34 commits behind head on master.

Files	Patch %	Lines
core/test/test_container.cpp	30.77%	9 Missing ⚠️
core/src/task.cpp	75.00%	1 Missing ⚠️

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

Additional details and impacted files

@@            Coverage Diff             @@
##           master     #597      +/-   ##
==========================================
- Coverage   58.82%   56.44%   -2.37%     
==========================================
  Files          91      131      +40     
  Lines        8623    10684    +2061     
  Branches        0      951     +951     
==========================================
+ Hits         5072     6030     +958     
- Misses       3551     4607    +1056     
- Partials        0       47      +47

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

rhaschke

When preempting early, this does not work because the preempt flag is reseted inside the plan() method.

I don't yet understand the race condition. Do you mean, preempt_requested_ may be set before it is initialized in plan() and then the request is lost?

rhaschke

I don't like that the user needs to remember to reset the preempt_requested_ flag.
What about resetting the flag when leaving Task::plan()?

rhaschke · 2024-07-16T14:59:36Z

core/include/moveit/task_constructor/task.h

 	moveit::core::MoveItErrorCode plan(size_t max_solutions = 0);
-	/// interrupt current planning (or execution)
-	void preempt();
+	/// interrupt current planning (or execution) or reset the preempt flag (false)


Execution cannot be preempted with this call.

Suggested change

/// interrupt current planning (or execution) or reset the preempt flag (false)

/// interrupt current planning or reset the preempt flag (false)

That would be more clean. We would need a try catch block and catch everything in the plan() to reset the value.

captain-yoshi · 2024-07-16T16:03:20Z

When preempting early, this does not work because the preempt flag is reseted inside the plan() method.

I don't yet understand the race condition. Do you mean, preempt_requested_ may be set before it is initialized in plan() and then the request is lost?

Yes exactly.

captain-yoshi · 2024-07-16T16:38:00Z

What about resetting the flag when leaving Task::plan()?

@rhaschke This would work in most of the cases (see last commit). But consider the following case:

Thread + Sequence	Method	Info
T1	Task::plan()	The plan finisihes and the flag is resetted.
Main	Task::preempt()	Flag is set to true;
Main	Task::plan()	This planning will not work as the preempted flag is set to true.

So I don't think it can be done it automatically...

EDIT Unless we always need to reset the task between 2 consecutive plan(), we could reset the preempt_requested_ in Task::reset(). This would always work.

rhaschke

I'm curious how you manager to call preempt() before plan() actually started its work.
I do see the theoretical possibility, but practically that shouldn't occur.

rhaschke · 2024-07-16T19:10:14Z

core/test/test_container.cpp

+	t.add(std::make_unique<GeneratorMockup>(PredefinedCosts::constant(0.0)));
+	t.add(std::make_unique<TimedForwardMockup>(timeout));
+
+	// preempt before any stage computation is done


Suggested change

// preempt before any stage computation is done

// preempt before preempt_request_ is reset in plan()

rhaschke · 2024-07-16T19:11:20Z

core/test/test_container.cpp

+
+	// preempt before any stage computation is done
+	{
+		std::thread thread{ [&ec, &t] { ec = t.plan(1); } };


What about introducing a small sleep before t.plan() in order to emphasize the issue even more?

rhaschke · 2024-07-16T19:25:36Z

core/src/task.cpp

+		pimpl()->preempt_requested_ = false;
+		throw;  // rethrow the original exception


The try-catch-block is the wrong tool to cleanup before leaving the function scope: it is only considered if an exception is thrown, not when leaving the function via return. Please revert this.
What you need is a scope guard:
https://stackoverflow.com/questions/50182244/simple-way-to-execute-code-at-the-end-of-function-scope

Would using this external library be acceptable ? At a first glance it seems well tested and easy to use.

A custom CMakeLists.txt is used to create the scope_guard libray. The original cmake libray file is only used to setup tests. See ricab/scope_guard#3.

Remove the initialization of preemt_requested_ in Task::plan(). Make preempted request atomic. Added method to reset the preempt request.

captain-yoshi · 2024-07-17T03:50:38Z

I'm curious how you manager to call preempt() before plan() actually started its work.

We started using BehaviorTree.CPP to create MTC tasks. I was adding a test to test the preempting of a BT node when planning. Here is the sample.

<root BTCPP_format="4" >
  <BehaviorTree ID="MainTree">
    <Sequence>
      <Script code="test := true"/>
      <ReactiveSequence name="root">
        <ForceFailure _skipIf="test">
          <Script code=" result:='error' " />
        </ForceFailure>

        <Script code=" result:='error' " _onSuccess="test = false" />
        <PlanMTCTask task="{mtc_task}" max_solutions="1000" />

      </ReactiveSequence>
    </Sequence>
  </BehaviorTree>
</root>

So planning is first started in another thread. Then the main thread will check if the BT node PlanMTCTask is done. After that the ReactiveSequence will halt all children because of ForceFailure triggering on the second pass.

captain-yoshi · 2024-07-17T03:57:04Z

@rhaschke Ready for another review with all changes requested.

This adds an external dependency to the scope_guard library.

rhaschke · 2024-07-17T08:08:25Z

If you agree to my simplification, this is ready to merge.

captain-yoshi · 2024-07-17T12:00:16Z

LGTM and thanks for the review.

rhaschke reviewed Jul 16, 2024

View reviewed changes

captain-yoshi force-pushed the fix-early-preempt branch 4 times, most recently from 6bbd283 to 2b82592 Compare July 16, 2024 18:28

rhaschke reviewed Jul 16, 2024

View reviewed changes

captain-yoshi force-pushed the fix-early-preempt branch from 2b82592 to 2da7ff8 Compare July 16, 2024 21:05

execution cannot be preempted within Task::preempt()

3e003fd

captain-yoshi force-pushed the fix-early-preempt branch from 2da7ff8 to 38a4801 Compare July 17, 2024 03:08

captain-yoshi added 2 commits July 16, 2024 23:19

[test] early preempt lost request

7222752

add scope_guard library as a submodule

d4671ee

A custom CMakeLists.txt is used to create the scope_guard libray. The original cmake libray file is only used to setup tests. See ricab/scope_guard#3.

captain-yoshi force-pushed the fix-early-preempt branch from 38a4801 to 69b7f22 Compare July 17, 2024 03:31

captain-yoshi added 2 commits July 16, 2024 23:33

[fix] early preempt lost request

1c0737f

Remove the initialization of preemt_requested_ in Task::plan(). Make preempted request atomic. Added method to reset the preempt request.

reset the preempt request when leaving Task::plan()

6761c68

captain-yoshi force-pushed the fix-early-preempt branch from 69b7f22 to 6761c68 Compare July 17, 2024 03:33

captain-yoshi mentioned this pull request Jul 17, 2024

Reduce preempt stop time in stages #598

Merged

Simplify use of scope_guard

167a57b

rhaschke approved these changes Jul 17, 2024

View reviewed changes

rhaschke merged commit cd28bdc into moveit:master Jul 17, 2024
7 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fix early preempt when planning #597

Fix early preempt when planning #597

captain-yoshi commented Jul 16, 2024

codecov-commenter commented Jul 16, 2024 •

edited

Loading

rhaschke left a comment

rhaschke left a comment

rhaschke Jul 16, 2024

captain-yoshi Jul 16, 2024

captain-yoshi commented Jul 16, 2024

captain-yoshi commented Jul 16, 2024 •

edited

Loading

rhaschke left a comment

rhaschke Jul 16, 2024

rhaschke Jul 16, 2024

rhaschke Jul 16, 2024

captain-yoshi Jul 16, 2024

captain-yoshi commented Jul 17, 2024

captain-yoshi commented Jul 17, 2024

rhaschke commented Jul 17, 2024

captain-yoshi commented Jul 17, 2024

	/// interrupt current planning (or execution) or reset the preempt flag (false)
	/// interrupt current planning or reset the preempt flag (false)

	// preempt before any stage computation is done
	// preempt before preempt_request_ is reset in plan()

		pimpl()->preempt_requested_ = false;
		throw; // rethrow the original exception

Fix early preempt when planning #597

Fix early preempt when planning #597

Conversation

captain-yoshi commented Jul 16, 2024

codecov-commenter commented Jul 16, 2024 • edited Loading

Codecov Report

rhaschke left a comment

Choose a reason for hiding this comment

rhaschke left a comment

Choose a reason for hiding this comment

rhaschke Jul 16, 2024

Choose a reason for hiding this comment

captain-yoshi Jul 16, 2024

Choose a reason for hiding this comment

captain-yoshi commented Jul 16, 2024

captain-yoshi commented Jul 16, 2024 • edited Loading

rhaschke left a comment

Choose a reason for hiding this comment

rhaschke Jul 16, 2024

Choose a reason for hiding this comment

rhaschke Jul 16, 2024

Choose a reason for hiding this comment

rhaschke Jul 16, 2024

Choose a reason for hiding this comment

captain-yoshi Jul 16, 2024

Choose a reason for hiding this comment

captain-yoshi commented Jul 17, 2024

captain-yoshi commented Jul 17, 2024

rhaschke commented Jul 17, 2024

captain-yoshi commented Jul 17, 2024

codecov-commenter commented Jul 16, 2024 •

edited

Loading

captain-yoshi commented Jul 16, 2024 •

edited

Loading