Skip to content

Commit c3bc675

Browse files
authored
Merge pull request #39 from dv-picknik/fix/20169-finally-halt-cleanup
Fix: Finish Finally cleanup when halted
2 parents 9daa27f + d17c8a5 commit c3bc675

3 files changed

Lines changed: 247 additions & 32 deletions

File tree

‎include/behaviortree_cpp/controls/finally_node.h‎

Lines changed: 20 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -14,18 +14,20 @@ namespace BT
1414
* - If main throws (any type), main is halted, cleanup runs, and then the
1515
* exception is rethrown.
1616
* - The node returns main's status, or FAILURE if cleanup fails.
17-
* - If this node is halted while main is RUNNING, main is halted and cleanup
18-
* is ticked once, synchronously, on the thread calling halt(). Halt-time
19-
* cleanup must therefore be synchronous: if it returns RUNNING, it is halted.
20-
* A slow cleanup delays the parent, for example a ReactiveSequence whose
21-
* condition changed. Call halt() from the thread that ticks the tree.
22-
* - If this node is halted while cleanup is RUNNING, cleanup is halted and
23-
* does not finish.
24-
* - Exceptions thrown by cleanup propagate from tick(), and the next tick
25-
* retries cleanup. If the tree is halted instead, cleanup is not retried.
26-
* - halt() never throws, because it also runs from ~Tree(). It prints
27-
* exceptions from cleanup or from halting a child, and a cleanup FAILURE,
28-
* to stderr.
17+
* - If this node is halted while main or cleanup is RUNNING, main is halted
18+
* and cleanup is ticked again every 10 ms on the thread calling halt(), so
19+
* asynchronous cleanup can finish. This stops when cleanup finishes, fails
20+
* or throws, or when "halt_timeout_msec" runs out, in which case cleanup is
21+
* halted unfinished. Cleanup always gets at least one tick, no later tick
22+
* starts after the timeout, and a tick that blocks is not cut short. halt() blocks its caller, and any lock the
23+
* caller holds, until then, so a slow cleanup also delays a parent such as
24+
* a ReactiveSequence. Call halt() from the thread that ticks the tree.
25+
* - An exception thrown by cleanup in tick() halts cleanup, leaves the node
26+
* IDLE and propagates. It replaces main's status, or main's exception,
27+
* which is dropped. The next tick starts with main.
28+
* - halt() never throws, because it also runs from ~Tree(). It prints to
29+
* stderr when cleanup throws, fails or times out, and when halting a child
30+
* throws. A pending exception from main is dropped by a halt.
2931
*
3032
* Requires exactly 2 children, checked when the XML is loaded and on tick.
3133
*/
@@ -43,17 +45,22 @@ class FinallyNode : public ControlNode
4345

4446
static PortsList providedPorts()
4547
{
46-
return {};
48+
return { InputPort<unsigned>("halt_timeout_msec", kDefaultHaltTimeoutMsec,
49+
"When halted, how long to keep ticking cleanup "
50+
"before halting it unfinished, in milliseconds") };
4751
}
4852

4953
void halt() override;
5054

5155
private:
56+
static constexpr unsigned kDefaultHaltTimeoutMsec = 10000;
57+
5258
bool in_cleanup_ = false;
5359
NodeStatus main_status_ = NodeStatus::IDLE;
5460
std::exception_ptr main_exception_;
5561

5662
void haltChildNoThrow(size_t i);
63+
void finishCleanupDuringHalt();
5764

5865
BT::NodeStatus tick() override;
5966
};

‎src/controls/finally_node.cpp‎

Lines changed: 73 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,18 @@
11
#include "behaviortree_cpp/controls/finally_node.h"
22

3+
#include <chrono>
34
#include <exception>
45
#include <iostream>
56
#include <string_view>
7+
#include <thread>
68
#include <utility>
79

810
namespace BT
911
{
1012
namespace
1113
{
14+
constexpr std::chrono::milliseconds kHaltTickPeriod{ 10 };
15+
1216
void printException(std::string_view node_name, const char* context,
1317
const std::exception_ptr& exception)
1418
{
@@ -37,21 +41,13 @@ FinallyNode::FinallyNode(const std::string& name, const NodeConfig& config)
3741
void FinallyNode::halt()
3842
{
3943
// halt() also runs from ~Tree(), where a propagating exception terminates, so nothing here throws.
40-
if(!in_cleanup_ && status() == NodeStatus::RUNNING && children_nodes_.size() == 2)
44+
if(status() == NodeStatus::RUNNING && children_nodes_.size() == 2)
4145
{
42-
haltChildNoThrow(0);
43-
try
44-
{
45-
if(children_nodes_[1]->executeTick() == NodeStatus::FAILURE)
46-
{
47-
std::cerr << "[" << name() << "]: cleanup returned FAILURE during halt"
48-
<< std::endl;
49-
}
50-
}
51-
catch(...)
46+
if(!in_cleanup_)
5247
{
53-
printException(name(), "cleanup threw during halt", std::current_exception());
48+
haltChildNoThrow(0);
5449
}
50+
finishCleanupDuringHalt();
5551
}
5652
for(size_t i = 0; i < children_nodes_.size(); i++)
5753
{
@@ -63,6 +59,51 @@ void FinallyNode::halt()
6359
resetStatus();
6460
}
6561

62+
void FinallyNode::finishCleanupDuringHalt()
63+
{
64+
unsigned timeout_msec = kDefaultHaltTimeoutMsec;
65+
if(auto input = getInput<unsigned>("halt_timeout_msec"))
66+
{
67+
timeout_msec = input.value();
68+
}
69+
else
70+
{
71+
std::cerr << "[" << name() << "]: cannot read halt_timeout_msec (" << input.error()
72+
<< "), using " << kDefaultHaltTimeoutMsec << " ms" << std::endl;
73+
}
74+
const auto deadline =
75+
std::chrono::steady_clock::now() + std::chrono::milliseconds(timeout_msec);
76+
try
77+
{
78+
// Cleanup is commonly asynchronous, so keep ticking it until it finishes rather than
79+
// halting it after its first tick.
80+
NodeStatus cleanup_status = children_nodes_[1]->executeTick();
81+
while(cleanup_status == NodeStatus::RUNNING)
82+
{
83+
// Start no tick after the deadline, so a tick that blocks cannot extend the wait.
84+
const auto next_tick = std::chrono::steady_clock::now() + kHaltTickPeriod;
85+
if(next_tick > deadline)
86+
{
87+
std::cerr << "[" << name()
88+
<< "]: cleanup did not finish within halt_timeout_msec ("
89+
<< timeout_msec << " ms), halting it unfinished" << std::endl;
90+
return;
91+
}
92+
std::this_thread::sleep_until(next_tick);
93+
cleanup_status = children_nodes_[1]->executeTick();
94+
}
95+
if(cleanup_status == NodeStatus::FAILURE)
96+
{
97+
std::cerr << "[" << name() << "]: cleanup returned FAILURE during halt"
98+
<< std::endl;
99+
}
100+
}
101+
catch(...)
102+
{
103+
printException(name(), "cleanup threw during halt", std::current_exception());
104+
}
105+
}
106+
66107
void FinallyNode::haltChildNoThrow(size_t i)
67108
{
68109
try
@@ -114,7 +155,26 @@ NodeStatus FinallyNode::tick()
114155
in_cleanup_ = true;
115156
}
116157

117-
const NodeStatus cleanup_status = children_nodes_[1]->executeTick();
158+
NodeStatus cleanup_status = NodeStatus::IDLE;
159+
try
160+
{
161+
cleanup_status = children_nodes_[1]->executeTick();
162+
}
163+
catch(...)
164+
{
165+
// As in a finally block, the exception replaces main's outcome and ends this run, so a later
166+
// halt, such as the one ~Tree() runs, does not retry cleanup.
167+
// Main finished but was not reset yet. haltChildNoThrow() resets both without throwing, so the
168+
// cleanup exception is the one that propagates.
169+
for(size_t i = 0; i < children_nodes_.size(); i++)
170+
{
171+
haltChildNoThrow(i);
172+
}
173+
in_cleanup_ = false;
174+
main_exception_ = nullptr;
175+
resetStatus();
176+
throw;
177+
}
118178
if(cleanup_status == NodeStatus::RUNNING)
119179
{
120180
return NodeStatus::RUNNING;

0 commit comments

Comments
 (0)