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
810namespace BT
911{
1012namespace
1113{
14+ constexpr std::chrono::milliseconds kHaltTickPeriod { 10 };
15+
1216void 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)
3741void 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+
66107void 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